Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

This commit is contained in:
Jonathan Challinger 2015-04-28 11:19:50 -07:00 committed by Randy Mackay
parent e2ba351149
commit 41ae7207c7
3 changed files with 24 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -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() +
_rc_pitch.pwm_out * _pitch_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_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()) {
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
if (yaw_allowed > _rc_yaw.pwm_out * get_compensation_gain()) {
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()) {
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
if (yaw_allowed < _rc_yaw.pwm_out * get_compensation_gain()) {
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;
}

12
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -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);

8
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -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
float get_voltage_comp_gain() const { return 1.0f/_lift_max; }
// return gain scheduling gain based on voltage and air density
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