From 2277f7045608d582efb01f49b44795971db0ae7b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 14 Dec 2018 17:00:51 +1100 Subject: [PATCH] AC_AutoTune: allow save of completed axes this allows you to stop the tune by disarming part way through and save the completed axes --- libraries/AC_AutoTune/AC_AutoTune.cpp | 180 ++++++++++++++------------ libraries/AC_AutoTune/AC_AutoTune.h | 1 + 2 files changed, 98 insertions(+), 83 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 8efff8be9c..06d1e4b9df 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -189,6 +189,8 @@ bool AC_AutoTune::init_internals(bool _use_poshold, // stop - should be called when the ch7/ch8 switch is switched OFF void AC_AutoTune::stop() { + axes_completed = 0; + // set gains to their original values load_orig_gains(); @@ -219,6 +221,8 @@ bool AC_AutoTune::start(void) pos_control->set_desired_velocity_z(inertial_nav->get_velocity_z()); } + axes_completed = 0; + return true; } @@ -803,6 +807,7 @@ void AC_AutoTune::control_attitude() bool complete = false; switch (axis) { case ROLL: + axes_completed |= AUTOTUNE_AXIS_BITMASK_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()) { @@ -814,6 +819,7 @@ void AC_AutoTune::control_attitude() } break; case PITCH: + axes_completed |= AUTOTUNE_AXIS_BITMASK_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()) { @@ -823,6 +829,7 @@ void AC_AutoTune::control_attitude() } break; case YAW: + axes_completed |= AUTOTUNE_AXIS_BITMASK_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; @@ -1050,89 +1057,93 @@ void AC_AutoTune::load_twitch_gains() // 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; + // see if we successfully completed tuning of at least one axis + if (axes_completed == 0) { + return; } + + 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 ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && 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 ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && 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 ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && 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; + axes_completed = 0; } // update_gcs - send message to ground station @@ -1152,7 +1163,10 @@ void AC_AutoTune::update_gcs(uint8_t message_id) gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Failed"); break; case AUTOTUNE_MESSAGE_SAVED_GAINS: - gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Saved gains"); + gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Saved gains for %s%s%s", + (axes_completed&AUTOTUNE_AXIS_BITMASK_ROLL)?"Roll ":"", + (axes_completed&AUTOTUNE_AXIS_BITMASK_PITCH)?"Pitch ":"", + (axes_completed&AUTOTUNE_AXIS_BITMASK_YAW)?"Yaw":""); break; } } diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index fc370aa14a..00ffd58486 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -164,6 +164,7 @@ private: bool use_poshold : 1; // true = enable position hold bool have_position : 1; // true = start_position is value Vector3f start_position; + uint8_t axes_completed; // bitmask of completed axes // variables uint32_t override_time; // the last time the pilot overrode the controls