Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Motors: thrust curve and voltage scaling for matrix supported frames

Browse Source
This commit is contained in:
Leonard Hall 2015-02-23 15:35:10 +09:00 committed by Randy Mackay
parent 3e8563dd8b
commit 83e3e2fec2
3 changed files with 97 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -160,6 +160,18 @@ void AP_MotorsMatrix::output_armed()
_rc_throttle.calc_pwm();
_rc_yaw.calc_pwm();
if(_throttle_curve_enabled && _batt_voltage_max > 0 && _batt_voltage_min < _batt_voltage_max) {
float batt_voltage = _batt_voltage + _batt_current * _batt_resistance;
batt_voltage = constrain_float(batt_voltage, _batt_voltage_min, _batt_voltage_max);
// filter at 0.5 Hz
// todo: replace with filter object
_batt_voltage_filt = _batt_voltage_filt + 0.007792f*(batt_voltage/_batt_voltage_max-_batt_voltage_filt); // ratio of current battery voltage to maximum battery voltage
_lift_max = _batt_voltage_filt*(1-_thrust_curve_expo) + _thrust_curve_expo*_batt_voltage_filt*_batt_voltage_filt;
} else {
_batt_voltage_filt = 1;
_lift_max = 1;
}
// if we are not sending a throttle output, we cut the motors
if (_rc_throttle.servo_out == 0) {
// range check spin_when_armed
@ -179,6 +191,9 @@ void AP_MotorsMatrix::output_armed()
// Every thing is limited
limit.roll_pitch = true;
limit.yaw = true;
_batt_voltage_resting = _batt_voltage;
_batt_current_resting = _batt_current;
_batt_timer = 0;
} else {
@ -187,12 +202,19 @@ void AP_MotorsMatrix::output_armed()
limit.throttle_lower = true;
}
// calculate battery resistance
if (_batt_timer < 400 && _rc_throttle.radio_out >= _hover_out && ((_batt_current_resting*2.0f) < _batt_current)) {
// filter reaches 90% in 1/4 the test time
_batt_resistance += 0.05*(( (_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_resting) ) - _batt_resistance);
_batt_timer += 1;
}
// calculate roll and pitch for each motor
// 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] +
_rc_pitch.pwm_out * _pitch_factor[i];
rpy_out[i] = _rc_roll.pwm_out * _roll_factor[i]/_lift_max +
_rc_pitch.pwm_out * _pitch_factor[i]/_lift_max;
// record lowest roll pitch command
if (rpy_out[i] < rpy_low) {
@ -224,16 +246,16 @@ void AP_MotorsMatrix::output_armed()
if (_rc_yaw.pwm_out >= 0) {
// if yawing right
if (yaw_allowed > _rc_yaw.pwm_out) {
yaw_allowed = _rc_yaw.pwm_out; // 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/_lift_max) {
yaw_allowed = _rc_yaw.pwm_out/_lift_max; // 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 ) {
yaw_allowed = _rc_yaw.pwm_out; // 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/_lift_max ) {
yaw_allowed = _rc_yaw.pwm_out/_lift_max; // 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;
}
@ -314,7 +336,11 @@ void AP_MotorsMatrix::output_armed()
if (_throttle_curve_enabled) {
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = _throttle_curve.get_y(motor_out[i]);
float temp_out = ((float)(motor_out[i]-out_min_pwm))/((float)(out_max_pwm-out_min_pwm));
if (_thrust_curve_expo > 0.0f){
temp_out = ((_thrust_curve_expo-1.0f) + safe_sqrt((1.0f-_thrust_curve_expo)*(1.0f-_thrust_curve_expo) + 4.0f*_thrust_curve_expo*_lift_max*temp_out))/(2.0f*_thrust_curve_expo*_batt_voltage_filt);
}
motor_out[i] = temp_out*(_thrust_curve_max*out_max_pwm-out_min_pwm)+out_min_pwm;
}
}
}

