From c9c803ffd499d1fdae3b4e32669bb9b0370688ac Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 9 Nov 2013 14:25:06 +0900 Subject: [PATCH] TradHeli: move STAB_COL_MIN to main parameter list --- ArduCopter/ArduCopter.pde | 2 +- ArduCopter/Parameters.h | 10 +++++--- ArduCopter/Parameters.pde | 19 +++++++++++++++ ArduCopter/config.h | 2 ++ ArduCopter/heli.pde | 15 ++++++++++++ libraries/AP_Motors/AP_MotorsHeli.cpp | 34 +-------------------------- libraries/AP_Motors/AP_MotorsHeli.h | 10 -------- 7 files changed, 45 insertions(+), 47 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 302017fa31..70dee0bf8c 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -2027,7 +2027,7 @@ void update_throttle_mode(void) case THROTTLE_MANUAL_HELI: // trad heli manual throttle controller // send pilot's output directly to swash plate - pilot_throttle_scaled = motors.get_pilot_desired_collective(g.rc_3.control_in); + pilot_throttle_scaled = get_pilot_desired_collective(g.rc_3.control_in); set_throttle_out(pilot_throttle_scaled, false); // update estimate of throttle cruise diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 7f50bf18b1..bfadeecd39 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -114,9 +114,11 @@ public: k_param_heli_servo_2, k_param_heli_servo_3, k_param_heli_servo_4, - k_param_heli_pitch_ff, - k_param_heli_roll_ff, - k_param_heli_yaw_ff, + k_param_heli_pitch_ff, + k_param_heli_roll_ff, + k_param_heli_yaw_ff, + k_param_heli_stab_col_min, + k_param_heli_stab_col_max, // 88 // // 90: Motors @@ -357,6 +359,8 @@ public: AP_Float heli_pitch_ff; // pitch rate feed-forward AP_Float heli_roll_ff; // roll rate feed-forward AP_Float heli_yaw_ff; // yaw rate feed-forward + AP_Int16 heli_stab_col_min; // min collective while pilot directly controls collective in stabilize mode + AP_Int16 heli_stab_col_max; // min collective while pilot directly controls collective in stabilize mode #endif #if FRAME_CONFIG == SINGLE_FRAME // Single diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index ba9fce3a3b..247a71546d 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -453,7 +453,26 @@ const AP_Param::Info var_info[] PROGMEM = { // @Increment: 0.01 // @User: Standard GSCALAR(heli_yaw_ff, "RATE_YAW_FF", HELI_YAW_FF), + + // @Param: STAB_COL_MIN + // @DisplayName: Heli Stabilize Throttle Collective Minimum + // @Description: Helicopter's minimum collective position while pilot directly controls collective in stabilize mode + // @Range: 0 500 + // @Units: pwm + // @Increment: 1 + // @User: Standard + GSCALAR(heli_stab_col_min, "STAB_COL_MIN", HELI_STAB_COLLECTIVE_MIN_DEFAULT), + + // @Param: STAB_COL_MAX + // @DisplayName: Stabilize Throttle Maximum + // @Description: Helicopter's maximum collective position while pilot directly controls collective in stabilize mode + // @Range: 500 1000 + // @Units: pwm + // @Increment: 1 + // @User: Standard + GSCALAR(heli_stab_col_max, "STAB_COL_MAX", HELI_STAB_COLLECTIVE_MAX_DEFAULT), #endif + #if FRAME_CONFIG == SINGLE_FRAME // @Group: SS1_ // @Path: ../libraries/RC_Channel/RC_Channel.cpp diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 9cf4a58443..a2726160b7 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -113,6 +113,8 @@ # define HELI_YAW_FF 0 # define STABILIZE_THR THROTTLE_MANUAL_HELI # define MPU6K_FILTER 10 + # define HELI_STAB_COLLECTIVE_MIN_DEFAULT 0 + # define HELI_STAB_COLLECTIVE_MAX_DEFAULT 1000 #endif ///////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/heli.pde b/ArduCopter/heli.pde index a621f8a887..bef862a934 100644 --- a/ArduCopter/heli.pde +++ b/ArduCopter/heli.pde @@ -16,6 +16,21 @@ static struct { uint8_t dynamic_flight : 1; // 0 // true if we are moving at a significant speed (used to turn on/off leaky I terms) } heli_flags; +// get_pilot_desired_collective - converts pilot input (from 0 ~ 1000) to a value that can be fed into the g.rc_3.servo_out function +static int16_t get_pilot_desired_collective(int16_t control_in) +{ + // return immediately if reduce collective range for manual flight has not been configured + if (g.heli_stab_col_min == 0 && g.heli_stab_col_max == 1000) { + return control_in; + } + + // scale pilot input to reduced collective range + float scalar = ((float)(g.heli_stab_col_max - g.heli_stab_col_min))/1000.0f; + int16_t collective_out = g.heli_stab_col_min + control_in * scalar; + collective_out = constrain_int16(collective_out, 0, 1000); + return collective_out; +} + // heli_check_dynamic_flight - updates the dynamic_flight flag based on our horizontal velocity // should be called at 50hz static void check_dynamic_flight(void) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 3058e78dad..2c029ad1eb 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -170,23 +170,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @User: Standard AP_GROUPINFO("FLYBAR_MODE", 18, AP_MotorsHeli, _flybar_mode, AP_MOTORS_HELI_NOFLYBAR), - // @Param: STAB_COL_MIN - // @DisplayName: Stabilize Throttle Minimum - // @Description: Minimum collective position while pilot directly controls collective - // @Range: 0 50 - // @Units: Percent - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("STAB_COL_MIN", 19, AP_MotorsHeli, _manual_collective_min, AP_MOTORS_HELI_MANUAL_COLLECTIVE_MIN), - - // @Param: STAB_COL_MAX - // @DisplayName: Stabilize Throttle Maximum - // @Description: Maximum collective position while pilot directly controls collective - // @Range: 50 100 - // @Units: Percent - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("STAB_COL_MAX", 20, AP_MotorsHeli, _manual_collective_max, AP_MOTORS_HELI_MANUAL_COLLECTIVE_MAX), + // 19,20 - was STAB_COL_MIN, STAB_COL_MAX now moved to main code's parameter list // @Param: LAND_COL_MIN // @DisplayName: Landing Collective Minimum @@ -354,21 +338,6 @@ bool AP_MotorsHeli::allow_arming() return true; } -// get_pilot_desired_collective - converts pilot input (from 0 ~ 1000) to a value that can be fed into the move_swash function -int16_t AP_MotorsHeli::get_pilot_desired_collective(int16_t control_in) -{ - // return immediately if reduce collective range for manual flight has not been configured - if (_manual_collective_min == 0 && _manual_collective_max == 100) { - return control_in; - } - - // scale - int16_t collective_out; - collective_out = _manual_collective_min*10 + control_in * _collective_scalar_manual; - collective_out = constrain_int16(collective_out, 0, 1000); - return collective_out; -} - // set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000 void AP_MotorsHeli::set_desired_rotor_speed(int16_t desired_speed) { @@ -490,7 +459,6 @@ void AP_MotorsHeli::init_swash() _roll_scaler = (float)_roll_max/4500.0f; _pitch_scaler = (float)_pitch_max/4500.0f; _collective_scalar = ((float)(_collective_max-_collective_min))/1000.0f; - _collective_scalar_manual = ((float)(_manual_collective_max - _manual_collective_min))/100.0f; // calculate factors based on swash type and servo position calculate_roll_pitch_collective_factors(); diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index e502e54457..51492eb24d 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -112,7 +112,6 @@ public: _roll_scaler(1), _pitch_scaler(1), _collective_scalar(1), - _collective_scalar_manual(1), _collective_out(0), _collective_mid_pwm(0), _rotor_desired(0), @@ -163,19 +162,12 @@ public: // has_flybar - returns true if we have a mechical flybar bool has_flybar() { return _flybar_mode; } - // get_pilot_desired_collective - converts pilot input (from 0 ~ 1000) to a value that can be fed into the move_swash function - int16_t get_pilot_desired_collective(int16_t control_in); - // get_collective_mid - returns collective mid position as a number from 0 ~ 1000 int16_t get_collective_mid() { return _collective_mid; } // get_collective_out - returns collective position from last output as a number from 0 ~ 1000 int16_t get_collective_out() { return _collective_out; } - // get min/max collective when controlled manually as a number from 0 ~ 1000 (note that parameter is stored as percentage) - int16_t get_manual_collective_min() { return _manual_collective_min*10; } - int16_t get_manual_collective_max() { return _manual_collective_max*10; } - // set_collective_for_landing - limits collective from going too low if we know we are landed void set_collective_for_landing(bool landing) { _heliflags.landing_collective = landing; } @@ -271,8 +263,6 @@ private: AP_Int8 _rsc_ramp_time; // Time in seconds for the output to the main rotor's ESC to reach full speed AP_Int8 _rsc_runup_time; // Time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time AP_Int8 _flybar_mode; // Flybar present or not. Affects attitude controller used during ACRO flight mode - AP_Int8 _manual_collective_min; // Minimum collective position while pilot directly controls the collective - AP_Int8 _manual_collective_max; // Maximum collective position while pilot directly controls the collective AP_Int16 _land_collective_min; // Minimum collective when landed or landing AP_Int16 _direct_drive_tailspeed; // Direct Drive VarPitch Tail ESC speed (0 ~ 1000)