From d5bbe6de0366bce970640f618bc64786d8999208 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 31 Jan 2015 16:04:54 +0900 Subject: [PATCH] Copter: remove TRIM_THROTTLE throttle_trim is replaced by throttle_average which is initialised to the throttle_mid parameter value at startup --- ArduCopter/ArduCopter.pde | 6 +++--- ArduCopter/Attitude.pde | 19 +++++++++---------- ArduCopter/Parameters.h | 3 +-- ArduCopter/Parameters.pde | 8 -------- ArduCopter/config.h | 4 ---- ArduCopter/motors.pde | 2 -- 6 files changed, 13 insertions(+), 29 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 4b44377cf5..b6d5dd9587 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -519,7 +519,7 @@ static int32_t initial_armed_bearing; //////////////////////////////////////////////////////////////////////////////// // Throttle variables //////////////////////////////////////////////////////////////////////////////// -static float throttle_avg; // g.throttle_cruise as a float +static float throttle_average; // estimated throttle required to hover static int16_t desired_climb_rate; // pilot desired climb rate - for logging purposes only @@ -758,7 +758,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { auto_trim, 40, 14 }, { update_altitude, 40, 100 }, { run_nav_updates, 8, 80 }, - { update_thr_cruise, 40, 10 }, + { update_thr_average, 40, 10 }, { three_hz_loop, 133, 9 }, { compass_accumulate, 8, 42 }, { barometer_accumulate, 8, 25 }, @@ -833,7 +833,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { auto_trim, 10, 140 }, { update_altitude, 10, 1000 }, { run_nav_updates, 4, 800 }, - { update_thr_cruise, 1, 50 }, + { update_thr_average, 1, 50 }, { three_hz_loop, 33, 90 }, { compass_accumulate, 2, 420 }, { barometer_accumulate, 2, 250 }, diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 23a9389805..69ffa1cc94 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -82,15 +82,15 @@ static float get_look_ahead_yaw() * throttle control ****************************************************************/ -// update_thr_cruise - update throttle cruise if necessary +// update_thr_average - update estimated throttle required to hover (if necessary) // should be called at 100hz -static void update_thr_cruise() +static void update_thr_average() { - // ensure throttle_avg has been initialised - if( throttle_avg == 0 ) { - throttle_avg = g.throttle_cruise; + // ensure throttle_average has been initialised + if( throttle_average == 0 ) { + throttle_average = g.throttle_mid; // update position controller - pos_control.set_throttle_hover(throttle_avg); + pos_control.set_throttle_hover(throttle_average); } // if not armed or landed exit @@ -103,10 +103,9 @@ static void update_thr_cruise() // calc average throttle if we are in a level hover if (throttle > g.throttle_min && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) { - throttle_avg = throttle_avg * 0.99f + (float)throttle * 0.01f; - g.throttle_cruise = throttle_avg; + throttle_average = throttle_average * 0.99f + (float)throttle * 0.01f; // update position controller - pos_control.set_throttle_hover(throttle_avg); + pos_control.set_throttle_hover(throttle_average); } } @@ -272,5 +271,5 @@ static float get_throttle_surface_tracking(int16_t target_rate, float current_al static void set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle) { // shift difference between pilot's throttle and hover throttle into accelerometer I - g.pid_accel_z.set_integrator(pilot_throttle-g.throttle_cruise); + g.pid_accel_z.set_integrator(pilot_throttle-throttle_average); } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 4fa0391d34..4dcb25644e 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -249,7 +249,7 @@ public: k_param_failsafe_throttle, k_param_throttle_fs_action, // remove k_param_failsafe_throttle_value, - k_param_throttle_cruise, + k_param_throttle_trim, // remove k_param_esc_calibrate, k_param_radio_tuning, k_param_radio_tuning_high, @@ -367,7 +367,6 @@ public: AP_Int16 throttle_max; AP_Int8 failsafe_throttle; AP_Int16 failsafe_throttle_value; - AP_Int16 throttle_cruise; AP_Int16 throttle_mid; AP_Int16 throttle_deadzone; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index dfed96fbc7..832daac88b 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -242,14 +242,6 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(failsafe_throttle_value, "FS_THR_VALUE", FS_THR_VALUE_DEFAULT), - // @Param: TRIM_THROTTLE - // @DisplayName: Throttle Trim - // @Description: The autopilot's estimate of the throttle required to maintain a level hover. Calculated automatically from the pilot's throttle input while in stabilize mode - // @Range: 0 1000 - // @Units: Percent*10 - // @User: Advanced - GSCALAR(throttle_cruise, "TRIM_THROTTLE", THROTTLE_CRUISE), - // @Param: THR_MID // @DisplayName: Throttle Mid Position // @Description: The throttle output (0 ~ 1000) when throttle stick is in mid position. Used to scale the manual throttle so that the mid throttle stick position is close to the throttle required to hover diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 4d2f951826..817bc55ab4 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -672,10 +672,6 @@ ////////////////////////////////////////////////////////////////////////////// // Throttle control gains // -#ifndef THROTTLE_CRUISE - # define THROTTLE_CRUISE 450 // default estimate of throttle required for vehicle to maintain a hover -#endif - #ifndef THR_MID_DEFAULT # define THR_MID_DEFAULT 500 // Throttle output (0 ~ 1000) when throttle stick is in mid position #endif diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 3d0c259148..083d6ba708 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -696,8 +696,6 @@ static void init_disarm_motors() compass.save_offsets(); } - g.throttle_cruise.save(); - #if AUTOTUNE_ENABLED == ENABLED // save auto tuned parameters autotune_save_tuning_gains();