Plane: stop passing speed_scaler all over the place

ArduPlane/Attitude.cpp

@@ -108,7 +108,7 @@ bool Plane::stick_mixing_enabled(void)
   previously set nav_roll calculates roll servo_out to try to
   stabilize the plane at the given roll
  */
-void Plane::stabilize_roll(float speed_scaler)
+void Plane::stabilize_roll()
 {
     if (fly_inverted()) {
         // we want to fly upside down. We need to cope with wrap of
@@ -120,12 +120,13 @@ void Plane::stabilize_roll(float speed_scaler)
         if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000;
     }
 
-    const float roll_out = stabilize_roll_get_roll_out(speed_scaler);
+    const float roll_out = stabilize_roll_get_roll_out();
     SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_out);
 }
 
-float Plane::stabilize_roll_get_roll_out(float speed_scaler)
+float Plane::stabilize_roll_get_roll_out()
 {
+    const float speed_scaler = get_speed_scaler();
 #if HAL_QUADPLANE_ENABLED
     if (!quadplane.use_fw_attitude_controllers()) {
         // use the VTOL rate for control, to ensure consistency
@@ -152,7 +153,7 @@ float Plane::stabilize_roll_get_roll_out(float speed_scaler)
   previously set nav_pitch and calculates servo_out values to try to
   stabilize the plane at the given attitude.
  */
-void Plane::stabilize_pitch(float speed_scaler)
+void Plane::stabilize_pitch()
 {
     int8_t force_elevator = takeoff_tail_hold();
     if (force_elevator != 0) {
@@ -162,12 +163,13 @@ void Plane::stabilize_pitch(float speed_scaler)
         return;
     }
 
-    const float pitch_out = stabilize_pitch_get_pitch_out(speed_scaler);
+    const float pitch_out = stabilize_pitch_get_pitch_out();
     SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_out);
 }
 
-float Plane::stabilize_pitch_get_pitch_out(float speed_scaler)
+float Plane::stabilize_pitch_get_pitch_out()
 {
+    const float speed_scaler = get_speed_scaler();
 #if HAL_QUADPLANE_ENABLED
     if (!quadplane.use_fw_attitude_controllers()) {
         // use the VTOL rate for control, to ensure consistency
@@ -296,7 +298,7 @@ void Plane::stabilize_stick_mixing_fbw()
     - rate controlled with ground steering
     - yaw control for coordinated flight    
  */
-void Plane::stabilize_yaw(float speed_scaler)
+void Plane::stabilize_yaw()
 {
     if (landing.is_flaring()) {
         // in flaring then enable ground steering
@@ -329,14 +331,14 @@ void Plane::stabilize_yaw(float speed_scaler)
     /*
       now calculate steering_control.rudder for the rudder
      */
-    calc_nav_yaw_coordinated(speed_scaler);
+    calc_nav_yaw_coordinated();
 }
 
 
 /*
   a special stabilization function for training mode
  */
-void Plane::stabilize_training(float speed_scaler)
+void Plane::stabilize_training()
 {
     const float rexpo = roll_in_expo(false);
     const float pexpo = pitch_in_expo(false);
@@ -344,7 +346,7 @@ void Plane::stabilize_training(float speed_scaler)
         SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo);
     } else {
         // calculate what is needed to hold
-        stabilize_roll(speed_scaler);
+        stabilize_roll();
         if ((nav_roll_cd > 0 && rexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)) ||
             (nav_roll_cd < 0 && rexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_aileron))) {
             // allow user to get out of the roll
@@ -355,7 +357,7 @@ void Plane::stabilize_training(float speed_scaler)
     if (training_manual_pitch) {
         SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo);
     } else {
-        stabilize_pitch(speed_scaler);
+        stabilize_pitch();
         if ((nav_pitch_cd > 0 && pexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)) ||
             (nav_pitch_cd < 0 && pexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_elevator))) {
             // allow user to get back to level
@@ -363,7 +365,7 @@ void Plane::stabilize_training(float speed_scaler)
         }
     }
 
-    stabilize_yaw(speed_scaler);
+    stabilize_yaw();
 }
 
 
@@ -371,14 +373,15 @@ void Plane::stabilize_training(float speed_scaler)
   this is the ACRO mode stabilization function. It does rate
   stabilization on roll and pitch axes
  */
