AP_Motors: learned THST_HOVER replace THR_MID

This holds the estimated thrust required for the vehicle to hover
expressed as a number from 0 to 1.  0 will be equivalent to outputting
SPIN_MIN (which is a number from 0~1 but in the full output range) and 1 is equivalent to SPIN_MAX (also 0~1 but in the full output range)

libraries/AP_Motors/AP_MotorsHeli.h

@@ -125,6 +125,8 @@ public:
     // supports_yaw_passthrough
     virtual bool supports_yaw_passthrough() const { return false; }
 
+    float get_throttle_hover() const { return 0.5f; };
+
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
 

libraries/AP_Motors/AP_MotorsMulticopter.cpp

@@ -108,13 +108,19 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("PWM_MAX", 17, AP_MotorsMulticopter, _pwm_max, 0),
 
+    // @Param: THST_HOVER
+    // @DisplayName: Thrust Hover Value
+    // @Description: Motor thrust needed to hover expressed as a number from 0 to 1
+    // @Range: 0.25 0.8
+    // @User: Advanced
+    AP_GROUPINFO("THST_HOVER", 21, AP_MotorsMulticopter, _throttle_hover, AP_MOTORS_THST_HOVER_DEFAULT),
+
     AP_GROUPEND
 };
 
 // Constructor
 AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz) :
     AP_Motors(loop_rate, speed_hz),
-    _hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE),
     _batt_voltage_resting(0.0f),
     _batt_current_resting(0.0f),
     _batt_resistance(0.0f),
@@ -209,8 +215,7 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle()
     _throttle_limit = constrain_float(_throttle_limit, 0.2f, 1.0f);
 
     // limit max throttle
-    float throttle_thrust_hover = get_hover_throttle_as_high_end_pct();
+    return get_throttle_hover() + ((1.0-get_throttle_hover())*_throttle_limit);
-    return throttle_thrust_hover + ((1.0-throttle_thrust_hover)*_throttle_limit);
 }
 
 // apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
@@ -263,7 +268,7 @@ void AP_MotorsMulticopter::update_battery_resistance()
     } else {
         // update battery resistance when throttle is over hover throttle
         if ((_batt_timer < 400) && ((_batt_current_resting*2.0f) < _batt_current)) {
-            if (get_throttle() >= _hover_out) {
+            if (get_throttle() >= get_throttle_hover()) {
                 // filter reaches 90% in 1/4 the test time
                 _batt_resistance += 0.05f*(( (_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_resting) ) - _batt_resistance);
                 _batt_timer += 1;
@@ -275,11 +280,6 @@ void AP_MotorsMulticopter::update_battery_resistance()
     }
 }
 
-float AP_MotorsMulticopter::get_hover_throttle_as_high_end_pct() const
-{
-    return (MAX(0,(float)_hover_out-_min_throttle) / (float)(1000-_min_throttle));
-}
-
 float AP_MotorsMulticopter::get_compensation_gain() const
 {
     // avoid divide by zero
@@ -338,6 +338,12 @@ void AP_MotorsMulticopter::set_throttle_range(uint16_t min_throttle, int16_t rad
     _min_throttle = (float)min_throttle * ((get_pwm_output_max() - get_pwm_output_min()) / 1000.0f);
 }
 
+// update the throttle input filter.  should be called at 100hz
+void AP_MotorsMulticopter::update_throttle_hover(float dt)
+{
+        _throttle_hover = _throttle_hover + (dt/(dt+AP_MOTORS_THST_HOVER_TC))*(_throttle_in-_throttle_hover);
+}
+
 void AP_MotorsMulticopter::output_logic()
 {
     // force desired and current spool mode if disarmed or not interlocked

libraries/AP_Motors/AP_MotorsMulticopter.h

@@ -16,6 +16,8 @@
 #define AP_MOTORS_YAW_HEADROOM_DEFAULT  200
 #define AP_MOTORS_THST_EXPO_DEFAULT     0.65f   // set to 0 for linear and 1 for second order approximation
 #define AP_MOTORS_SPIN_MAX_DEFAULT      0.95f   // throttle which produces the maximum thrust.  (i.e. 0 ~ 1 ) of the full throttle range
+#define AP_MOTORS_THST_HOVER_DEFAULT    0.5f    // the estimated hover throttle, 0 ~ 1
+#define AP_MOTORS_THST_HOVER_TC         10.0f   // time constant used to update estimated hover throttle, 0 ~ 1
 #define AP_MOTORS_BAT_VOLT_MAX_DEFAULT  0.0f    // voltage limiting max default
 #define AP_MOTORS_BAT_VOLT_MIN_DEFAULT  0.0f    // voltage limiting min default (voltage dropping below this level will have no effect)
 #define AP_MOTORS_BAT_CURR_MAX_DEFAULT  0.0f    // current limiting max default
@@ -48,9 +50,9 @@ public:
     // also sets minimum and maximum pwm values that will be sent to the motors
     void                set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max);
 
-    // set_hover_throttle - sets the mid throttle which is close to the hover throttle of the copter
+    // update estimated throttle required to hover
-    // this is used to limit the amount that the stability patch will increase the throttle to give more room for roll, pitch and yaw control
+    void                update_throttle_hover(float dt);
-    void                set_hover_throttle(uint16_t hov_thr) { _hover_out = hov_thr; }
+    virtual float       get_throttle_hover() const { return _throttle_hover; };
 
     // spool up states
     enum spool_up_down_mode {
@@ -123,9 +125,6 @@ protected:
     // return gain scheduling gain based on voltage and air density
     float               get_compensation_gain() const;
 
-    // get_hover_throttle_as_high_end_pct - return hover throttle in the 0 to 1 range
-    float               get_hover_throttle_as_high_end_pct() const;
-
     // convert thrust (0~1) range back to pwm range
     int16_t             calc_thrust_to_pwm(float thrust_in) const;
 
@@ -148,11 +147,11 @@ protected:
     AP_Float            _batt_current_max;      // current over which maximum throttle is limited
     AP_Int16            _pwm_min;               // minimum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's min pwm used)
     AP_Int16            _pwm_max;               // maximum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's max pwm used)
+    AP_Float            _throttle_hover;        // estimated throttle required to hover throttle in the range 0 ~ 1
 
     // internal variables
     bool                motor_enabled[AP_MOTORS_MAX_NUM_MOTORS];    // true if motor is enabled
     int16_t             _min_throttle;              // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying)
-    int16_t             _hover_out;                 // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000)
     int16_t             _throttle_radio_min;        // minimum PWM from RC input's throttle channel (i.e. minimum PWM input from receiver, RC3_MIN)
     int16_t             _throttle_radio_max;        // maximum PWM from RC input's throttle channel (i.e. maximum PWM input from receiver, RC3_MAX)
     float               _throttle_thrust_max;       // the maximum allowed throttle thrust 0.0 to 1.0 in the range throttle_min to throttle_max

libraries/AP_Motors/AP_Motors_Class.h

@@ -71,6 +71,7 @@ public:
     float               get_pitch() const { return _pitch_in; }
     float               get_yaw() const { return _yaw_in; }
     float               get_throttle() const { return constrain_float(_throttle_filter.get(),0.0f,1.0f); }
+    virtual float       get_throttle_hover() const = 0;
 
     // spool up states
     enum spool_up_down_desired {