From 83e3e2fec2774f60c0a9e1d42e8124b968774f95 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 23 Feb 2015 15:35:10 +0900 Subject: [PATCH] Motors: thrust curve and voltage scaling for matrix supported frames --- libraries/AP_Motors/AP_MotorsMatrix.cpp | 40 ++++++++++++++++++++----- libraries/AP_Motors/AP_Motors_Class.cpp | 40 ++++++++++++++++++++++++- libraries/AP_Motors/AP_Motors_Class.h | 25 ++++++++++++++++ 3 files changed, 97 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 0cd214a473..5890f8c33a 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -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= 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 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; } } } diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index e79886e6c1..9bd48784fc 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -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); diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 8c4f065b21..99241d188c 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -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__