Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: autotune to use 2pos switch only 

method of recording state also changed
other code clean-up
pair programmed withe Leonard
This commit is contained in:
Randy Mackay 2013-10-18 13:57:50 +09:00
parent e0c4785b2a
commit 0cbedded0d
4 changed files with 135 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

221
ArduCopter/auto_tune.pde
View File

@ -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();
}
}
}

5
ArduCopter/control_modes.pde
View File

@ -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();

12
ArduCopter/defines.h
View File

@ -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. */

2
ArduCopter/motors.pde
View File

@ -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