Copter: make get_pilot_desired_throttle a method on Mode

ArduCopter/Attitude.cpp

@@ -70,17 +70,21 @@ void Copter::set_throttle_takeoff()
     pos_control->init_takeoff();
 }
 
+float Copter::Mode::throttle_hover() const
+{
+    return motors->get_throttle_hover();
+}
+
 // transform pilot's manual throttle input to make hover throttle mid stick
 // used only for manual throttle modes
 // thr_mid should be in the range 0 to 1
 // returns throttle output 0 to 1
-float Copter::get_pilot_desired_throttle(int16_t throttle_control, float thr_mid)
+float Copter::Mode::get_pilot_desired_throttle() const
 {
-    if (thr_mid <= 0.0f) {
+    const float thr_mid = throttle_hover();
-        thr_mid = motors->get_throttle_hover();
+    int16_t throttle_control = channel_throttle->get_control_in();
-    }
 
-    int16_t mid_stick = get_throttle_mid();
+    int16_t mid_stick = copter.get_throttle_mid();
     // protect against unlikely divide by zero
     if (mid_stick <= 0) {
         mid_stick = 500;

ArduCopter/Copter.h

@@ -618,7 +618,6 @@ private:
     float get_pilot_desired_yaw_rate(int16_t stick_angle);
     void update_throttle_hover();
     void set_throttle_takeoff();
-    float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid = 0.0f);
     float get_pilot_desired_climb_rate(float throttle_control);
     float get_non_takeoff_throttle();
     float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);

ArduCopter/mode.cpp

@@ -563,11 +563,6 @@ float Copter::Mode::get_pilot_desired_climb_rate(float throttle_control)
     return copter.get_pilot_desired_climb_rate(throttle_control);
 }
 
-float Copter::Mode::get_pilot_desired_throttle(int16_t throttle_control, float thr_mid)
-{
-    return copter.get_pilot_desired_throttle(throttle_control, thr_mid);
-}
-
 float Copter::Mode::get_non_takeoff_throttle()
 {
     return copter.get_non_takeoff_throttle();

ArduCopter/mode.h

@@ -121,6 +121,9 @@ protected:
     void land_run_horizontal_control();
     void land_run_vertical_control(bool pause_descent = false);
 
+    // return expected input throttle setting to hover:
+    virtual float throttle_hover() const;
+
     // convenience references to avoid code churn in conversion:
     Parameters &g;
     ParametersG2 &g2;
@@ -185,7 +188,7 @@ protected:
     float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);
     float get_pilot_desired_yaw_rate(int16_t stick_angle);
     float get_pilot_desired_climb_rate(float throttle_control);
-    float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid = 0.0f);
+    float get_pilot_desired_throttle() const;
     float get_non_takeoff_throttle(void);
     void update_simple_mode(void);
     bool set_mode(control_mode_t mode, mode_reason_t reason);
@@ -222,6 +225,13 @@ protected:
 
     void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
 
+    float throttle_hover() const override {
+        if (g2.acro_thr_mid > 0) {
+            return g2.acro_thr_mid;
+        }
+        return Copter::Mode::throttle_hover();
+    }
+
 private:
 
 };

ArduCopter/mode_acro.cpp

@@ -12,7 +12,7 @@ bool Copter::ModeAcro::init(bool ignore_checks)
 {
    // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
    if (motors->armed() && ap.land_complete && !copter.flightmode->has_manual_throttle() &&
-           (get_pilot_desired_throttle(channel_throttle->get_control_in(), copter.g2.acro_thr_mid) > copter.get_non_takeoff_throttle())) {
+           (get_pilot_desired_throttle() > copter.get_non_takeoff_throttle())) {
        return false;
    }
 
@@ -22,7 +22,6 @@ bool Copter::ModeAcro::init(bool ignore_checks)
 void Copter::ModeAcro::run()
 {
     float target_roll, target_pitch, target_yaw;
-    float pilot_throttle_scaled;
 
     // if not armed set throttle to zero and exit immediately
     if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) {
@@ -38,14 +37,13 @@ void Copter::ModeAcro::run()
     // convert the input to the desired body frame rate
     get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw);
 
-    // get pilot's desired throttle
-    pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in(), g2.acro_thr_mid);
-
     // run attitude controller
     attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
 
     // output pilot's throttle without angle boost
-    attitude_control->set_throttle_out(pilot_throttle_scaled, false, copter.g.throttle_filt);
+    attitude_control->set_throttle_out(get_pilot_desired_throttle(),
+                                       false,
+                                       copter.g.throttle_filt);
 }
 
 

ArduCopter/mode_drift.cpp

@@ -42,7 +42,6 @@ void Copter::ModeDrift::run()
     static float roll_input = 0.0f;
     float target_roll, target_pitch;
     float target_yaw_rate;
-    float pilot_throttle_scaled;
 
     // if landed and throttle at zero, set throttle to zero and exit immediately
     if (!motors->armed() || !motors->get_interlock() || (ap.land_complete && ap.throttle_zero)) {
@@ -58,9 +57,6 @@ void Copter::ModeDrift::run()
     // convert pilot input to lean angles
     get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
 
-    // get pilot's desired throttle
-    pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());
-
     // Grab inertial velocity
     const Vector3f& vel = inertial_nav.get_velocity();
 
@@ -101,7 +97,8 @@ void Copter::ModeDrift::run()
     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
 
     // output pilot's throttle with angle boost
-    attitude_control->set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt);
+    const float assisted_throttle = get_throttle_assist(vel.z, get_pilot_desired_throttle());
+    attitude_control->set_throttle_out(assisted_throttle, true, g.throttle_filt);
 }
 
 // get_throttle_assist - return throttle output (range 0 ~ 1) based on pilot input and z-axis velocity

ArduCopter/mode_flip.cpp

@@ -100,7 +100,7 @@ void Copter::ModeFlip::run()
     }
 
     // get pilot's desired throttle
-    float throttle_out = get_pilot_desired_throttle(channel_throttle->get_control_in());
+    float throttle_out = get_pilot_desired_throttle();
 
     // get corrected angle based on direction and axis of rotation
     // we flip the sign of flip_angle to minimize the code repetition

ArduCopter/mode_stabilize.cpp

@@ -9,7 +9,7 @@ bool Copter::ModeStabilize::init(bool ignore_checks)
 {
     // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
     if (motors->armed() && ap.land_complete && !copter.flightmode->has_manual_throttle() &&
-            (get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) {
+            (get_pilot_desired_throttle() > get_non_takeoff_throttle())) {
         return false;
     }
 
@@ -22,7 +22,6 @@ void Copter::ModeStabilize::run()
 {
     float target_roll, target_pitch;
     float target_yaw_rate;
-    float pilot_throttle_scaled;
 
     // if not armed set throttle to zero and exit immediately
     if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) {
@@ -44,14 +43,13 @@ void Copter::ModeStabilize::run()
     // get pilot's desired yaw rate
     target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
 
-    // get pilot's desired throttle
-    pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());
-
     // call attitude controller
     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
 
     // body-frame rate controller is run directly from 100hz loop
 
     // output pilot's throttle
-    attitude_control->set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt);
+    attitude_control->set_throttle_out(get_pilot_desired_throttle(),
+                                       true,
+                                       g.throttle_filt);
 }