From 5cc4f41d852c9b316864de0a1a572ed1212a023d Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Mon, 12 Oct 2015 21:05:49 -0400 Subject: [PATCH] Copter: Helicopter: to use new Stab_Col and Acro_Col functions. --- ArduCopter/Copter.h | 1 - ArduCopter/Parameters.cpp | 18 ------------------ ArduCopter/Parameters.h | 6 ++---- ArduCopter/config.h | 2 -- ArduCopter/flight_mode.cpp | 11 +++++++++++ ArduCopter/heli.cpp | 17 +++-------------- ArduCopter/heli_control_acro.cpp | 9 ++++++++- ArduCopter/heli_control_stabilize.cpp | 6 +++++- 8 files changed, 29 insertions(+), 41 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index e3b357aff3..b64ee5990f 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -828,7 +828,6 @@ private: bool mode_allows_arming(uint8_t mode, bool arming_from_gcs); void notify_flight_mode(uint8_t mode); void heli_init(); - int16_t get_pilot_desired_collective(int16_t control_in); void check_dynamic_flight(void); void update_heli_control_dynamics(void); void heli_update_landing_swash(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index b91791e4c9..5e1f2e6431 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -519,24 +519,6 @@ const AP_Param::Info Copter::var_info[] = { // @Group: H_RSC_ // @Path: ../libraries/RC_Channel/RC_Channel.cpp GGROUP(heli_servo_rsc, "H_RSC_", RC_Channel), - - // @Param: H_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: Percent*10 - // @Increment: 1 - // @User: Standard - GSCALAR(heli_stab_col_min, "H_STAB_COL_MIN", HELI_STAB_COLLECTIVE_MIN_DEFAULT), - - // @Param: H_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: Percent*10 - // @Increment: 1 - // @User: Standard - GSCALAR(heli_stab_col_max, "H_STAB_COL_MAX", HELI_STAB_COLLECTIVE_MAX_DEFAULT), #endif // RC channel diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 553bcfcb54..c0b5817dfd 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -166,8 +166,8 @@ public: k_param_heli_pitch_ff, // remove k_param_heli_roll_ff, // remove k_param_heli_yaw_ff, // remove - k_param_heli_stab_col_min, - k_param_heli_stab_col_max, // 88 + k_param_heli_stab_col_min, // remove + k_param_heli_stab_col_max, // remove k_param_heli_servo_rsc, // 89 = full! // @@ -447,8 +447,6 @@ public: // Heli RC_Channel heli_servo_1, heli_servo_2, heli_servo_3, heli_servo_4; // servos for swash plate and tail RC_Channel heli_servo_rsc; // servo for rotor speed control output - 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/config.h b/ArduCopter/config.h index 6145d3665e..4b1e7bc6af 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -117,8 +117,6 @@ # define RATE_YAW_IMAX 4500 # define RATE_YAW_FF 0.02 # define RATE_YAW_FILT_HZ 20.0f - # define HELI_STAB_COLLECTIVE_MIN_DEFAULT 0 - # define HELI_STAB_COLLECTIVE_MAX_DEFAULT 1000 # define THR_MIN_DEFAULT 0 # define AUTOTUNE_ENABLED DISABLED #endif diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index d6e623d3da..6d4390ceb1 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -244,6 +244,17 @@ void Copter::exit_mode(uint8_t old_control_mode, uint8_t new_control_mode) motors.set_acro_tail(false); } + // if we are changing from a mode that did not use manual throttle, + // stab col ramp value should be pre-loaded to the correct value to avoid a twitch + // heli_stab_col_ramp should really only be active switching between Stabilize and Acro modes + if (!mode_has_manual_throttle(old_control_mode)){ + if (new_control_mode == STABILIZE){ + input_manager.set_stab_col_ramp(1.0); + } else if (new_control_mode == ACRO){ + input_manager.set_stab_col_ramp(0.0); + } + } + // reset RC Passthrough to motors motors.reset_radio_passthrough(); #endif //HELI_FRAME diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 5470a22c59..8fb46d2b64 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -31,21 +31,10 @@ void Copter::heli_init() if (!g.heli_servo_rsc.radio_max.configured()) { g.heli_servo_rsc.radio_max.set_and_save(g.rc_8.radio_max.get()); } -} - -// get_pilot_desired_collective - converts pilot input (from 0 ~ 1000) to a value that can be fed into the channel_throttle->servo_out function -int16_t Copter::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; + // pre-load stab col values as mode is initialized as Stabilize, but stabilize_init() function is not run on start-up. + input_manager.set_use_stab_col(true); + input_manager.set_stab_col_ramp(1.0); } // heli_check_dynamic_flight - updates the dynamic_flight flag based on our horizontal velocity diff --git a/ArduCopter/heli_control_acro.cpp b/ArduCopter/heli_control_acro.cpp index 730d01448e..fab3319740 100644 --- a/ArduCopter/heli_control_acro.cpp +++ b/ArduCopter/heli_control_acro.cpp @@ -15,6 +15,9 @@ bool Copter::heli_acro_init(bool ignore_checks) motors.set_acro_tail(true); + // set stab collective false to use full collective pitch range + input_manager.set_use_stab_col(false); + // always successfully enter acro return true; } @@ -24,6 +27,7 @@ bool Copter::heli_acro_init(bool ignore_checks) void Copter::heli_acro_run() { float target_roll, target_pitch, target_yaw; + int16_t pilot_throttle_scaled; // Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because // we may be in autorotation flight. These should be reset only when transitioning from disarmed @@ -76,8 +80,11 @@ void Copter::heli_acro_run() attitude_control.passthrough_bf_roll_pitch_rate_yaw(roll_in, pitch_in, yaw_in); } + // get pilot's desired throttle + pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->control_in); + // output pilot's throttle without angle boost - attitude_control.set_throttle_out(channel_throttle->control_in, false, g.throttle_filt); + attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); } #endif //HELI_FRAME diff --git a/ArduCopter/heli_control_stabilize.cpp b/ArduCopter/heli_control_stabilize.cpp index 61ac9a38f8..fc502cd06a 100644 --- a/ArduCopter/heli_control_stabilize.cpp +++ b/ArduCopter/heli_control_stabilize.cpp @@ -13,6 +13,10 @@ bool Copter::heli_stabilize_init(bool ignore_checks) // set target altitude to zero for reporting // To-Do: make pos controller aware when it's active/inactive so it can always report the altitude error? pos_control.set_alt_target(0); + + // set stab collective true to use stabilize scaled collective pitch range + input_manager.set_use_stab_col(true); + return true; } @@ -56,7 +60,7 @@ void Copter::heli_stabilize_run() target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); // get pilot's desired throttle - pilot_throttle_scaled = get_pilot_desired_collective(channel_throttle->control_in); + pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->control_in); // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());