AP_Motors: Low throttle compensation setters

libraries/AP_Motors/AP_Motors_Class.cpp

@@ -179,6 +179,9 @@ void AP_Motors::output()
     // calc filtered battery voltage and lift_max
     update_lift_max_from_batt_voltage();
 
+    // move throttle_low_comp towards desired throttle low comp
+    update_throttle_low_comp();
+
     // output to motors
     if (_flags.armed ) {
         output_armed();
@@ -318,3 +321,17 @@ void AP_Motors::update_battery_resistance()
         }
     }
 }
+
+// update_throttle_low_comp - slew set_throttle_low_comp to requested value
+void AP_Motors::update_throttle_low_comp()
+{
+    // slew _throttle_low_comp to _throttle_low_comp_desired
+    if (_throttle_low_comp < _throttle_low_comp_desired) {
+        // increase quickly (i.e. from 0.1 to 0.9 in 0.8 seconds)
+        _throttle_low_comp += min(1.0f/_loop_rate, _throttle_low_comp_desired-_throttle_low_comp);
+    } else if (_throttle_low_comp > _throttle_low_comp_desired) {
+        // reduce more slowly (from 0.9 to 0.1 in 1.8 seconds)
+        _throttle_low_comp -= min(0.5f/_loop_rate, _throttle_low_comp-_throttle_low_comp_desired);
+    }
+    _throttle_low_comp = constrain_float(_throttle_low_comp, 0.1f, 1.0f);
+}

libraries/AP_Motors/AP_Motors_Class.h

@@ -156,6 +156,11 @@ public:
     // set_current - set current to be used for output scaling
     virtual void        set_current(float current){ _batt_current = current; }
 
+    // set_throttle_low_comp - set desired throttle_low_comp (actual throttle_low_comp is slewed towards this value over 1~2 seconds)
+    //  low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
+    //  has no effect when throttle is above hover throttle
+    void                set_throttle_low_comp(float throttle_low_comp) { _throttle_low_comp_desired = throttle_low_comp; }
+
     // get_lift_max - get maximum lift ratio
     float               get_lift_max() { return _lift_max; }
 
@@ -211,6 +216,9 @@ protected:
     // update_battery_resistance - calculate battery resistance when throttle is above hover_out
     void                update_battery_resistance();
 
+    // update_throttle_low_comp - updates thr_low_comp value towards the target
+    void                update_throttle_low_comp();
+
     // get_voltage_comp_gain - return battery voltage compensation gain
     float               get_voltage_comp_gain() const { return 1.0f/_lift_max; }
 
@@ -247,6 +255,7 @@ protected:
     int16_t             _max_throttle;          // the maximum throttle to be sent to the motors (sometimes limited by slow start)
     int16_t             _hover_out;             // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000)
     int16_t             _spin_when_armed_ramped;// equal to _spin_when_armed parameter but slowly ramped up from zero
+    float               _throttle_low_comp_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
 
     // battery voltage compensation variables
     float               _batt_voltage;          // latest battery voltage reading