Line data Source code
1 : #include "Plane.h"
2 :
3 : /*
4 : calculate speed scaling number for control surfaces. This is applied
5 : to PIDs to change the scaling of the PID with speed. At high speed
6 : we move the surfaces less, and at low speeds we move them more.
7 : */
8 547097 : float Plane::calc_speed_scaler(void)
9 : {
10 : float aspeed, speed_scaler;
11 547097 : if (ahrs.airspeed_EAS(aspeed)) {
12 542890 : if (aspeed > auto_state.highest_airspeed && arming.is_armed_and_safety_off()) {
13 28536 : auto_state.highest_airspeed = aspeed;
14 : }
15 : // ensure we have scaling over the full configured airspeed
16 542890 : const float airspeed_min = MAX(aparm.airspeed_min, MIN_AIRSPEED_MIN);
17 542890 : const float scale_min = MIN(0.5, g.scaling_speed / (2.0 * aparm.airspeed_max));
18 : #if HAL_QUADPLANE_ENABLED
19 : float scale_max;
20 542890 : if (quadplane.is_flying_vtol()) {
21 : // vtol aircraft can generate excessive large aero control surface deflections
22 : // during VTOL operation because the low airspeed can create large speed scaler
23 : // values at a flight condition where aero control surfaces have little influence.
24 79175 : const float stall_airspeed_1g = is_positive(aparm.airspeed_stall)
25 79175 : ? aparm.airspeed_stall : 0.7f * airspeed_min;
26 79175 : scale_max = g.scaling_speed / stall_airspeed_1g;
27 : } else {
28 : // use the legacy scaling limit for FW flight
29 463715 : scale_max = MAX(2.0, g.scaling_speed / (0.7 * airspeed_min));
30 : }
31 : #else
32 : const float scale_max = MAX(2.0, g.scaling_speed / (0.7 * airspeed_min));
33 : #endif
34 542890 : if (aspeed > 0.0001f) {
35 541676 : speed_scaler = g.scaling_speed / aspeed;
36 : } else {
37 1214 : speed_scaler = scale_max;
38 : }
39 542890 : speed_scaler = constrain_float(speed_scaler, scale_min, scale_max);
40 :
41 : #if HAL_QUADPLANE_ENABLED
42 542890 : if ((quadplane.in_vtol_mode() || quadplane.in_assisted_flight()) && arming.is_armed_and_safety_off()) {
43 : // when in VTOL modes limit surface movement at low speed to prevent instability
44 88733 : float threshold = airspeed_min * 0.5;
45 88733 : if (aspeed < threshold) {
46 68104 : float new_scaler = linear_interpolate(0.001, g.scaling_speed / threshold, aspeed, 0, threshold);
47 68104 : speed_scaler = MIN(speed_scaler, new_scaler);
48 :
49 : // we also decay the integrator to prevent an integrator from before
50 : // we were at low speed persistent at high speed
51 68104 : rollController.decay_I();
52 68104 : pitchController.decay_I();
53 68104 : yawController.decay_I();
54 : }
55 : }
56 : #endif
57 4207 : } else if (arming.is_armed_and_safety_off()) {
58 : // scale assumed surface movement using throttle output
59 0 : float throttle_out = MAX(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 1);
60 : // we use a fixed value here as changing the trim throttle
61 : // value is often done at runtime - and changing that
62 : // shouldn't change the speed scaler here (it will change the
63 : // effective PID values)
64 0 : speed_scaler = sqrtf(AP_PLANE_TRIM_THROTTLE_DEFAULT / throttle_out);
65 : // This case is constrained tighter as we don't have real speed info
66 0 : speed_scaler = constrain_float(speed_scaler, 0.6f, 1.67f);
67 : } else {
68 : // no speed estimate and not armed, use a unit scaling
69 4207 : speed_scaler = 1;
70 : }
71 567415 : if (!plane.ahrs.using_airspeed_sensor() &&
72 567415 : (plane.flight_option_enabled(FlightOptions::SURPRESS_TKOFF_SCALING)) &&
73 0 : (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) { //scaling is suppressed during climb phase of automatic takeoffs with no airspeed sensor being used due to problems with inaccurate airspeed estimates
74 0 : return MIN(speed_scaler, 1.0f) ;
75 : }
76 547097 : return speed_scaler;
77 : }
78 :
79 : /*
80 : return true if the current settings and mode should allow for stick mixing
81 : */
82 12689547 : bool Plane::stick_mixing_enabled(void)
83 : {
84 12689547 : if (!rc().has_valid_input()) {
85 : // never stick mix without valid RC
86 3112 : return false;
87 : }
88 : #if AP_FENCE_ENABLED
89 12686435 : const bool stickmixing = fence_stickmixing();
90 : #else
91 : const bool stickmixing = true;
92 : #endif
93 : #if HAL_QUADPLANE_ENABLED
94 13168843 : if (control_mode == &mode_qrtl &&
95 482408 : quadplane.poscontrol.get_state() >= QuadPlane::QPOS_POSITION1) {
96 : // user may be repositioning
97 263938 : return false;
98 : }
99 12422497 : if (quadplane.in_vtol_land_poscontrol()) {
100 : // user may be repositioning
101 1178010 : return false;
102 : }
103 : #endif
104 11244487 : if (control_mode->does_auto_throttle() && plane.control_mode->does_auto_navigation()) {
105 : // we're in an auto mode. Check the stick mixing flag
106 16820316 : if (g.stick_mixing != StickMixing::NONE &&
107 16820316 : g.stick_mixing != StickMixing::VTOL_YAW &&
108 : stickmixing) {
109 8380876 : return true;
110 : } else {
111 29282 : return false;
112 : }
113 : }
114 :
115 2834329 : if (failsafe.rc_failsafe && g.fs_action_short == FS_ACTION_SHORT_FBWA) {
116 : // don't do stick mixing in FBWA glide mode
117 0 : return false;
118 : }
119 :
120 : // non-auto mode. Always do stick mixing
121 2834329 : return true;
122 : }
123 :
124 :
125 : /*
126 : this is the main roll stabilization function. It takes the
127 : previously set nav_roll calculates roll servo_out to try to
128 : stabilize the plane at the given roll
129 : */
130 7541881 : void Plane::stabilize_roll()
131 : {
132 7541881 : if (fly_inverted()) {
133 5135 : nav_roll_cd += 18000;
134 : }
135 7541881 : float roll_out = stabilize_roll_get_roll_out();
136 :
137 : #if AP_PLANE_SYSTEMID_ENABLED
138 7541881 : const auto &systemid = plane.g2.systemid;
139 7541881 : if (systemid.is_running_fw()) {
140 0 : Vector3f offset;
141 0 : offset = systemid.get_output_offset();
142 0 : roll_out += offset.x * 100.0f;
143 : }
144 : #endif
145 :
146 7541881 : SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_out);
147 7541881 : }
148 :
149 7541881 : float Plane::stabilize_roll_get_roll_out()
150 : {
151 7541881 : const float speed_scaler = get_speed_scaler();
152 : #if HAL_QUADPLANE_ENABLED
153 7541881 : if (!quadplane.use_fw_attitude_controllers()) {
154 : // use the VTOL rate for control, to ensure consistency
155 2195258 : const auto &pid_info = quadplane.attitude_control->get_rate_roll_pid().get_pid_info();
156 :
157 : // scale FF to angle P
158 2195258 : if (quadplane.option_is_set(QuadPlane::Option::SCALE_FF_ANGLE_P)) {
159 0 : const float mc_angR = quadplane.attitude_control->get_angle_roll_p().kP()
160 0 : * quadplane.attitude_control->get_last_angle_P_scale().x;
161 0 : if (is_positive(mc_angR)) {
162 0 : rollController.set_ff_scale(MIN(1.0, rollController.get_angle_p() / mc_angR));
163 : }
164 : }
165 :
166 2195258 : const float roll_out = rollController.run_rate_control(degrees(pid_info.target), speed_scaler);
167 : /* when slaving fixed wing control to VTOL control we need to decay the integrator to prevent
168 : opposing integrators balancing between the two controllers
169 : */
170 2195258 : rollController.decay_I();
171 2195258 : return roll_out;
172 : }
173 : #endif
174 :
175 5346623 : bool disable_integrator = false;
176 5346623 : if (control_mode == &mode_stabilize && channel_roll->get_control_in() != 0) {
177 49 : disable_integrator = true;
178 : }
179 5346623 : return rollController.run_angle_control(nav_roll_cd, speed_scaler, disable_integrator,
180 10693246 : ground_mode && !(plane.flight_option_enabled(FlightOptions::DISABLE_GROUND_PID_SUPPRESSION)));
181 : }
182 :
183 : /*
184 : this is the main pitch stabilization function. It takes the
185 : previously set nav_pitch and calculates servo_out values to try to
186 : stabilize the plane at the given attitude.
187 : */
188 7540677 : void Plane::stabilize_pitch()
189 : {
190 7540677 : int8_t force_elevator = takeoff_tail_hold();
191 7540677 : if (force_elevator != 0) {
192 : // we are holding the tail down during takeoff. Just convert
193 : // from a percentage to a -4500..4500 centidegree angle
194 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 45*force_elevator);
195 0 : return;
196 : }
197 :
198 7540677 : float pitch_out = stabilize_pitch_get_pitch_out();
199 :
200 : #if AP_PLANE_SYSTEMID_ENABLED
201 7540677 : const auto &systemid = plane.g2.systemid;
202 7540677 : if (systemid.is_running_fw()) {
203 0 : Vector3f offset;
204 0 : offset = systemid.get_output_offset();
205 0 : pitch_out += offset.y * 100.0f;
206 : }
207 : #endif
208 :
209 7540677 : SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_out);
210 : }
211 :
212 7540677 : float Plane::stabilize_pitch_get_pitch_out()
213 : {
214 7540677 : const float speed_scaler = get_speed_scaler();
215 : #if HAL_QUADPLANE_ENABLED
216 7540677 : if (!quadplane.use_fw_attitude_controllers()) {
217 : // use the VTOL rate for control, to ensure consistency
218 2195258 : const auto &pid_info = quadplane.attitude_control->get_rate_pitch_pid().get_pid_info();
219 :
220 : // scale FF to angle P
221 2195258 : if (quadplane.option_is_set(QuadPlane::Option::SCALE_FF_ANGLE_P)) {
222 0 : const float mc_angP = quadplane.attitude_control->get_angle_pitch_p().kP()
223 0 : * quadplane.attitude_control->get_last_angle_P_scale().y;
224 0 : if (is_positive(mc_angP)) {
225 0 : pitchController.set_ff_scale(MIN(1.0, pitchController.get_angle_p() / mc_angP));
226 : }
227 : }
228 :
229 2195258 : const int32_t pitch_out = pitchController.run_rate_control(degrees(pid_info.target), speed_scaler);
230 : /* when slaving fixed wing control to VTOL control we need to decay the integrator to prevent
231 : opposing integrators balancing between the two controllers
232 : */
233 2195258 : pitchController.decay_I();
234 2195258 : return pitch_out;
235 : }
236 : #endif
237 : // if LANDING_FLARE RCx_OPTION switch is set and in FW mode, manual throttle,throttle idle then set pitch to LAND_PITCH_DEG if flight option FORCE_FLARE_ATTITUDE is set
238 : #if HAL_QUADPLANE_ENABLED
239 5345419 : const bool quadplane_in_frwd_transition = quadplane.in_frwd_transition();
240 : #else
241 : const bool quadplane_in_frwd_transition = false;
242 : #endif
243 :
244 5345419 : int32_t demanded_pitch = nav_pitch_cd + int32_t(g.pitch_trim * 100.0) + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch;
245 5345419 : bool disable_integrator = false;
246 5345419 : if (control_mode == &mode_stabilize && channel_pitch->get_control_in() != 0) {
247 0 : disable_integrator = true;
248 : }
249 : /* force landing pitch if:
250 : - flare switch high
251 : - throttle stick at zero thrust
252 : - in fixed wing non auto-throttle mode
253 : */
254 15566145 : if (!quadplane_in_frwd_transition &&
255 4875307 : !control_mode->is_vtol_mode() &&
256 4417824 : !control_mode->does_auto_throttle() &&
257 10220726 : flare_mode == FlareMode::ENABLED_PITCH_TARGET &&
258 0 : throttle_at_zero()) {
259 0 : demanded_pitch = landing.get_pitch_cd();
260 : }
261 :
262 5345419 : return pitchController.run_angle_control(demanded_pitch, speed_scaler, disable_integrator,
263 10690838 : ground_mode && !(plane.flight_option_enabled(FlightOptions::DISABLE_GROUND_PID_SUPPRESSION)));
264 : }
265 :
266 : /*
267 : this gives the user control of the aircraft in stabilization modes, only used in Stabilize Mode
268 : to be moved to mode_stabilize.cpp in future
269 : */
270 1389 : void ModeStabilize::stabilize_stick_mixing_direct()
271 : {
272 1389 : if (!plane.stick_mixing_enabled()) {
273 0 : return;
274 : }
275 : #if HAL_QUADPLANE_ENABLED
276 1389 : if (!plane.quadplane.allow_stick_mixing()) {
277 0 : return;
278 : }
279 : #endif
280 1389 : float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
281 1389 : aileron = plane.channel_roll->stick_mixing(aileron);
282 1389 : SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
283 :
284 1389 : float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
285 1389 : elevator = plane.channel_pitch->stick_mixing(elevator);
286 1389 : SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator);
287 : }
288 :
289 : /*
290 : this gives the user control of the aircraft in stabilization modes
291 : using FBW style controls
292 : */
293 6031906 : void Plane::stabilize_stick_mixing_fbw()
294 : {
295 6031906 : if (!stick_mixing_enabled() ||
296 5426704 : control_mode == &mode_acro ||
297 5426704 : control_mode == &mode_fbwa ||
298 4332166 : control_mode == &mode_autotune ||
299 4312421 : control_mode == &mode_fbwb ||
300 4295248 : control_mode == &mode_cruise ||
301 : #if HAL_QUADPLANE_ENABLED
302 4131854 : control_mode == &mode_qstabilize ||
303 4131854 : control_mode == &mode_qhover ||
304 4131853 : control_mode == &mode_qloiter ||
305 4131853 : control_mode == &mode_qland ||
306 4131426 : control_mode == &mode_qacro ||
307 : #if QAUTOTUNE_ENABLED
308 4131426 : control_mode == &mode_qautotune ||
309 : #endif
310 15587390 : !quadplane.allow_stick_mixing() ||
311 : #endif // HAL_QUADPLANE_ENABLED
312 4128780 : control_mode == &mode_training) {
313 1903126 : return;
314 : }
315 : // do FBW style roll stick mixing. We don't treat it linearly however. For
316 : // inputs up to half the maximum, we use linear addition to the nav_roll.
317 : // Above that it goes non-linear and ends up as 2x the maximum, to ensure
318 : // that the user can direct the plane in any direction with stick mixing.
319 4128780 : float roll_input = channel_roll->norm_input_dz();
320 4128780 : if (roll_input > 0.5f) {
321 484 : roll_input = (3*roll_input - 1);
322 4128296 : } else if (roll_input < -0.5f) {
323 0 : roll_input = (3*roll_input + 1);
324 : }
325 4128780 : nav_roll_cd += roll_input * roll_limit_cd;
326 4128780 : nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
327 :
328 4128780 : if (plane.g.stick_mixing == StickMixing::FBW_NO_PITCH) {
329 0 : return;
330 : }
331 4128780 : if ((control_mode == &mode_loiter) && (plane.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL))) {
332 : // loiter is using altitude control based on the pitch stick, don't use it again here
333 8407 : return;
334 : }
335 :
336 : // do FBW-A style pitch stick mixing. Use the same non-linear approach as
337 : // roll, but based on the pitch range rather than the limits to ensure full
338 : // stick deflection can override either limit regardless of current
339 : // attitude.
340 4120373 : float pitch_input = channel_pitch->norm_input_dz();
341 4120373 : if (pitch_input > 0.5f) {
342 0 : pitch_input = (3*pitch_input - 1);
343 4120373 : } else if (pitch_input < -0.5f) {
344 228 : pitch_input = (3*pitch_input + 1);
345 : }
346 4120373 : if (fly_inverted()) {
347 875 : pitch_input = -pitch_input;
348 : }
349 4120373 : const float pitch_range = aparm.pitch_limit_max.get() - pitch_limit_min;
350 4120373 : nav_pitch_cd += pitch_input * (pitch_range / 2.0f) * 100;
351 4120373 : nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min*100, aparm.pitch_limit_max.get()*100);
352 : }
353 :
354 :
355 : /*
356 : stabilize the yaw axis. There are 3 modes of operation:
357 :
358 : - hold a specific heading with ground steering
359 : - rate controlled with ground steering
360 : - yaw control for coordinated flight
361 : */
362 6297553 : void Plane::stabilize_yaw()
363 : {
364 6297553 : bool ground_steering = false;
365 6297553 : if (landing.is_flaring()) {
366 : // in flaring then enable ground steering
367 123090 : ground_steering = true;
368 : } else {
369 : // otherwise use ground steering when no input control and we
370 : // are below the GROUND_STEER_ALT
371 12325549 : ground_steering = (channel_roll->get_control_in() == 0 &&
372 6151086 : fabsf(relative_altitude) < g.ground_steer_alt);
373 6174463 : if (!landing.is_ground_steering_allowed()) {
374 : // don't use ground steering on landing approach
375 33795 : ground_steering = false;
376 : }
377 : }
378 :
379 :
380 : /*
381 : first calculate steering for a nose or tail
382 : wheel. We use "course hold" mode for the rudder when either performing
383 : a flare (when the wings are held level) or when in course hold in
384 : FBWA mode (when we are below GROUND_STEER_ALT)
385 : */
386 6297553 : float steering_output = 0.0;
387 12472016 : if (landing.is_flaring() ||
388 6174463 : (steer_state.hold_course_cd != -1 && ground_steering)) {
389 123090 : steering_output = calc_nav_yaw_course();
390 6174463 : } else if (ground_steering) {
391 0 : steering_output = calc_nav_yaw_ground();
392 : }
393 :
394 : /*
395 : now calculate rudder for the rudder
396 : */
397 6297553 : const float rudder_output = calc_nav_yaw_coordinated();
398 :
399 6297553 : if (!ground_steering) {
400 : // Not doing ground steering, output rudder on steering channel
401 6174463 : SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder_output);
402 6174463 : SRV_Channels::set_output_scaled(SRV_Channel::k_steering, rudder_output);
403 :
404 123090 : } else if (!SRV_Channels::function_assigned(SRV_Channel::k_steering)) {
405 : // Ground steering active but no steering output configured, output steering on rudder channel
406 123090 : SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_output);
407 123090 : SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_output);
408 :
409 : } else {
410 : // Ground steering with both steering and rudder channels
411 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder_output);
412 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_output);
413 : }
414 :
415 : #if HAL_QUADPLANE_ENABLED
416 : // possibly recover from a spin
417 6297553 : quadplane.assist.output_spin_recovery();
418 : #endif
419 6297553 : }
420 :
421 : /*
422 : main stabilization function for all 3 axes
423 : */
424 8377547 : void Plane::stabilize()
425 : {
426 8377547 : uint32_t now = AP_HAL::millis();
427 : #if HAL_QUADPLANE_ENABLED
428 8377547 : if (quadplane.available()) {
429 6504054 : quadplane.transition->set_FW_roll_pitch(nav_pitch_cd, nav_roll_cd);
430 : }
431 : #endif
432 :
433 : #if AP_PLANE_SYSTEMID_ENABLED
434 8377547 : if (plane.g2.systemid.is_running_fw()) {
435 0 : plane.g2.systemid.fw_update();
436 : }
437 : #endif
438 :
439 8377547 : if (now - last_stabilize_ms > 2000) {
440 : // if we haven't run the rate controllers for 2 seconds then reset
441 92 : control_mode->reset_controllers();
442 : }
443 8377547 : last_stabilize_ms = now;
444 :
445 8377547 : if (control_mode == &mode_training ||
446 8376167 : control_mode == &mode_manual) {
447 615622 : plane.control_mode->run();
448 : #if AP_SCRIPTING_ENABLED
449 7761925 : } else if (nav_scripting_active()) {
450 : // scripting is in control of roll and pitch rates and throttle
451 6657 : const float speed_scaler = get_speed_scaler();
452 6657 : const float aileron = rollController.run_rate_control(nav_scripting.roll_rate_dps, speed_scaler);
453 6657 : const float elevator = pitchController.run_rate_control(nav_scripting.pitch_rate_dps, speed_scaler);
454 6657 : SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
455 6657 : SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator);
456 6657 : float rudder = 0;
457 6657 : if (yawController.rate_control_enabled()) {
458 6657 : rudder = nav_scripting.rudder_offset_pct * 45;
459 6657 : if (nav_scripting.run_yaw_rate_controller) {
460 6655 : rudder += yawController.get_rate_out(nav_scripting.yaw_rate_dps, speed_scaler, false);
461 : } else {
462 2 : yawController.reset_I();
463 : }
464 : }
465 6657 : SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder);
466 6657 : SRV_Channels::set_output_scaled(SRV_Channel::k_steering, rudder);
467 6657 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct);
468 : #endif
469 : } else {
470 7755268 : plane.control_mode->run();
471 : }
472 :
473 : /*
474 : see if we should zero the attitude controller integrators.
475 : */
476 8377547 : if (is_zero(get_throttle_input()) &&
477 6139714 : fabsf(relative_altitude) < 5.0f &&
478 16293483 : fabsf(barometer.get_climb_rate()) < 0.5f &&
479 1776222 : ahrs.groundspeed() < 3) {
480 : // we are low, with no climb rate, and zero throttle, and very
481 : // low ground speed. Zero the attitude controller
482 : // integrators. This prevents integrator buildup pre-takeoff.
483 1720634 : rollController.reset_I();
484 1720634 : pitchController.reset_I();
485 1720634 : yawController.reset_I();
486 :
487 : // if moving very slowly also zero the steering integrator
488 1720634 : if (ahrs.groundspeed() < 1) {
489 1653122 : steerController.reset_I();
490 : }
491 : }
492 8377547 : }
493 :
494 :
495 : /*
496 : * Set the throttle output.
497 : * This is called by TECS-enabled flight modes, e.g. AUTO, GUIDED, etc.
498 : */
499 3609969 : void Plane::calc_throttle()
500 : {
501 3609969 : if (aparm.throttle_cruise <= 1) {
502 : // user has asked for zero throttle - this may be done by a
503 : // mission which wants to turn off the engine for a parachute
504 : // landing
505 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
506 0 : return;
507 : }
508 :
509 : // Read the TECS throttle output and set it to the throttle channel.
510 3609969 : float commanded_throttle = TECS_controller.get_throttle_demand();
511 3609969 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, commanded_throttle);
512 : }
513 :
514 : /*****************************************
515 : * Calculate desired roll/pitch/yaw angles (in medium freq loop)
516 : *****************************************/
517 :
518 : /*
519 : calculate yaw control for coordinated flight
520 : */
521 6297553 : int16_t Plane::calc_nav_yaw_coordinated()
522 : {
523 6297553 : const float speed_scaler = get_speed_scaler();
524 6297553 : bool disable_integrator = false;
525 6297553 : int16_t rudder_in = rudder_input();
526 :
527 : int16_t commanded_rudder;
528 6297553 : bool using_rate_controller = false;
529 :
530 : // Received an external msg that guides yaw within g2.guided_timeout?
531 6297553 : if (control_mode->is_guided_mode() &&
532 6302794 : plane.guided_state.last_forced_rpy_ms.z > 0 &&
533 5241 : millis() - plane.guided_state.last_forced_rpy_ms.z < g2.guided_timeout*1000.0f) {
534 5241 : commanded_rudder = plane.guided_state.forced_rpy_cd.z;
535 6292312 : } else if (autotuning && g.acro_yaw_rate > 0 && yawController.rate_control_enabled()) {
536 : // user is doing an AUTOTUNE with yaw rate control
537 0 : const float rudd_expo = rudder_in_expo(true);
538 0 : const float yaw_rate = (rudd_expo/SERVO_MAX) * g.acro_yaw_rate;
539 : // add in the coordinated turn yaw rate to make it easier to fly while tuning the yaw rate controller
540 0 : const float coordination_yaw_rate = degrees(GRAVITY_MSS * tanf(cd_to_rad(nav_roll_cd))/MAX(aparm.airspeed_min,smoothed_airspeed));
541 0 : commanded_rudder = yawController.get_rate_out(yaw_rate+coordination_yaw_rate, speed_scaler, false);
542 0 : using_rate_controller = true;
543 : } else {
544 6292312 : if (control_mode == &mode_stabilize && rudder_in != 0) {
545 0 : disable_integrator = true;
546 : }
547 :
548 6292312 : commanded_rudder = yawController.get_servo_out(speed_scaler, disable_integrator);
549 :
550 : // add in rudder mixing from roll
551 6292312 : commanded_rudder += SRV_Channels::get_output_scaled(SRV_Channel::k_aileron) * g.kff_rudder_mix;
552 6292312 : commanded_rudder += rudder_in;
553 : }
554 :
555 6297553 : if (!using_rate_controller) {
556 : /*
557 : When not running the yaw rate controller, we need to reset the rate
558 : */
559 6297553 : yawController.reset_rate_PID();
560 : }
561 :
562 6297553 : return constrain_int16(commanded_rudder, -4500, 4500);
563 : }
564 :
565 : /*
566 : calculate yaw control for ground steering with specific course
567 : */
568 123090 : int16_t Plane::calc_nav_yaw_course(void)
569 : {
570 : // holding a specific navigation course on the ground. Used in
571 : // auto-takeoff and landing
572 123090 : int32_t bearing_error_cd = nav_controller->bearing_error_cd();
573 123090 : int16_t steering = steerController.get_steering_out_angle_error(bearing_error_cd);
574 123090 : if (stick_mixing_enabled()) {
575 123090 : steering = channel_rudder->stick_mixing(steering);
576 : }
577 123090 : return constrain_int16(steering, -4500, 4500);
578 : }
579 :
580 : /*
581 : calculate yaw control for ground steering
582 : */
583 0 : int16_t Plane::calc_nav_yaw_ground(void)
584 : {
585 0 : if (gps.ground_speed() < 1 &&
586 0 : is_zero(get_throttle_input()) &&
587 0 : flight_stage != AP_FixedWing::FlightStage::TAKEOFF &&
588 0 : flight_stage != AP_FixedWing::FlightStage::ABORT_LANDING) {
589 : // manual rudder control while still
590 0 : steer_state.locked_course = false;
591 0 : steer_state.locked_course_err = 0;
592 0 : return rudder_input();
593 : }
594 :
595 : // if we haven't been steering for 1s then clear locked course
596 0 : const uint32_t now_ms = AP_HAL::millis();
597 0 : if (now_ms - steer_state.last_steer_ms > 1000) {
598 0 : steer_state.locked_course = false;
599 : }
600 0 : steer_state.last_steer_ms = now_ms;
601 :
602 0 : float steer_rate = (rudder_input()/4500.0f) * g.ground_steer_dps;
603 0 : if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF ||
604 0 : flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
605 0 : steer_rate = 0;
606 : }
607 0 : if (!is_zero(steer_rate)) {
608 : // pilot is giving rudder input
609 0 : steer_state.locked_course = false;
610 0 : } else if (!steer_state.locked_course) {
611 : // pilot has released the rudder stick or we are still - lock the course
612 0 : steer_state.locked_course = true;
613 0 : if (flight_stage != AP_FixedWing::FlightStage::TAKEOFF &&
614 0 : flight_stage != AP_FixedWing::FlightStage::ABORT_LANDING) {
615 0 : steer_state.locked_course_err = 0;
616 : }
617 : }
618 :
619 : int16_t steering;
620 0 : if (!steer_state.locked_course) {
621 : // use a rate controller at the pilot specified rate
622 0 : steering = steerController.get_steering_out_rate(steer_rate);
623 : } else {
624 : // use a error controller on the summed error
625 0 : int32_t yaw_error_cd = -degrees(steer_state.locked_course_err)*100;
626 0 : steering = steerController.get_steering_out_angle_error(yaw_error_cd);
627 : }
628 0 : return constrain_int16(steering, -4500, 4500);
629 : }
630 :
631 :
632 : /*
633 : calculate a new nav_pitch_cd from the speed height controller
634 : */
635 3731360 : void Plane::calc_nav_pitch()
636 : {
637 3731360 : int32_t commanded_pitch = TECS_controller.get_pitch_demand();
638 3731360 : nav_pitch_cd = constrain_int32(commanded_pitch, pitch_limit_min*100, aparm.pitch_limit_max.get()*100);
639 3731360 : }
640 :
641 :
642 : /*
643 : calculate a new nav_roll_cd from the navigation controller
644 : */
645 3791334 : void Plane::calc_nav_roll()
646 : {
647 3791334 : int32_t commanded_roll = nav_controller->nav_roll_cd();
648 3791334 : nav_roll_cd = constrain_int32(commanded_roll, -roll_limit_cd, roll_limit_cd);
649 3791334 : update_load_factor();
650 3791334 : }
651 :
652 : /*
653 : adjust nav_pitch_cd for STAB_PITCH_DOWN_CD. This is used to make
654 : keeping up good airspeed in FBWA mode easier, as the plane will
655 : automatically pitch down a little when at low throttle. It makes
656 : FBWA landings without stalling much easier.
657 : */
658 1114368 : void Plane::adjust_nav_pitch_throttle(void)
659 : {
660 1114368 : int8_t throttle = throttle_percentage();
661 1114368 : if (throttle >= 0 && throttle < aparm.throttle_cruise && flight_stage != AP_FixedWing::FlightStage::VTOL) {
662 830311 : float p = (aparm.throttle_cruise - throttle) / (float)aparm.throttle_cruise;
663 830311 : nav_pitch_cd -= g.stab_pitch_down * 100.0f * p;
664 : }
665 1114368 : }
666 :
667 :
668 : /*
669 : calculate a new aerodynamic_load_factor
670 : */
671 4981710 : void Plane::update_load_factor(void)
672 : {
673 4981710 : float demanded_roll = fabsf(nav_roll_cd*0.01f);
674 4981710 : if (demanded_roll > 85) {
675 : // limit to 85 degrees to prevent numerical errors
676 0 : demanded_roll = 85;
677 : }
678 :
679 : // loadFactor = liftForce / gravityForce, where gravityForce = liftForce * cos(roll) on balanced horizontal turn
680 4981710 : aerodynamic_load_factor = 1.0f / cosf(radians(demanded_roll));
681 :
682 4981710 : apply_load_factor_roll_limits();
683 4981710 : }
684 :
685 : /*
686 : limit nav_roll_cd to ensure that the load factor does not take us below the
687 : sustainable airspeed
688 : */
689 4981710 : void Plane::apply_load_factor_roll_limits(void)
690 : {
691 : #if HAL_QUADPLANE_ENABLED
692 4981710 : if (quadplane.available() && quadplane.transition->set_FW_roll_limit(roll_limit_cd)) {
693 0 : nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
694 332168 : return;
695 : }
696 : #endif
697 :
698 4981710 : if (!aparm.stall_prevention) {
699 : // stall prevention is disabled
700 324926 : return;
701 : }
702 4656784 : if (fly_inverted()) {
703 : // no roll limits when inverted
704 5135 : return;
705 : }
706 : #if HAL_QUADPLANE_ENABLED
707 4651649 : if (quadplane.tailsitter.active()) {
708 : // no limits while hovering
709 2107 : return;
710 : }
711 : #endif
712 :
713 4649542 : float stall_airspeed_1g = is_positive(aparm.airspeed_stall)
714 9299084 : ? aparm.airspeed_stall
715 4649542 : : aparm.airspeed_min;
716 :
717 : float max_load_factor =
718 4649542 : sq(smoothed_airspeed / MAX(stall_airspeed_1g, 1));
719 :
720 : // allow limiting roll command down to wings-level when necessary if
721 : // airspeed is accurate and airspeed stall is set (implying low load factor
722 : // overhead)
723 : const bool enforce_full_roll_limit =
724 4649542 : flight_option_enabled(FlightOptions::ENABLE_FULL_AERO_LF_ROLL_LIMITS) &&
725 4649542 : ahrs.using_airspeed_sensor() && is_positive(aparm.airspeed_stall);
726 :
727 4649542 : const float level_roll_limit_deg = g.level_roll_limit;
728 4649542 : float lf_roll_limit_deg = aparm.roll_limit;
729 4649542 : if (max_load_factor <= 1) {
730 1067119 : if (enforce_full_roll_limit) {
731 0 : lf_roll_limit_deg = level_roll_limit_deg;
732 : } else {
733 : // 25° limit to ensure maneuverability if airspeed estimate is wrong
734 1067119 : lf_roll_limit_deg = 25;
735 : }
736 3582423 : } else if (max_load_factor < aerodynamic_load_factor) {
737 : // the demanded nav_roll would take us past the aerodynamic
738 : // load limit. Limit our roll to a bank angle that will keep
739 : // the load within what the airframe can handle.
740 14941 : lf_roll_limit_deg = degrees(acosf(1.0f / max_load_factor));
741 :
742 : // unless enforcing full limits, allow at least 25 degrees of roll to
743 : // ensure the aircraft can be manoeuvered with a bad airspeed estimate.
744 : // At 25 degrees the load factor is 1.1 (10%)
745 14941 : if (!enforce_full_roll_limit && lf_roll_limit_deg < 25) {
746 3370 : lf_roll_limit_deg = 25;
747 : }
748 :
749 : // always allow at least the wings level threshold to prevent flyaways
750 14941 : if (lf_roll_limit_deg < level_roll_limit_deg) {
751 0 : lf_roll_limit_deg = level_roll_limit_deg;
752 : }
753 : }
754 :
755 4649542 : nav_roll_cd = constrain_int32(nav_roll_cd, -lf_roll_limit_deg * 100, lf_roll_limit_deg * 100);
756 4649542 : roll_limit_cd = MIN(roll_limit_cd, lf_roll_limit_deg * 100);
757 : }
758 :
759 : // Check if there has been a change in attitude estimate which the attitude controllers should be told about
760 : // This allows them to compensate for the change so smooth control is maintained
761 8377547 : void Plane::check_ahrs_reset()
762 : {
763 8377547 : bool should_reset = false;
764 :
765 : // Check for yaw reset
766 8377547 : const uint16_t new_yaw_reset_count = ahrs.get_yaw_reset_count();
767 8377547 : if (ahrs_check.ahrs_yaw_reset_count != new_yaw_reset_count) {
768 421 : should_reset = true;
769 421 : ahrs_check.ahrs_yaw_reset_count = new_yaw_reset_count;
770 : }
771 :
772 : // check for change in primary EKF, or EKF lane.
773 8377547 : const uint16_t new_reset_count = ahrs.get_last_attitude_reset_count();
774 8377547 : if (new_reset_count != ahrs_check.attitude_reset_count) {
775 336 : should_reset = true;
776 336 : ahrs_check.attitude_reset_count = new_reset_count;
777 : }
778 :
779 : // Nothing to do if there are no resets
780 8377547 : if (!should_reset) {
781 8377123 : return;
782 : }
783 :
784 : // Reset fixed wing controllers
785 424 : rollController.ahrs_reset();
786 424 : pitchController.ahrs_reset();
787 :
788 : #if HAL_QUADPLANE_ENABLED
789 : // Reset vtol controllers
790 424 : if (quadplane.initialised) {
791 198 : quadplane.attitude_control->inertial_frame_reset();
792 : }
793 : #endif
794 :
795 : }
|
