AP_Motors: scale roll/pitch/yaw gain for air density

libraries/AP_Motors/AP_MotorsMatrix.cpp

@@ -216,8 +216,8 @@ void AP_MotorsMatrix::output_armed_stabilizing()
     // set rpy_low and rpy_high to the lowest and highest values of the motors
     for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
         if (motor_enabled[i]) {
-            rpy_out[i] = _rc_roll.pwm_out * _roll_factor[i] * get_voltage_comp_gain() +
+            rpy_out[i] = _rc_roll.pwm_out * _roll_factor[i] * get_compensation_gain() +
-                            _rc_pitch.pwm_out * _pitch_factor[i] * get_voltage_comp_gain();
+                            _rc_pitch.pwm_out * _pitch_factor[i] * get_compensation_gain();
 
             // record lowest roll pitch command
             if (rpy_out[i] < rpy_low) {
@@ -249,16 +249,16 @@ void AP_MotorsMatrix::output_armed_stabilizing()
 
     if (_rc_yaw.pwm_out >= 0) {
         // if yawing right
-        if (yaw_allowed > _rc_yaw.pwm_out * get_voltage_comp_gain()) {
+        if (yaw_allowed > _rc_yaw.pwm_out * get_compensation_gain()) {
-            yaw_allowed = _rc_yaw.pwm_out * get_voltage_comp_gain(); // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
+            yaw_allowed = _rc_yaw.pwm_out * get_compensation_gain(); // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
         }else{
             limit.yaw = true;
         }
     }else{
         // if yawing left
         yaw_allowed = -yaw_allowed;
-        if (yaw_allowed < _rc_yaw.pwm_out * get_voltage_comp_gain()) {
+        if (yaw_allowed < _rc_yaw.pwm_out * get_compensation_gain()) {
-            yaw_allowed = _rc_yaw.pwm_out * get_voltage_comp_gain(); // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
+            yaw_allowed = _rc_yaw.pwm_out * get_compensation_gain(); // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
         }else{
             limit.yaw = true;
         }

libraries/AP_Motors/AP_Motors_Class.cpp

@@ -116,6 +116,7 @@ AP_Motors::AP_Motors(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_t
     _batt_current_resting(0.0f),
     _batt_resistance(0.0f),
     _batt_timer(0),
+    _air_density_ratio(1.0f),
     _lift_max(1.0f),
     _throttle_limit(1.0f),
     _throttle_in(0.0f),
@@ -365,6 +366,17 @@ void AP_Motors::update_throttle_low_comp()
     _throttle_low_comp = constrain_float(_throttle_low_comp, 0.1f, 1.0f);
 }
 
+float AP_Motors::get_compensation_gain() const
+{
+    float ret = 1.0f / _lift_max;
+
+    // air density ratio is increasing in density / decreasing in altitude
+    if (_air_density_ratio > 0.3f && _air_density_ratio < 1.5f) {
+        ret *= 1.0f / constrain_float(_air_density_ratio,0.5f,1.25f);
+    }
+    return ret;
+}
+
 float AP_Motors::rel_pwm_to_thr_range(float pwm) const
 {
     return 1000.0f*pwm/(_rc_throttle.radio_max-_rc_throttle.radio_min);

libraries/AP_Motors/AP_Motors_Class.h

@@ -156,6 +156,9 @@ public:
     // set_current - set current to be used for output scaling
     virtual void        set_current(float current){ _batt_current = current; }
 
+    // set_density_ratio - sets air density as a proportion of sea level density
+    void                set_air_density_ratio(float ratio) { _air_density_ratio = ratio; }
+
     // 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
@@ -225,8 +228,8 @@ protected:
     // 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
+    // return gain scheduling gain based on voltage and air density
-    float               get_voltage_comp_gain() const { return 1.0f/_lift_max; }
+    float               get_compensation_gain() const;
 
     float               rel_pwm_to_thr_range(float pwm) const;
     float               thr_range_to_rel_pwm(float thr) const;
@@ -275,6 +278,7 @@ protected:
     float               _batt_current_resting;  // battery's current when motors at minimum
     float               _batt_resistance;       // battery's resistance calculated by comparing resting voltage vs in flight voltage
     int16_t             _batt_timer;            // timer used in battery resistance calcs
+    float               _air_density_ratio;     // air density / sea level density - decreases in altitude
     float               _lift_max;              // maximum lift ratio from battery voltage
     float               _throttle_limit;        // ratio of throttle limit between hover and maximum
     float               _throttle_in;           // last throttle input from set_throttle caller