|
|
@ -333,10 +333,12 @@ float AP_MotorsMulticopter::get_compensation_gain() const |
|
|
|
|
|
|
|
|
|
|
|
float ret = 1.0f / _lift_max; |
|
|
|
float ret = 1.0f / _lift_max; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if AP_MOTORS_DENSITY_COMP == 1 |
|
|
|
// air density ratio is increasing in density / decreasing in altitude
|
|
|
|
// air density ratio is increasing in density / decreasing in altitude
|
|
|
|
if (_air_density_ratio > 0.3f && _air_density_ratio < 1.5f) { |
|
|
|
if (_air_density_ratio > 0.3f && _air_density_ratio < 1.5f) { |
|
|
|
ret *= 1.0f / constrain_float(_air_density_ratio,0.5f,1.25f); |
|
|
|
ret *= 1.0f / constrain_float(_air_density_ratio,0.5f,1.25f); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
#endif |
|
|
|
return ret; |
|
|
|
return ret; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|