diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp new file mode 100644 index 0000000000..a3c3bda46a --- /dev/null +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -0,0 +1,1628 @@ +#include "AC_AutoTune.h" +#include +#include + +/* + * autotune support for multicopters + * + * Instructions: + * 1) Set up one flight mode switch position to be AltHold. + * 2) Set the Ch7 Opt or Ch8 Opt to AutoTune to allow you to turn the auto tuning on/off with the ch7 or ch8 switch. + * 3) Ensure the ch7 or ch8 switch is in the LOW position. + * 4) Wait for a calm day and go to a large open area. + * 5) Take off and put the vehicle into AltHold mode at a comfortable altitude. + * 6) Set the ch7/ch8 switch to the HIGH position to engage auto tuning: + * a) You will see it twitch about 20 degrees left and right for a few minutes, then it will repeat forward and back. + * b) Use the roll and pitch stick at any time to reposition the copter if it drifts away (it will use the original PID gains during repositioning and between tests). + * When you release the sticks it will continue auto tuning where it left off. + * c) Move the ch7/ch8 switch into the LOW position at any time to abandon the autotuning and return to the origin PIDs. + * d) Make sure that you do not have any trim set on your transmitter or the autotune may not get the signal that the sticks are centered. + * 7) When the tune completes the vehicle will change back to the original PID gains. + * 8) Put the ch7/ch8 switch into the LOW position then back to the HIGH position to test the tuned PID gains. + * 9) Put the ch7/ch8 switch into the LOW position to fly using the original PID gains. + * 10) If you are happy with the autotuned PID gains, leave the ch7/ch8 switch in the HIGH position, land and disarm to save the PIDs permanently. + * If you DO NOT like the new PIDS, switch ch7/ch8 LOW to return to the original PIDs. The gains will not be saved when you disarm + * + * What it's doing during each "twitch": + * 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 request 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% + * + */ + +#define AUTOTUNE_AXIS_BITMASK_ROLL 1 +#define AUTOTUNE_AXIS_BITMASK_PITCH 2 +#define AUTOTUNE_AXIS_BITMASK_YAW 4 + +#define AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS 500 // restart tuning if pilot has left sticks in middle for 2 seconds +#define AUTOTUNE_TESTING_STEP_TIMEOUT_MS 1000 // timeout for tuning mode's testing step +#define AUTOTUNE_LEVEL_ANGLE_CD 500 // angle which qualifies as level +#define AUTOTUNE_LEVEL_RATE_RP_CD 1000 // rate which qualifies as level for roll and pitch +#define AUTOTUNE_LEVEL_RATE_Y_CD 750 // rate which qualifies as level for yaw +#define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 500 // time we require the copter to be level +#define AUTOTUNE_RD_STEP 0.05f // minimum increment when increasing/decreasing Rate D term +#define AUTOTUNE_RP_STEP 0.05f // minimum increment when increasing/decreasing Rate P term +#define AUTOTUNE_SP_STEP 0.05f // minimum increment when increasing/decreasing Stab P term +#define AUTOTUNE_PI_RATIO_FOR_TESTING 0.1f // I is set 10x smaller than P during testing +#define AUTOTUNE_PI_RATIO_FINAL 1.0f // I is set 1x P after testing +#define AUTOTUNE_YAW_PI_RATIO_FINAL 0.1f // I is set 1x P after testing +#define AUTOTUNE_RD_MAX 0.200f // maximum Rate D value +#define AUTOTUNE_RLPF_MIN 1.0f // minimum Rate Yaw filter value +#define AUTOTUNE_RLPF_MAX 5.0f // maximum Rate Yaw filter value +#define AUTOTUNE_RP_MIN 0.01f // minimum Rate P value +#define AUTOTUNE_RP_MAX 2.0f // maximum Rate P value +#define AUTOTUNE_SP_MAX 20.0f // maximum Stab P value +#define AUTOTUNE_SP_MIN 0.5f // maximum Stab P value +#define AUTOTUNE_RP_ACCEL_MIN 4000.0f // Minimum acceleration for Roll and Pitch +#define AUTOTUNE_Y_ACCEL_MIN 1000.0f // Minimum acceleration for Yaw +#define AUTOTUNE_Y_FILT_FREQ 10.0f // Autotune filter frequency when testing Yaw +#define AUTOTUNE_SUCCESS_COUNT 4 // The number of successful iterations we need to freeze at current gains +#define AUTOTUNE_D_UP_DOWN_MARGIN 0.2f // The margin below the target that we tune D in +#define AUTOTUNE_RD_BACKOFF 1.0f // Rate D gains are reduced to 50% of their maximum value discovered during tuning +#define AUTOTUNE_RP_BACKOFF 1.0f // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning +#define AUTOTUNE_SP_BACKOFF 0.9f // Stab P gains are reduced to 90% of their maximum value discovered during tuning +#define AUTOTUNE_ACCEL_RP_BACKOFF 1.0f // back off from maximum acceleration +#define AUTOTUNE_ACCEL_Y_BACKOFF 1.0f // back off from maximum acceleration + +// roll and pitch axes +#define AUTOTUNE_TARGET_ANGLE_RLLPIT_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_RATE_RLLPIT_CDS 18000 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step +#define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD 1000 // minimum target angle during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS 4500 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step + +// yaw axis +#define AUTOTUNE_TARGET_ANGLE_YAW_CD 3000 // target angle during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_RATE_YAW_CDS 9000 // target yaw rate during AUTOTUNE_STEP_TWITCHING step +#define AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD 500 // minimum target angle during TESTING_RATE step that will cause us to move to next step +#define AUTOTUNE_TARGET_MIN_RATE_YAW_CDS 1500 // minimum target yaw rate during AUTOTUNE_STEP_TWITCHING step + +// Auto Tune message ids for ground station +#define AUTOTUNE_MESSAGE_STARTED 0 +#define AUTOTUNE_MESSAGE_STOPPED 1 +#define AUTOTUNE_MESSAGE_SUCCESS 2 +#define AUTOTUNE_MESSAGE_FAILED 3 +#define AUTOTUNE_MESSAGE_SAVED_GAINS 4 + +#define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000 + +// second table of user settable parameters for quadplanes, this +// allows us to go beyond the 64 parameter limit +const AP_Param::GroupInfo AC_AutoTune::var_info[] = { + // @Param: AXES + // @DisplayName: Autotune axis bitmask + // @Description: 1-byte bitmap of axes to autotune + // @Values: 7:All,1:Roll Only,2:Pitch Only,4:Yaw Only,3:Roll and Pitch,5:Roll and Yaw,6:Pitch and Yaw + // @Bitmask: 0:Roll,1:Pitch,2:Yaw + // @User: Standard + AP_GROUPINFO("AXES", 1, AC_AutoTune, axis_bitmask, 7), // AUTOTUNE_AXIS_BITMASK_DEFAULT + + // @Param: AGGR + // @DisplayName: Autotune aggressiveness + // @Description: Autotune aggressiveness. Defines the bounce back used to detect size of the D term. + // @Range: 0.05 0.10 + // @User: Standard + AP_GROUPINFO("AGGR", 2, AC_AutoTune, aggressiveness, 0.1f), + + // @Param: MIN_D + // @DisplayName: AutoTune minimum D + // @Description: Defines the minimum D gain + // @Range: 0.001 0.006 + // @User: Standard + AP_GROUPINFO("MIN_D", 3, AC_AutoTune, min_d, 0.001f), + + AP_GROUPEND +}; + +AC_AutoTune::AC_AutoTune() +{ + AP_Param::setup_object_defaults(this, var_info); +} + +// autotune_init - should be called when autotune mode is selected +bool AC_AutoTune::init_internals(bool _use_poshold, + AC_AttitudeControl_Multi *_attitude_control, + AC_PosControl *_pos_control, + AP_AHRS_View *_ahrs_view, + AP_InertialNav *_inertial_nav) +{ + bool success = true; + + use_poshold = _use_poshold; + attitude_control = _attitude_control; + pos_control = _pos_control; + ahrs_view = _ahrs_view; + inertial_nav = _inertial_nav; + motors = AP_Motors::get_instance(); + + switch (mode) { + case FAILED: + // autotune has been run but failed so reset state to uninitialized + mode = UNINITIALISED; + // fall through to restart the tuning + FALLTHROUGH; + + case UNINITIALISED: + // autotune has never been run + success = start(); + if (success) { + // so store current gains as original gains + backup_gains_and_initialise(); + // advance mode to tuning + mode = TUNING; + // send message to ground station that we've started tuning + update_gcs(AUTOTUNE_MESSAGE_STARTED); + } + break; + + case TUNING: + // we are restarting tuning after the user must have switched ch7/ch8 off so we restart tuning where we left off + success = start(); + if (success) { + // reset gains to tuning-start gains (i.e. low I term) + load_intra_test_gains(); + // write dataflash log even and send message to ground station + Log_Write_Event(EVENT_AUTOTUNE_RESTART); + update_gcs(AUTOTUNE_MESSAGE_STARTED); + } + break; + + case SUCCESS: + // we have completed a tune and the pilot wishes to test the new gains in the current flight mode + // so simply apply tuning gains (i.e. do not change flight mode) + load_tuned_gains(); + Log_Write_Event(EVENT_AUTOTUNE_PILOT_TESTING); + break; + } + + have_position = false; + + return success; +} + +// stop - should be called when the ch7/ch8 switch is switched OFF +void AC_AutoTune::stop() +{ + // set gains to their original values + load_orig_gains(); + + // re-enable angle-to-rate request limits + attitude_control->use_sqrt_controller(true); + + // log off event and send message to ground station + update_gcs(AUTOTUNE_MESSAGE_STOPPED); + Log_Write_Event(EVENT_AUTOTUNE_OFF); + + // Note: we leave the mode as it was so that we know how the autotune ended + // we expect the caller will change the flight mode back to the flight mode indicated by the flight mode switch +} + +// start - Initialize autotune flight mode +bool AC_AutoTune::start(void) +{ + if (!motors->armed()) { + return false; + } + + // initialize vertical speeds and leash lengths + init_z_limits(); + + // initialise position and desired velocity + if (!pos_control->is_active_z()) { + pos_control->set_alt_target_to_current_alt(); + pos_control->set_desired_velocity_z(inertial_nav->get_velocity_z()); + } + + return true; +} + +const char *AC_AutoTune::level_issue_string() const +{ + switch (level_problem.issue) { + case LEVEL_ISSUE_NONE: + return "None"; + case LEVEL_ISSUE_ANGLE_ROLL: + return "Angle(R)"; + case LEVEL_ISSUE_ANGLE_PITCH: + return "Angle(P)"; + case LEVEL_ISSUE_ANGLE_YAW: + return "Angle(Y)"; + case LEVEL_ISSUE_RATE_ROLL: + return "Rate(R)"; + case LEVEL_ISSUE_RATE_PITCH: + return "Rate(P)"; + case LEVEL_ISSUE_RATE_YAW: + return "Rate(Y)"; + } + return "Bug"; +} + +void AC_AutoTune::send_step_string() +{ + if (pilot_override) { + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Paused: Pilot Override Active"); + return; + } + switch (step) { + case WAITING_FOR_LEVEL: + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: WFL (%s) (%f > %f)", level_issue_string(), (double)(level_problem.current*0.01f), (double)(level_problem.maximum*0.01f)); + return; + case UPDATE_GAINS: + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: UPDATING_GAINS"); + return; + case TWITCHING: + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: TWITCHING"); + return; + } + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: unknown step"); +} + +const char *AC_AutoTune::type_string() const +{ + switch (tune_type) { + case RD_UP: + return "Rate D Up"; + case RD_DOWN: + return "Rate D Down"; + case RP_UP: + return "Rate P Up"; + case SP_DOWN: + return "Angle P Down"; + case SP_UP: + return "Angle P Up"; + } + return "Bug"; +} + +void AC_AutoTune::do_gcs_announcements() +{ + const uint32_t now = AP_HAL::millis(); + if (now - announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) { + return; + } + float tune_rp = 0.0f; + float tune_rd = 0.0f; + float tune_sp = 0.0f; + float tune_accel = 0.0f; + char axis_char = '?'; + switch (axis) { + case ROLL: + tune_rp = tune_roll_rp; + tune_rd = tune_roll_rd; + tune_sp = tune_roll_sp; + tune_accel = tune_roll_accel; + axis_char = 'R'; + break; + case PITCH: + tune_rp = tune_pitch_rp; + tune_rd = tune_pitch_rd; + tune_sp = tune_pitch_sp; + tune_accel = tune_pitch_accel; + axis_char = 'P'; + break; + case YAW: + tune_rp = tune_yaw_rp; + tune_rd = tune_yaw_rLPF; + tune_sp = tune_yaw_sp; + tune_accel = tune_yaw_accel; + axis_char = 'Y'; + break; + } + + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: (%c) %s", axis_char, type_string()); + send_step_string(); + if (!is_zero(lean_angle)) { + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: lean=%f target=%f", (double)lean_angle, (double)target_angle); + } + if (!is_zero(rotation_rate)) { + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: rotation=%f target=%f", (double)(rotation_rate*0.01f), (double)(target_rate*0.01f)); + } + switch (tune_type) { + case RD_UP: + case RD_DOWN: + case RP_UP: + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f d=%f", (double)tune_rp, (double)tune_rd); + break; + case SP_DOWN: + case SP_UP: + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f accel=%f", (double)tune_sp, (double)tune_accel); + break; + } + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: success %u/%u", counter, AUTOTUNE_SUCCESS_COUNT); + + announce_time = now; +} + +// run - runs the autotune flight mode +// should be called at 100hz or more +void AC_AutoTune::run() +{ + int32_t target_climb_rate_cms; + + // initialize vertical speeds and acceleration + init_z_limits(); + + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately + // this should not actually be possible because of the init() checks + if (!motors->armed() || !motors->get_interlock()) { + motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); + attitude_control->set_throttle_out_unstabilized(0.0f, true, 0); + pos_control->relax_alt_hold_controllers(0.0f); + return; + } + + + int32_t target_roll_cd, target_pitch_cd, target_yaw_rate_cds; + get_pilot_desired_rp_yrate_cd(target_roll_cd, target_pitch_cd, target_yaw_rate_cds); + + // get pilot desired climb rate + target_climb_rate_cms = get_pilot_desired_climb_rate_cms(); + + bool zero_rp_input = target_roll_cd == 0 && target_pitch_cd == 0; + if (!zero_rp_input || target_yaw_rate_cds != 0 || target_climb_rate_cms != 0) { + if (!pilot_override) { + pilot_override = true; + // set gains to their original values + load_orig_gains(); + attitude_control->use_sqrt_controller(true); + } + // reset pilot override time + override_time = AP_HAL::millis(); + if (!zero_rp_input) { + // only reset position on roll or pitch input + have_position = false; + } + } else if (pilot_override) { + // check if we should resume tuning after pilot's override + if (AP_HAL::millis() - override_time > AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS) { + pilot_override = false; // turn off pilot override + // set gains to their intra-test values (which are very close to the original gains) + // load_intra_test_gains(); //I think we should be keeping the originals here to let the I term settle quickly + step = WAITING_FOR_LEVEL; // set tuning step back from beginning + desired_yaw_cd = ahrs_view->yaw_sensor; + } + } + + if (zero_rp_input) { + // pilot input on throttle and yaw will still use position hold if enabled + get_poshold_attitude(target_roll_cd, target_pitch_cd, desired_yaw_cd); + } + + // set motors to full range + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + + // if pilot override call attitude controller + if (pilot_override || mode != TUNING) { + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll_cd, target_pitch_cd, target_yaw_rate_cds); + } else { + // somehow get attitude requests from autotuning + control_attitude(); + // tell the user what's going on + do_gcs_announcements(); + } + + // call position controller + pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate_cms, AP::scheduler().get_last_loop_time_s(), false); + pos_control->update_z_controller(); + +} + +bool AC_AutoTune::check_level(const LEVEL_ISSUE issue, const float current, const float maximum) +{ + if (current > maximum) { + level_problem.current = current; + level_problem.maximum = maximum; + level_problem.issue = issue; + return false; + } + return true; +} + +bool AC_AutoTune::currently_level() +{ + if (!check_level(LEVEL_ISSUE_ANGLE_ROLL, + fabsf(ahrs_view->roll_sensor - roll_cd), + AUTOTUNE_LEVEL_ANGLE_CD)) { + return false; + } + + if (!check_level(LEVEL_ISSUE_ANGLE_PITCH, + fabsf(ahrs_view->pitch_sensor - pitch_cd), + AUTOTUNE_LEVEL_ANGLE_CD)) { + return false; + } + if (!check_level(LEVEL_ISSUE_ANGLE_YAW, + fabsf(wrap_180_cd(ahrs_view->yaw_sensor - desired_yaw_cd)), + AUTOTUNE_LEVEL_ANGLE_CD)) { + return false; + } + if (!check_level(LEVEL_ISSUE_RATE_ROLL, + (ToDeg(ahrs_view->get_gyro().x) * 100.0f), + AUTOTUNE_LEVEL_RATE_RP_CD)) { + return false; + } + if (!check_level(LEVEL_ISSUE_RATE_PITCH, + (ToDeg(ahrs_view->get_gyro().y) * 100.0f), + AUTOTUNE_LEVEL_RATE_RP_CD)) { + return false; + } + if (!check_level(LEVEL_ISSUE_RATE_YAW, + (ToDeg(ahrs_view->get_gyro().z) * 100.0f), + AUTOTUNE_LEVEL_RATE_Y_CD)) { + return false; + } + return true; +} + +// attitude_controller - sets attitude control targets during tuning +void AC_AutoTune::control_attitude() +{ + rotation_rate = 0.0f; // rotation rate in radians/second + lean_angle = 0.0f; + const float direction_sign = positive_direction ? 1.0f : -1.0f; + const uint32_t now = AP_HAL::millis(); + + // check tuning step + switch (step) { + + case WAITING_FOR_LEVEL: { + // Note: we should be using intra-test gains (which are very close to the original gains but have lower I) + // re-enable rate limits + attitude_control->use_sqrt_controller(true); + + get_poshold_attitude(roll_cd, pitch_cd, desired_yaw_cd); + + // hold level attitude + attitude_control->input_euler_angle_roll_pitch_yaw(roll_cd, pitch_cd, desired_yaw_cd, true); + + // hold the copter level for 0.5 seconds before we begin a twitch + // reset counter if we are no longer level + if (!currently_level()) { + step_start_time = now; + } + + // if we have been level for a sufficient amount of time (0.5 seconds) move onto tuning step + if (now - step_start_time >= AUTOTUNE_REQUIRED_LEVEL_TIME_MS) { + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Twitch"); + // initiate variables for next step + step = TWITCHING; + step_start_time = now; + step_stop_time = step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS; + twitch_first_iter = true; + test_rate_max = 0.0f; + test_rate_min = 0.0f; + test_angle_max = 0.0f; + test_angle_min = 0.0f; + rotation_rate_filt.reset(0.0f); + rate_max = 0.0f; + // set gains to their to-be-tested values + load_twitch_gains(); + } + + switch (axis) { + case ROLL: + target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS); + target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD); + start_rate = ToDeg(ahrs_view->get_gyro().x) * 100.0f; + start_angle = ahrs_view->roll_sensor; + rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_hz()*2.0f); + break; + case PITCH: + target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS); + target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD); + start_rate = ToDeg(ahrs_view->get_gyro().y) * 100.0f; + start_angle = ahrs_view->pitch_sensor; + rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_hz()*2.0f); + break; + case YAW: + target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, AUTOTUNE_TARGET_RATE_YAW_CDS); + target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD, AUTOTUNE_TARGET_ANGLE_YAW_CD); + start_rate = ToDeg(ahrs_view->get_gyro().z) * 100.0f; + start_angle = ahrs_view->yaw_sensor; + rotation_rate_filt.set_cutoff_frequency(AUTOTUNE_Y_FILT_FREQ); + break; + } + if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { + rotation_rate_filt.reset(start_rate); + } else { + rotation_rate_filt.reset(0); + } + break; + } + + case TWITCHING: { + // Run the twitching step + // Note: we should be using intra-test gains (which are very close to the original gains but have lower I) + + // disable rate limits + attitude_control->use_sqrt_controller(false); + // hold current attitude + attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); + + if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { + // step angle targets on first iteration + if (twitch_first_iter) { + twitch_first_iter = false; + // Testing increasing stabilize P gain so will set lean angle target + switch (axis) { + case ROLL: + // request roll to 20deg + attitude_control->input_angle_step_bf_roll_pitch_yaw(direction_sign * target_angle, 0.0f, 0.0f); + break; + case PITCH: + // request pitch to 20deg + attitude_control->input_angle_step_bf_roll_pitch_yaw(0.0f, direction_sign * target_angle, 0.0f); + break; + case YAW: + // request pitch to 20deg + attitude_control->input_angle_step_bf_roll_pitch_yaw(0.0f, 0.0f, direction_sign * target_angle); + break; + } + } + } else { + // Testing rate P and D gains so will set body-frame rate targets. + // Rate controller will use existing body-frame rates and convert to motor outputs + // for all axes except the one we override here. + switch (axis) { + case ROLL: + // override body-frame roll rate + attitude_control->rate_bf_roll_target(direction_sign * target_rate + start_rate); + break; + case PITCH: + // override body-frame pitch rate + attitude_control->rate_bf_pitch_target(direction_sign * target_rate + start_rate); + break; + case YAW: + // override body-frame yaw rate + attitude_control->rate_bf_yaw_target(direction_sign * target_rate + start_rate); + break; + } + } + + // capture this iterations rotation rate and lean angle + float gyro_reading = 0; + switch (axis) { + case ROLL: + gyro_reading = ahrs_view->get_gyro().x; + lean_angle = direction_sign * (ahrs_view->roll_sensor - (int32_t)start_angle); + break; + case PITCH: + gyro_reading = ahrs_view->get_gyro().y; + lean_angle = direction_sign * (ahrs_view->pitch_sensor - (int32_t)start_angle); + break; + case YAW: + gyro_reading = ahrs_view->get_gyro().z; + lean_angle = direction_sign * wrap_180_cd(ahrs_view->yaw_sensor-(int32_t)start_angle); + break; + } + + // Add filter to measurements + float filter_value; + switch (tune_type) { + case SP_DOWN: + case SP_UP: + filter_value = direction_sign * (ToDeg(gyro_reading) * 100.0f); + break; + default: + filter_value = direction_sign * (ToDeg(gyro_reading) * 100.0f - start_rate); + break; + } + rotation_rate = rotation_rate_filt.apply(filter_value, + AP::scheduler().get_loop_period_s()); + + switch (tune_type) { + case RD_UP: + case RD_DOWN: + twitching_test_rate(rotation_rate, target_rate, test_rate_min, test_rate_max); + twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max); + if (lean_angle >= target_angle) { + step = UPDATE_GAINS; + } + break; + case RP_UP: + twitching_test_rate(rotation_rate, target_rate*(1+0.5f*aggressiveness), test_rate_min, test_rate_max); + twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max); + if (lean_angle >= target_angle) { + step = UPDATE_GAINS; + } + break; + case SP_DOWN: + case SP_UP: + twitching_test_angle(lean_angle, rotation_rate, target_angle*(1+0.5f*aggressiveness), test_angle_min, test_angle_max, test_rate_min, test_rate_max); + twitching_measure_acceleration(test_accel_max, rotation_rate - direction_sign * start_rate, rate_max); + break; + } + + // log this iterations lean angle and rotation rate + Log_Write_AutoTuneDetails(lean_angle, rotation_rate); + DataFlash_Class::instance()->Log_Write_Rate(AP::ahrs(), *motors, *attitude_control, *pos_control); + + break; + } + + case UPDATE_GAINS: + + // re-enable rate limits + attitude_control->use_sqrt_controller(true); + + + // log the latest gains + if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { + switch (axis) { + case ROLL: + Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max); + break; + case PITCH: + Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max); + break; + case YAW: + Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max); + break; + } + } else { + switch (axis) { + case ROLL: + Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max); + break; + case PITCH: + Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max); + break; + case YAW: + Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max); + break; + } + } + + + // Check results after mini-step to increase rate D gain + switch (tune_type) { + case RD_UP: + switch (axis) { + case ROLL: + updating_rate_d_up(tune_roll_rd, min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + break; + case PITCH: + updating_rate_d_up(tune_pitch_rd, min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + break; + case YAW: + updating_rate_d_up(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RLPF_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + break; + } + break; + // Check results after mini-step to decrease rate D gain + case RD_DOWN: + switch (axis) { + case ROLL: + updating_rate_d_down(tune_roll_rd, min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + break; + case PITCH: + updating_rate_d_down(tune_pitch_rd, min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + break; + case YAW: + updating_rate_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + break; + } + break; + // Check results after mini-step to increase rate P gain + case RP_UP: + switch (axis) { + case ROLL: + updating_rate_p_up_d_down(tune_roll_rd, min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + break; + case PITCH: + updating_rate_p_up_d_down(tune_pitch_rd, min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + break; + case YAW: + updating_rate_p_up_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max); + break; + } + break; + // Check results after mini-step to increase stabilize P gain + case SP_DOWN: + switch (axis) { + case ROLL: + updating_angle_p_down(tune_roll_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); + break; + case PITCH: + updating_angle_p_down(tune_pitch_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); + break; + case YAW: + updating_angle_p_down(tune_yaw_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); + break; + } + break; + // Check results after mini-step to increase stabilize P gain + case SP_UP: + switch (axis) { + case ROLL: + updating_angle_p_up(tune_roll_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); + break; + case PITCH: + updating_angle_p_up(tune_pitch_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); + break; + case YAW: + updating_angle_p_up(tune_yaw_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max); + break; + } + break; + } + + // we've complete this step, finalize pids and move to next step + if (counter >= AUTOTUNE_SUCCESS_COUNT) { + + // reset counter + counter = 0; + + // move to the next tuning type + switch (tune_type) { + case RD_UP: + tune_type = TuneType(tune_type + 1); + break; + case RD_DOWN: + tune_type = TuneType(tune_type + 1); + switch (axis) { + case ROLL: + tune_roll_rd = MAX(min_d, tune_roll_rd * AUTOTUNE_RD_BACKOFF); + tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RD_BACKOFF); + break; + case PITCH: + tune_pitch_rd = MAX(min_d, tune_pitch_rd * AUTOTUNE_RD_BACKOFF); + tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RD_BACKOFF); + break; + case YAW: + tune_yaw_rLPF = MAX(AUTOTUNE_RLPF_MIN, tune_yaw_rLPF * AUTOTUNE_RD_BACKOFF); + tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RD_BACKOFF); + break; + } + break; + case RP_UP: + tune_type = TuneType(tune_type + 1); + switch (axis) { + case ROLL: + tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RP_BACKOFF); + break; + case PITCH: + tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RP_BACKOFF); + break; + case YAW: + tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF); + break; + } + break; + case SP_DOWN: + tune_type = TuneType(tune_type + 1); + break; + case SP_UP: + // we've reached the end of a D-up-down PI-up-down tune type cycle + tune_type = RD_UP; + + // advance to the next axis + bool complete = false; + switch (axis) { + case ROLL: + tune_roll_sp = MAX(AUTOTUNE_SP_MIN, tune_roll_sp * AUTOTUNE_SP_BACKOFF); + tune_roll_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF); + if (pitch_enabled()) { + axis = PITCH; + } else if (yaw_enabled()) { + axis = YAW; + } else { + complete = true; + } + break; + case PITCH: + tune_pitch_sp = MAX(AUTOTUNE_SP_MIN, tune_pitch_sp * AUTOTUNE_SP_BACKOFF); + tune_pitch_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF); + if (yaw_enabled()) { + axis = YAW; + } else { + complete = true; + } + break; + case YAW: + tune_yaw_sp = MAX(AUTOTUNE_SP_MIN, tune_yaw_sp * AUTOTUNE_SP_BACKOFF); + tune_yaw_accel = MAX(AUTOTUNE_Y_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_Y_BACKOFF); + complete = true; + break; + } + + // if we've just completed all axes we have successfully completed the autotune + // change to TESTING mode to allow user to fly with new gains + if (complete) { + mode = SUCCESS; + update_gcs(AUTOTUNE_MESSAGE_SUCCESS); + Log_Write_Event(EVENT_AUTOTUNE_SUCCESS); + AP_Notify::events.autotune_complete = true; + } else { + AP_Notify::events.autotune_next_axis = true; + } + break; + } + } + + // reverse direction + positive_direction = !positive_direction; + + if (axis == YAW) { + attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs_view->yaw_sensor, false); + } + + // set gains to their intra-test values (which are very close to the original gains) + load_intra_test_gains(); + + // reset testing step + step = WAITING_FOR_LEVEL; + step_start_time = now; + break; + } +} + +// backup_gains_and_initialise - store current gains as originals +// called before tuning starts to backup original gains +void AC_AutoTune::backup_gains_and_initialise() +{ + // initialise state because this is our first time + if (roll_enabled()) { + axis = ROLL; + } else if (pitch_enabled()) { + axis = PITCH; + } else if (yaw_enabled()) { + axis = YAW; + } + positive_direction = false; + step = WAITING_FOR_LEVEL; + step_start_time = AP_HAL::millis(); + tune_type = RD_UP; + + desired_yaw_cd = ahrs_view->yaw_sensor; + + aggressiveness = constrain_float(aggressiveness, 0.05f, 0.2f); + + orig_bf_feedforward = attitude_control->get_bf_feedforward(); + + // backup original pids and initialise tuned pid values + orig_roll_rp = attitude_control->get_rate_roll_pid().kP(); + orig_roll_ri = attitude_control->get_rate_roll_pid().kI(); + orig_roll_rd = attitude_control->get_rate_roll_pid().kD(); + orig_roll_sp = attitude_control->get_angle_roll_p().kP(); + orig_roll_accel = attitude_control->get_accel_roll_max(); + tune_roll_rp = attitude_control->get_rate_roll_pid().kP(); + tune_roll_rd = attitude_control->get_rate_roll_pid().kD(); + tune_roll_sp = attitude_control->get_angle_roll_p().kP(); + tune_roll_accel = attitude_control->get_accel_roll_max(); + + orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP(); + orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI(); + orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD(); + orig_pitch_sp = attitude_control->get_angle_pitch_p().kP(); + orig_pitch_accel = attitude_control->get_accel_pitch_max(); + tune_pitch_rp = attitude_control->get_rate_pitch_pid().kP(); + tune_pitch_rd = attitude_control->get_rate_pitch_pid().kD(); + tune_pitch_sp = attitude_control->get_angle_pitch_p().kP(); + tune_pitch_accel = attitude_control->get_accel_pitch_max(); + + orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP(); + orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI(); + orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD(); + orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_hz(); + orig_yaw_accel = attitude_control->get_accel_yaw_max(); + orig_yaw_sp = attitude_control->get_angle_yaw_p().kP(); + tune_yaw_rp = attitude_control->get_rate_yaw_pid().kP(); + tune_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_hz(); + tune_yaw_sp = attitude_control->get_angle_yaw_p().kP(); + tune_yaw_accel = attitude_control->get_accel_yaw_max(); + + Log_Write_Event(EVENT_AUTOTUNE_INITIALISED); +} + +// load_orig_gains - set gains to their original values +// called by stop and failed functions +void AC_AutoTune::load_orig_gains() +{ + attitude_control->bf_feedforward(orig_bf_feedforward); + if (roll_enabled()) { + if (!is_zero(orig_roll_rp)) { + attitude_control->get_rate_roll_pid().kP(orig_roll_rp); + attitude_control->get_rate_roll_pid().kI(orig_roll_ri); + attitude_control->get_rate_roll_pid().kD(orig_roll_rd); + attitude_control->get_angle_roll_p().kP(orig_roll_sp); + attitude_control->set_accel_roll_max(orig_roll_accel); + } + } + if (pitch_enabled()) { + if (!is_zero(orig_pitch_rp)) { + attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp); + attitude_control->get_rate_pitch_pid().kI(orig_pitch_ri); + attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd); + attitude_control->get_angle_pitch_p().kP(orig_pitch_sp); + attitude_control->set_accel_pitch_max(orig_pitch_accel); + } + } + if (yaw_enabled()) { + if (!is_zero(orig_yaw_rp)) { + attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp); + attitude_control->get_rate_yaw_pid().kI(orig_yaw_ri); + attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd); + attitude_control->get_rate_yaw_pid().filt_hz(orig_yaw_rLPF); + attitude_control->get_angle_yaw_p().kP(orig_yaw_sp); + attitude_control->set_accel_yaw_max(orig_yaw_accel); + } + } +} + +// load_tuned_gains - load tuned gains +void AC_AutoTune::load_tuned_gains() +{ + if (!attitude_control->get_bf_feedforward()) { + attitude_control->bf_feedforward(true); + attitude_control->set_accel_roll_max(0.0f); + attitude_control->set_accel_pitch_max(0.0f); + } + if (roll_enabled()) { + if (!is_zero(tune_roll_rp)) { + attitude_control->get_rate_roll_pid().kP(tune_roll_rp); + attitude_control->get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL); + attitude_control->get_rate_roll_pid().kD(tune_roll_rd); + attitude_control->get_angle_roll_p().kP(tune_roll_sp); + attitude_control->set_accel_roll_max(tune_roll_accel); + } + } + if (pitch_enabled()) { + if (!is_zero(tune_pitch_rp)) { + attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); + attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL); + attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); + attitude_control->get_angle_pitch_p().kP(tune_pitch_sp); + attitude_control->set_accel_pitch_max(tune_pitch_accel); + } + } + if (yaw_enabled()) { + if (!is_zero(tune_yaw_rp)) { + attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp); + attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); + attitude_control->get_rate_yaw_pid().kD(0.0f); + attitude_control->get_rate_yaw_pid().filt_hz(tune_yaw_rLPF); + attitude_control->get_angle_yaw_p().kP(tune_yaw_sp); + attitude_control->set_accel_yaw_max(tune_yaw_accel); + } + } +} + +// load_intra_test_gains - gains used between tests +// called during testing mode's update-gains step to set gains ahead of return-to-level step +void AC_AutoTune::load_intra_test_gains() +{ + // we are restarting tuning so reset gains to tuning-start gains (i.e. low I term) + // sanity check the gains + attitude_control->bf_feedforward(true); + if (roll_enabled()) { + attitude_control->get_rate_roll_pid().kP(orig_roll_rp); + attitude_control->get_rate_roll_pid().kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); + attitude_control->get_rate_roll_pid().kD(orig_roll_rd); + attitude_control->get_angle_roll_p().kP(orig_roll_sp); + } + if (pitch_enabled()) { + attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp); + attitude_control->get_rate_pitch_pid().kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); + attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd); + attitude_control->get_angle_pitch_p().kP(orig_pitch_sp); + } + if (yaw_enabled()) { + attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp); + attitude_control->get_rate_yaw_pid().kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); + attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd); + attitude_control->get_rate_yaw_pid().filt_hz(orig_yaw_rLPF); + attitude_control->get_angle_yaw_p().kP(orig_yaw_sp); + } +} + +// load_twitch_gains - load the to-be-tested gains for a single axis +// called by control_attitude() just before it beings testing a gain (i.e. just before it twitches) +void AC_AutoTune::load_twitch_gains() +{ + switch (axis) { + case ROLL: + attitude_control->get_rate_roll_pid().kP(tune_roll_rp); + attitude_control->get_rate_roll_pid().kI(tune_roll_rp*0.01f); + attitude_control->get_rate_roll_pid().kD(tune_roll_rd); + attitude_control->get_angle_roll_p().kP(tune_roll_sp); + break; + case PITCH: + attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); + attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*0.01f); + attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); + attitude_control->get_angle_pitch_p().kP(tune_pitch_sp); + break; + case YAW: + attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp); + attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*0.01f); + attitude_control->get_rate_yaw_pid().kD(0.0f); + attitude_control->get_rate_yaw_pid().filt_hz(tune_yaw_rLPF); + attitude_control->get_angle_yaw_p().kP(tune_yaw_sp); + break; + } +} + +// save_tuning_gains - save the final tuned gains for each axis +// save discovered gains to eeprom if autotuner is enabled (i.e. switch is in the high position) +void AC_AutoTune::save_tuning_gains() +{ + // if we successfully completed tuning + if (mode == SUCCESS) { + + if (!attitude_control->get_bf_feedforward()) { + attitude_control->bf_feedforward_save(true); + attitude_control->save_accel_roll_max(0.0f); + attitude_control->save_accel_pitch_max(0.0f); + } + + // sanity check the rate P values + if (roll_enabled() && !is_zero(tune_roll_rp)) { + // rate roll gains + attitude_control->get_rate_roll_pid().kP(tune_roll_rp); + attitude_control->get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL); + attitude_control->get_rate_roll_pid().kD(tune_roll_rd); + attitude_control->get_rate_roll_pid().save_gains(); + + // stabilize roll + attitude_control->get_angle_roll_p().kP(tune_roll_sp); + attitude_control->get_angle_roll_p().save_gains(); + + // acceleration roll + attitude_control->save_accel_roll_max(tune_roll_accel); + + // resave pids to originals in case the autotune is run again + orig_roll_rp = attitude_control->get_rate_roll_pid().kP(); + orig_roll_ri = attitude_control->get_rate_roll_pid().kI(); + orig_roll_rd = attitude_control->get_rate_roll_pid().kD(); + orig_roll_sp = attitude_control->get_angle_roll_p().kP(); + orig_roll_accel = attitude_control->get_accel_roll_max(); + } + + if (pitch_enabled() && !is_zero(tune_pitch_rp)) { + // rate pitch gains + attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); + attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL); + attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); + attitude_control->get_rate_pitch_pid().save_gains(); + + // stabilize pitch + attitude_control->get_angle_pitch_p().kP(tune_pitch_sp); + attitude_control->get_angle_pitch_p().save_gains(); + + // acceleration pitch + attitude_control->save_accel_pitch_max(tune_pitch_accel); + + // resave pids to originals in case the autotune is run again + orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP(); + orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI(); + orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD(); + orig_pitch_sp = attitude_control->get_angle_pitch_p().kP(); + orig_pitch_accel = attitude_control->get_accel_pitch_max(); + } + + if (yaw_enabled() && !is_zero(tune_yaw_rp)) { + // rate yaw gains + attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp); + attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); + attitude_control->get_rate_yaw_pid().kD(0.0f); + attitude_control->get_rate_yaw_pid().filt_hz(tune_yaw_rLPF); + attitude_control->get_rate_yaw_pid().save_gains(); + + // stabilize yaw + attitude_control->get_angle_yaw_p().kP(tune_yaw_sp); + attitude_control->get_angle_yaw_p().save_gains(); + + // acceleration yaw + attitude_control->save_accel_yaw_max(tune_yaw_accel); + + // resave pids to originals in case the autotune is run again + orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP(); + orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI(); + orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD(); + orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_hz(); + orig_yaw_sp = attitude_control->get_angle_yaw_p().kP(); + orig_yaw_accel = attitude_control->get_accel_pitch_max(); + } + // update GCS and log save gains event + update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS); + Log_Write_Event(EVENT_AUTOTUNE_SAVEDGAINS); + // reset Autotune so that gains are not saved again and autotune can be run again. + mode = UNINITIALISED; + } +} + +// update_gcs - send message to ground station +void AC_AutoTune::update_gcs(uint8_t message_id) +{ + switch (message_id) { + case AUTOTUNE_MESSAGE_STARTED: + gcs().send_text(MAV_SEVERITY_INFO,"AutoTune: Started"); + break; + case AUTOTUNE_MESSAGE_STOPPED: + gcs().send_text(MAV_SEVERITY_INFO,"AutoTune: Stopped"); + break; + case AUTOTUNE_MESSAGE_SUCCESS: + gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Success"); + break; + case AUTOTUNE_MESSAGE_FAILED: + gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Failed"); + break; + case AUTOTUNE_MESSAGE_SAVED_GAINS: + gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Saved gains"); + break; + } +} + +// axis helper functions +inline bool AC_AutoTune::roll_enabled() +{ + return axis_bitmask & AUTOTUNE_AXIS_BITMASK_ROLL; +} + +inline bool AC_AutoTune::pitch_enabled() +{ + return axis_bitmask & AUTOTUNE_AXIS_BITMASK_PITCH; +} + +inline bool AC_AutoTune::yaw_enabled() +{ + return axis_bitmask & AUTOTUNE_AXIS_BITMASK_YAW; +} + +// twitching_test_rate - twitching tests +// update min and max and test for end conditions +void AC_AutoTune::twitching_test_rate(float rate, float rate_target_max, float &meas_rate_min, float &meas_rate_max) +{ + const uint32_t now = AP_HAL::millis(); + + // capture maximum rate + if (rate > meas_rate_max) { + // the measurement is continuing to increase without stopping + meas_rate_max = rate; + meas_rate_min = rate; + } + + // capture minimum measurement after the measurement has peaked (aka "bounce back") + if ((rate < meas_rate_min) && (meas_rate_max > rate_target_max * 0.5f)) { + // the measurement is bouncing back + meas_rate_min = rate; + } + + // calculate early stopping time based on the time it takes to get to 75% + if (meas_rate_max < rate_target_max * 0.75f) { + // the measurement not reached the 75% threshold yet + step_stop_time = step_start_time + (now - step_start_time) * 3.0f; + step_stop_time = MIN(step_stop_time, step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS); + } + + if (meas_rate_max > rate_target_max) { + // the measured rate has passed the maximum target rate + step = UPDATE_GAINS; + } + + if (meas_rate_max-meas_rate_min > meas_rate_max*aggressiveness) { + // the measurement has passed 50% of the maximum rate and bounce back is larger than the threshold + step = UPDATE_GAINS; + } + + if (now >= step_stop_time) { + // we have passed the maximum stop time + step = UPDATE_GAINS; + } +} + +// twitching_test_angle - twitching tests +// update min and max and test for end conditions +void AC_AutoTune::twitching_test_angle(float angle, float rate, float angle_target_max, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max) +{ + const uint32_t now = AP_HAL::millis(); + + // capture maximum angle + if (angle > meas_angle_max) { + // the angle still increasing + meas_angle_max = angle; + meas_angle_min = angle; + } + + // capture minimum angle after we have reached a reasonable maximum angle + if ((angle < meas_angle_min) && (meas_angle_max > angle_target_max * 0.5f)) { + // the measurement is bouncing back + meas_angle_min = angle; + } + + // capture maximum rate + if (rate > meas_rate_max) { + // the measurement is still increasing + meas_rate_max = rate; + meas_rate_min = rate; + } + + // capture minimum rate after we have reached maximum rate + if (rate < meas_rate_min) { + // the measurement is still decreasing + meas_rate_min = rate; + } + + // calculate early stopping time based on the time it takes to get to 75% + if (meas_angle_max < angle_target_max * 0.75f) { + // the measurement not reached the 75% threshold yet + step_stop_time = step_start_time + (now - step_start_time) * 3.0f; + step_stop_time = MIN(step_stop_time, step_start_time + AUTOTUNE_TESTING_STEP_TIMEOUT_MS); + } + + if (meas_angle_max > angle_target_max) { + // the measurement has passed the maximum angle + step = UPDATE_GAINS; + } + + if (meas_angle_max-meas_angle_min > meas_angle_max*aggressiveness) { + // the measurement has passed 50% of the maximum angle and bounce back is larger than the threshold + step = UPDATE_GAINS; + } + + if (now >= step_stop_time) { + // we have passed the maximum stop time + step = UPDATE_GAINS; + } +} + +// twitching_measure_acceleration - measure rate of change of measurement +void AC_AutoTune::twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) +{ + if (rate_measurement_max < rate_measurement) { + rate_measurement_max = rate_measurement; + rate_of_change = (1000.0f*rate_measurement_max)/(AP_HAL::millis() - step_start_time); + } +} + +// updating_rate_d_up - increase D and adjust P to optimize the D term for a little bounce back +// optimize D term while keeping the maximum just below the target by adjusting P +void AC_AutoTune::updating_rate_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max) +{ + if (meas_rate_max > rate_target) { + // if maximum measurement was higher than target + // reduce P gain (which should reduce maximum) + tune_p -= tune_p*tune_p_step_ratio; + if (tune_p < tune_p_min) { + // P gain is at minimum so start reducing D + tune_p = tune_p_min; + tune_d -= tune_d*tune_d_step_ratio; + if (tune_d <= tune_d_min) { + // We have reached minimum D gain so stop tuning + tune_d = tune_d_min; + counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(EVENT_AUTOTUNE_REACHED_LIMIT); + } + } + } else if ((meas_rate_max < rate_target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) { + // we have not achieved a high enough maximum to get a good measurement of bounce back. + // increase P gain (which should increase maximum) + tune_p += tune_p*tune_p_step_ratio; + if (tune_p >= tune_p_max) { + tune_p = tune_p_max; + Log_Write_Event(EVENT_AUTOTUNE_REACHED_LIMIT); + } + } else { + // we have a good measurement of bounce back + if (meas_rate_max-meas_rate_min > meas_rate_max*aggressiveness) { + // ignore the next result unless it is the same as this one + ignore_next = true; + // bounce back is bigger than our threshold so increment the success counter + counter++; + } else { + if (ignore_next == false) { + // bounce back is smaller than our threshold so decrement the success counter + if (counter > 0) { + counter--; + } + // increase D gain (which should increase bounce back) + tune_d += tune_d*tune_d_step_ratio*2.0f; + // stop tuning if we hit maximum D + if (tune_d >= tune_d_max) { + tune_d = tune_d_max; + counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(EVENT_AUTOTUNE_REACHED_LIMIT); + } + } else { + ignore_next = false; + } + } + } +} + +// updating_rate_d_down - decrease D and adjust P to optimize the D term for no bounce back +// optimize D term while keeping the maximum just below the target by adjusting P +void AC_AutoTune::updating_rate_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max) +{ + if (meas_rate_max > rate_target) { + // if maximum measurement was higher than target + // reduce P gain (which should reduce maximum) + tune_p -= tune_p*tune_p_step_ratio; + if (tune_p < tune_p_min) { + // P gain is at minimum so start reducing D gain + tune_p = tune_p_min; + tune_d -= tune_d*tune_d_step_ratio; + if (tune_d <= tune_d_min) { + // We have reached minimum D so stop tuning + tune_d = tune_d_min; + counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(EVENT_AUTOTUNE_REACHED_LIMIT); + } + } + } else if ((meas_rate_max < rate_target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) { + // we have not achieved a high enough maximum to get a good measurement of bounce back. + // increase P gain (which should increase maximum) + tune_p += tune_p*tune_p_step_ratio; + if (tune_p >= tune_p_max) { + tune_p = tune_p_max; + Log_Write_Event(EVENT_AUTOTUNE_REACHED_LIMIT); + } + } else { + // we have a good measurement of bounce back + if (meas_rate_max-meas_rate_min < meas_rate_max*aggressiveness) { + if (ignore_next == false) { + // bounce back is less than our threshold so increment the success counter + counter++; + } else { + ignore_next = false; + } + } else { + // ignore the next result unless it is the same as this one + ignore_next = true; + // bounce back is larger than our threshold so decrement the success counter + if (counter > 0) { + counter--; + } + // decrease D gain (which should decrease bounce back) + tune_d -= tune_d*tune_d_step_ratio; + // stop tuning if we hit minimum D + if (tune_d <= tune_d_min) { + tune_d = tune_d_min; + counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(EVENT_AUTOTUNE_REACHED_LIMIT); + } + } + } +} + +// updating_rate_p_up_d_down - increase P to ensure the target is reached while checking bounce back isn't increasing +// P is increased until we achieve our target within a reasonable time while reducing D if bounce back increases above the threshold +void AC_AutoTune::updating_rate_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max) +{ + if (meas_rate_max > rate_target*(1+0.5f*aggressiveness)) { + // ignore the next result unless it is the same as this one + ignore_next = true; + // if maximum measurement was greater than target so increment the success counter + counter++; + } else if ((meas_rate_max < rate_target) && (meas_rate_max > rate_target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (meas_rate_max-meas_rate_min > meas_rate_max*aggressiveness) && (tune_d > tune_d_min)) { + // if bounce back was larger than the threshold so decrement the success counter + if (counter > 0) { + counter--; + } + // decrease D gain (which should decrease bounce back) + tune_d -= tune_d*tune_d_step_ratio; + // do not decrease the D term past the minimum + if (tune_d <= tune_d_min) { + tune_d = tune_d_min; + Log_Write_Event(EVENT_AUTOTUNE_REACHED_LIMIT); + } + // decrease P gain to match D gain reduction + tune_p -= tune_p*tune_p_step_ratio; + // do not decrease the P term past the minimum + if (tune_p <= tune_p_min) { + tune_p = tune_p_min; + Log_Write_Event(EVENT_AUTOTUNE_REACHED_LIMIT); + } + // cancel change in direction + positive_direction = !positive_direction; + } else { + if (ignore_next == false) { + // if maximum measurement was lower than target so decrement the success counter + if (counter > 0) { + counter--; + } + // increase P gain (which should increase the maximum) + tune_p += tune_p*tune_p_step_ratio; + // stop tuning if we hit maximum P + if (tune_p >= tune_p_max) { + tune_p = tune_p_max; + counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(EVENT_AUTOTUNE_REACHED_LIMIT); + } + } else { + ignore_next = false; + } + } +} + +// updating_angle_p_down - decrease P until we don't reach the target before time out +// P is decreased to ensure we are not overshooting the target +void AC_AutoTune::updating_angle_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max) +{ + if (meas_angle_max < angle_target*(1+0.5f*aggressiveness)) { + if (ignore_next == false) { + // if maximum measurement was lower than target so increment the success counter + counter++; + } else { + ignore_next = false; + } + } else { + // ignore the next result unless it is the same as this one + ignore_next = true; + // if maximum measurement was higher than target so decrement the success counter + if (counter > 0) { + counter--; + } + // decrease P gain (which should decrease the maximum) + tune_p -= tune_p*tune_p_step_ratio; + // stop tuning if we hit maximum P + if (tune_p <= tune_p_min) { + tune_p = tune_p_min; + counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(EVENT_AUTOTUNE_REACHED_LIMIT); + } + } +} + +// updating_angle_p_up - increase P to ensure the target is reached +// P is increased until we achieve our target within a reasonable time +void AC_AutoTune::updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max) +{ + if ((meas_angle_max > angle_target*(1+0.5f*aggressiveness)) || + ((meas_angle_max > angle_target) && (meas_rate_min < -meas_rate_max*aggressiveness))) { + // ignore the next result unless it is the same as this one + ignore_next = true; + // if maximum measurement was greater than target so increment the success counter + counter++; + } else { + if (ignore_next == false) { + // if maximum measurement was lower than target so decrement the success counter + if (counter > 0) { + counter--; + } + // increase P gain (which should increase the maximum) + tune_p += tune_p*tune_p_step_ratio; + // stop tuning if we hit maximum P + if (tune_p >= tune_p_max) { + tune_p = tune_p_max; + counter = AUTOTUNE_SUCCESS_COUNT; + Log_Write_Event(EVENT_AUTOTUNE_REACHED_LIMIT); + } + } else { + ignore_next = false; + } + } +} + +/* + check if we have a good position estimate + */ +bool AC_AutoTune::position_ok(void) +{ + if (!AP::ahrs().have_inertial_nav()) { + // do not allow navigation with dcm position + return false; + } + + // with EKF use filter status and ekf check + nav_filter_status filt_status = inertial_nav->get_filter_status(); + + // require a good absolute position and EKF must not be in const_pos_mode + return (filt_status.flags.horiz_pos_abs && !filt_status.flags.const_pos_mode); +} + +// get attitude for slow position hold in autotune mode +void AC_AutoTune::get_poshold_attitude(int32_t &roll_cd_out, int32_t &pitch_cd_out, int32_t &yaw_cd_out) +{ + roll_cd_out = pitch_cd_out = 0; + + if (!use_poshold) { + // we are not trying to hold position + return; + } + + // do we know where we are? If not then don't do poshold + if (!position_ok()) { + return; + } + + if (!have_position) { + have_position = true; + start_position = inertial_nav->get_position(); + } + + // don't go past 10 degrees, as autotune result would deteriorate too much + const float angle_max_cd = 1000; + + // hit the 10 degree limit at 20 meters position error + const float dist_limit_cm = 2000; + + // we only start adjusting yaw if we are more than 5m from the + // target position. That corresponds to a lean angle of 2.5 degrees + const float yaw_dist_limit_cm = 500; + + Vector3f pdiff = inertial_nav->get_position() - start_position; + pdiff.z = 0; + float dist_cm = pdiff.length(); + if (dist_cm < 10) { + // don't do anything within 10cm + return; + } + + /* + very simple linear controller + */ + float scaling = constrain_float(angle_max_cd * dist_cm / dist_limit_cm, 0, angle_max_cd); + Vector2f angle_ne(pdiff.x, pdiff.y); + angle_ne *= scaling / dist_cm; + + // rotate into body frame + pitch_cd_out = angle_ne.x * ahrs_view->cos_yaw() + angle_ne.y * ahrs_view->sin_yaw(); + roll_cd_out = angle_ne.x * ahrs_view->sin_yaw() - angle_ne.y * ahrs_view->cos_yaw(); + + if (dist_cm < yaw_dist_limit_cm) { + // no yaw adjustment + return; + } + + /* + also point so that twitching occurs perpendicular to the wind, + if we have drifted more than yaw_dist_limit_cm from the desired + position. This ensures that autotune doesn't have to deal with + more than 2.5 degrees of attitude on the axis it is tuning + */ + float target_yaw_cd = degrees(atan2f(pdiff.y, pdiff.x)) * 100; + if (axis == PITCH) { + // for roll and yaw tuning we point along the wind, for pitch + // we point across the wind + target_yaw_cd += 9000; + } + // go to the nearest 180 degree mark, with 5 degree slop to prevent oscillation + if (fabsf(yaw_cd_out - target_yaw_cd) > 9500) { + target_yaw_cd += 18000; + } + + yaw_cd_out = target_yaw_cd; +} + +// Write an Autotune data packet +void AC_AutoTune::Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt) +{ + DataFlash_Class::instance()->Log_Write( + "ATUN", + "TimeUS,Axis,TuneStep,Targ,Min,Max,RP,RD,SP,ddt", + "s--ddd---o", + "F--BBB---0", + "QBBfffffff", + AP_HAL::micros64(), + axis, + tune_step, + meas_target, + meas_min, + meas_max, + new_gain_rp, + new_gain_rd, + new_gain_sp, + new_ddt); +} + +// Write an Autotune data packet +void AC_AutoTune::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds) +{ + DataFlash_Class::instance()->Log_Write( + "ATDE", + "TimeUS,Angle,Rate", + "sdk", + "FBB", + "Qff", + AP_HAL::micros64(), + angle_cd, + rate_cds); +} diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h new file mode 100644 index 0000000000..1bf529e23b --- /dev/null +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -0,0 +1,216 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + support for autotune of multirotors. Based on original autotune code from ArduCopter, written by Leonard Hall + Converted to a library by Andrew Tridgell + */ + +#pragma once + +#include +#include +#include + +class AC_AutoTune { +public: + // constructor + AC_AutoTune(); + + // main run loop + virtual void run(); + + // save gained, called on disarm + void save_tuning_gains(); + + // stop tune, reverting gains + void stop(); + + // var_info for holding Parameter information + static const struct AP_Param::GroupInfo var_info[]; + +protected: + // methods that must be supplied by the vehicle specific subclass + virtual bool init(void) = 0; + + // get pilot input for desired cimb rate + virtual float get_pilot_desired_climb_rate_cms(void) const = 0; + + // get pilot input for designed roll and pitch, and yaw rate + virtual void get_pilot_desired_rp_yrate_cd(int32_t &roll_cd, int32_t &pitch_cd, int32_t &yaw_rate_cds) = 0; + + // init pos controller Z velocity and accel limits + virtual void init_z_limits() = 0; + + // start tune - virtual so that vehicle code can add additional pre-conditions + virtual bool start(void); + + // return true if we have a good position estimate + virtual bool position_ok(); + + enum at_event { + EVENT_AUTOTUNE_INITIALISED = 0, + EVENT_AUTOTUNE_OFF = 1, + EVENT_AUTOTUNE_RESTART = 2, + EVENT_AUTOTUNE_SUCCESS = 3, + EVENT_AUTOTUNE_FAILED = 4, + EVENT_AUTOTUNE_REACHED_LIMIT = 5, + EVENT_AUTOTUNE_PILOT_TESTING = 6, + EVENT_AUTOTUNE_SAVEDGAINS = 7 + }; + + // write a log event + virtual void Log_Write_Event(enum at_event id) = 0; + + // internal init function, should be called from init() + bool init_internals(bool use_poshold, + AC_AttitudeControl_Multi *attitude_control, + AC_PosControl *pos_control, + AP_AHRS_View *ahrs_view, + AP_InertialNav *inertial_nav); + +private: + void control_attitude(); + void backup_gains_and_initialise(); + void load_orig_gains(); + void load_tuned_gains(); + void load_intra_test_gains(); + void load_twitch_gains(); + void update_gcs(uint8_t message_id); + bool roll_enabled(); + bool pitch_enabled(); + bool yaw_enabled(); + void twitching_test_rate(float rate, float rate_target, float &meas_rate_min, float &meas_rate_max); + void twitching_test_angle(float angle, float rate, float angle_target, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max); + void twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max); + void updating_rate_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max); + void updating_rate_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max); + void updating_rate_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max); + void updating_angle_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max); + void updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max); + void get_poshold_attitude(int32_t &roll_cd, int32_t &pitch_cd, int32_t &yaw_cd); + + void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt); + void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds); + + void send_step_string(); + const char *level_issue_string() const; + const char * type_string() const; + void announce_state_to_gcs(); + void do_gcs_announcements(); + + enum LEVEL_ISSUE { + LEVEL_ISSUE_NONE, + LEVEL_ISSUE_ANGLE_ROLL, + LEVEL_ISSUE_ANGLE_PITCH, + LEVEL_ISSUE_ANGLE_YAW, + LEVEL_ISSUE_RATE_ROLL, + LEVEL_ISSUE_RATE_PITCH, + LEVEL_ISSUE_RATE_YAW, + }; + bool check_level(const enum LEVEL_ISSUE issue, const float current, const float maximum); + bool currently_level(); + + // autotune modes (high level states) + enum TuneMode { + UNINITIALISED = 0, // autotune has never been run + TUNING = 1, // autotune is testing gains + SUCCESS = 2, // tuning has completed, user is flight testing the new gains + FAILED = 3, // tuning has failed, user is flying on original gains + }; + + // steps performed while in the tuning mode + enum StepType { + WAITING_FOR_LEVEL = 0, // autotune is waiting for vehicle to return to level before beginning the next twitch + TWITCHING = 1, // autotune has begun a twitch and is watching the resulting vehicle movement + UPDATE_GAINS = 2 // autotune has completed a twitch and is updating the gains based on the results + }; + + // things that can be tuned + enum AxisType { + ROLL = 0, // roll axis is being tuned (either angle or rate) + PITCH = 1, // pitch axis is being tuned (either angle or rate) + YAW = 2, // pitch axis is being tuned (either angle or rate) + }; + + // mini steps performed while in Tuning mode, Testing step + enum TuneType { + RD_UP = 0, // rate D is being tuned up + RD_DOWN = 1, // rate D is being tuned down + RP_UP = 2, // rate P is being tuned up + SP_DOWN = 3, // angle P is being tuned down + SP_UP = 4 // angle P is being tuned up + }; + + TuneMode mode : 2; // see TuneMode for what modes are allowed + bool pilot_override : 1; // true = pilot is overriding controls so we suspend tuning temporarily + AxisType axis : 2; // see AxisType for which things can be tuned + bool positive_direction : 1; // false = tuning in negative direction (i.e. left for roll), true = positive direction (i.e. right for roll) + StepType step : 2; // see StepType for what steps are performed + TuneType tune_type : 3; // see TuneType + bool ignore_next : 1; // true = ignore the next test + bool twitch_first_iter : 1; // true on first iteration of a twitch (used to signal we must step the attitude or rate target) + bool use_poshold : 1; // true = enable position hold + bool have_position : 1; // true = start_position is value + Vector3f start_position; + + // variables + uint32_t override_time; // the last time the pilot overrode the controls + float test_rate_min; // the minimum angular rate achieved during TESTING_RATE step + float test_rate_max; // the maximum angular rate achieved during TESTING_RATE step + float test_angle_min; // the minimum angle achieved during TESTING_ANGLE step + float test_angle_max; // the maximum angle achieved during TESTING_ANGLE step + uint32_t step_start_time; // start time of current tuning step (used for timeout checks) + uint32_t step_stop_time; // start time of current tuning step (used for timeout checks) + int8_t counter; // counter for tuning gains + float target_rate, start_rate; // target and start rate + float target_angle, start_angle; // target and start angles + int32_t desired_yaw_cd; // yaw heading during tune + float rate_max, test_accel_max; // maximum acceleration variables + + LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second + + // backup of currently being tuned parameter values + float orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_sp, orig_roll_accel; + float orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_sp, orig_pitch_accel; + float orig_yaw_rp, orig_yaw_ri, orig_yaw_rd, orig_yaw_rLPF, orig_yaw_sp, orig_yaw_accel; + bool orig_bf_feedforward; + + // currently being tuned parameter values + float tune_roll_rp, tune_roll_rd, tune_roll_sp, tune_roll_accel; + float tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, tune_pitch_accel; + float tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, tune_yaw_accel; + + uint32_t announce_time; + float lean_angle; + float rotation_rate; + int32_t roll_cd, pitch_cd; + + struct { + LEVEL_ISSUE issue{LEVEL_ISSUE_NONE}; + float maximum; + float current; + } level_problem; + + AP_Int8 axis_bitmask; + AP_Float aggressiveness; + AP_Float min_d; + + // copies of object pointers to make code a bit clearer + AC_AttitudeControl_Multi *attitude_control; + AC_PosControl *pos_control; + AP_AHRS_View *ahrs_view; + AP_InertialNav *inertial_nav; + AP_Motors *motors; +};