40
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -77,6 +77,36 @@ const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = {
// @User: Advanced
AP_GROUPINFO("YAW_HEADROOM", 6, AP_Motors, _yaw_headroom, AP_MOTORS_YAW_HEADROOM_DEFAULT),
// @Param: THST_EXPO
// @DisplayName: Thrust Curve Expo
// @Description: Motor thrust curve exponent (from 0 for linear to 1.0 for second order curve)
// @Range: 0.25 0.8
// @User: Advanced
AP_GROUPINFO("THST_EXPO", 8, AP_Motors, _thrust_curve_expo, AP_MOTORS_THST_EXPO_DEFAULT),
// @Param: THST_MAX
// @DisplayName: Thrust Curve Max
// @Description: Point at which the thrust saturates
// @Values: 0.9:Low, 1.0:High
// @User: Advanced
AP_GROUPINFO("THST_MAX", 9, AP_Motors, _thrust_curve_max, AP_MOTORS_THST_MAX_DEFAULT),
// @Param: THST_BAT_MAX
// @DisplayName: Battery voltage compensation maximum voltage
// @Description: Battery voltage compensation maximum voltage (voltage above this will have no additional scaling effect on thrust). Recommend 4.4 * cell count, 0 = Disabled
// @Range: 6 35
// @Units: Volts
// @User: Advanced
AP_GROUPINFO("THST_BAT_MAX", 10, AP_Motors, _batt_voltage_max, AP_MOTORS_THST_BAT_MAX_DEFAULT),
// @Param: THST_BAT_MIN
// @DisplayName: Battery voltage compensation minimum voltage
// @Description: Battery voltage compensation minimum voltage (voltage below this will have no additional scaling effect on thrust). Recommend 3.5 * cell count, 0 = Disabled
// @Range: 6 35
// @Units: Volts
// @User: Advanced
AP_GROUPINFO("THST_BAT_MIN", 11, AP_Motors, _batt_voltage_min, AP_MOTORS_THST_BAT_MIN_DEFAULT),
AP_GROUPEND
};
@ -90,7 +120,15 @@ AP_Motors::AP_Motors( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_
_min_throttle(AP_MOTORS_DEFAULT_MIN_THROTTLE),
_max_throttle(AP_MOTORS_DEFAULT_MAX_THROTTLE),
_hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE),
_spin_when_armed_ramped(0)
_spin_when_armed_ramped(0),
_batt_voltage(0.0f),
_batt_voltage_resting(0.0f),
_batt_voltage_filt(1.0f),
_batt_current(0.0f),
_batt_current_resting(0.0f),
_batt_resistance(0.0f),
_batt_timer(0),
_lift_max(1.0f)
{
AP_Param::setup_object_defaults(this, var_info);

25
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -56,6 +56,11 @@
#define AP_MOTORS_YAW_HEADROOM_DEFAULT 200
#define AP_MOTORS_THST_EXPO_DEFAULT 0.5f // set to 0 for linear and 1 for second order approximation
#define AP_MOTORS_THST_MAX_DEFAULT 0.95f // throttle which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range
#define AP_MOTORS_THST_BAT_MAX_DEFAULT 0.0f
#define AP_MOTORS_THST_BAT_MIN_DEFAULT 0.0f
// bit mask for recording which limits we have reached when outputting to motors
#define AP_MOTOR_NO_LIMITS_REACHED 0x00
#define AP_MOTOR_ROLLPITCH_LIMIT 0x01
@ -132,6 +137,12 @@ public:
// set_yaw_headroom - set yaw headroom (yaw is given at least this amount of pwm)
virtual void set_yaw_headroom(int16_t pwm) { _yaw_headroom = pwm; }
// set_voltage - set voltage to be used for output scaling
virtual void set_voltage(float volts){ _batt_voltage = volts; }
// set_current - set current to be used for output scaling
virtual void set_current(float current){ _batt_current = current; }
// setup_throttle_curve - used to linearlise thrust output by motors
// returns true if curve is created successfully
bool setup_throttle_curve();
@ -186,6 +197,10 @@ protected:
AP_Int16 _spin_when_armed; // used to control whether the motors always spin when armed. pwm value above radio_min
AP_Int16 _yaw_headroom; // yaw control is given at least this pwm range
AP_Float _thrust_curve_expo; // curve used to linearize pwm to thrust conversion. set to 0 for linear and 1 for second order approximation
AP_Float _thrust_curve_max; // throttle which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range
AP_Float _batt_voltage_max; // maximum voltage used to scale lift
AP_Float _batt_voltage_min; // minimum voltage used to scale lift
// internal variables
RC_Channel& _rc_roll; // roll input in from users is held in servo_out
@ -197,5 +212,15 @@ 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 in pwm (i.e. 1000 ~ 2000). calculated from the THR_MID parameter
int16_t _spin_when_armed_ramped;// equal to _spin_when_armed parameter but slowly ramped up from zero
// battery voltage compensation variables
float _batt_voltage; // latest battery voltage reading
float _batt_voltage_resting; // battery voltage reading at minimum throttle
float _batt_voltage_filt; // filtered battery voltage
float _batt_current; // latest battery current reading
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 _lift_max; // maximum lift ratio from battery voltage
};
#endif // __AP_MOTORS_CLASS_H__