diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index ed83503a77..7789a65f68 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -116,6 +116,14 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = { // @User: Advanced AP_GROUPINFO("SPIN_ARM", 19, AP_MotorsMulticopter, _spin_arm, AP_MOTORS_SPIN_ARM_DEFAULT), + // @Param: BAT_CURR_TC + // @DisplayName: Motor Current Max Time Constant + // @Description: Time constant used to limit the maximum current + // @Range: 0 10 + // @Units: Seconds + // @User: Advanced + AP_GROUPINFO("BAT_CURR_TC", 20, AP_MotorsMulticopter, _batt_current_time_constant, AP_MOTORS_BAT_CURR_TC_DEFAULT), + // @Param: THST_HOVER // @DisplayName: Thrust Hover Value // @Description: Motor thrust needed to hover expressed as a number from 0 to 1 @@ -207,7 +215,6 @@ void AP_MotorsMulticopter::update_throttle_filter() } // return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max -//todo: replace this with a variable P term float AP_MotorsMulticopter::get_current_limit_max_throttle() { // return maximum if current limiting is disabled @@ -224,7 +231,8 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle() float batt_current_ratio = _batt_current/_batt_current_max; - _throttle_limit += AP_MOTORS_CURRENT_LIMIT_P*(1.0f - batt_current_ratio)/_loop_rate; + float loop_interval = 1.0f/_loop_rate; + _throttle_limit += (loop_interval/(loop_interval+_batt_current_time_constant))*(1.0f - batt_current_ratio); // throttle limit drops to 20% between hover and full throttle _throttle_limit = constrain_float(_throttle_limit, 0.2f, 1.0f); diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index 53a9f3879a..b94649b1e9 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -23,7 +23,7 @@ #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 -#define AP_MOTORS_CURRENT_LIMIT_P 0.2f // replace with parameter - Sets the current limit P term +#define AP_MOTORS_BAT_CURR_TC_DEFAULT 5.0f // Time constant used to limit the maximum current #define AP_MOTORS_BATT_VOLT_FILT_HZ 0.5f // battery voltage filtered at 0.5hz // spool definition @@ -153,6 +153,7 @@ protected: AP_Float _batt_voltage_max; // maximum voltage used to scale lift AP_Float _batt_voltage_min; // minimum voltage used to scale lift AP_Float _batt_current_max; // current over which maximum throttle is limited + AP_Float _batt_current_time_constant; // Time constant used to limit the maximum current 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