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

585 lines
29 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.0f // minimum Rate D value
#define AUTO_TUNE_RD_MAX 0.1f // maximum Rate D value
#define AUTO_TUNE_RP_MIN 0.02f // minimum Rate P value
#define AUTO_TUNE_RP_MAX 1.0f // maximum Rate P value
#define AUTO_TUNE_SP_MIN 1.0f // minimum Stab 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
// 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 {
uint8_t enabled : 1; // 0 = disabled, 1 = enabled
uint8_t active : 1; // 0 = inactive (temporarily suspended), 1 = actively tuning
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
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
// store current pids as originals
void auto_tune_save_orig_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();
}
// auto_tune_restore_orig_pids - restore pids to their original values
void auto_tune_restore_orig_pids()
{
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 original values
void auto_tune_load_tuned_pids()
{
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;
}
}
// start an auto tuning session
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 (i.e. we were not just suspended)
if (!auto_tune_state.active) {
auto_tune_state.active = true;
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;
Log_Write_Event(DATA_AUTOTUNE_ON);
}
}
}
// turn off tuning and return to standard pids
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);
}
}
// 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()
{
if (auto_tune_state.enabled) {
auto_tune_load_tuned_pids();
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);
}
}
// Auto tuning roll-pitch controller
static void
get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch_angle)
{
int32_t target_roll_rate, target_pitch_rate;
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) {
return;
}
// check for pilot override
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_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 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
}
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 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;
// 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)) ) {
Log_Write_Event(DATA_AUTOTUNE_ABANDONED);
auto_tune_stop();
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;
}
}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;
}
}
}
}
} 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)) ) {
Log_Write_Event(DATA_AUTOTUNE_ABANDONED);
auto_tune_stop();
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;
}
}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;
}
}
}
}
} 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;
}
}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;
}
}
}
} 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;
}
}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;
}
}
}
}
// 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 so suspend tuning
auto_tune_suspend();
}
}
}
// 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