Plane: rework set_servos_controlled function

ArduPilot · Oct 31, 2023 · 7e4b5b0 · 7e4b5b0
1 parent 069507c
commit 7e4b5b0
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 18 deletions.
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
@@ -1096,6 +1096,7 @@ class Plane : public AP_Vehicle {
     void set_servos_idle(void);
     void set_servos();
     void set_servos_controlled(void);
+    void set_takeoff_expected(void);
     void set_servos_old_elevons(void);
     void set_servos_flaps(void);
     void set_landing_gear(void);

diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
@@ -485,28 +485,22 @@ void Plane::set_servos_controlled(void)
         min_throttle = 0;
     }
 
-    bool flight_stage_determines_max_throttle = false;
-    if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF || 
-        flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING
-        ) {
-        flight_stage_determines_max_throttle = true;
-    }
+    const bool use_takeoff_throttle_max =
 #if HAL_QUADPLANE_ENABLED
-    if (quadplane.in_transition()) {
-        flight_stage_determines_max_throttle = true;
-    }
+        quadplane.in_transition() ||
 #endif
-    if (flight_stage_determines_max_throttle) {
+        (flight_stage == AP_FixedWing::FlightStage::TAKEOFF) ||
+        (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING);
+
+    if (use_takeoff_throttle_max) {
         if (aparm.takeoff_throttle_max != 0) {
-            max_throttle = aparm.takeoff_throttle_max;
-        } else {
-            max_throttle = aparm.throttle_max;
+            max_throttle = aparm.takeoff_throttle_max.get();
         }
     } else if (landing.is_flaring()) {
         min_throttle = 0;
     }
 
-    // conpensate for battery voltage drop
+    // compensate for battery voltage drop
     throttle_voltage_comp(min_throttle, max_throttle);
 
     // apply watt limiter
@@ -516,14 +510,16 @@ void Plane::set_servos_controlled(void)
                                     constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle));
 
     if (!arming.is_armed_and_safety_off()) {
+        // Always set 0 scaled even if overriding to zero pwm.
+        // This ensures slew limits and other functions using the scaled value pick up in the correct place
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0);
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0);
+
         if (arming.arming_required() == AP_Arming::Required::YES_ZERO_PWM) {
             SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::ZERO_PWM);
             SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::ZERO_PWM);
             SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM);
-        } else {
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0);
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0);
         }
     } else if (suppress_throttle()) {
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); // default
@@ -573,6 +569,13 @@ void Plane::set_servos_controlled(void)
 #endif  // HAL_QUADPLANE_ENABLED
     }
 
+}
+
+/*
+  Warn AHRS that we might take off soon
+ */
+void Plane::set_takeoff_expected(void)
+{
     // let EKF know to start GSF yaw estimator before takeoff movement starts so that yaw angle is better estimated
     const float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
     if (!is_flying() && arming.is_armed()) {
@@ -821,6 +824,7 @@ void Plane::set_servos(void)
 
     if (control_mode != &mode_manual) {
         set_servos_controlled();
+        set_takeoff_expected();
     }
 
     // setup flap outputs