diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index d63e4db6ea..2260836385 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -125,6 +125,8 @@ public: // supports_yaw_passthrough virtual bool supports_yaw_passthrough() const { return false; } + float get_throttle_hover() const { return 0.5f; }; + // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index ea817e3733..bb4da7ec90 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -108,13 +108,19 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = { // @User: Advanced AP_GROUPINFO("PWM_MAX", 17, AP_MotorsMulticopter, _pwm_max, 0), + // @Param: THST_HOVER + // @DisplayName: Thrust Hover Value + // @Description: Motor thrust needed to hover expressed as a number from 0 to 1 + // @Range: 0.25 0.8 + // @User: Advanced + AP_GROUPINFO("THST_HOVER", 21, AP_MotorsMulticopter, _throttle_hover, AP_MOTORS_THST_HOVER_DEFAULT), + AP_GROUPEND }; // Constructor AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz) : AP_Motors(loop_rate, speed_hz), - _hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE), _batt_voltage_resting(0.0f), _batt_current_resting(0.0f), _batt_resistance(0.0f), @@ -209,8 +215,7 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle() _throttle_limit = constrain_float(_throttle_limit, 0.2f, 1.0f); // limit max throttle - float throttle_thrust_hover = get_hover_throttle_as_high_end_pct(); - return throttle_thrust_hover + ((1.0-throttle_thrust_hover)*_throttle_limit); + return get_throttle_hover() + ((1.0-get_throttle_hover())*_throttle_limit); } // apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1 @@ -263,7 +268,7 @@ void AP_MotorsMulticopter::update_battery_resistance() } else { // update battery resistance when throttle is over hover throttle if ((_batt_timer < 400) && ((_batt_current_resting*2.0f) < _batt_current)) { - if (get_throttle() >= _hover_out) { + if (get_throttle() >= get_throttle_hover()) { // filter reaches 90% in 1/4 the test time _batt_resistance += 0.05f*(( (_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_resting) ) - _batt_resistance); _batt_timer += 1; @@ -275,11 +280,6 @@ void AP_MotorsMulticopter::update_battery_resistance() } } -float AP_MotorsMulticopter::get_hover_throttle_as_high_end_pct() const -{ - return (MAX(0,(float)_hover_out-_min_throttle) / (float)(1000-_min_throttle)); -} - float AP_MotorsMulticopter::get_compensation_gain() const { // avoid divide by zero @@ -338,6 +338,12 @@ void AP_MotorsMulticopter::set_throttle_range(uint16_t min_throttle, int16_t rad _min_throttle = (float)min_throttle * ((get_pwm_output_max() - get_pwm_output_min()) / 1000.0f); } +// update the throttle input filter. should be called at 100hz +void AP_MotorsMulticopter::update_throttle_hover(float dt) +{ + _throttle_hover = _throttle_hover + (dt/(dt+AP_MOTORS_THST_HOVER_TC))*(_throttle_in-_throttle_hover); +} + void AP_MotorsMulticopter::output_logic() { // force desired and current spool mode if disarmed or not interlocked diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index 7d06d41443..049ca8dea7 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -16,6 +16,8 @@ #define AP_MOTORS_YAW_HEADROOM_DEFAULT 200 #define AP_MOTORS_THST_EXPO_DEFAULT 0.65f // set to 0 for linear and 1 for second order approximation #define AP_MOTORS_SPIN_MAX_DEFAULT 0.95f // throttle which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range +#define AP_MOTORS_THST_HOVER_DEFAULT 0.5f // the estimated hover throttle, 0 ~ 1 +#define AP_MOTORS_THST_HOVER_TC 10.0f // time constant used to update estimated hover throttle, 0 ~ 1 #define AP_MOTORS_BAT_VOLT_MAX_DEFAULT 0.0f // voltage limiting max default #define AP_MOTORS_BAT_VOLT_MIN_DEFAULT 0.0f // voltage limiting min default (voltage dropping below this level will have no effect) #define AP_MOTORS_BAT_CURR_MAX_DEFAULT 0.0f // current limiting max default @@ -48,9 +50,9 @@ public: // also sets minimum and maximum pwm values that will be sent to the motors void set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max); - // set_hover_throttle - sets the mid throttle which is close to the hover throttle of the copter - // this is used to limit the amount that the stability patch will increase the throttle to give more room for roll, pitch and yaw control - void set_hover_throttle(uint16_t hov_thr) { _hover_out = hov_thr; } + // update estimated throttle required to hover + void update_throttle_hover(float dt); + virtual float get_throttle_hover() const { return _throttle_hover; }; // spool up states enum spool_up_down_mode { @@ -123,9 +125,6 @@ protected: // return gain scheduling gain based on voltage and air density float get_compensation_gain() const; - // get_hover_throttle_as_high_end_pct - return hover throttle in the 0 to 1 range - float get_hover_throttle_as_high_end_pct() const; - // convert thrust (0~1) range back to pwm range int16_t calc_thrust_to_pwm(float thrust_in) const; @@ -148,11 +147,11 @@ protected: AP_Float _batt_current_max; // current over which maximum throttle is limited AP_Int16 _pwm_min; // minimum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's min pwm used) AP_Int16 _pwm_max; // maximum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's max pwm used) + AP_Float _throttle_hover; // estimated throttle required to hover throttle in the range 0 ~ 1 // internal variables bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS]; // true if motor is enabled int16_t _min_throttle; // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying) - int16_t _hover_out; // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000) int16_t _throttle_radio_min; // minimum PWM from RC input's throttle channel (i.e. minimum PWM input from receiver, RC3_MIN) int16_t _throttle_radio_max; // maximum PWM from RC input's throttle channel (i.e. maximum PWM input from receiver, RC3_MAX) float _throttle_thrust_max; // the maximum allowed throttle thrust 0.0 to 1.0 in the range throttle_min to throttle_max diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 8d204b0241..0c82fd544b 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -71,6 +71,7 @@ public: float get_pitch() const { return _pitch_in; } float get_yaw() const { return _yaw_in; } float get_throttle() const { return constrain_float(_throttle_filter.get(),0.0f,1.0f); } + virtual float get_throttle_hover() const = 0; // spool up states enum spool_up_down_desired {