-void Plane::stabilize_acro(float speed_scaler)
+void Plane::stabilize_acro()
 {
     if (g.acro_locking == 2 && g.acro_yaw_rate > 0 &&
         yawController.rate_control_enabled()) {
         // we can do 3D acro locking
-        stabilize_acro_quaternion(speed_scaler);
+        stabilize_acro_quaternion();
         return;
     }
+    const float speed_scaler = get_speed_scaler();
     const float rexpo = roll_in_expo(true);
     const float pexpo = pitch_in_expo(true);
     float roll_rate = (rexpo/SERVO_MAX) * g.acro_roll_rate;
@@ -448,7 +451,7 @@ void Plane::stabilize_acro(float speed_scaler)
         steering_control.steering = steering_control.rudder = yawController.get_rate_out(yaw_rate,  speed_scaler, false);
     } else if (plane.g2.flight_options & FlightOptions::ACRO_YAW_DAMPER) {
         // use yaw controller
-        calc_nav_yaw_coordinated(speed_scaler);
+        calc_nav_yaw_coordinated();
     } else {
         /*
           manual rudder
@@ -460,8 +463,9 @@ void Plane::stabilize_acro(float speed_scaler)
 /*
   quaternion based acro stabilization with continuous locking. Enabled with ACRO_LOCKING=2
  */
-void Plane::stabilize_acro_quaternion(float speed_scaler)
+void Plane::stabilize_acro_quaternion()
 {
+    const float speed_scaler = get_speed_scaler();
     auto &q = acro_state.q;
     const float rexpo = roll_in_expo(true);
     const float pexpo = pitch_in_expo(true);
@@ -567,7 +571,6 @@ void Plane::stabilize()
         steer_state.locked_course_err = 0;
         return;
     }
-    float speed_scaler = get_speed_scaler();
 
     uint32_t now = AP_HAL::millis();
     bool allow_stick_mixing = true;
@@ -591,10 +594,11 @@ void Plane::stabilize()
     last_stabilize_ms = now;
 
     if (control_mode == &mode_training) {
-        stabilize_training(speed_scaler);
+        stabilize_training();
 #if AP_SCRIPTING_ENABLED
     } else if (nav_scripting_active()) {
         // scripting is in control of roll and pitch rates and throttle
+        const float speed_scaler = get_speed_scaler();
         const float aileron = rollController.get_rate_out(nav_scripting.roll_rate_dps, speed_scaler);
         const float elevator = pitchController.get_rate_out(nav_scripting.pitch_rate_dps, speed_scaler);
         SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
@@ -605,14 +609,14 @@ void Plane::stabilize()
         }
 #endif
     } else if (control_mode == &mode_acro) {
-        stabilize_acro(speed_scaler);
+        stabilize_acro();
     } else if (control_mode == &mode_stabilize) {
-        stabilize_roll(speed_scaler);
+        stabilize_roll();
-        stabilize_pitch(speed_scaler);
+        stabilize_pitch();
         if (allow_stick_mixing) {
             stabilize_stick_mixing_direct();
         }
-        stabilize_yaw(speed_scaler);
+        stabilize_yaw();
 #if HAL_QUADPLANE_ENABLED
     } else if (control_mode->is_vtol_mode() && !quadplane.tailsitter.in_vtol_transition(now)) {
         // run controlers specific to this mode
@@ -620,10 +624,10 @@ void Plane::stabilize()
 
         // we also stabilize using fixed wing surfaces
         if (plane.control_mode->mode_number() == Mode::Number::QACRO) {
-            stabilize_acro(speed_scaler);
+            stabilize_acro();
         } else {
-            stabilize_roll(speed_scaler);
+            stabilize_roll();
-            stabilize_pitch(speed_scaler);
+            stabilize_pitch();
         }
 #endif
     } else {
@@ -632,9 +636,9 @@ void Plane::stabilize()
         if (allow_stick_mixing && ((g.stick_mixing == StickMixing::FBW) || (g.stick_mixing == StickMixing::DIRECT_REMOVED))) {
             stabilize_stick_mixing_fbw();
         }
-        stabilize_roll(speed_scaler);
+        stabilize_roll();
-        stabilize_pitch(speed_scaler);
+        stabilize_pitch();
-        stabilize_yaw(speed_scaler);
+        stabilize_yaw();
     }
 
     /*
@@ -688,8 +692,9 @@ void Plane::calc_throttle()
 /*
   calculate yaw control for coordinated flight
  */
-void Plane::calc_nav_yaw_coordinated(float speed_scaler)
+void Plane::calc_nav_yaw_coordinated()
 {
+    const float speed_scaler = get_speed_scaler();
     bool disable_integrator = false;
     int16_t rudder_in = rudder_input();
 

ArduPlane/Plane.h

@@ -856,17 +856,17 @@ private:
     float calc_speed_scaler(void);
     float get_speed_scaler(void) const { return surface_speed_scaler; }
     bool stick_mixing_enabled(void);
-    void stabilize_roll(float speed_scaler);
+    void stabilize_roll();
-    float stabilize_roll_get_roll_out(float speed_scaler);
+    float stabilize_roll_get_roll_out();
-    void stabilize_pitch(float speed_scaler);
+    void stabilize_pitch();
-    float stabilize_pitch_get_pitch_out(float speed_scaler);
+    float stabilize_pitch_get_pitch_out();
     void stabilize_stick_mixing_direct();
     void stabilize_stick_mixing_fbw();
-    void stabilize_yaw(float speed_scaler);
+    void stabilize_yaw();
-    void stabilize_training(float speed_scaler);
+    void stabilize_training();
-    void stabilize_acro(float speed_scaler);
+    void stabilize_acro();
-    void stabilize_acro_quaternion(float speed_scaler);
+    void stabilize_acro_quaternion();
-    void calc_nav_yaw_coordinated(float speed_scaler);
+    void calc_nav_yaw_coordinated();
     void calc_nav_yaw_course(void);
     void calc_nav_yaw_ground(void);