Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : main logic for servo control
17 : */
18 :
19 : #include "Plane.h"
20 : #include <utility>
21 :
22 : /*****************************************
23 : * Throttle slew limit
24 : *****************************************/
25 8377244 : void Plane::throttle_slew_limit()
26 : {
27 : #if HAL_QUADPLANE_ENABLED
28 8377244 : const bool do_throttle_slew = (control_mode->does_auto_throttle() || quadplane.in_assisted_flight() || quadplane.in_vtol_mode());
29 : #else
30 : const bool do_throttle_slew = control_mode->does_auto_throttle();
31 : #endif
32 :
33 8377244 : if (!do_throttle_slew) {
34 : // only do throttle slew limiting in modes where throttle control is automatic
35 1661897 : SRV_Channels::set_slew_rate(SRV_Channel::k_throttle, 0.0, 100, G_Dt);
36 1661897 : SRV_Channels::set_slew_rate(SRV_Channel::k_throttleLeft, 0.0, 100, G_Dt);
37 1661897 : SRV_Channels::set_slew_rate(SRV_Channel::k_throttleRight, 0.0, 100, G_Dt);
38 1661897 : return;
39 : }
40 :
41 6715347 : uint16_t slewrate = aparm.throttle_slewrate;
42 6715347 : if (control_mode == &mode_auto) {
43 3603874 : if (auto_state.takeoff_complete == false && g.takeoff_throttle_slewrate != 0) {
44 0 : slewrate = g.takeoff_throttle_slewrate;
45 3603874 : } else if (landing.get_throttle_slewrate() != 0 && flight_stage == AP_FixedWing::FlightStage::LAND) {
46 0 : slewrate = landing.get_throttle_slewrate();
47 : }
48 : }
49 6715347 : if (g.takeoff_throttle_slewrate != 0 &&
50 0 : (flight_stage == AP_FixedWing::FlightStage::TAKEOFF ||
51 0 : flight_stage == AP_FixedWing::FlightStage::VTOL)) {
52 : // for VTOL we use takeoff slewrate, which helps with transition
53 0 : slewrate = g.takeoff_throttle_slewrate;
54 : }
55 : #if HAL_QUADPLANE_ENABLED
56 6715347 : if (g.takeoff_throttle_slewrate != 0 && quadplane.in_frwd_transition()) {
57 0 : slewrate = g.takeoff_throttle_slewrate;
58 : }
59 : #endif
60 6715347 : SRV_Channels::set_slew_rate(SRV_Channel::k_throttle, slewrate, 100, G_Dt);
61 6715347 : SRV_Channels::set_slew_rate(SRV_Channel::k_throttleLeft, slewrate, 100, G_Dt);
62 6715347 : SRV_Channels::set_slew_rate(SRV_Channel::k_throttleRight, slewrate, 100, G_Dt);
63 : }
64 :
65 : /* We want to suppress the throttle if we think we are on the ground and in an autopilot controlled throttle mode.
66 :
67 : Disable throttle if following conditions are met:
68 : * 1 - We are in Circle mode (which we use for short term failsafe), or in FBW-B or higher
69 : * AND
70 : * 2 - Our reported altitude is within 10 meters of the home altitude.
71 : * 3 - Our reported speed is under 5 meters per second.
72 : * 4 - We are not performing a takeoff in Auto mode or takeoff speed/accel not yet reached
73 : * OR
74 : * 5 - Home location is not set
75 : * OR
76 : * 6- Landing does not want to allow throttle
77 : */
78 8377244 : bool Plane::suppress_throttle(void)
79 : {
80 8377244 : if (control_mode == &mode_manual) {
81 : // Throttle is never suppressed in manual mode
82 613998 : return false;
83 : }
84 :
85 : #if HAL_PARACHUTE_ENABLED
86 7763246 : if (control_mode->does_auto_throttle() && parachute.release_initiated()) {
87 : // throttle always suppressed in auto-throttle modes after parachute release initiated
88 246 : throttle_suppressed = true;
89 246 : return true;
90 : }
91 : #endif
92 :
93 7763000 : if (landing.is_throttle_suppressed()) {
94 123870 : return true;
95 : }
96 :
97 7639130 : if (!throttle_suppressed) {
98 : // we've previously met a condition for unsupressing the throttle
99 7412472 : return false;
100 : }
101 226658 : if (!control_mode->does_auto_throttle()) {
102 : // the user controls the throttle
103 0 : throttle_suppressed = false;
104 0 : return false;
105 : }
106 :
107 226658 : bool gps_movement = (gps.status() >= AP_GPS_FixType::FIX_2D && gps.ground_speed() >= 5);
108 :
109 226658 : if ((control_mode == &mode_auto &&
110 140578 : auto_state.takeoff_complete == false) ||
111 218617 : control_mode == &mode_takeoff) {
112 :
113 36959 : uint32_t launch_duration_ms = ((int32_t)g.takeoff_throttle_delay)*100 + 2000;
114 36959 : if (is_flying() &&
115 220 : millis() - started_flying_ms > MAX(launch_duration_ms, 5000U) && // been flying >5s in any mode
116 66 : adjusted_relative_altitude_cm() > 500 && // are >5m above AGL/home
117 37113 : fabsf(ahrs.get_pitch_deg()) < 30 && // not high pitch, which happens when held before launch
118 : gps_movement) { // definite gps movement
119 : // we're already flying, do not suppress the throttle. We can get
120 : // stuck in this condition if we reset a mission and cmd 1 is takeoff
121 : // but we're currently flying around below the takeoff altitude
122 65 : throttle_suppressed = false;
123 65 : return false;
124 : }
125 36894 : if (auto_takeoff_check()) {
126 : // we're in auto takeoff
127 102 : throttle_suppressed = false;
128 102 : auto_state.baro_takeoff_alt = barometer.get_altitude();
129 102 : return false;
130 : }
131 : // keep throttle suppressed
132 36792 : return true;
133 : }
134 :
135 189699 : if (fabsf(relative_altitude) >= 10.0f) {
136 : // we're more than 10m from the home altitude
137 321 : throttle_suppressed = false;
138 321 : return false;
139 : }
140 :
141 189378 : if (gps_movement) {
142 : // if we have an airspeed sensor, then check it too, and
143 : // require 5m/s. This prevents throttle up due to spiky GPS
144 : // groundspeed with bad GPS reception
145 : #if AP_AIRSPEED_ENABLED
146 414 : if ((!ahrs.using_airspeed_sensor()) || airspeed.get_airspeed() >= 5) {
147 : // we're moving at more than 5 m/s
148 2 : throttle_suppressed = false;
149 2 : return false;
150 : }
151 : #else
152 : // no airspeed sensor, so we trust that the GPS's movement is truthful
153 : throttle_suppressed = false;
154 : return false;
155 : #endif
156 : }
157 :
158 : #if HAL_QUADPLANE_ENABLED
159 189376 : if (quadplane.is_flying()) {
160 50 : throttle_suppressed = false;
161 50 : return false;
162 : }
163 : #endif
164 :
165 : // throttle remains suppressed
166 189326 : return true;
167 : }
168 :
169 :
170 : /*
171 : mixer for elevon and vtail channels setup using designated servo
172 : function values. This mixer operates purely on scaled values,
173 : allowing the user to trim and limit individual servos using the
174 : SERVOn_* parameters
175 : */
176 16877342 : void Plane::channel_function_mixer(SRV_Channel::Function func1_in, SRV_Channel::Function func2_in,
177 : SRV_Channel::Function func1_out, SRV_Channel::Function func2_out) const
178 : {
179 : // the order is setup so that non-reversed servos go "up", and
180 : // func1 is the "left" channel. Users can adjust with channel
181 : // reversal as needed
182 16877342 : float in1 = SRV_Channels::get_output_scaled(func1_in);
183 16877342 : float in2 = SRV_Channels::get_output_scaled(func2_in);
184 :
185 : // apply MIXING_OFFSET to input channels
186 16877342 : if (g.mixing_offset < 0) {
187 0 : in2 *= (100 - g.mixing_offset) * 0.01;
188 16877342 : } else if (g.mixing_offset > 0) {
189 0 : in1 *= (100 + g.mixing_offset) * 0.01;
190 : }
191 :
192 16877342 : float out1 = constrain_float((in2 - in1) * g.mixing_gain, -4500, 4500);
193 16877342 : float out2 = constrain_float((in2 + in1) * g.mixing_gain, -4500, 4500);
194 16877342 : SRV_Channels::set_output_scaled(func1_out, out1);
195 16877342 : SRV_Channels::set_output_scaled(func2_out, out2);
196 16877342 : }
197 :
198 :
199 : /*
200 : setup flaperon output channels
201 : */
202 8437677 : void Plane::flaperon_update()
203 : {
204 : /*
205 : flaperons are implemented as a mixer between aileron and a
206 : percentage of flaps. Flap input can come from a manual channel
207 : or from auto flaps.
208 : */
209 8437677 : float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
210 8437677 : float flap_percent = SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto);
211 8437677 : float flaperon_left = constrain_float(aileron + flap_percent * 45, -4500, 4500);
212 8437677 : float flaperon_right = constrain_float(aileron - flap_percent * 45, -4500, 4500);
213 8437677 : SRV_Channels::set_output_scaled(SRV_Channel::k_flaperon_left, flaperon_left);
214 8437677 : SRV_Channels::set_output_scaled(SRV_Channel::k_flaperon_right, flaperon_right);
215 8437677 : }
216 :
217 :
218 : /*
219 : setup differential spoiler output channels
220 :
221 : Differential spoilers are a type of elevon that is split on each
222 : wing to give yaw control, mixed from rudder
223 : */
224 8438671 : void Plane::dspoiler_update(void)
225 : {
226 8438671 : const int8_t bitmask = g2.crow_flap_options.get();
227 8438671 : const bool flying_wing = (bitmask & CrowFlapOptions::FLYINGWING) != 0;
228 8438671 : const bool full_span_aileron = (bitmask & CrowFlapOptions::FULLSPAN) != 0;
229 : //progressive crow when option is set or RC switch is set to progressive
230 8438671 : const bool progressive_crow = (bitmask & CrowFlapOptions::PROGRESSIVE_CROW) != 0 || crow_mode == CrowMode::PROGRESSIVE;
231 :
232 : // if flying wing use elevons else use ailerons
233 : float elevon_left;
234 : float elevon_right;
235 8438671 : if (flying_wing) {
236 8438671 : elevon_left = SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_left);
237 8438671 : elevon_right = SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_right);
238 : } else {
239 0 : const float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
240 0 : elevon_left = -aileron;
241 0 : elevon_right = aileron;
242 : }
243 :
244 8438671 : const float rudder_rate = g.dspoiler_rud_rate * 0.01f;
245 8438671 : const float rudder = SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) * rudder_rate;
246 8438671 : float dspoiler_outer_left = elevon_left;
247 8438671 : float dspoiler_outer_right = elevon_right;
248 :
249 8438671 : float dspoiler_inner_left = 0;
250 8438671 : float dspoiler_inner_right = 0;
251 :
252 : // full span ailerons / elevons
253 8438671 : if (full_span_aileron) {
254 8438671 : dspoiler_inner_left = elevon_left;
255 8438671 : dspoiler_inner_right = elevon_right;
256 : }
257 :
258 8438671 : if (rudder > 0) {
259 : // apply rudder to right wing
260 2625312 : dspoiler_outer_right = constrain_float(dspoiler_outer_right + rudder, -4500, 4500);
261 2625312 : dspoiler_inner_right = constrain_float(dspoiler_inner_right - rudder, -4500, 4500);
262 : } else {
263 : // apply rudder to left wing
264 5813359 : dspoiler_outer_left = constrain_float(dspoiler_outer_left - rudder, -4500, 4500);
265 5813359 : dspoiler_inner_left = constrain_float(dspoiler_inner_left + rudder, -4500, 4500);
266 : }
267 :
268 : // limit flap throw used for aileron
269 8438671 : const int8_t aileron_matching = g2.crow_flap_aileron_matching.get();
270 8438671 : if (aileron_matching < 100) {
271 : // only do matching if it will make a difference
272 0 : const float aileron_matching_scaled = aileron_matching * 0.01;
273 0 : if (is_negative(dspoiler_inner_left)) {
274 0 : dspoiler_inner_left *= aileron_matching_scaled;
275 : }
276 0 : if (is_negative(dspoiler_inner_right)) {
277 0 : dspoiler_inner_right *= aileron_matching_scaled;
278 : }
279 : }
280 :
281 8438671 : int16_t weight_outer = g2.crow_flap_weight_outer.get();
282 8438671 : if (crow_mode == Plane::CrowMode::CROW_DISABLED) { //override totally aileron crow if crow RC switch set to disabled
283 0 : weight_outer = 0;
284 : }
285 8438671 : const int16_t weight_inner = g2.crow_flap_weight_inner.get();
286 8438671 : if (weight_outer > 0 || weight_inner > 0) {
287 : /*
288 : apply crow flaps by apply the same split of the differential
289 : spoilers to both wings. Get flap percentage from k_flap_auto, which is set
290 : in set_servos_flaps() as the maximum of manual and auto flap control
291 : */
292 0 : const float flap_percent = SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto);
293 :
294 0 : if (is_positive(flap_percent)) {
295 0 : float inner_flap_scaled = flap_percent;
296 0 : float outer_flap_scaled = flap_percent;
297 0 : if (progressive_crow) {
298 : // apply 0 - full inner from 0 to 50% flap then add in outer above 50%
299 0 : inner_flap_scaled = constrain_float(inner_flap_scaled * 2, 0,100);
300 0 : outer_flap_scaled = constrain_float(outer_flap_scaled - 50, 0,50) * 2;
301 : }
302 : // scale flaps so when weights are 100 they give full up or down
303 0 : dspoiler_outer_left = constrain_float(dspoiler_outer_left + outer_flap_scaled * weight_outer * 0.45, -4500, 4500);
304 0 : dspoiler_inner_left = constrain_float(dspoiler_inner_left - inner_flap_scaled * weight_inner * 0.45, -4500, 4500);
305 0 : dspoiler_outer_right = constrain_float(dspoiler_outer_right + outer_flap_scaled * weight_outer * 0.45, -4500, 4500);
306 0 : dspoiler_inner_right = constrain_float(dspoiler_inner_right - inner_flap_scaled * weight_inner * 0.45, -4500, 4500);
307 : }
308 : }
309 :
310 8438671 : SRV_Channels::set_output_scaled(SRV_Channel::k_dspoilerLeft1, dspoiler_outer_left);
311 8438671 : SRV_Channels::set_output_scaled(SRV_Channel::k_dspoilerLeft2, dspoiler_inner_left);
312 8438671 : SRV_Channels::set_output_scaled(SRV_Channel::k_dspoilerRight1, dspoiler_outer_right);
313 8438671 : SRV_Channels::set_output_scaled(SRV_Channel::k_dspoilerRight2, dspoiler_inner_right);
314 8438671 : }
315 :
316 : /*
317 : set airbrakes based on reverse thrust and/or manual input RC channel
318 : */
319 8377244 : void Plane::airbrake_update(void)
320 : {
321 : // Calculate any manual airbrake input from RC channel option.
322 8377244 : float manual_airbrake_percent = 0;
323 :
324 8377244 : if (channel_airbrake != nullptr && !failsafe.rc_failsafe && failsafe.throttle_counter == 0) {
325 0 : manual_airbrake_percent = channel_airbrake->percent_input();
326 : }
327 :
328 : // Calculate auto airbrake from negative throttle.
329 8377244 : float throttle_min = aparm.throttle_min.get();
330 8377244 : float airbrake_pc = 0;
331 :
332 8377244 : float throttle_pc = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
333 :
334 8377244 : if (throttle_min < 0) {
335 0 : if (landing.is_flaring()) {
336 : // Full airbrakes during the flare.
337 0 : airbrake_pc = 100;
338 : }
339 : else {
340 : // Determine fraction between zero and full negative throttle.
341 0 : airbrake_pc = constrain_float(-throttle_pc, 0, 100);
342 : }
343 : }
344 :
345 : // Manual overrides auto airbrake setting.
346 8377244 : if (airbrake_pc < manual_airbrake_percent) {
347 0 : airbrake_pc = manual_airbrake_percent;
348 : }
349 :
350 : // Output to airbrake servo types.
351 8377244 : SRV_Channels::set_output_scaled(SRV_Channel::k_airbrake, airbrake_pc);
352 8377244 : }
353 :
354 : /*
355 : setup servos for idle wiggle mode
356 : Idle mode is used during balloon launch to keep servos still, apart
357 : from occasional wiggle to prevent freezing up
358 : */
359 199220 : void ModeAuto::wiggle_servos()
360 : {
361 : // This is only active while in AUTO running NAV_ALTITUDE_WAIT with wiggle_time > 0
362 199220 : if (wiggle.last_ms == 0) {
363 5198 : return;
364 : }
365 :
366 : int16_t servo_valueElevator;
367 : int16_t servo_valueAileronRudder;
368 : // Wiggle the control surfaces in stages: elevators first, then rudders + ailerons, through the full range over 4 seconds
369 194022 : if (wiggle.stage != 0) {
370 6766 : wiggle.stage += 1;
371 : }
372 194022 : if (wiggle.stage == 0) {
373 187256 : servo_valueElevator = 0;
374 187256 : servo_valueAileronRudder = 0;
375 6766 : } else if (wiggle.stage < 25) {
376 782 : servo_valueElevator = wiggle.stage * (4500 / 25);
377 782 : servo_valueAileronRudder = 0;
378 5984 : } else if (wiggle.stage < 75) {
379 1700 : servo_valueElevator = (50 - wiggle.stage) * (4500 / 25);
380 1700 : servo_valueAileronRudder = 0;
381 4284 : } else if (wiggle.stage < 100) {
382 850 : servo_valueElevator = (wiggle.stage - 100) * (4500 / 25);
383 850 : servo_valueAileronRudder = 0;
384 3434 : } else if (wiggle.stage < 125) {
385 850 : servo_valueElevator = 0;
386 850 : servo_valueAileronRudder = (wiggle.stage - 100) * (4500 / 25);
387 2584 : } else if (wiggle.stage < 175) {
388 1700 : servo_valueElevator = 0;
389 1700 : servo_valueAileronRudder = (150 - wiggle.stage) * (4500 / 25);
390 884 : } else if (wiggle.stage < 200) {
391 850 : servo_valueElevator = 0;
392 850 : servo_valueAileronRudder = (wiggle.stage - 200) * (4500 / 25);
393 : } else {
394 34 : wiggle.stage = 0;
395 34 : servo_valueElevator = 0;
396 34 : servo_valueAileronRudder = 0;
397 : }
398 194022 : SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, servo_valueAileronRudder);
399 194022 : SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, servo_valueElevator);
400 194022 : SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, servo_valueAileronRudder);
401 :
402 : }
403 :
404 :
405 : /*
406 : Calculate the throttle scale to compensate for battery voltage drop
407 : */
408 8377244 : void ParametersG2::FWD_BATT_CMP::update()
409 : {
410 : // Assume disabled
411 8377244 : enabled = false;
412 :
413 : // return if not enabled, or setup incorrectly
414 8377244 : if (!is_positive(batt_voltage_min) || batt_voltage_min >= batt_voltage_max) {
415 8377244 : return;
416 : }
417 :
418 0 : float batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(batt_idx);
419 : // Return for a very low battery
420 0 : if (batt_voltage_resting_estimate < 0.25f * batt_voltage_min) {
421 0 : return;
422 : }
423 :
424 : // constrain read voltage to min and max params
425 0 : batt_voltage_resting_estimate = constrain_float(batt_voltage_resting_estimate, batt_voltage_min, batt_voltage_max);
426 :
427 : // don't apply compensation if the voltage is excessively low
428 0 : if (batt_voltage_resting_estimate < 1) {
429 0 : return;
430 : }
431 :
432 : // Scale the throttle up to compensate for voltage drop
433 : // Ratio = 1 when voltage = voltage max, ratio increases as voltage drops
434 0 : ratio = batt_voltage_max / batt_voltage_resting_estimate;
435 :
436 : // Got this far then ratio is valid
437 0 : enabled = true;
438 : }
439 :
440 : // Apply throttle scale to min and max limits
441 7338760 : void ParametersG2::FWD_BATT_CMP::apply_min_max(int8_t &min_throttle, int8_t &max_throttle) const
442 : {
443 : // Cut off throttle if FWD_BAT_IDX battery resting voltage is below
444 : // FWD_THR_CUTOFF_V (if set), to preserve battery life for the electronics
445 : // and actuators. Only applies when the battery monitor is working and the
446 : // current mode does auto-throttle.
447 7338760 : if (is_positive(batt_voltage_throttle_cutoff) &&
448 7338760 : plane.control_mode->does_auto_throttle() && AP::battery().healthy(batt_idx) &&
449 0 : (AP::battery().voltage_resting_estimate(batt_idx) < batt_voltage_throttle_cutoff)) {
450 0 : min_throttle = 0;
451 0 : max_throttle = 0;
452 :
453 0 : return;
454 : }
455 :
456 : // return if not enabled
457 7338760 : if (!enabled) {
458 7338760 : return;
459 : }
460 :
461 : // Scale the throttle limits to prevent subsequent clipping
462 : // Ratio will always be >= 1, ensure still within max limits
463 0 : min_throttle = int8_t(MAX((ratio * (float)min_throttle), -100));
464 0 : max_throttle = int8_t(MIN((ratio * (float)max_throttle), 100));
465 :
466 : }
467 :
468 : // Apply throttle scale to throttle demand
469 3895383 : float ParametersG2::FWD_BATT_CMP::apply_throttle(float throttle) const
470 : {
471 : // return if not enabled
472 3895383 : if (!enabled) {
473 3895383 : return throttle;
474 : }
475 :
476 : // Ratio will always be >= 1, ensure still within max limits
477 0 : return constrain_float(throttle * ratio, -100, 100);
478 :
479 : }
480 :
481 : /*
482 : calculate any throttle limits based on the watt limiter
483 : */
484 : #if AP_BATTERY_WATT_MAX_ENABLED
485 7338760 : void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle)
486 : {
487 7338760 : uint32_t now = millis();
488 7338760 : if (battery.overpower_detected()) {
489 : // overpower detected, cut back on the throttle if we're maxing it out by calculating a limiter value
490 : // throttle limit will attack by 10% per second
491 :
492 0 : if (is_positive(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)) && // demanding too much positive thrust
493 0 : throttle_watt_limit_max < max_throttle - 25 &&
494 0 : now - throttle_watt_limit_timer_ms >= 1) {
495 : // always allow for 25% throttle available regardless of battery status
496 0 : throttle_watt_limit_timer_ms = now;
497 0 : throttle_watt_limit_max++;
498 :
499 0 : } else if (is_negative(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)) &&
500 0 : min_throttle < 0 && // reverse thrust is available
501 0 : throttle_watt_limit_min < -(min_throttle) - 25 &&
502 0 : now - throttle_watt_limit_timer_ms >= 1) {
503 : // always allow for 25% throttle available regardless of battery status
504 0 : throttle_watt_limit_timer_ms = now;
505 0 : throttle_watt_limit_min++;
506 : }
507 :
508 7338760 : } else if (now - throttle_watt_limit_timer_ms >= 1000) {
509 : // it has been 1 second since last over-current, check if we can resume higher throttle.
510 : // this throttle release is needed to allow raising the max_throttle as the battery voltage drains down
511 : // throttle limit will release by 1% per second
512 11157204 : if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > throttle_watt_limit_max && // demanding max forward thrust
513 3818444 : throttle_watt_limit_max > 0) { // and we're currently limiting it
514 0 : throttle_watt_limit_timer_ms = now;
515 0 : throttle_watt_limit_max--;
516 :
517 7338760 : } else if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < throttle_watt_limit_min && // demanding max negative thrust
518 0 : throttle_watt_limit_min > 0) { // and we're limiting it
519 0 : throttle_watt_limit_timer_ms = now;
520 0 : throttle_watt_limit_min--;
521 : }
522 : }
523 :
524 7338760 : max_throttle = constrain_int16(max_throttle, 0, max_throttle - throttle_watt_limit_max);
525 7338760 : if (min_throttle < 0) {
526 0 : min_throttle = constrain_int16(min_throttle, min_throttle + throttle_watt_limit_min, 0);
527 : }
528 7338760 : }
529 : #endif // #if AP_BATTERY_WATT_MAX_ENABLED
530 :
531 : /*
532 : Apply min/max safety limits to throttle.
533 : */
534 7338760 : float Plane::apply_throttle_limits(float throttle_in)
535 : {
536 : // Pull the base throttle limits.
537 : // These are usually set to map the ESC operating range.
538 7338760 : int8_t min_throttle = aparm.throttle_min.get();
539 7338760 : int8_t max_throttle = aparm.throttle_max.get();
540 :
541 : #if AP_ICENGINE_ENABLED
542 : // Get the idle throttle (parameter or idle governor) from AP_ICEngine
543 7338760 : min_throttle = MAX(min_throttle, g2.ice_control.get_min_throttle_pct());
544 : #endif
545 :
546 : // If reverse thrust is enabled not allowed right now, the minimum throttle must not fall below 0.
547 7338760 : if (min_throttle < 0 && !allow_reverse_thrust()) {
548 : // reverse thrust is available but inhibited.
549 0 : min_throttle = 0;
550 : }
551 :
552 : // Handle throttle limits for takeoff conditions.
553 : // Query the conditions where TKOFF_THR_MAX applies.
554 7338760 : const bool use_takeoff_throttle =
555 14615651 : (flight_stage == AP_FixedWing::FlightStage::TAKEOFF) ||
556 7276891 : (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING);
557 :
558 : // Handle throttle limits for takeoff conditions.
559 7338760 : if (use_takeoff_throttle) {
560 : // Read from takeoff_state
561 63104 : max_throttle = takeoff_state.throttle_lim_max;
562 63104 : min_throttle = takeoff_state.throttle_lim_min;
563 7275656 : } else if (landing.is_flaring()) {
564 : // Allow throttle cutoff when flaring.
565 : // This is to allow the aircraft to bleed speed faster and land with a shut off thruster.
566 123090 : min_throttle = 0;
567 : }
568 :
569 : // Handle throttle limits for transition conditions.
570 : #if HAL_QUADPLANE_ENABLED
571 7338760 : if (quadplane.in_frwd_transition()) {
572 467918 : if (aparm.takeoff_throttle_max != 0) {
573 0 : max_throttle = aparm.takeoff_throttle_max.get();
574 : }
575 :
576 : // Apply minimum throttle limits only for SLT thrust types.
577 : // The other types don't support it well.
578 467918 : if (quadplane.get_thrust_type() == QuadPlane::ThrustType::SLT
579 467918 : && control_mode->does_auto_throttle()
580 : ) {
581 290609 : if (aparm.takeoff_throttle_min.get() != 0) {
582 3061 : min_throttle = MAX(min_throttle, aparm.takeoff_throttle_min.get());
583 : } else {
584 287548 : min_throttle = MAX(min_throttle, aparm.throttle_cruise.get());
585 : }
586 : }
587 : }
588 : #endif
589 :
590 : // Compensate the limits for battery voltage drop.
591 : // This relaxes the limits when the battery is getting depleted.
592 7338760 : g2.fwd_batt_cmp.apply_min_max(min_throttle, max_throttle);
593 :
594 : #if AP_BATTERY_WATT_MAX_ENABLED
595 : // Ensure that the power draw limits are not exceeded.
596 7338760 : throttle_watt_limiter(min_throttle, max_throttle);
597 : #endif
598 :
599 : // Do a sanity check on them. Constrain down if necessary.
600 7338760 : min_throttle = MIN(min_throttle, max_throttle);
601 :
602 : // Let TECS know about the updated throttle limits.
603 : // These will be taken into account on the next iteration.
604 7338760 : TECS_controller.set_throttle_min(0.01f*min_throttle);
605 7338760 : TECS_controller.set_throttle_max(0.01f*max_throttle);
606 7338760 : return constrain_float(throttle_in, min_throttle, max_throttle);
607 : }
608 :
609 : /*
610 : setup output channels all non-manual modes
611 : */
612 8377244 : void Plane::set_throttle(void)
613 : {
614 :
615 : // Update voltage scaling
616 8377244 : g2.fwd_batt_cmp.update();
617 :
618 8377244 : if (control_mode->use_battery_compensation()) {
619 : // Apply voltage compensation to throttle output from flight mode
620 3895383 : const float throttle = g2.fwd_batt_cmp.apply_throttle(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle));
621 3895383 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
622 : }
623 :
624 8377244 : if (control_mode->use_throttle_limits()) {
625 : // Apply min/max throttle limits
626 7338760 : const float limited_throttle = apply_throttle_limits(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle));
627 7338760 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, limited_throttle);
628 : }
629 :
630 8377244 : if (suppress_throttle()) {
631 350234 : if (g.throttle_suppress_manual) {
632 : // manual pass through of throttle while throttle is suppressed
633 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));
634 :
635 350234 : } else if (landing.is_flaring() && landing.use_thr_min_during_flare() ) {
636 : // throttle is suppressed (above) to zero in final flare in auto mode, but we allow instead thr_min if user prefers, eg turbines:
637 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, aparm.throttle_min.get());
638 :
639 700468 : } else if ((flight_stage == AP_FixedWing::FlightStage::TAKEOFF)
640 350234 : && (aparm.takeoff_throttle_idle.get() > 0)
641 : ) {
642 : // we want to spin at idle throttle before the takeoff conditions are met
643 661 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, aparm.takeoff_throttle_idle.get());
644 : } else {
645 : // default
646 349573 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
647 :
648 : }
649 : }
650 :
651 8377244 : }
652 :
653 : /*
654 : Warn AHRS that we might take off soon
655 : */
656 8377244 : void Plane::set_takeoff_expected(void)
657 : {
658 : // let EKF know to start GSF yaw estimator before takeoff movement starts so that yaw angle is better estimated
659 8377244 : const float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
660 8377244 : if (!is_flying() && arming.is_armed()) {
661 : // Check if rate of change of velocity along X axis exceeds 1-g which normally indicates a throw.
662 : // Tests with hand carriage of micro UAS indicates that a 1-g threshold does not false trigger prior
663 : // to the throw, but there is margin to increase this threshold if false triggering becomes problematic.
664 382722 : const float accel_x_due_to_gravity = GRAVITY_MSS * ahrs.sin_pitch();
665 382722 : const float accel_x_due_to_throw = ahrs.get_accel().x - accel_x_due_to_gravity;
666 382722 : bool throw_detected = accel_x_due_to_throw > GRAVITY_MSS;
667 382722 : bool throttle_up_detected = throttle > aparm.throttle_cruise;
668 382722 : if (throw_detected || throttle_up_detected) {
669 28348 : plane.ahrs.set_takeoff_expected(true);
670 : }
671 : }
672 8377244 : }
673 :
674 : // Return the speed which should be used for auto flap deployment calculation
675 5388522 : float Plane::get_auto_flap_speed() const
676 : {
677 : float est_airspeed;
678 5388522 : const auto flap_actual_speed = flight_option_enabled(FlightOptions::FLAP_ACTUAL_SPEED) && ahrs.airspeed_EAS(est_airspeed) && TECS_controller.use_airspeed();
679 5388522 : const bool has_target_airspeed = control_mode->does_auto_throttle() && (target_airspeed_cm > 0);
680 :
681 : // Use airspeed if enabled and available
682 5388522 : if (flap_actual_speed) {
683 : // If there is a target airspeed return the smaller of the measured and the target
684 : // This means the flaps are deployed in anticipation of slowing down
685 45 : if (has_target_airspeed) {
686 9 : return MIN(est_airspeed, target_airspeed_cm * 0.01f);
687 : }
688 :
689 : // No target, use measurement only
690 36 : return est_airspeed;
691 : }
692 :
693 : // If there is a target airspeed use it
694 5388477 : if (has_target_airspeed) {
695 5388442 : return target_airspeed_cm * 0.01f;
696 : }
697 :
698 : // Default to cruise speed
699 35 : return aparm.airspeed_cruise.get();
700 : }
701 :
702 : /*
703 : setup flap outputs
704 : */
705 8377244 : void Plane::set_servos_flaps(void)
706 : {
707 : // Auto flap deployment
708 8377244 : int8_t auto_flap_percent = 0;
709 8377244 : int8_t manual_flap_percent = 0;
710 :
711 : // work out any manual flap input
712 8377244 : if (channel_flap != nullptr && rc().has_valid_input()) {
713 13279 : manual_flap_percent = channel_flap->percent_input();
714 : }
715 :
716 8377244 : const auto autoflap_in_manual = flight_option_enabled(FlightOptions::FLAP_ACTUAL_SPEED);
717 8377244 : const bool has_target_airspeed = control_mode->does_auto_throttle();
718 8377244 : if (has_target_airspeed || autoflap_in_manual) {
719 5388522 : const float flapSpeedSource = get_auto_flap_speed();
720 5388522 : if (g.flap_2_speed != 0 && flapSpeedSource <= g.flap_2_speed) {
721 25 : auto_flap_percent = g.flap_2_percent;
722 5388497 : } else if ( g.flap_1_speed != 0 && flapSpeedSource <= g.flap_1_speed) {
723 3161 : auto_flap_percent = g.flap_1_percent;
724 : } //else flaps stay at default zero deflection
725 :
726 : #if HAL_SOARING_ENABLED
727 5388522 : if (control_mode == &mode_thermal) {
728 13618 : auto_flap_percent = g2.soaring_controller.get_thermalling_flap();
729 : }
730 : #endif
731 :
732 : /*
733 : special flap levels for takeoff and landing. This works
734 : better than speed based flaps as it leads to less
735 : possibility of oscillation
736 : */
737 5388522 : switch (flight_stage) {
738 63104 : case AP_FixedWing::FlightStage::TAKEOFF:
739 : case AP_FixedWing::FlightStage::ABORT_LANDING:
740 63104 : if (g.takeoff_flap_percent != 0) {
741 0 : auto_flap_percent = g.takeoff_flap_percent;
742 : }
743 63104 : break;
744 4002630 : case AP_FixedWing::FlightStage::NORMAL:
745 4002630 : if (g.takeoff_flap_percent != 0 && in_preLaunch_flight_stage()) {
746 : // TODO: move this to a new FLIGHT_PRE_TAKEOFF stage
747 0 : auto_flap_percent = g.takeoff_flap_percent;
748 : }
749 4002630 : break;
750 197641 : case AP_FixedWing::FlightStage::LAND:
751 197641 : if (landing.get_flap_percent() != 0) {
752 1420 : auto_flap_percent = landing.get_flap_percent();
753 : }
754 197641 : break;
755 1125147 : default:
756 1125147 : break;
757 : }
758 : }
759 :
760 : // manual flap input overrides auto flap input
761 8377244 : if (abs(manual_flap_percent) > auto_flap_percent) {
762 91 : auto_flap_percent = manual_flap_percent;
763 : }
764 :
765 8377244 : SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, auto_flap_percent);
766 8377244 : SRV_Channels::set_output_scaled(SRV_Channel::k_flap, manual_flap_percent);
767 :
768 8377244 : SRV_Channels::set_slew_rate(SRV_Channel::k_flap_auto, g.flap_slewrate, 100, G_Dt);
769 8377244 : SRV_Channels::set_slew_rate(SRV_Channel::k_flap, g.flap_slewrate, 100, G_Dt);
770 :
771 : // output to flaperons, if any
772 8377244 : flaperon_update();
773 8377244 : }
774 :
775 : /*
776 : support for twin-engine planes
777 : */
778 8438671 : void Plane::servos_twin_engine_mix(void)
779 : {
780 8438671 : float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
781 8438671 : float rud_gain = float(plane.g2.rudd_dt_gain) * 0.01f;
782 8438671 : rudder_dt = rud_gain * SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) / SERVO_MAX;
783 :
784 : #if AP_ADVANCEDFAILSAFE_ENABLED
785 8438671 : if (afs.should_crash_vehicle()) {
786 : // when in AFS failsafe force rudder input for differential thrust to zero
787 303 : rudder_dt = 0;
788 : }
789 : #endif
790 :
791 : float throttle_left, throttle_right;
792 :
793 8438671 : if (throttle < 0 && have_reverse_thrust() && allow_reverse_thrust()) {
794 : // doing reverse thrust
795 0 : throttle_left = constrain_float(throttle + 50 * rudder_dt, -100, 0);
796 0 : throttle_right = constrain_float(throttle - 50 * rudder_dt, -100, 0);
797 8438671 : } else if (throttle <= 0) {
798 4736379 : throttle_left = throttle_right = 0;
799 : } else {
800 : // doing forward thrust
801 3702292 : throttle_left = constrain_float(throttle + 50 * rudder_dt, 0, 100);
802 3702292 : throttle_right = constrain_float(throttle - 50 * rudder_dt, 0, 100);
803 : }
804 8438671 : if (!arming.is_armed_and_safety_off()) {
805 1979600 : if (arming.arming_required() == AP_Arming::Required::YES_ZERO_PWM) {
806 0 : SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::ZERO_PWM);
807 0 : SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM);
808 : } else {
809 1979600 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0);
810 1979600 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0);
811 : }
812 : } else {
813 6459071 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle_left);
814 6459071 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle_right);
815 : }
816 8438671 : }
817 :
818 : /*
819 : Set throttle,attitude(in Attitude.cpp), and tilt servos for forced flare by RCx_OPTION switch for landing in FW mode
820 : For Fixed Wind modes with manual throttle control only. Forces tilts up and throttle to THR_MIN.
821 : Throttle stick must be in idle deadzone. This allows non-momentary switch to be used and quick bailouts
822 : for go-arounds. Also helps prevent propstrike after landing with switch release on ground.
823 : */
824 8438671 : void Plane::force_flare(void)
825 : {
826 : #if HAL_QUADPLANE_ENABLED
827 8438671 : if (quadplane.in_frwd_transition() && plane.arming.is_armed()) { //allows for ground checking of flare tilts
828 416489 : return;
829 : }
830 8022182 : if (control_mode->is_vtol_mode()) {
831 1601324 : return;
832 : }
833 : /* to be active must be:
834 : -manual throttle mode
835 : -in an enabled flare mode (RC switch active)
836 : -at zero thrust: in throttle trim dz except for sprung throttle option where trim is at hover stick
837 : */
838 6420858 : if (!control_mode->does_auto_throttle() && flare_mode != FlareMode::FLARE_DISABLED && throttle_at_zero()) {
839 0 : int32_t tilt = -SERVO_MAX; //this is tilts up for a normal tiltrotor if at zero thrust throttle stick
840 0 : if (quadplane.tiltrotor.enabled() && (quadplane.tiltrotor.type == Tiltrotor::TILT_TYPE_BICOPTER)) {
841 0 : tilt = 0; // this is tilts up for a Bicopter
842 : }
843 0 : if (quadplane.tailsitter.enabled()) {
844 0 : tilt = SERVO_MAX; //this is tilts up for a tailsitter
845 : }
846 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, tilt);
847 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt);
848 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt);
849 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRear, tilt);
850 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearLeft, tilt);
851 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearRight, tilt);
852 0 : float throttle_min = MAX(aparm.throttle_min.get(),0); //allows ICE to run if used but accounts for reverse thrust setups
853 0 : if (arming.is_armed()) { //prevent running motors if unarmed
854 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle_min);
855 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle_min);
856 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle_min);
857 : }
858 : }
859 : #endif
860 : }
861 :
862 : /* Set the flight control servos based on the current calculated values
863 :
864 : This function operates by first building up output values for
865 : channels using set_servo() and set_radio_out(). Using
866 : set_radio_out() is for when a raw PWM value of output is given which
867 : does not depend on any output scaling. Using set_servo() is for when
868 : scaling and mixing will be needed.
869 :
870 : Finally servos_output() is called to push the final PWM values
871 : for output channels
872 : */
873 8377547 : void Plane::set_servos(void)
874 : {
875 : // start with output corked. the cork is released when we run
876 : // servos_output(), which is run from all code paths in this
877 : // function
878 8377547 : AP::srv().cork();
879 :
880 : // this is to allow the failsafe module to deliberately crash
881 : // the plane. Only used in extreme circumstances to meet the
882 : // OBC rules
883 : #if AP_ADVANCEDFAILSAFE_ENABLED
884 8377547 : if (afs.should_crash_vehicle()) {
885 303 : afs.terminate_vehicle();
886 303 : if (!afs.terminating_vehicle_via_landing()) {
887 303 : return;
888 : }
889 : }
890 : #endif
891 :
892 : // do any transition updates for quadplane
893 : #if HAL_QUADPLANE_ENABLED
894 8377244 : quadplane.update();
895 : #endif
896 :
897 8377244 : if (flight_stage == AP_FixedWing::FlightStage::LAND) {
898 : // allow landing to override servos if it would like to
899 197641 : landing.override_servos();
900 : }
901 :
902 8377244 : set_throttle();
903 :
904 8377244 : if ((control_mode != &mode_manual) && !arming.is_armed_and_safety_off()) {
905 : // Always set 0 scaled even if overriding to zero pwm.
906 : // This ensures slew limits and other functions using the scaled value pick up in the correct place
907 1366804 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
908 1366804 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0);
909 1366804 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0);
910 :
911 1366804 : if (arming.arming_required() == AP_Arming::Required::YES_ZERO_PWM) {
912 0 : SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::ZERO_PWM);
913 0 : SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::ZERO_PWM);
914 0 : SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM);
915 : }
916 : }
917 :
918 : // Warn AHRS if we might take off soon
919 8377244 : set_takeoff_expected();
920 :
921 : // setup flap outputs
922 8377244 : set_servos_flaps();
923 :
924 : // set airbrake outputs
925 8377244 : airbrake_update();
926 :
927 : // slew rate limit throttle
928 8377244 : throttle_slew_limit();
929 :
930 8377244 : int8_t min_throttle = 0;
931 : #if AP_ICENGINE_ENABLED
932 8377244 : if (g2.ice_control.allow_throttle_while_disarmed()) {
933 72114 : min_throttle = MAX(aparm.throttle_min.get(), 0);
934 : }
935 8377244 : const float base_throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
936 : #endif
937 :
938 8377244 : if (!arming.is_armed()) {
939 : //Some ESCs get noisy (beep error msgs) if PWM == 0.
940 : //This little segment aims to avoid this.
941 1918176 : switch (arming.arming_required()) {
942 0 : case AP_Arming::Required::NO:
943 : //keep existing behavior: do nothing to radio_out
944 : //(don't disarm throttle channel even if AP_Arming class is)
945 0 : break;
946 :
947 0 : case AP_Arming::Required::YES_ZERO_PWM:
948 0 : SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, 0);
949 0 : SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, 0);
950 0 : SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, 0);
951 0 : break;
952 :
953 1918176 : case AP_Arming::Required::YES_MIN_PWM:
954 : default:
955 1918176 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, min_throttle);
956 1918176 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, min_throttle);
957 1918176 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, min_throttle);
958 1918176 : break;
959 : }
960 : }
961 :
962 : #if AP_ICENGINE_ENABLED
963 8377244 : float override_pct = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
964 8377244 : if (g2.ice_control.throttle_override(override_pct, base_throttle)) {
965 : // the ICE controller wants to override the throttle for starting, idle, or redline
966 107170 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, override_pct);
967 : #if HAL_QUADPLANE_ENABLED
968 107170 : quadplane.vel_forward.integrator = 0;
969 : #endif
970 : }
971 : #endif // AP_ICENGINE_ENABLED
972 :
973 : // run output mixer and send values to the hal for output
974 8377244 : servos_output();
975 : }
976 :
977 : /*
978 : This sets servos to neutral if it is a control surface servo in auto mode
979 : */
980 8438671 : void Plane::landing_neutral_control_surface_servos(void)
981 : {
982 8438671 : if (!(landing.get_then_servos_neutral() > 0 &&
983 0 : control_mode == &mode_auto &&
984 0 : landing.get_disarm_delay() > 0 &&
985 0 : landing.is_complete() &&
986 0 : !arming.is_armed())) {
987 8438671 : return;
988 : }
989 :
990 :
991 : // after an auto land and auto disarm, set the servos to be neutral just
992 : // in case we're upside down or some crazy angle and straining the servos.
993 0 : for (uint8_t i = 0; i < NUM_SERVO_CHANNELS ; i++) {
994 0 : SRV_Channel *chan = SRV_Channels::srv_channel(i);
995 0 : if (chan == nullptr || !SRV_Channel::is_control_surface(chan->get_function())) {
996 0 : continue;
997 : }
998 :
999 0 : if (landing.get_then_servos_neutral() == 1) {
1000 0 : SRV_Channels::set_output_scaled(chan->get_function(), 0);
1001 0 : } else if (landing.get_then_servos_neutral() == 2) {
1002 0 : SRV_Channels::set_output_limit(chan->get_function(), SRV_Channel::Limit::ZERO_PWM);
1003 : }
1004 : }
1005 :
1006 : }
1007 :
1008 : /*
1009 : sets rudder/vtail , and elevon to indicator positions that we are in a rudder arming waiting for neutral stick state
1010 : */
1011 0 : void Plane::indicate_waiting_for_rud_neutral_to_takeoff(void)
1012 : {
1013 0 : if (takeoff_state.waiting_for_rudder_neutral) {
1014 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0);
1015 0 : channel_function_mixer(SRV_Channel::k_rudder, SRV_Channel::k_elevator, SRV_Channel::k_vtail_right, SRV_Channel::k_vtail_left);
1016 0 : if (!SRV_Channels::function_assigned(SRV_Channel::k_rudder) && !SRV_Channels::function_assigned(SRV_Channel::k_vtail_left)) {
1017 : // if no rudder indication possible, neutral elevons during wait because on takeoff stance they are usually both full up
1018 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_right, 0);
1019 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_left, 0);
1020 : }
1021 : }
1022 0 : }
1023 :
1024 :
1025 : /*
1026 : run configured output mixer. This takes calculated servo_out values
1027 : for each channel and calculates PWM values, then pushes them to
1028 : hal.rcout
1029 : */
1030 8438671 : void Plane::servos_output(void)
1031 : {
1032 8438671 : auto &srv = AP::srv();
1033 8438671 : srv.cork();
1034 :
1035 : // support twin-engine aircraft
1036 8438671 : servos_twin_engine_mix();
1037 :
1038 : // run vtail and elevon mixers
1039 8438671 : channel_function_mixer(SRV_Channel::k_aileron, SRV_Channel::k_elevator, SRV_Channel::k_elevon_left, SRV_Channel::k_elevon_right);
1040 8438671 : channel_function_mixer(SRV_Channel::k_rudder, SRV_Channel::k_elevator, SRV_Channel::k_vtail_right, SRV_Channel::k_vtail_left);
1041 :
1042 : #if HAL_QUADPLANE_ENABLED
1043 : // cope with tailsitters and bicopters
1044 8438671 : quadplane.tailsitter.output();
1045 8438671 : quadplane.tiltrotor.bicopter_output();
1046 : #endif
1047 :
1048 : // support forced flare option
1049 8438671 : force_flare();
1050 :
1051 : // implement differential spoilers
1052 8438671 : dspoiler_update();
1053 :
1054 : // set control surface servos to neutral
1055 8438671 : landing_neutral_control_surface_servos();
1056 :
1057 : // set rudder arm waiting for neutral control throws (rudder neutral, aileron/rt vtail/rt elevon to full right)
1058 8438671 : if (flight_option_enabled(FlightOptions::INDICATE_WAITING_FOR_RUDDER_NEUTRAL)) {
1059 0 : indicate_waiting_for_rud_neutral_to_takeoff();
1060 : }
1061 :
1062 : // support MANUAL_RCMASK
1063 8438671 : if (g2.manual_rc_mask.get() != 0 && control_mode == &mode_manual) {
1064 0 : SRV_Channels::copy_radio_in_out_mask(uint32_t(g2.manual_rc_mask.get()));
1065 : }
1066 :
1067 8438671 : SRV_Channels::calc_pwm();
1068 :
1069 8438671 : SRV_Channels::output_ch_all();
1070 :
1071 8438671 : srv.push();
1072 :
1073 8438671 : if (g2.servo_channels.auto_trim_enabled()) {
1074 0 : servos_auto_trim();
1075 : }
1076 8438671 : }
1077 :
1078 3894351 : void Plane::update_throttle_hover() {
1079 : // update hover throttle at 100Hz
1080 : #if HAL_QUADPLANE_ENABLED
1081 3894351 : quadplane.update_throttle_hover();
1082 : #endif
1083 3894351 : }
1084 :
1085 : /*
1086 : implement automatic persistent trim of control surfaces with
1087 : AUTO_TRIM=2, only available when SERVO_RNG_ENABLE=1 as otherwise it
1088 : would impact R/C transmitter calibration
1089 : */
1090 0 : void Plane::servos_auto_trim(void)
1091 : {
1092 : // only in auto modes and FBWA
1093 0 : if (!control_mode->does_auto_throttle() && control_mode != &mode_fbwa) {
1094 0 : return;
1095 : }
1096 0 : if (!arming.is_armed_and_safety_off()) {
1097 0 : return;
1098 : }
1099 0 : if (!is_flying()) {
1100 0 : return;
1101 : }
1102 : #if HAL_QUADPLANE_ENABLED
1103 0 : if (!quadplane.allow_servo_auto_trim()) {
1104 : // can't auto-trim with quadplane motors running
1105 0 : return;
1106 : }
1107 : #endif
1108 0 : if (abs(nav_roll_cd) > 700 || abs(nav_pitch_cd) > 700) {
1109 : // only when close to level
1110 0 : return;
1111 : }
1112 0 : uint32_t now = AP_HAL::millis();
1113 0 : if (now - auto_trim.last_trim_check < 500) {
1114 : // check twice a second. We want slow trim update
1115 0 : return;
1116 : }
1117 0 : if (ahrs.groundspeed() < 8 || smoothed_airspeed < 8) {
1118 : // only when definitely moving
1119 0 : return;
1120 : }
1121 :
1122 : // adjust trim on channels by a small amount according to I value
1123 0 : float roll_I = rollController.get_pid_info().I;
1124 0 : float pitch_I = pitchController.get_pid_info().I;
1125 :
1126 0 : g2.servo_channels.adjust_trim(SRV_Channel::k_aileron, roll_I);
1127 0 : g2.servo_channels.adjust_trim(SRV_Channel::k_elevator, pitch_I);
1128 :
1129 0 : g2.servo_channels.adjust_trim(SRV_Channel::k_elevon_left, pitch_I - roll_I);
1130 0 : g2.servo_channels.adjust_trim(SRV_Channel::k_elevon_right, pitch_I + roll_I);
1131 :
1132 0 : g2.servo_channels.adjust_trim(SRV_Channel::k_vtail_left, pitch_I);
1133 0 : g2.servo_channels.adjust_trim(SRV_Channel::k_vtail_right, pitch_I);
1134 :
1135 0 : g2.servo_channels.adjust_trim(SRV_Channel::k_flaperon_left, roll_I);
1136 0 : g2.servo_channels.adjust_trim(SRV_Channel::k_flaperon_right, roll_I);
1137 :
1138 : // cope with various dspoiler options
1139 0 : const int8_t bitmask = g2.crow_flap_options.get();
1140 0 : const bool flying_wing = (bitmask & CrowFlapOptions::FLYINGWING) != 0;
1141 0 : const bool full_span_aileron = (bitmask & CrowFlapOptions::FULLSPAN) != 0;
1142 :
1143 0 : float dspoiler_outer_left = - roll_I;
1144 0 : float dspoiler_inner_left = 0.0f;
1145 0 : float dspoiler_outer_right = roll_I;
1146 0 : float dspoiler_inner_right = 0.0f;
1147 :
1148 0 : if (flying_wing) {
1149 0 : dspoiler_outer_left += pitch_I;
1150 0 : dspoiler_outer_right += pitch_I;
1151 : }
1152 0 : if (full_span_aileron) {
1153 0 : dspoiler_inner_left = dspoiler_outer_left;
1154 0 : dspoiler_inner_right = dspoiler_outer_right;
1155 : }
1156 :
1157 0 : g2.servo_channels.adjust_trim(SRV_Channel::k_dspoilerLeft1, dspoiler_outer_left);
1158 0 : g2.servo_channels.adjust_trim(SRV_Channel::k_dspoilerLeft2, dspoiler_inner_left);
1159 0 : g2.servo_channels.adjust_trim(SRV_Channel::k_dspoilerRight1, dspoiler_outer_right);
1160 0 : g2.servo_channels.adjust_trim(SRV_Channel::k_dspoilerRight2, dspoiler_inner_right);
1161 :
1162 0 : auto_trim.last_trim_check = now;
1163 :
1164 0 : if (now - auto_trim.last_trim_save > 10000) {
1165 0 : auto_trim.last_trim_save = now;
1166 0 : g2.servo_channels.save_trim();
1167 : }
1168 :
1169 : }
|
