From aec67cd3df0af386ac5fc0ed86cfd334a2350def Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 2 Feb 2014 16:58:36 +0900 Subject: [PATCH] Copter: integrate control_autotune --- ArduCopter/ArduCopter.pde | 40 ++++++------------------------------ ArduCopter/Log.pde | 4 ++-- ArduCopter/config.h | 6 +++--- ArduCopter/control_modes.pde | 15 +++++++------- ArduCopter/defines.h | 12 +++++++---- ArduCopter/motors.pde | 4 ++-- ArduCopter/system.pde | 21 +++++++++++++++++++ 7 files changed, 50 insertions(+), 52 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 79cd19e84f..a5a8d89034 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1397,18 +1397,8 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode) case ROLL_PITCH_SPORT: roll_pitch_initialised = true; break; - -#if AUTOTUNE == ENABLED - case ROLL_PITCH_AUTOTUNE: - // only enter autotune mode from stabilized roll-pitch mode when armed and flying - if (roll_pitch_mode == ROLL_PITCH_STABLE && motors.armed() && !ap.land_complete) { reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in); - // auto_tune_start returns true if it wants the roll-pitch mode changed to autotune - roll_pitch_initialised = auto_tune_start(); } - break; -#endif - } // if initialisation has been successful update the yaw mode if( roll_pitch_initialised ) { @@ -1423,11 +1413,6 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode) // exit_roll_pitch_mode - peforms any code required when exiting the current roll-pitch mode void exit_roll_pitch_mode(uint8_t old_roll_pitch_mode) { -#if AUTOTUNE == ENABLED - if (old_roll_pitch_mode == ROLL_PITCH_AUTOTUNE) { - auto_tune_stop(); - } -#endif } // update_flight_mode - calls the appropriate attitude controllers based on flight mode @@ -1494,6 +1479,12 @@ static void update_flight_mode() case FLIP: flip_run(); break; + +#if AUTOTUNE_ENABLED == ENABLED + case AUTOTUNE: + autotune_run(); + break; +#endif } } @@ -1579,27 +1570,8 @@ void update_roll_pitch_mode(void) get_roll_rate_stabilized_ef(g.rc_1.control_in); get_pitch_rate_stabilized_ef(g.rc_2.control_in); break; - -#if AUTOTUNE == ENABLED - case ROLL_PITCH_AUTOTUNE: - // apply SIMPLE mode transform - if(ap.simple_mode && ap.new_radio_frame) { - update_simple_mode(); } - // convert pilot input to lean angles - get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch); - - // pass desired roll, pitch to stabilize attitude controllers - get_stabilize_roll(control_roll); - get_stabilize_pitch(control_pitch); - - // copy user input for reporting purposes - get_autotune_roll_pitch_controller(g.rc_1.control_in, g.rc_2.control_in, g.rc_4.control_in); - break; -#endif - } - #if FRAME_CONFIG != HELI_FRAME if(g.rc_3.control_in == 0 && control_mode <= ACRO) { reset_rate_I(); diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 64a41a2d60..b763576d63 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -157,7 +157,7 @@ process_logs(uint8_t argc, const Menu::arg *argv) return 0; } -#if AUTOTUNE == ENABLED +#if AUTOTUNE_ENABLED == ENABLED struct PACKED log_AutoTune { LOG_PACKET_HEADER; uint8_t axis; // roll or pitch @@ -774,7 +774,7 @@ static void Log_Write_Cmd(uint8_t num, const struct Location *wp) {} static void Log_Write_Mode(uint8_t mode) {} static void Log_Write_IMU() {} static void Log_Write_GPS() {} -#if AUTOTUNE == ENABLED +#if AUTOTUNE_ENABLED == ENABLED static void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float rate_min, float rate_max, float new_gain_rp, float new_gain_rd, float new_gain_sp) {} static void Log_Write_AutoTuneDetails(int16_t angle_cd, float rate_cds) {} #endif diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 46c981f62a..4d081b7cff 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -130,7 +130,7 @@ # define HELI_STAB_COLLECTIVE_MIN_DEFAULT 0 # define HELI_STAB_COLLECTIVE_MAX_DEFAULT 1000 # define THR_MIN_DEFAULT 0 - # define AUTOTUNE DISABLED + # define AUTOTUNE_ENABLED DISABLED # ifndef HELI_CC_COMP #define HELI_CC_COMP DISABLED @@ -411,8 +411,8 @@ ////////////////////////////////////////////////////////////////////////////// // Auto Tuning -#ifndef AUTOTUNE - # define AUTOTUNE ENABLED +#ifndef AUTOTUNE_ENABLED + # define AUTOTUNE_ENABLED ENABLED #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 44a0e45c52..93ca383c09 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -340,21 +340,22 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) } break; -#if AUTOTUNE == ENABLED +#if AUTOTUNE_ENABLED == ENABLED case AUX_SWITCH_AUTOTUNE: // turn on auto tuner switch(ch_flag) { case AUX_SWITCH_LOW: case AUX_SWITCH_MIDDLE: - // turn off tuning and return to standard pids - if (roll_pitch_mode == ROLL_PITCH_AUTOTUNE) { - set_roll_pitch_mode(ROLL_PITCH_STABLE); + // stop the autotune and return to original gains + autotune_stop(); + // restore flight mode based on flight mode switch position + if (control_mode == AUTOTUNE) { + reset_control_switch(); } break; case AUX_SWITCH_HIGH: - // start an auto tuning session - // set roll-pitch mode to our special auto tuning stabilize roll-pitch mode - set_roll_pitch_mode(ROLL_PITCH_AUTOTUNE); + // start an autotuning session + autotune_start(); break; } break; diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index c6b66f237f..cfb1e7ac7d 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -128,7 +128,8 @@ #define DRIFT 11 // DRIFT mode (Note: 12 is no longer used) #define SPORT 13 // earth frame rate control #define FLIP 14 // flip the vehicle on the roll axis -#define NUM_MODES 15 +#define AUTOTUNE 15 // autotune the vehicle's roll and pitch gains +#define NUM_MODES 16 // CH_6 Tuning @@ -313,10 +314,10 @@ enum FlipState { #define DATA_AUTOTUNE_INITIALISED 30 #define DATA_AUTOTUNE_OFF 31 #define DATA_AUTOTUNE_RESTART 32 -#define DATA_AUTOTUNE_COMPLETE 33 -#define DATA_AUTOTUNE_ABANDONED 34 +#define DATA_AUTOTUNE_SUCCESS 33 +#define DATA_AUTOTUNE_FAILED 34 #define DATA_AUTOTUNE_REACHED_LIMIT 35 -#define DATA_AUTOTUNE_TESTING 36 +#define DATA_AUTOTUNE_PILOT_TESTING 36 #define DATA_AUTOTUNE_SAVEDGAINS 37 #define DATA_SAVE_TRIM 38 #define DATA_SAVEWP_ADD_WP 39 @@ -383,6 +384,7 @@ enum FlipState { #define ERROR_SUBSYSTEM_GPS 11 #define ERROR_SUBSYSTEM_CRASH_CHECK 12 #define ERROR_SUBSYSTEM_FLIP 13 +#define ERROR_SUBSYSTEM_AUTOTUNE 14 // general error codes #define ERROR_CODE_ERROR_RESOLVED 0 #define ERROR_CODE_FAILED_TO_INITIALISE 1 @@ -401,6 +403,8 @@ enum FlipState { #define ERROR_CODE_CRASH_CHECK_CRASH 1 // subsystem specific error codes -- flip #define ERROR_CODE_FLIP_ABANDONED 2 +// subsystem specific error codes -- autotune +#define ERROR_CODE_AUTOTUNE_BAD_GAINS 2 // Arming Check Enable/Disable bits #define ARMING_CHECK_NONE 0x00 diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index e723b1368f..1a94fc27d6 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -514,9 +514,9 @@ static void init_disarm_motors() g.throttle_cruise.save(); -#if AUTOTUNE == ENABLED +#if AUTOTUNE_ENABLED == ENABLED // save auto tuned parameters - auto_tune_save_tuning_gains_and_reset(); + autotune_save_tuning_gains(); #endif // we are not in the air diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 5432a48f77..684f1fc069 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -450,6 +450,12 @@ static bool set_mode(uint8_t mode) success = flip_init(ignore_checks); break; +#if AUTOTUNE_ENABLED == ENABLED + case AUTOTUNE: + success = autotune_init(ignore_checks); + break; +#endif + default: success = false; break; @@ -457,6 +463,8 @@ static bool set_mode(uint8_t mode) // update flight mode if (success) { + // perform any cleanup required by previous flight mode + exit_mode(control_mode); control_mode = mode; Log_Write_Mode(control_mode); }else{ @@ -468,6 +476,16 @@ static bool set_mode(uint8_t mode) return success; } +// exit_mode - high level call to organise cleanup as a flight mode is exited +static void exit_mode(uint8_t old_control_mode) +{ +#if AUTOTUNE_ENABLED == ENABLED + if (old_control_mode == AUTOTUNE) { + autotune_stop(); + } +#endif +} + // update_auto_armed - update status of auto_armed flag static void update_auto_armed() { @@ -588,6 +606,9 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) case FLIP: port->print_P(PSTR("FLIP")); break; + case AUTOTUNE: + port->print_P(PSTR("AUTOTUNE")); + break; default: port->printf_P(PSTR("Mode(%u)"), (unsigned)mode); break;