From 2c811364d3720aeed1ac9214c08836cfc422f364 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 9 Jun 2016 12:42:38 +0900 Subject: [PATCH] AP_MotorsMulticopter: SPIN_ARM param replaces SPIN_ARMED --- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 17 +++++++++-------- libraries/AP_Motors/AP_MotorsMulticopter.h | 6 +++--- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 44548a2899..6079b4a309 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -29,13 +29,7 @@ extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = { // 0 was used by TB_RATIO // 1,2,3 were used by throttle curve - - // @Param: SPIN_ARMED - // @DisplayName: Motors always spin when armed - // @Description: Controls whether motors always spin when armed (must be below THR_MIN) - // @Values: 0:Do Not Spin,70:VerySlow,100:Slow,130:Medium,150:Fast - // @User: Standard - AP_GROUPINFO("SPIN_ARMED", 5, AP_MotorsMulticopter, _spin_when_armed, AP_MOTORS_SPIN_WHEN_ARMED), + // 5 was SPIN_ARMED // @Param: YAW_HEADROOM // @DisplayName: Matrix Yaw Min @@ -108,6 +102,13 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = { // @User: Advanced AP_GROUPINFO("PWM_MAX", 17, AP_MotorsMulticopter, _pwm_max, 0), + // @Param: SPIN_ARM + // @DisplayName: Motor Spin armed + // @Description: Point at which the motors start to spin expressed as a number from 0 to 1 in the entire output range + // @Values: 0.0:Low, 0.1:Default, 0.2:High + // @User: Advanced + AP_GROUPINFO("SPIN_ARM", 19, AP_MotorsMulticopter, _thrust_curve_arm, AP_MOTORS_SPIN_ARM_DEFAULT), + // @Param: THST_HOVER // @DisplayName: Thrust Hover Value // @Description: Motor thrust needed to hover expressed as a number from 0 to 1 @@ -415,7 +416,7 @@ void AP_MotorsMulticopter::output_logic() } else { // _spool_desired == SPIN_WHEN_ARMED float spin_up_armed_ratio = 0.0f; if (_min_throttle > 0) { - spin_up_armed_ratio = (float)_spin_when_armed / _min_throttle; + spin_up_armed_ratio = _thrust_curve_arm / _min_throttle; } _spin_up_ratio += constrain_float(spin_up_armed_ratio-_spin_up_ratio, -spool_step, spool_step); } diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index b9ce18fae0..fc5e43e247 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -15,9 +15,10 @@ #define AP_MOTORS_SPIN_WHEN_ARMED 70 // spin motors at this PWM value when armed #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_SPIN_MAX_DEFAULT 0.95f // throttle out ratio which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range +#define AP_MOTORS_SPIN_ARM_DEFAULT 0.10f // throttle out ratio which produces the armed spin rate. (i.e. 0 ~ 1 ) of the full throttle range #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 @@ -143,11 +144,10 @@ protected: } _multicopter_flags; // parameters - 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 _thrust_curve_arm; // throttle out ratio which produces the armed spin rate. (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 AP_Float _batt_current_max; // current over which maximum throttle is limited