From 5e26450a6f4e12294f33f572578dd6cf342afcc9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 9 Mar 2015 22:23:16 +0900 Subject: [PATCH] AP_Motors: make THR_LOW_COMP a variable instead of param --- libraries/AP_Motors/AP_Motors_Class.cpp | 9 +++------ libraries/AP_Motors/AP_Motors_Class.h | 2 +- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 2d27eb98bf..18b401d274 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -53,12 +53,7 @@ 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: THR_LOW_CMP - // @DisplayName: Motor low throttle compensation - // @Description: Ratio controlling the max throttle output during competing requests of low throttle from the pilot (or autopilot) and higher throttle for attitude control - // @Values: 0.2:Favour Throttle Control, 0.5:Equal Weighting, 1:Favour Attitude Control - // @User: Advanced - AP_GROUPINFO("THR_LOW_CMP", 7, AP_Motors, _throttle_low_comp, AP_MOTORS_THR_LOW_CMP_DEFAULT), + // 7 was THR_LOW_CMP // @Param: THST_EXPO // @DisplayName: Thrust Curve Expo @@ -113,6 +108,8 @@ AP_Motors::AP_Motors(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_t _max_throttle(AP_MOTORS_DEFAULT_MAX_THROTTLE), _hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE), _spin_when_armed_ramped(0), + _throttle_low_comp(AP_MOTORS_THR_LOW_CMP_DEFAULT), + _throttle_low_comp_desired(AP_MOTORS_THR_LOW_CMP_DEFAULT), _batt_voltage(0.0f), _batt_voltage_resting(0.0f), _batt_current(0.0f), diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 5355ea4c0b..1ff7cd62f0 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -237,7 +237,6 @@ 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 _throttle_low_comp; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1 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 @@ -255,6 +254,7 @@ 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 as pct * 10 (i.e. 0 ~ 1000) int16_t _spin_when_armed_ramped;// equal to _spin_when_armed parameter but slowly ramped up from zero + float _throttle_low_comp; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1 float _throttle_low_comp_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds // battery voltage compensation variables