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

657 lines
32 KiB
Plaintext
Raw Blame History

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if AUTOTUNE == ENABLED
/*
Auto tuning works in this way:
i) set up 3-position ch7 or ch8 switch to "AutoTune"
2) take-off in stabilize mode, put the copter into a level hover and switch ch7/ch8 to high position to start tuning
3) auto tuner brings roll and pitch level
4) the following procedure is run for roll and then pitch
a) invokes 90 deg/sec rate request
b) records maximum "forward" roll rate and bounce back rate
c) when copter reaches 20 degrees or 1 second has passed, it commands level
d) tries to keep max rotation rate between 80% ~ 100% of requested rate (90deg/sec) by adjusting rate P
e) increases rate D until the bounce back becomes greater than 10% of requested rate (90deg/sec)
f) decreases rate D until the bounce back becomes less than 10% of requested rate (90deg/sec)
g) increases rate P until the max rotate rate becomes greater than the requeste rate (90deg/sec)
h) invokes a 20deg angle request on roll or pitch
i) increases stab P until the maximum angle becomes greater than 110% of the requested angle (20deg)
j) decreases stab P by 25%
If pilot inputs any stick inputs these becomes the desired roll, pitch angles sent to the stabilize controller and the tuner is disabled until the sticks are put back into the middle for 1 second
*/
#define AUTO_TUNE_PILOT_OVERRIDE_TIMEOUT_MS 500 // restart tuning if pilot has left sticks in middle for 2 seconds
#define AUTO_TUNE_TARGET_RATE_TIMEOUT_MS 500 // timeout for rate test step
#define AUTO_TUNE_TARGET_RATE_CDS 9000 // target roll/pitch rate during AUTO_TUNE_STEP_TESTING step
#define AUTO_TUNE_LEVEL_ANGLE_CD 300 // angle which qualifies as level
#define AUTO_TUNE_TARGET_ANGLE_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step
#define AUTO_TUNE_REQUIRED_LEVEL_TIME_MS 250 // time we require the copter to be level
#define AUTO_TUNE_AGGRESSIVENESS 0.1f // tuning for 10% overshoot
#define AUTO_TUNE_RD_STEP 0.0005f // minimum increment when increasing/decreasing Rate D term
#define AUTO_TUNE_RP_STEP 0.005f // minimum increment when increasing/decreasing Rate P term
#define AUTO_TUNE_SP_STEP 0.5f // minimum increment when increasing/decreasing Stab P term
#define AUTO_TUNE_SP_BACKOFF 0.75f // back off on the Stab P tune
#define AUTO_TUNE_PI_RATIO_FOR_TESTING 0.1f // I is set 10x smaller than P during testing
#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.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
// Auto Tune message ids for ground station
#define AUTO_TUNE_MESSAGE_STARTED 0
#define AUTO_TUNE_MESSAGE_SUCCESS 1
#define AUTO_TUNE_MESSAGE_FAILED 2
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,
AUTO_TUNE_AXIS_PITCH = 1
};
// steps performed during tuning
enum AutoTuneStepType {
AUTO_TUNE_STEP_WAITING_FOR_LEVEL = 0,
AUTO_TUNE_STEP_TESTING = 1,
AUTO_TUNE_STEP_UPDATE_GAINS = 2
};
// steps performed during tuning
enum AutoTuneTuneType {
AUTO_TUNE_TYPE_RD_UP = 0,
AUTO_TUNE_TYPE_RD_DOWN = 1,
AUTO_TUNE_TYPE_RP_UP = 2,
AUTO_TUNE_TYPE_SP_UP = 3
};
// state
struct auto_tune_state_struct {
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)
AutoTuneStepType step : 2; // see AutoTuneStepType for what steps are performed
AutoTuneTuneType tune_type : 2; // see AutoTuneTuneType
} auto_tune_state;
// variables
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
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();
orig_roll_sp = g.pi_stabilize_roll.kP();
orig_pitch_rp = g.pid_rate_pitch.kP();
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();
Log_Write_Event(DATA_AUTOTUNE_INITIALISED);
}
// 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);
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_ri);
g.pid_rate_pitch.kD(orig_pitch_rd);
g.pi_stabilize_pitch.kP(orig_pitch_sp);
ap.disable_stab_rate_limit = false;
}
// 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);
g.pid_rate_roll.kI(tune_roll_rp*AUTO_TUNE_RP_RATIO_FINAL);
g.pid_rate_roll.kD(tune_roll_rd);
g.pi_stabilize_roll.kP(tune_roll_sp);
g.pid_rate_pitch.kP(tune_pitch_rp);
g.pid_rate_pitch.kI(tune_pitch_rp*AUTO_TUNE_RP_RATIO_FINAL);
g.pid_rate_pitch.kD(tune_pitch_rd);
g.pi_stabilize_pitch.kP(tune_pitch_sp);
ap.disable_stab_rate_limit = false;
}
}
// 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
// returns true if we should change the roll-pitch mode to ROLL_PITCH_AUTOTUNE
// To-Do: make autotune a flight mode so that this slightly non-intuitive returning of a flag is not required
static bool auto_tune_start()
{
bool requires_autotune_rp_mode = false;
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_update_gcs(AUTO_TUNE_MESSAGE_STARTED);
auto_tune_state.mode = AUTO_TUNE_MODE_TUNING;
requires_autotune_rp_mode = true;
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();
Log_Write_Event(DATA_AUTOTUNE_RESTART);
auto_tune_update_gcs(AUTO_TUNE_MESSAGE_STARTED);
requires_autotune_rp_mode = true;
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);
requires_autotune_rp_mode = false;
break;
case AUTO_TUNE_MODE_FAILED:
Log_Write_Event(DATA_AUTOTUNE_ABANDONED);
requires_autotune_rp_mode = false;
break;
}
// tell caller we require roll-pitch mode to be changed to ROLL_PITCH_AUTOTUNE
return requires_autotune_rp_mode;
}
// turn off tuning and return to standard pids
static void auto_tune_stop()
{
ap.disable_stab_rate_limit = false;
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)
static void auto_tune_save_tuning_gains_and_reset()
{
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();
orig_roll_rp = g.pid_rate_roll.kP();
orig_roll_ri = g.pid_rate_roll.kI();
orig_roll_rd = g.pid_rate_roll.kD();
orig_roll_sp = g.pi_stabilize_roll.kP();
orig_pitch_rp = g.pid_rate_pitch.kP();
orig_pitch_ri = g.pid_rate_pitch.kI();
orig_pitch_rd = g.pid_rate_pitch.kD();
orig_pitch_sp = g.pi_stabilize_pitch.kP();
Log_Write_Event(DATA_AUTOTUNE_SAVEDGAINS);
}
// reset state of autotune
auto_tune_state.mode = AUTO_TUNE_MODE_UNINITIALISED;
}
// send message to ground station
void auto_tune_update_gcs(uint8_t message_id)
{
switch (message_id) {
case AUTO_TUNE_MESSAGE_STARTED:
gcs_send_text_P(SEVERITY_HIGH,PSTR("AutoTune: Started"));
break;
case AUTO_TUNE_MESSAGE_SUCCESS:
gcs_send_text_P(SEVERITY_HIGH,PSTR("AutoTune: Success"));
break;
case AUTO_TUNE_MESSAGE_FAILED:
gcs_send_text_P(SEVERITY_HIGH,PSTR("AutoTune: Failed"));
break;
}
}
// Auto tuning roll-pitch controller
static void
get_autotune_roll_pitch_controller(int16_t pilot_roll_angle, int16_t pilot_pitch_angle, int16_t pilot_yaw_command)
{
int32_t target_roll_rate, target_pitch_rate;
float rotation_rate; // rotation rate in radians/second
int32_t lean_angle;
// exit immediately if not actively tuning
if (!auto_tune_state.mode == AUTO_TUNE_MODE_TUNING) {
return;
}
// check for pilot override
if (pilot_roll_angle != 0 || pilot_pitch_angle != 0 || pilot_yaw_command != 0) {
if (!auto_tune_state.pilot_override) {
// restore pids to their original values
auto_tune_restore_orig_gains();
}
auto_tune_state.pilot_override = true;
auto_tune_override_time = millis();
}else if (auto_tune_state.pilot_override) {
// check if we should resume tuning after pilot's override
if (millis() - auto_tune_override_time > AUTO_TUNE_PILOT_OVERRIDE_TIMEOUT_MS) {
auto_tune_state.pilot_override = false; // turn off pilot override
auto_tune_state.step = AUTO_TUNE_STEP_WAITING_FOR_LEVEL; // set tuning step back from beginning
auto_tune_timer = millis();
}
}
// check tuning step
if (!auto_tune_state.pilot_override) {
switch (auto_tune_state.step) {
case AUTO_TUNE_STEP_WAITING_FOR_LEVEL:
// reset counter if we are no longer level
if ((labs(ahrs.roll_sensor) > AUTO_TUNE_LEVEL_ANGLE_CD) || (labs(ahrs.pitch_sensor) > AUTO_TUNE_LEVEL_ANGLE_CD)) {
auto_tune_timer = millis();
}
// if we have been level for a sufficient amount of time (0.5 seconds) move onto next step
if (millis() - auto_tune_timer >= AUTO_TUNE_REQUIRED_LEVEL_TIME_MS) {
auto_tune_state.step = AUTO_TUNE_STEP_TESTING;
// init variables for next step
auto_tune_test_max = 0;
auto_tune_test_min = 0;
rotation_rate = 0;
auto_tune_timer = millis();
// initialise rate controller targets
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 to-be-tested values
auto_tune_load_test_gains();
ap.disable_stab_rate_limit = true; // disable rate limits
}
break;
case AUTO_TUNE_STEP_TESTING:
// Run the test
// update rotation targets in body-earth frame
if(auto_tune_state.tune_type == AUTO_TUNE_TYPE_SP_UP){
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
// override roll angle
if (auto_tune_state.positive_direction) {
control_roll = AUTO_TUNE_TARGET_ANGLE_CD;
}else{
control_roll = -AUTO_TUNE_TARGET_ANGLE_CD;
}
get_stabilize_roll(control_roll);
}else{
// override pitch angle
if (auto_tune_state.positive_direction) {
control_pitch = AUTO_TUNE_TARGET_ANGLE_CD;
}else{
control_pitch = -AUTO_TUNE_TARGET_ANGLE_CD;
}
get_stabilize_pitch(control_pitch);
}
} else {
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
// override roll rate
if (auto_tune_state.positive_direction) {
target_roll_rate = AUTO_TUNE_TARGET_RATE_CDS;
}else{
target_roll_rate = -AUTO_TUNE_TARGET_RATE_CDS;
}
// set body frame targets for rate controller
set_roll_rate_target(target_roll_rate, BODY_FRAME);
set_pitch_rate_target(acro_pitch_rate, BODY_FRAME);
set_yaw_rate_target(acro_yaw_rate, BODY_FRAME);
}else{
// override pitch rate
if (auto_tune_state.positive_direction) {
target_pitch_rate = AUTO_TUNE_TARGET_RATE_CDS;
}else{
target_pitch_rate = -AUTO_TUNE_TARGET_RATE_CDS;
}
// set body frame targets for rate controller
set_pitch_rate_target(target_pitch_rate, BODY_FRAME);
set_roll_rate_target(acro_roll_rate, BODY_FRAME);
set_yaw_rate_target(acro_yaw_rate, BODY_FRAME);
}
rate_targets_frame = BODY_EARTH_FRAME;
}
// Get Rate and Angle
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
// 20 Hz filter on rate
rotation_rate = ToDeg(fabs(ahrs.get_gyro().x)) * 100.0f;
lean_angle = labs(ahrs.roll_sensor);
}else{
// 20 Hz filter on rate
// rotation_rate = rotation_rate + 0.55686f*(ToDeg(fabs(ahrs.get_gyro().y))*100.0f-rotation_rate);
rotation_rate = ToDeg(fabs(ahrs.get_gyro().y)) * 100.0f;
lean_angle = labs(ahrs.pitch_sensor);
}
// Make measurements
if(auto_tune_state.tune_type == AUTO_TUNE_TYPE_SP_UP){
// capture max angle
if (lean_angle > auto_tune_test_max) {
auto_tune_test_max = lean_angle;
auto_tune_test_min = lean_angle;
}
// capture min rotation rate
if (lean_angle < auto_tune_test_min && auto_tune_test_max > AUTO_TUNE_TARGET_ANGLE_CD*(1-AUTO_TUNE_AGGRESSIVENESS)) {
auto_tune_test_min = lean_angle;
}
}else{
// capture max rotation rate
if (rotation_rate > auto_tune_test_max) {
auto_tune_test_max = rotation_rate;
auto_tune_test_min = rotation_rate;
}
// capture min rotation rate
if (rotation_rate < auto_tune_test_min && auto_tune_test_max > AUTO_TUNE_TARGET_RATE_CDS*(1-2*AUTO_TUNE_AGGRESSIVENESS)) {
auto_tune_test_min = rotation_rate;
}
}
// check for end of test conditions
if(millis() - auto_tune_timer >= AUTO_TUNE_TARGET_RATE_TIMEOUT_MS) {
auto_tune_state.step = AUTO_TUNE_STEP_UPDATE_GAINS;
}
if(auto_tune_state.tune_type == AUTO_TUNE_TYPE_SP_UP){
if ((lean_angle >= AUTO_TUNE_TARGET_ANGLE_CD*(1+AUTO_TUNE_AGGRESSIVENESS)) ||
(auto_tune_test_max-auto_tune_test_min > AUTO_TUNE_TARGET_ANGLE_CD*AUTO_TUNE_AGGRESSIVENESS)) {
auto_tune_state.step = AUTO_TUNE_STEP_UPDATE_GAINS;
}
}else{
if (lean_angle >= AUTO_TUNE_TARGET_ANGLE_CD) {
auto_tune_state.step = AUTO_TUNE_STEP_UPDATE_GAINS;
}
if (auto_tune_state.tune_type == AUTO_TUNE_TYPE_RD_UP || auto_tune_state.tune_type == AUTO_TUNE_TYPE_RD_DOWN) {
if(auto_tune_test_max-auto_tune_test_min > AUTO_TUNE_TARGET_RATE_CDS*AUTO_TUNE_AGGRESSIVENESS) {
auto_tune_state.step = AUTO_TUNE_STEP_UPDATE_GAINS;
}
}
}
// logging
Log_Write_AutoTuneDetails((int16_t)lean_angle, rotation_rate);
break;
case AUTO_TUNE_STEP_UPDATE_GAINS:
// restore gains to their intra-test values
auto_tune_intra_test_gains();
// logging
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
Log_Write_AutoTune(auto_tune_state.axis, auto_tune_state.tune_type, auto_tune_test_min, auto_tune_test_max, tune_roll_rp, tune_roll_rd, tune_roll_sp);
}else{
Log_Write_AutoTune(auto_tune_state.axis, auto_tune_state.tune_type, auto_tune_test_min, auto_tune_test_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp);
}
// do gain updates
if (auto_tune_state.tune_type == AUTO_TUNE_TYPE_RD_UP) {
if (auto_tune_test_max > AUTO_TUNE_TARGET_RATE_CDS) {
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_rp -= AUTO_TUNE_RP_STEP;
}else{
tune_pitch_rp -= AUTO_TUNE_RP_STEP;
}
// 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);
set_roll_pitch_mode(ROLL_PITCH_STABLE);
auto_tune_update_gcs(AUTO_TUNE_MESSAGE_FAILED);
return;
}
}else if(auto_tune_test_max < AUTO_TUNE_TARGET_RATE_CDS*(1.0f-AUTO_TUNE_AGGRESSIVENESS*2.0f) &&
((auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL && tune_roll_rp <= AUTO_TUNE_RP_MAX) ||
(auto_tune_state.axis == AUTO_TUNE_AXIS_PITCH && tune_pitch_rp <= AUTO_TUNE_RP_MAX)) ) {
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_rp += AUTO_TUNE_RP_STEP*2.0f;
}else{
tune_pitch_rp += AUTO_TUNE_RP_STEP*2.0f;
}
}else{
if (auto_tune_test_max-auto_tune_test_min > AUTO_TUNE_TARGET_RATE_CDS*AUTO_TUNE_AGGRESSIVENESS) {
auto_tune_counter++;
}else{
if (auto_tune_counter > 0 ) {
auto_tune_counter--;
}
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_rd += AUTO_TUNE_RD_STEP*2.0f;
// stop tuning if we hit max D
if (tune_roll_rd >= AUTO_TUNE_RD_MAX) {
tune_roll_rd = AUTO_TUNE_RD_MAX;
auto_tune_counter = AUTO_TUNE_SUCCESS_COUNT;
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
}
}else{
tune_pitch_rd += AUTO_TUNE_RD_STEP*2.0f;
// stop tuning if we hit max D
if (tune_pitch_rd >= AUTO_TUNE_RD_MAX) {
tune_pitch_rd = AUTO_TUNE_RD_MAX;
auto_tune_counter = AUTO_TUNE_SUCCESS_COUNT;
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
}
}
}
}
} else if (auto_tune_state.tune_type == AUTO_TUNE_TYPE_RD_DOWN) {
if (auto_tune_test_max > AUTO_TUNE_TARGET_RATE_CDS) {
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_rp -= AUTO_TUNE_RP_STEP;
}else{
tune_pitch_rp -= AUTO_TUNE_RP_STEP;
}
// 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);
set_roll_pitch_mode(ROLL_PITCH_STABLE);
auto_tune_update_gcs(AUTO_TUNE_MESSAGE_FAILED);
return;
}
}else if(auto_tune_test_max < AUTO_TUNE_TARGET_RATE_CDS*(1-AUTO_TUNE_AGGRESSIVENESS*2.0f) &&
((auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL && tune_roll_rp <= AUTO_TUNE_RP_MAX) ||
(auto_tune_state.axis == AUTO_TUNE_AXIS_PITCH && tune_pitch_rp <= AUTO_TUNE_RP_MAX)) ) {
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_rp += AUTO_TUNE_RP_STEP;
}else{
tune_pitch_rp += AUTO_TUNE_RP_STEP;
}
}else{
if (auto_tune_test_max-auto_tune_test_min < AUTO_TUNE_TARGET_RATE_CDS*AUTO_TUNE_AGGRESSIVENESS) {
auto_tune_counter++;
}else{
if (auto_tune_counter > 0 ) {
auto_tune_counter--;
}
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_rd -= AUTO_TUNE_RD_STEP;
// stop tuning if we hit max D
if (tune_roll_rd <= AUTO_TUNE_RD_MIN) {
tune_roll_rd = AUTO_TUNE_RD_MIN;
auto_tune_counter = AUTO_TUNE_SUCCESS_COUNT;
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
}
}else{
tune_pitch_rd -= AUTO_TUNE_RD_STEP;
// stop tuning if we hit max D
if (tune_pitch_rd <= AUTO_TUNE_RD_MIN) {
tune_pitch_rd = AUTO_TUNE_RD_MIN;
auto_tune_counter = AUTO_TUNE_SUCCESS_COUNT;
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
}
}
}
}
} else if (auto_tune_state.tune_type == AUTO_TUNE_TYPE_RP_UP) {
if (auto_tune_test_max > AUTO_TUNE_TARGET_RATE_CDS) {
auto_tune_counter++;
}else{
if (auto_tune_counter > 0 ) {
auto_tune_counter--;
}
// increase P & I or D term
// update PI term
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_rp += AUTO_TUNE_RP_STEP;
// stop tuning if we hit max P
if (tune_roll_rp >= AUTO_TUNE_RP_MAX) {
tune_roll_rp = AUTO_TUNE_RP_MAX;
auto_tune_counter = AUTO_TUNE_SUCCESS_COUNT;
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
}
}else{
tune_pitch_rp += AUTO_TUNE_RP_STEP;
// stop tuning if we hit max P
if (tune_pitch_rp >= AUTO_TUNE_RP_MAX) {
tune_pitch_rp = AUTO_TUNE_RP_MAX;
auto_tune_counter = AUTO_TUNE_SUCCESS_COUNT;
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
}
}
}
} else if (auto_tune_state.tune_type == AUTO_TUNE_TYPE_SP_UP) {
if (auto_tune_test_max > AUTO_TUNE_TARGET_ANGLE_CD*(1+AUTO_TUNE_AGGRESSIVENESS) ||
(auto_tune_test_max-auto_tune_test_min > AUTO_TUNE_TARGET_ANGLE_CD*AUTO_TUNE_AGGRESSIVENESS)) {
auto_tune_counter++;
}else{
if (auto_tune_counter > 0 ) {
auto_tune_counter--;
}
// increase P & I or D term
// update PI term
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_sp += AUTO_TUNE_SP_STEP;
// stop tuning if we hit max P
if (tune_roll_sp >= AUTO_TUNE_SP_MAX) {
tune_roll_sp = AUTO_TUNE_SP_MAX;
auto_tune_counter = AUTO_TUNE_SUCCESS_COUNT;
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
}
}else{
tune_pitch_sp += AUTO_TUNE_SP_STEP;
// stop tuning if we hit max P
if (tune_pitch_sp >= AUTO_TUNE_SP_MAX) {
tune_pitch_sp = AUTO_TUNE_SP_MAX;
auto_tune_counter = AUTO_TUNE_SUCCESS_COUNT;
Log_Write_Event(DATA_AUTOTUNE_REACHED_LIMIT);
}
}
}
}
// reverse direction
auto_tune_state.positive_direction = !auto_tune_state.positive_direction;
// we've complete this step, finalise pids and move to next step
if (auto_tune_counter >= AUTO_TUNE_SUCCESS_COUNT) {
// reset counter
auto_tune_counter = 0;
// move to the next tuning type
if (auto_tune_state.tune_type < AUTO_TUNE_TYPE_SP_UP) {
auto_tune_state.tune_type++;
}else{
// we've reached the end of a D-up-down PI-up-down tune type cycle
auto_tune_state.tune_type = AUTO_TUNE_TYPE_RD_UP;
// if we've just completed roll move onto pitch
if (auto_tune_state.axis == AUTO_TUNE_AXIS_ROLL) {
tune_roll_sp = tune_roll_sp * AUTO_TUNE_SP_BACKOFF;
auto_tune_state.axis = AUTO_TUNE_AXIS_PITCH;
}else{
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 tuning and are moving onto testing
auto_tune_state.mode = AUTO_TUNE_MODE_TESTING;
auto_tune_update_gcs(AUTO_TUNE_MESSAGE_SUCCESS);
Log_Write_Event(DATA_AUTOTUNE_COMPLETE);
set_roll_pitch_mode(ROLL_PITCH_STABLE);
}
}
}
// reset testing step
auto_tune_state.step = AUTO_TUNE_STEP_WAITING_FOR_LEVEL;
auto_tune_timer = millis();
break;
}
}
}
#endif // AUTOTUNE == ENABLED
Reference in New Issue View Git Blame Copy Permalink