From 0cbedded0df8728126ee5328c6d5996a896397a3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 18 Oct 2013 13:57:50 +0900 Subject: [PATCH] Copter: autotune to use 2pos switch only method of recording state also changed other code clean-up pair programmed withe Leonard --- ArduCopter/auto_tune.pde | 221 ++++++++++++++++++++--------------- ArduCopter/control_modes.pde | 5 +- ArduCopter/defines.h | 12 +- ArduCopter/motors.pde | 2 +- 4 files changed, 135 insertions(+), 105 deletions(-) diff --git a/ArduCopter/auto_tune.pde b/ArduCopter/auto_tune.pde index 693cabcc1d..11613fcf5a 100644 --- a/ArduCopter/auto_tune.pde +++ b/ArduCopter/auto_tune.pde @@ -35,11 +35,18 @@ #define AUTO_TUNE_RP_RATIO_FINAL 1.0f // I is set 1x P after testing #define AUTO_TUNE_RD_MIN 0.004f // minimum Rate D value #define AUTO_TUNE_RD_MAX 0.015f // maximum Rate D value -#define AUTO_TUNE_RP_MIN 0.05f // minimum Rate P value +#define AUTO_TUNE_RP_MIN 0.02f // minimum Rate P value #define AUTO_TUNE_RP_MAX 0.25f // maximum Rate P value #define AUTO_TUNE_SP_MAX 15.0f // maximum Stab P value #define AUTO_TUNE_SUCCESS_COUNT 4 // how many successful iterations we need to freeze at current gains +enum AutoTuneTuneMode { + AUTO_TUNE_MODE_UNINITIALISED = 0, + AUTO_TUNE_MODE_TUNING = 1, + AUTO_TUNE_MODE_TESTING = 2, + AUTO_TUNE_MODE_FAILED = 3 +}; + // things that can be tuned enum AutoTuneAxisType { AUTO_TUNE_AXIS_ROLL = 0, @@ -63,8 +70,7 @@ enum AutoTuneTuneType { // state struct auto_tune_state_struct { - uint8_t enabled : 1; // 0 = disabled, 1 = enabled - uint8_t active : 1; // 0 = inactive (temporarily suspended), 1 = actively tuning + AutoTuneTuneMode mode : 2; // see AutoTuneTuneMode for what modes are allowed uint8_t pilot_override : 1; // 1 = pilot is overriding controls so we suspend tuning temporarily AutoTuneAxisType axis : 1; // see AutoTuneAxisType for which things can be tuned uint8_t positive_direction : 1; // 0 = tuning in negative direction (i.e. left for roll), 1 = positive direction (i.e. right for roll) @@ -73,19 +79,27 @@ struct auto_tune_state_struct { } auto_tune_state; // variables -uint32_t auto_tune_override_time; // the last time the pilot overrode the controls -float auto_tune_test_min; // the minimum angular rate achieved during TESTING_RATE step -float auto_tune_test_max; // the maximum angular rate achieved during TESTING_RATE step -uint32_t auto_tune_timer; // generic timer variable -int8_t auto_tune_counter; // counter for tuning gains -float orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_sp; // backup of currently being tuned parameter values -float orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_sp; // backup of currently being tuned parameter values -float tune_roll_rp, tune_roll_rd, tune_roll_sp; // currently being tuned parameter values -float tune_pitch_rp, tune_pitch_rd, tune_pitch_sp; // currently being tuned parameter values +static uint32_t auto_tune_override_time; // the last time the pilot overrode the controls +static float auto_tune_test_min; // the minimum angular rate achieved during TESTING_RATE step +static float auto_tune_test_max; // the maximum angular rate achieved during TESTING_RATE step +static uint32_t auto_tune_timer; // generic timer variable +static int8_t auto_tune_counter; // counter for tuning gains +static float orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_sp; // backup of currently being tuned parameter values +static float orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_sp; // backup of currently being tuned parameter values +static float tune_roll_rp, tune_roll_rd, tune_roll_sp; // currently being tuned parameter values +static float tune_pitch_rp, tune_pitch_rd, tune_pitch_sp; // currently being tuned parameter values // store current pids as originals -void auto_tune_save_orig_pids() +static void auto_tune_initialise() { + // initialise gains and axis because this is our first time + auto_tune_state.axis = AUTO_TUNE_AXIS_ROLL; + auto_tune_state.positive_direction = false; + auto_tune_state.step = AUTO_TUNE_STEP_WAITING_FOR_LEVEL; + auto_tune_timer = millis(); + auto_tune_state.tune_type = AUTO_TUNE_TYPE_RD_UP; + + // backup original pids orig_roll_rp = g.pid_rate_roll.kP(); orig_roll_ri = g.pid_rate_roll.kI(); orig_roll_rd = g.pid_rate_roll.kD(); @@ -94,10 +108,40 @@ void auto_tune_save_orig_pids() orig_pitch_ri = g.pid_rate_pitch.kI(); orig_pitch_rd = g.pid_rate_pitch.kD(); orig_pitch_sp = g.pi_stabilize_pitch.kP(); + + // initialise tuned pid values + tune_roll_rp = g.pid_rate_roll.kP(); + tune_roll_rd = g.pid_rate_roll.kD(); + tune_roll_sp = g.pi_stabilize_roll.kP(); + tune_pitch_rp = g.pid_rate_pitch.kP(); + tune_pitch_rd = g.pid_rate_pitch.kD(); + tune_pitch_sp = g.pi_stabilize_pitch.kP(); + + // set roll-pitch mode to our special auto tuning stabilize roll-pitch mode + set_roll_pitch_mode(ROLL_PITCH_AUTOTUNE); + + Log_Write_Event(DATA_AUTOTUNE_INITIALISED); } -// auto_tune_restore_orig_pids - restore pids to their original values -void auto_tune_restore_orig_pids() +// auto_tune_intra_test_gains - gains used between tests +static void auto_tune_intra_test_gains() +{ + // we are restarting tuning so reset gains to tuning-start gains (i.e. low I term) + g.pid_rate_roll.kP(orig_roll_rp); + g.pid_rate_roll.kI(orig_roll_rp*AUTO_TUNE_PI_RATIO_FOR_TESTING); + g.pid_rate_roll.kD(orig_roll_rd); + g.pi_stabilize_roll.kP(orig_roll_sp); + g.pid_rate_pitch.kP(orig_pitch_rp); + g.pid_rate_pitch.kI(orig_pitch_rp*AUTO_TUNE_PI_RATIO_FOR_TESTING); + g.pid_rate_pitch.kD(orig_pitch_rd); + g.pi_stabilize_pitch.kP(orig_pitch_sp); + + // re-enable the rate limits + ap.disable_stab_rate_limit = false; +} + +// auto_tune_restore_orig_gains - restore pids to their original values +static void auto_tune_restore_orig_gains() { g.pid_rate_roll.kP(orig_roll_rp); g.pid_rate_roll.kI(orig_roll_ri); @@ -110,8 +154,8 @@ void auto_tune_restore_orig_pids() ap.disable_stab_rate_limit = false; } -// auto_tune_load_tuned_pids - restore pids to their original values -void auto_tune_load_tuned_pids() +// auto_tune_load_tuned_pids - restore pids to their tuned values +static void auto_tune_load_tuned_gains() { if (tune_roll_rp != 0 && tune_pitch_rp != 0) { g.pid_rate_roll.kP(tune_roll_rp); @@ -126,77 +170,79 @@ void auto_tune_load_tuned_pids() } } +// auto_tune_load_test_gains - load the to-be-tested gains for a single axis +static void auto_tune_load_test_gains() +{ + // restore pids to their tuning values + if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) { + g.pid_rate_roll.kP(tune_roll_rp); + g.pid_rate_roll.kI(tune_roll_rp*0.01f); + g.pid_rate_roll.kD(tune_roll_rd); + g.pi_stabilize_roll.kP(tune_roll_sp); + }else{ + g.pid_rate_pitch.kP(tune_pitch_rp); + g.pid_rate_pitch.kI(tune_pitch_rp*0.01f); + g.pid_rate_pitch.kD(tune_pitch_rd); + g.pi_stabilize_pitch.kP(tune_pitch_sp); + } + ap.disable_stab_rate_limit = true; // disable rate limits +} + // start an auto tuning session -void auto_tune_start() +static void auto_tune_start() { // check we are in stabilize mode if (control_mode == STABILIZE || control_mode == ALT_HOLD) { - // reset axis if this is our first time - if (!auto_tune_state.enabled) { - auto_tune_state.axis = AUTO_TUNE_AXIS_ROLL; - auto_tune_state.positive_direction = false; - auto_tune_state.step = AUTO_TUNE_STEP_WAITING_FOR_LEVEL; - auto_tune_timer = millis(); - auto_tune_state.tune_type = AUTO_TUNE_TYPE_RD_UP; - auto_tune_save_orig_pids(); - // initialise tuned pid values - tune_roll_rp = g.pid_rate_roll.kP(); - tune_roll_rd = g.pid_rate_roll.kD(); - tune_roll_sp = g.pi_stabilize_roll.kP(); - tune_pitch_rp = g.pid_rate_pitch.kP(); - tune_pitch_rd = g.pid_rate_pitch.kD(); - tune_pitch_sp = g.pi_stabilize_pitch.kP(); - }else{ - // restarting from suspended state - auto_tune_restore_orig_pids(); - } - // set roll-pitch mode to our special auto tuning stabilize roll-pitch mode - if (set_roll_pitch_mode(ROLL_PITCH_AUTOTUNE)) { - auto_tune_state.enabled = true; - auto_tune_state.active = true; - Log_Write_Event(DATA_AUTOTUNE_ON); + switch (auto_tune_state.mode) { + case AUTO_TUNE_MODE_UNINITIALISED: + // initialise gains and axis because this is our first time + auto_tune_initialise(); + auto_tune_state.mode = AUTO_TUNE_MODE_TUNING; + break; + case AUTO_TUNE_MODE_TUNING: + // we are restarting tuning so reset gains to tuning-start gains (i.e. low I term) + auto_tune_intra_test_gains(); + set_roll_pitch_mode(ROLL_PITCH_AUTOTUNE); // set roll-pitch mode to our special auto tuning stabilize roll-pitch mode + Log_Write_Event(DATA_AUTOTUNE_RESTART); + break; + case AUTO_TUNE_MODE_TESTING: + // we have completed a tune and are testing the new gains + auto_tune_load_tuned_gains(); + Log_Write_Event(DATA_AUTOTUNE_TESTING); + break; + case AUTO_TUNE_MODE_FAILED: + Log_Write_Event(DATA_AUTOTUNE_ABANDONED); + break; } } } // turn off tuning and return to standard pids -void auto_tune_stop() +static void auto_tune_stop() { - if (auto_tune_state.enabled) { - auto_tune_state.enabled = false; - auto_tune_state.active = false; - ap.disable_stab_rate_limit = false; - if (roll_pitch_mode == ROLL_PITCH_AUTOTUNE) { - set_roll_pitch_mode(ROLL_PITCH_STABLE); // restore roll-pitch mode - rate_targets_frame = EARTH_FRAME; // regular stabilize mode frame - } - // restore pids to their original values - auto_tune_restore_orig_pids(); - Log_Write_Event(DATA_AUTOTUNE_OFF); - } -} - -// stop tuning but remain with tuned pids -void auto_tune_suspend() -{ - auto_tune_load_tuned_pids(); - if (auto_tune_state.active) { - auto_tune_state.active = false; - Log_Write_Event(DATA_AUTOTUNE_SUSPENDED); + ap.disable_stab_rate_limit = false; + if (roll_pitch_mode == ROLL_PITCH_AUTOTUNE) { + set_roll_pitch_mode(ROLL_PITCH_STABLE); // restore roll-pitch mode + rate_targets_frame = EARTH_FRAME; // regular stabilize mode frame } + // restore pids to their original values + auto_tune_restore_orig_gains(); + Log_Write_Event(DATA_AUTOTUNE_OFF); } // save discovered gains to eeprom if auto tuner is enabled (i.e. switch is in middle or high position) -void auto_tune_save_tuning_gains() +static void auto_tune_save_tuning_gains_and_reset() { - if (auto_tune_state.enabled) { - auto_tune_load_tuned_pids(); + if (auto_tune_state.mode == AUTO_TUNE_MODE_TESTING) { + auto_tune_load_tuned_gains(); g.pid_rate_roll.save_gains(); g.pid_rate_pitch.save_gains(); g.pi_stabilize_roll.save_gains(); g.pi_stabilize_pitch.save_gains(); Log_Write_Event(DATA_AUTOTUNE_SAVEDGAINS); } + // reset state of autotune + auto_tune_state.mode = AUTO_TUNE_MODE_UNINITIALISED; } // Auto tuning roll-pitch controller @@ -207,8 +253,8 @@ get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch float rotation_rate; // rotation rate in radians/second int32_t lean_angle; - // exit immediately if not enabled or not actively tuning - if (!auto_tune_state.enabled || !auto_tune_state.active) { + // exit immediately if not actively tuning + if (!auto_tune_state.mode == AUTO_TUNE_MODE_TUNING) { return; } @@ -216,7 +262,7 @@ get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch if (pilot_roll_angle != 0 || pilot_pitch_angle != 0 ) { if (!auto_tune_state.pilot_override) { // restore pids to their original values - auto_tune_restore_orig_pids(); + auto_tune_restore_orig_gains(); } auto_tune_state.pilot_override = true; auto_tune_override_time = millis(); @@ -250,18 +296,8 @@ get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch acro_roll_rate = roll_rate_target_bf; acro_pitch_rate = pitch_rate_target_bf; acro_yaw_rate = yaw_rate_target_bf; - // restore pids to their tuning values - if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) { - g.pid_rate_roll.kP(tune_roll_rp); - g.pid_rate_roll.kI(tune_roll_rp*0.01f); - g.pid_rate_roll.kD(tune_roll_rd); - g.pi_stabilize_roll.kP(tune_roll_sp); - }else{ - g.pid_rate_pitch.kP(tune_pitch_rp); - g.pid_rate_pitch.kI(tune_pitch_rp*0.01f); - g.pid_rate_pitch.kD(tune_pitch_rd); - g.pi_stabilize_pitch.kP(tune_pitch_sp); - } + // restore pids to their to-be-tested values + auto_tune_load_test_gains(); ap.disable_stab_rate_limit = true; // disable rate limits } break; @@ -378,18 +414,8 @@ get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch break; case AUTO_TUNE_STEP_UPDATE_GAINS: - // restore pids to their original values - g.pid_rate_roll.kP(orig_roll_rp); - g.pid_rate_roll.kI(orig_roll_rp*AUTO_TUNE_PI_RATIO_FOR_TESTING); - g.pid_rate_roll.kD(orig_roll_rd); - g.pi_stabilize_roll.kP(orig_roll_sp); - g.pid_rate_pitch.kP(orig_pitch_rp); - g.pid_rate_pitch.kI(orig_pitch_rp*AUTO_TUNE_PI_RATIO_FOR_TESTING); - g.pid_rate_pitch.kD(orig_pitch_rd); - g.pi_stabilize_pitch.kP(orig_pitch_sp); - - // re-enable the rate limits - ap.disable_stab_rate_limit = false; + // restore gains to their intra-test values + auto_tune_intra_test_gains(); // logging if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) { @@ -409,6 +435,7 @@ get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch // stop the auto tune if we have hit the minimum roll or pitch rate P if(((auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL && tune_roll_rp < AUTO_TUNE_RP_MIN) || (auto_tune_state.axis == AUTO_TUNE_AXIS_PITCH && tune_pitch_rp < AUTO_TUNE_RP_MIN)) ) { + auto_tune_state.mode = AUTO_TUNE_MODE_FAILED; Log_Write_Event(DATA_AUTOTUNE_ABANDONED); auto_tune_stop(); return; @@ -457,6 +484,7 @@ get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch // stop the auto tune if we have hit the minimum roll or pitch rate P if(((auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL && tune_roll_rp < AUTO_TUNE_RP_MIN) || (auto_tune_state.axis == AUTO_TUNE_AXIS_PITCH && tune_pitch_rp < AUTO_TUNE_RP_MIN)) ) { + auto_tune_state.mode = AUTO_TUNE_MODE_FAILED; Log_Write_Event(DATA_AUTOTUNE_ABANDONED); auto_tune_stop(); return; @@ -576,8 +604,9 @@ get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch tune_pitch_sp = tune_pitch_sp * AUTO_TUNE_SP_BACKOFF; tune_roll_sp = min(tune_roll_sp, tune_pitch_sp); tune_pitch_sp = min(tune_roll_sp, tune_pitch_sp); - // if we've just completed pitch we are done so suspend tuning - auto_tune_suspend(); + // if we've just completed pitch we are done tuning and are moving onto testing + auto_tune_state.mode = AUTO_TUNE_MODE_TESTING; + auto_tune_stop(); } } } diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 7642037607..26d6b98621 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -297,13 +297,10 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) // turn on auto tuner switch(ch_flag) { case AUX_SWITCH_LOW: + case AUX_SWITCH_MIDDLE: // turn off tuning and return to standard pids auto_tune_stop(); break; - case AUX_SWITCH_MIDDLE: - // stop tuning but remain with tuned pids - auto_tune_suspend(); - break; case AUX_SWITCH_HIGH: // start an auto tuning session auto_tune_start(); diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 87b1e6150b..bd4ef358c1 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -336,12 +336,16 @@ enum ap_message { #define DATA_SET_SIMPLE_ON 26 #define DATA_SET_SIMPLE_OFF 27 #define DATA_SET_SUPERSIMPLE_ON 28 -#define DATA_AUTOTUNE_ON 29 -#define DATA_AUTOTUNE_SUSPENDED 30 -#define DATA_AUTOTUNE_OFF 31 -#define DATA_AUTOTUNE_SAVEDGAINS 32 +#define DATA_AUTOTUNE_INITIALISED 29 +#define DATA_AUTOTUNE_OFF 30 +#define DATA_AUTOTUNE_RESTART 31 +#define DATA_AUTOTUNE_COMPLETE 32 #define DATA_AUTOTUNE_ABANDONED 33 #define DATA_AUTOTUNE_REACHED_LIMIT 34 +#define DATA_AUTOTUNE_TESTING 35 +#define DATA_AUTOTUNE_SAVEDGAINS 36 + + /* ************************************************************** */ /* Expansion PIN's that people can use for various things. */ diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 14a1636eed..ef6ea139b5 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -411,7 +411,7 @@ static void init_disarm_motors() #if AUTOTUNE == ENABLED // save auto tuned parameters - auto_tune_save_tuning_gains(); + auto_tune_save_tuning_gains_and_reset(); #endif // we are not in the air