From f795e4acb35b481cb10fc96c4cd366115fbdb19d Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 29 May 2025 13:29:17 +0930 Subject: [PATCH] AC_AutoTune: Rename for clarity and units --- libraries/AC_AutoTune/AC_AutoTune.cpp | 272 ++++++++---------- libraries/AC_AutoTune/AC_AutoTune.h | 111 +++---- .../AC_AutoTune/AC_AutoTune_FreqResp.cpp | 26 +- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 144 +++++----- libraries/AC_AutoTune/AC_AutoTune_Heli.h | 2 +- libraries/AC_AutoTune/AC_AutoTune_Multi.cpp | 150 +++++----- libraries/AC_AutoTune/AC_AutoTune_Multi.h | 188 ++++++------ 7 files changed, 438 insertions(+), 455 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 892a8ef9bf5..fe7e6cce112 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -38,7 +38,7 @@ bool AC_AutoTune::init_internals(bool _use_poshold, pos_control = _pos_control; ahrs_view = _ahrs_view; motors = AP_Motors::get_singleton(); - const uint32_t now = AP_HAL::millis(); + const uint32_t now_ms = AP_HAL::millis(); // exit immediately if motor are not armed if ((motors == nullptr) || !motors->armed()) { @@ -50,37 +50,33 @@ bool AC_AutoTune::init_internals(bool _use_poshold, switch (mode) { case TuneMode::FAILED: - // fall through to restart the tuning + // Fall through to restart the tuning process from scratch FALLTHROUGH; case TuneMode::UNINITIALISED: - // autotune has never been run - // so store current gains as original gains + // First-time run: store the current gains as the baseline (original gains) backup_gains_and_initialise(); - // advance mode to tuning + // Set the mode to TUNING to begin the autotune process mode = TuneMode::TUNING; - // send message to ground station that we've started tuning + // Notify GCS that autotune has started update_gcs(AUTOTUNE_MESSAGE_STARTED); break; case TuneMode::TUNING: - // reset test variables for each vehicle + // Resuming from previous tuning session, restart from current axis and tune step reset_vehicle_test_variables(); - - // we are restarting tuning so restart where we left off step = StepType::WAITING_FOR_LEVEL; - step_start_time_ms = now; - level_start_time_ms = now; - // reset gains to tuning-start gains (i.e. low I term) - load_gains(GainType::GAIN_INTRA_TEST); + step_start_time_ms = now_ms; + level_start_time_ms = now_ms; + // Reload gains with low I-term and restart logging LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_RESTART); update_gcs(AUTOTUNE_MESSAGE_STARTED); break; case TuneMode::FINISHED: - case TuneMode::TESTING: - // we have completed a tune and the pilot wishes to test the new gains - mode = TuneMode::TESTING; + case TuneMode::VALIDATING: + // The user is now validating the tuned gains in flight + mode = TuneMode::VALIDATING; update_gcs(AUTOTUNE_MESSAGE_TESTING); LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_PILOT_TESTING); break; @@ -95,7 +91,7 @@ bool AC_AutoTune::init_internals(bool _use_poshold, void AC_AutoTune::stop() { // set gains to their original values - load_gains(GainType::GAIN_ORIGINAL); + load_gains(GainType::ORIGINAL); update_gcs(AUTOTUNE_MESSAGE_STOPPED); @@ -118,30 +114,30 @@ void AC_AutoTune::do_aux_function(const RC_Channel::AuxSwitchPos ch_flag) switch(ch_flag) { case RC_Channel::AuxSwitchPos::LOW: // load original gains - load_gains(GainType::GAIN_ORIGINAL); + load_gains(GainType::ORIGINAL); update_gcs(AUTOTUNE_MESSAGE_TESTING_END); break; case RC_Channel::AuxSwitchPos::MIDDLE: - // Middle position is unused for now + // Middle position is unused for now_ms break; case RC_Channel::AuxSwitchPos::HIGH: // Load tuned gains - load_gains(GainType::GAIN_TUNED); + load_gains(GainType::TUNED); update_gcs(AUTOTUNE_MESSAGE_TESTING); break; } - have_pilot_testing_command = true; + testing_switch_used = true; } // Possibly save gains, called on disarm void AC_AutoTune::disarmed(const bool in_autotune_mode) { // True if pilot is testing tuned gains - const bool testing_tuned = have_pilot_testing_command && (loaded_gains == GainType::GAIN_TUNED); + const bool testing_tuned = testing_switch_used && (loaded_gains == GainType::TUNED); // True if in autotune mode and no pilot testing commands have been received - const bool tune_complete_no_testing = !have_pilot_testing_command && in_autotune_mode; + const bool tune_complete_no_testing = !testing_switch_used && in_autotune_mode; if (tune_complete_no_testing || testing_tuned) { save_tuning_gains(); @@ -178,27 +174,27 @@ void AC_AutoTune::send_step_string() case StepType::ABORT: GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Aborting Test"); return; - case StepType::TESTING: + case StepType::EXECUTING_TEST: GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Testing"); return; } GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: unknown step"); } -const char *AC_AutoTune::type_string() const +const char *AC_AutoTune::get_tune_type_name() const { switch (tune_type) { - case TuneType::RD_UP: + case TuneType::RATE_D_UP: return "Rate D Up"; - case TuneType::RD_DOWN: + case TuneType::RATE_D_DOWN: return "Rate D Down"; - case TuneType::RP_UP: + case TuneType::RATE_P_UP: return "Rate P Up"; - case TuneType::RFF_UP: + case TuneType::RATE_FF_UP: return "Rate FF Up"; - case TuneType::SP_UP: + case TuneType::ANGLE_P_UP: return "Angle P Up"; - case TuneType::SP_DOWN: + case TuneType::ANGLE_P_DOWN: return "Angle P Down"; case TuneType::MAX_GAINS: return "Find Max Gains"; @@ -213,7 +209,7 @@ const char *AC_AutoTune::type_string() const } // return current axis string -const char *AC_AutoTune::axis_string() const +const char *AC_AutoTune::get_axis_name() const { switch (axis) { case AxisType::ROLL: @@ -228,15 +224,15 @@ const char *AC_AutoTune::axis_string() const return ""; } -// run - runs the autotune flight mode -// should be called at 100hz or more +// Main update loop for Autotune mode. Handles all states: tuning, validating, or idle. +// Should be called at ≥100Hz for consistent performance. void AC_AutoTune::run() { - // initialize vertical speeds and acceleration + // Initialise vertical climb rate and acceleration limits 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 + // Exit early if the vehicle is disarmed or motor interlock is not enabled + // (this condition should not occur if init() is working correctly) if (!motors->armed() || !motors->get_interlock()) { motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control->set_throttle_out(0.0f, true, 0.0f); @@ -247,81 +243,82 @@ void AC_AutoTune::run() float 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 + // Get pilot's desired climb rate const float target_climb_rate_cms = get_pilot_desired_climb_rate_cms(); const bool zero_rp_input = is_zero(target_roll_cd) && is_zero(target_pitch_cd); if (zero_rp_input) { - // pilot input on throttle and yaw will still use position hold if enabled + // Use position hold if enabled get_poshold_attitude(target_roll_cd, target_pitch_cd, desired_yaw_cd); } - const uint32_t now = AP_HAL::millis(); + const uint32_t now_ms = AP_HAL::millis(); switch (mode) { case TuneMode::TUNING: + // Detect pilot override if (!zero_rp_input || !is_zero(target_yaw_rate_cds) || !is_zero(target_climb_rate_cms)) { if (!pilot_override) { pilot_override = true; - // set gains to their original values + // Restore original gains while pilot is in control } - // reset pilot override time - override_time = now; + // Update last override time + override_time = now_ms; if (!zero_rp_input) { - // only reset position on roll or pitch input + // Invalidate position hold if pilot inputs roll/pitch have_position = false; } } else if (pilot_override) { - // check if we should resume tuning after pilot's override - if (now - override_time > AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS) { - pilot_override = false; // turn off pilot override - step = StepType::WAITING_FOR_LEVEL; // set tuning step back from beginning - step_start_time_ms = now; - level_start_time_ms = now; - // todo: use our current target. - desired_yaw_cd = ahrs_view->yaw_sensor; + // Check if pilot has released sticks long enough to resume tuning + if (now_ms - override_time > AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS) { + pilot_override = false; + step = StepType::WAITING_FOR_LEVEL; + step_start_time_ms = now_ms; + level_start_time_ms = now_ms; + // TODO: Consider using our current target. + desired_yaw_cd = ahrs_view->yaw_sensor; // Reset yaw reference } } if (pilot_override) { - if (now - last_pilot_override_warning > 1000) { + // Pilot is actively controlling the vehicle; fly on original gains + if (now_ms - last_pilot_override_warning > 1000) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: pilot overrides active"); - last_pilot_override_warning = now; + last_pilot_override_warning = now_ms; } - load_gains(GainType::GAIN_ORIGINAL); + load_gains(GainType::ORIGINAL); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll_cd, target_pitch_cd, target_yaw_rate_cds); - } else { - // Autotune will take control of the aircraft while conducting the test + // Autotune controls the aircraft control_attitude(); - // tell the user what's going on do_gcs_announcements(); } break; case TuneMode::UNINITIALISED: - // we should never get here + // Should never reach this state; init() must be called before run() INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); FALLTHROUGH; - + case TuneMode::FAILED: FALLTHROUGH; case TuneMode::FINISHED: - load_gains(GainType::GAIN_ORIGINAL); + // Tuning is complete or failed; fly using original gains + load_gains(GainType::ORIGINAL); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll_cd, target_pitch_cd, target_yaw_rate_cds); break; - - case TuneMode::TESTING: - load_gains(GainType::GAIN_TUNED); + + case TuneMode::VALIDATING: + // Pilot is evaluating tuned gains + load_gains(GainType::TUNED); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll_cd, target_pitch_cd, target_yaw_rate_cds); break; } - // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - // call position controller + // Update vertical position controller with pilot climb rate input pos_control->set_pos_target_U_from_climb_rate_cm(target_climb_rate_cms); pos_control->update_U_controller(); } @@ -363,40 +360,36 @@ bool AC_AutoTune::currently_level() return true; } -// main state machine to level vehicle, perform a test and update gains -// directly updates attitude controller with targets +// Main tuning state machine. Handles all StepTypes: WAITING_FOR_LEVEL, EXECUTING_TEST, UPDATE_GAINS, ABORT. +// Updates attitude controller targets and processes test results to adjust PID gains. void AC_AutoTune::control_attitude() { - rotation_rate = 0.0f; // rotation rate in radians/second + rotation_rate = 0.0f; lean_angle = 0.0f; const float direction_sign = positive_direction ? 1.0f : -1.0f; - const uint32_t now = AP_HAL::millis(); + const uint32_t now_ms = AP_HAL::millis(); - // check tuning step switch (step) { case StepType::WAITING_FOR_LEVEL: { - - // when waiting for level we use the intra-test gains - load_gains(GainType::GAIN_INTRA_TEST); + // Use intra-test gains while holding level between twitches + load_gains(GainType::INTRA_TEST); get_poshold_attitude(roll_cd, pitch_cd, desired_yaw_cd); attitude_control->input_euler_angle_roll_pitch_yaw_cd(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 + // Require a short stable period before executing the next twitch if (!currently_level()) { - step_start_time_ms = now; + step_start_time_ms = now_ms; } - // if we have been level for a sufficient amount of time (0.5 seconds) move onto tuning step - if (now - step_start_time_ms > AUTOTUNE_REQUIRED_LEVEL_TIME_MS) { - // initiate variables for next step - step = StepType::TESTING; - step_start_time_ms = now; - step_time_limit_ms = get_testing_step_timeout_ms(); + if (now_ms - step_start_time_ms > AUTOTUNE_REQUIRED_LEVEL_TIME_MS) { + // Begin the test phase + step = StepType::EXECUTING_TEST; + step_start_time_ms = now_ms; + step_timeout_ms = get_testing_step_timeout_ms(); - // Initialize test-specific variables + // Record starting angular position and rate switch (axis) { case AxisType::ROLL: start_rate = degrees(ahrs_view->get_gyro().x) * 100.0f; @@ -413,38 +406,31 @@ void AC_AutoTune::control_attitude() break; } - // make sure to-be-tested gains are loaded before commanding the step - load_gains(GainType::GAIN_TEST); - // tests must be initialized last as some rely on variables above + // Apply test gains and initialise test-specific variables + load_gains(GainType::TEST); test_init(); } break; } - case StepType::TESTING: { - // Run the twitching step - load_gains(GainType::GAIN_TEST); - - // run the test + case StepType::EXECUTING_TEST: { + // Run the test with current trial gains + load_gains(GainType::TEST); test_run(axis, direction_sign); - // Check for failure causing reverse response - if (lean_angle <= -angle_lim_neg_rpy_cd()) { - step = StepType::ABORT; - } - - // protect from roll over - if (attitude_control->lean_angle_deg() * 100 > angle_lim_max_rp_cd()) { + // Detect failure due to reverse response or excessive lean angle + if (lean_angle <= -angle_lim_neg_rpy_cd() || + attitude_control->lean_angle_deg() * 100 > angle_lim_max_rp_cd()) { step = StepType::ABORT; } #if HAL_LOGGING_ENABLED - // log this iterations lean angle and rotation rate Log_AutoTuneDetails(); attitude_control->Write_Rate(*pos_control); log_pids(); #endif + // Update yaw target for next test if required if (axis == AxisType::YAW || axis == AxisType::YAW_D) { desired_yaw_cd = ahrs_view->yaw_sensor; } @@ -454,74 +440,54 @@ void AC_AutoTune::control_attitude() case StepType::UPDATE_GAINS: #if HAL_LOGGING_ENABLED - // log the latest gains - Log_AutoTune(); + Log_AutoTune(); // Log gain adjustment results #endif - // Announce tune type test results - // must be done before updating method because this method changes parameters for next test + // Announce test results before gains are changed do_post_test_gcs_announcements(); + // Update gains based on the current tuning strategy switch (tune_type) { - // Check results after mini-step to increase rate D gain - case TuneType::RD_UP: + case TuneType::RATE_D_UP: updating_rate_d_up_all(axis); break; - // Check results after mini-step to decrease rate D gain - case TuneType::RD_DOWN: + case TuneType::RATE_D_DOWN: updating_rate_d_down_all(axis); break; - // Check results after mini-step to increase rate P gain - case TuneType::RP_UP: + case TuneType::RATE_P_UP: updating_rate_p_up_all(axis); break; - // Check results after mini-step to increase stabilize P gain - case TuneType::SP_DOWN: + case TuneType::ANGLE_P_DOWN: updating_angle_p_down_all(axis); break; - // Check results after mini-step to increase stabilize P gain - case TuneType::SP_UP: + case TuneType::ANGLE_P_UP: updating_angle_p_up_all(axis); break; - case TuneType::RFF_UP: + case TuneType::RATE_FF_UP: updating_rate_ff_up_all(axis); break; case TuneType::MAX_GAINS: updating_max_gains_all(axis); break; case TuneType::TUNE_CHECK: - counter = AUTOTUNE_SUCCESS_COUNT; + success_counter = AUTOTUNE_SUCCESS_COUNT; FALLTHROUGH; case TuneType::TUNE_COMPLETE: break; } - // we've complete this step, finalize pids and move to next step - if (counter >= AUTOTUNE_SUCCESS_COUNT) { - // The current step has completed - // Reset the state and move onto the next step - - // reset counter - counter = 0; - - // reset scaling factor + // If tuning step was successful, proceed to the next step + if (success_counter >= AUTOTUNE_SUCCESS_COUNT) { + success_counter = 0; step_scaler = 1.0f; - - // reduce the gains after tuning step before moving to the next tuning type - // this is to provide additional gain margin if required. set_tuning_gains_with_backoff(axis); - - // increment the tune type to the next one in tune sequence next_tune_type(tune_type, false); - // check if all steps have been completed on this axis if (tune_type == TuneType::TUNE_COMPLETE) { - // Determine the next axis and reset + // Complete tuning for this axis and determine the next one next_tune_type(tune_type, true); - report_final_gains(axis); - // advance to the next axis bool complete = false; switch (axis) { case AxisType::ROLL: @@ -560,16 +526,12 @@ void AC_AutoTune::control_attitude() 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 = TuneMode::FINISHED; update_gcs(AUTOTUNE_MESSAGE_SUCCESS); LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_SUCCESS); AP_Notify::events.autotune_complete = true; - - // Return to original gains for landing - load_gains(GainType::GAIN_ORIGINAL); + load_gains(GainType::ORIGINAL); // Reset for landing } else { AP_Notify::events.autotune_next_axis = true; reset_update_gain_variables(); @@ -579,20 +541,18 @@ void AC_AutoTune::control_attitude() FALLTHROUGH; case StepType::ABORT: - // we have aborted or finished a test and need to reset for the next test + // Recover from failed test or move on after a successful one get_poshold_attitude(roll_cd, pitch_cd, desired_yaw_cd); attitude_control->input_euler_angle_roll_pitch_yaw_cd(roll_cd, pitch_cd, desired_yaw_cd, true); - // set gains to their intra-test values (which are very close to the original gains) - load_gains(GainType::GAIN_INTRA_TEST); + load_gains(GainType::INTRA_TEST); - // reset testing step step = StepType::WAITING_FOR_LEVEL; positive_direction = twitch_reverse_direction(); - step_start_time_ms = now; - level_start_time_ms = now; - step_time_limit_ms = AUTOTUNE_REQUIRED_LEVEL_TIME_MS; + step_start_time_ms = now_ms; + level_start_time_ms = now_ms; + step_timeout_ms = AUTOTUNE_REQUIRED_LEVEL_TIME_MS; break; } } @@ -601,7 +561,7 @@ void AC_AutoTune::control_attitude() // called before tuning starts to backup original gains void AC_AutoTune::backup_gains_and_initialise() { - const uint32_t now = AP_HAL::millis(); + const uint32_t now_ms = AP_HAL::millis(); // initialise state because this is our first time if (roll_enabled()) { @@ -624,8 +584,8 @@ void AC_AutoTune::backup_gains_and_initialise() step = StepType::WAITING_FOR_LEVEL; positive_direction = false; - step_start_time_ms = now; - level_start_time_ms = now; + step_start_time_ms = now_ms; + level_start_time_ms = now_ms; step_scaler = 1.0f; desired_yaw_cd = ahrs_view->yaw_sensor; @@ -643,16 +603,16 @@ void AC_AutoTune::load_gains(enum GainType gain_type) loaded_gains = gain_type; switch (gain_type) { - case GainType::GAIN_ORIGINAL: + case GainType::ORIGINAL: load_orig_gains(); break; - case GainType::GAIN_INTRA_TEST: + case GainType::INTRA_TEST: load_intra_test_gains(); break; - case GainType::GAIN_TEST: + case GainType::TEST: load_test_gains(); break; - case GainType::GAIN_TUNED: + case GainType::TUNED: load_tuned_gains(); break; } @@ -811,15 +771,15 @@ void AC_AutoTune::next_tune_type(TuneType &curr_tune_type, bool reset) { if (reset) { set_tune_sequence(); - tune_seq_curr = 0; + tune_seq_index = 0; } else if (curr_tune_type == TuneType::TUNE_COMPLETE) { // leave tune_type as TUNE_COMPLETE to initiate next axis or exit autotune return; } else { - tune_seq_curr++; + tune_seq_index++; } - curr_tune_type = tune_seq[tune_seq_curr]; + curr_tune_type = tune_seq[tune_seq_index]; } #endif // AC_AUTOTUNE_ENABLED diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index a1008b99041..57d561904fc 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -52,7 +52,8 @@ public: // constructor AC_AutoTune(); - // main run loop + // Main update loop for Autotune mode. Handles all states: tuning, testing, or idle. + // Should be called at >=100Hz for reliable performance. virtual void run(); // Possibly save gains, called on disarm @@ -73,7 +74,7 @@ protected: void reset() { mode = TuneMode::UNINITIALISED; axes_completed = 0; - have_pilot_testing_command = false; + testing_switch_used = false; } // axis that can be tuned @@ -122,7 +123,7 @@ protected: // load gains for next test. relies on axis variable being set virtual void load_test_gains() = 0; - // reset the test vaariables for each vehicle + // reset the test variables for each vehicle virtual void reset_vehicle_test_variables() = 0; // reset the update gain variables for each vehicle @@ -185,10 +186,10 @@ protected: void send_step_string(); // convert tune type to string for reporting - const char *type_string() const; + const char *get_tune_type_name() const; // return current axis string - const char *axis_string() const; + const char *get_axis_name() const; // report final gains for a given axis to GCS virtual void report_final_gains(AxisType test_axis) const = 0; @@ -204,28 +205,28 @@ protected: // steps performed while in the tuning mode enum class StepType { - WAITING_FOR_LEVEL = 0, // autotune is waiting for vehicle to return to level before beginning the next twitch - TESTING = 1, // autotune has begun a test and is watching the resulting vehicle movement - UPDATE_GAINS = 2, // autotune has completed a test and is updating the gains based on the results - ABORT = 3 // load normal gains and return to WAITING_FOR_LEVEL + WAITING_FOR_LEVEL = 0, // Waiting for the vehicle to stabilize at level before starting a twitch test. + EXECUTING_TEST = 1, // Performing a twitch test and monitoring the vehicle's response. + UPDATE_GAINS = 2, // Updating gains based on test results. + ABORT = 3 // Aborting the current test; revert to safe gains and return to WAITING_FOR_LEVEL. }; - StepType step; // see StepType for what steps are performed + StepType step; // see StepType for what steps are performed // mini steps performed while in Tuning mode, Testing step enum class 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 - RFF_UP = 3, // rate FF is being tuned up - SP_DOWN = 4, // angle P is being tuned down - SP_UP = 5, // angle P is being tuned up - MAX_GAINS = 6, // max allowable stable gains are determined - TUNE_CHECK = 7, // frequency sweep with tuned gains - TUNE_COMPLETE = 8 // Reached end of tuning + RATE_D_UP = 0, // rate D is being tuned up + RATE_D_DOWN = 1, // rate D is being tuned down + RATE_P_UP = 2, // rate P is being tuned up + RATE_FF_UP = 3, // rate FF is being tuned up + ANGLE_P_DOWN = 4, // angle P is being tuned down + ANGLE_P_UP = 5, // angle P is being tuned up + MAX_GAINS = 6, // max allowable stable gains are determined + TUNE_CHECK = 7, // frequency sweep with tuned gains + TUNE_COMPLETE = 8 // Reached end of tuning }; - TuneType tune_type; // see TuneType - TuneType tune_seq[6]; // holds sequence of tune_types to be performed - uint8_t tune_seq_curr; // current tune sequence step + TuneType tune_type; // see TuneType + TuneType tune_seq[6]; // holds sequence of tune_types to be performed + uint8_t tune_seq_index; // current tune sequence step // get the next tune type void next_tune_type(TuneType &curr_tune_type, bool reset); @@ -244,20 +245,20 @@ protected: // type of gains to load enum class GainType { - GAIN_ORIGINAL = 0, - GAIN_TEST = 1, - GAIN_INTRA_TEST = 2, - GAIN_TUNED = 3, + ORIGINAL = 0, // Gains as configured before autotune started + TEST = 1, // Gains applied during an active test + INTRA_TEST = 2, // Gains applied between tests to maintain safe control while returning to level, with slower I-term buildup + TUNED = 3, // Gains discovered by the autotune process, used for flight testing or final use } loaded_gains; void load_gains(enum GainType gain_type); - // autotune modes (high level states) + // TuneMode defines the high-level state of the autotune process. enum class TuneMode { - UNINITIALISED = 0, // autotune has never been run - TUNING = 1, // autotune is testing gains - FINISHED = 2, // tuning has completed, user is flight testing the new gains - FAILED = 3, // tuning has failed, user is flying on original gains - TESTING = 4, // tuning has completed, user is flight testing the new gains + UNINITIALISED = 0, // Autotune has not yet been started. + TUNING = 1, // Autotune is actively running and tuning PID gains. + FINISHED = 2, // Tuning is complete, original (pre-tune) gains are restored. + FAILED = 3, // Tuning failed, vehicle is flying with original gains. + VALIDATING = 4, // Tuning complete, user is flight testing the newly tuned gains. }; TuneMode mode; // see TuneMode for what modes are allowed @@ -267,21 +268,21 @@ protected: AP_AHRS_View *ahrs_view; AP_Motors *motors; - AxisType axis; // current axis being tuned. see AxisType enum - bool positive_direction; // false = tuning in negative direction (i.e. left for roll), true = positive direction (i.e. right for roll) - bool twitch_first_iter; // true on first iteration of a twitch (used to signal we must step the attitude or rate target) - uint8_t axes_completed; // bitmask of completed axes - uint32_t step_start_time_ms; // start time of current tuning step (used for timeout checks) - uint32_t step_time_limit_ms; // time limit of current autotune process - uint32_t level_start_time_ms; // start time of waiting for level - int8_t counter; // counter for tuning gains - float start_angle; // start angle - float start_rate; // start rate - parent and multi - float test_accel_max; // maximum acceleration variable - float desired_yaw_cd; // yaw heading during tune - parent and Tradheli - float step_scaler; // scaler to reduce maximum target step - parent and multi + AxisType axis; // current axis being tuned. see AxisType enum + bool positive_direction; // false = tuning in negative direction (i.e. left for roll), true = positive direction (i.e. right for roll) + bool angle_step_commanded; // true on first iteration of a twitch (used to signal we must step the attitude or rate target) + uint8_t axes_completed; // bitmask of completed axes + uint32_t step_start_time_ms; // start time of current tuning step (used for timeout checks) + uint32_t step_timeout_ms; // time limit of current autotune process + uint32_t level_start_time_ms; // start time of waiting for level + int8_t success_counter; // counter for tuning gains + float start_angle; // start angle + float start_rate; // start rate - parent and multi + float test_accel_max_cdss; // maximum acceleration variable + float desired_yaw_cd; // yaw heading during tune - parent and Tradheli + float step_scaler; // scaler to reduce maximum target step - parent and multi - LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second + 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_rff, orig_roll_dff, orig_roll_fltt, orig_roll_smax, orig_roll_sp, orig_roll_accel, orig_roll_rate; @@ -295,14 +296,14 @@ protected: float tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, tune_yaw_accel; float tune_roll_rff, tune_pitch_rff, tune_yaw_rd, tune_yaw_rff; - uint32_t announce_time; - float lean_angle; - float rotation_rate; - float roll_cd, pitch_cd; + uint32_t last_announce_ms; + float lean_angle; + float rotation_rate; + float roll_cd, pitch_cd; // heli specific variables - float start_freq; //start freq for dwell test - float stop_freq; //ending freq for dwell test + float start_freq; //start freq for dwell test + float stop_freq; //ending freq for dwell test private: // return true if we have a good position estimate @@ -329,8 +330,8 @@ private: // initialise position controller bool init_position_controller(); - // main state machine to level vehicle, perform a test and update gains - // directly updates attitude controller with targets + // Main tuning state machine. Handles WAITING_FOR_LEVEL, EXECUTING_TEST, UPDATE_GAINS, ABORT. + // Updates attitude controller targets and evaluates test responses to tune gains. void control_attitude(); // returns true if vehicle is close to level @@ -349,7 +350,7 @@ private: // True if we ever got a pilot testing command of tuned gains. // If true then disarming will save if the tuned gains are currently active. - bool have_pilot_testing_command; + bool testing_switch_used; }; diff --git a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp index c85aa33f659..aab09869639 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp @@ -39,7 +39,7 @@ void AC_AutoTune_FreqResp::init(InputType input_type, ResponseType response_type void AC_AutoTune_FreqResp::update(float command, float tgt_resp, float meas_resp, float tgt_freq) { - uint32_t now = AP_HAL::millis(); + uint32_t now_ms = AP_HAL::millis(); float dt = 0.0025; uint32_t half_cycle_time_ms = 0; uint32_t cycle_time_ms = 0; @@ -54,7 +54,7 @@ void AC_AutoTune_FreqResp::update(float command, float tgt_resp, float meas_resp } if (input_start_time_ms == 0) { - input_start_time_ms = now; + input_start_time_ms = now_ms; if (response == ANGLE) { prev_tgt_resp = tgt_resp; prev_meas_resp = meas_resp; @@ -140,9 +140,9 @@ void AC_AutoTune_FreqResp::update(float command, float tgt_resp, float meas_resp // Indicates when the target(input) is positive or negative half of the cycle to notify when the max or min should be sought if (((response == ANGLE && is_positive(prev_target) && !is_positive(target_rate)) || (response == RATE && !is_positive(prev_target) && is_positive(target_rate))) - && !new_target && now > new_tgt_time_ms) { + && !new_target && now_ms > new_tgt_time_ms) { new_target = true; - new_tgt_time_ms = now + half_cycle_time_ms; + new_tgt_time_ms = now_ms + half_cycle_time_ms; // reset max_target max_target = 0.0f; max_target_cnt++; @@ -160,9 +160,9 @@ void AC_AutoTune_FreqResp::update(float command, float tgt_resp, float meas_resp } else if (((response == ANGLE && !is_positive(prev_target) && is_positive(target_rate)) || (response == RATE && is_positive(prev_target) && !is_positive(target_rate))) - && new_target && now > new_tgt_time_ms && max_target_cnt > 0) { + && new_target && now_ms > new_tgt_time_ms && max_target_cnt > 0) { new_target = false; - new_tgt_time_ms = now + half_cycle_time_ms; + new_tgt_time_ms = now_ms + half_cycle_time_ms; min_target_cnt++; temp_max_target = max_target; min_target = 0.0f; @@ -171,9 +171,9 @@ void AC_AutoTune_FreqResp::update(float command, float tgt_resp, float meas_resp // Indicates when the measured value (output) is positive or negative half of the cycle to notify when the max or min should be sought if (((response == ANGLE && is_positive(prev_meas) && !is_positive(measured_rate)) || (response == RATE && !is_positive(prev_meas) && is_positive(measured_rate))) - && !new_meas && now > new_meas_time_ms && max_target_cnt > 0) { + && !new_meas && now_ms > new_meas_time_ms && max_target_cnt > 0) { new_meas = true; - new_meas_time_ms = now + half_cycle_time_ms; + new_meas_time_ms = now_ms + half_cycle_time_ms; // reset max_meas max_meas = 0.0f; max_meas_cnt++; @@ -205,9 +205,9 @@ void AC_AutoTune_FreqResp::update(float command, float tgt_resp, float meas_resp } } else if (((response == ANGLE && !is_positive(prev_meas) && is_positive(measured_rate)) || (response == RATE && is_positive(prev_meas) && !is_positive(measured_rate))) - && new_meas && now > new_meas_time_ms && max_meas_cnt > 0) { + && new_meas && now_ms > new_meas_time_ms && max_meas_cnt > 0) { new_meas = false; - new_meas_time_ms = now + half_cycle_time_ms; + new_meas_time_ms = now_ms + half_cycle_time_ms; min_meas_cnt++; temp_max_meas = max_meas; min_meas = 0.0f; @@ -216,7 +216,7 @@ void AC_AutoTune_FreqResp::update(float command, float tgt_resp, float meas_resp if (new_target) { if (tgt_resp > max_target) { max_target = tgt_resp; - max_tgt_time = now; + max_tgt_time = now_ms; } } else { if (tgt_resp < min_target) { @@ -227,7 +227,7 @@ void AC_AutoTune_FreqResp::update(float command, float tgt_resp, float meas_resp if (new_meas) { if (meas_resp > max_meas) { max_meas = meas_resp; - max_meas_time = now; + max_meas_time = now_ms; } } else { if (meas_resp < min_meas) { @@ -236,7 +236,7 @@ void AC_AutoTune_FreqResp::update(float command, float tgt_resp, float meas_resp } if (response == ANGLE) { - if (now > (uint32_t)(input_start_time_ms + 7.0f * cycle_time_ms) && now < (uint32_t)(input_start_time_ms + 9.0f * cycle_time_ms)) { + if (now_ms > (uint32_t)(input_start_time_ms + 7.0f * cycle_time_ms) && now_ms < (uint32_t)(input_start_time_ms + 9.0f * cycle_time_ms)) { if (measured_rate > max_meas_rate) { max_meas_rate = measured_rate; } diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 218353e70cf..bb552afe5d1 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -136,7 +136,7 @@ AC_AutoTune_Heli::AC_AutoTune_Heli() AP_Param::setup_object_defaults(this, var_info); } -// initialize tests for each tune type +// Prepares all tuning state variables and target values for a new twitch test. void AC_AutoTune_Heli::test_init() { AC_AutoTune_FreqResp::ResponseType resp_type = AC_AutoTune_FreqResp::ResponseType::RATE; @@ -145,7 +145,7 @@ void AC_AutoTune_Heli::test_init() float freq_resp_amplitude = 5.0f; // amplitude in deg float filter_freq = 10.0f; switch (tune_type) { - case TuneType::RFF_UP: + case TuneType::RATE_FF_UP: if (!is_positive(next_test_freq)) { start_freq = 0.25f * M_2PI; } else { @@ -172,7 +172,7 @@ void AC_AutoTune_Heli::test_init() } else { start_freq = next_test_freq; stop_freq = start_freq; - test_accel_max = 0.0f; + test_accel_max_cdss = 0.0f; } filter_freq = start_freq; @@ -185,12 +185,12 @@ void AC_AutoTune_Heli::test_init() pre_calc_cycles = 6.25f; num_dwell_cycles = 6; break; - case TuneType::RP_UP: - case TuneType::RD_UP: + case TuneType::RATE_P_UP: + case TuneType::RATE_D_UP: // initialize start frequency if (!is_positive(next_test_freq)) { - // continue using frequency where testing left off with RD_UP completed - if (curr_data.phase > 150.0f && curr_data.phase < 180.0f && tune_type == TuneType::RP_UP) { + // continue using frequency where testing left off with RATE_D_UP completed + if (curr_data.phase > 150.0f && curr_data.phase < 180.0f && tune_type == TuneType::RATE_P_UP) { start_freq = curr_data.freq; // start with freq found for sweep where phase was 180 deg } else if (!is_zero(sweep_tgt.ph180.freq)) { @@ -214,7 +214,7 @@ void AC_AutoTune_Heli::test_init() pre_calc_cycles = 6.25f; num_dwell_cycles = 6; break; - case TuneType::SP_UP: + case TuneType::ANGLE_P_UP: // initialize start frequency for sweep if (!is_positive(next_test_freq)) { start_freq = min_sweep_freq; @@ -223,7 +223,7 @@ void AC_AutoTune_Heli::test_init() } else { start_freq = next_test_freq; stop_freq = start_freq; - test_accel_max = 0.0f; + test_accel_max_cdss = 0.0f; } filter_freq = start_freq; attitude_control->bf_feedforward(false); @@ -239,7 +239,7 @@ void AC_AutoTune_Heli::test_init() // initialize start frequency start_freq = min_sweep_freq; stop_freq = max_sweep_freq; - test_accel_max = 0.0f; + test_accel_max_cdss = 0.0f; filter_freq = start_freq; // variables needed to initialize frequency response object and test method @@ -269,28 +269,28 @@ void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign) { // if tune complete or beyond frequency range or no max allowed gains then exit testing if (tune_type == TuneType::TUNE_COMPLETE || - ((tune_type == TuneType::RP_UP || tune_type == TuneType::RD_UP) && (max_rate_p.max_allowed <= 0.0f || max_rate_d.max_allowed <= 0.0f)) || - ((tune_type == TuneType::MAX_GAINS || tune_type == TuneType::RP_UP || tune_type == TuneType::RD_UP || tune_type == TuneType::SP_UP) && exceeded_freq_range(start_freq))){ + ((tune_type == TuneType::RATE_P_UP || tune_type == TuneType::RATE_D_UP) && (max_rate_p.max_allowed <= 0.0f || max_rate_d.max_allowed <= 0.0f)) || + ((tune_type == TuneType::MAX_GAINS || tune_type == TuneType::RATE_P_UP || tune_type == TuneType::RATE_D_UP || tune_type == TuneType::ANGLE_P_UP) && exceeded_freq_range(start_freq))){ - load_gains(GainType::GAIN_ORIGINAL); + load_gains(GainType::ORIGINAL); get_poshold_attitude(roll_cd, pitch_cd, desired_yaw_cd); // hold level attitude attitude_control->input_euler_angle_roll_pitch_yaw_cd(roll_cd, pitch_cd, desired_yaw_cd, true); - if ((tune_type == TuneType::RP_UP || tune_type == TuneType::RD_UP) && (max_rate_p.max_allowed <= 0.0f || max_rate_d.max_allowed <= 0.0f)) { + if ((tune_type == TuneType::RATE_P_UP || tune_type == TuneType::RATE_D_UP) && (max_rate_p.max_allowed <= 0.0f || max_rate_d.max_allowed <= 0.0f)) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Max Gain Determination Failed"); mode = TuneMode::FAILED; LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED); update_gcs(AUTOTUNE_MESSAGE_FAILED); - } else if ((tune_type == TuneType::MAX_GAINS || tune_type == TuneType::RP_UP || tune_type == TuneType::RD_UP || tune_type == TuneType::SP_UP) && exceeded_freq_range(start_freq)){ + } else if ((tune_type == TuneType::MAX_GAINS || tune_type == TuneType::RATE_P_UP || tune_type == TuneType::RATE_D_UP || tune_type == TuneType::ANGLE_P_UP) && exceeded_freq_range(start_freq)){ GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Exceeded frequency range"); mode = TuneMode::FAILED; LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED); update_gcs(AUTOTUNE_MESSAGE_FAILED); } else if (tune_type == TuneType::TUNE_COMPLETE) { - counter = AUTOTUNE_SUCCESS_COUNT; + success_counter = AUTOTUNE_SUCCESS_COUNT; step = StepType::UPDATE_GAINS; } return; @@ -303,19 +303,19 @@ void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign) // heli specific gcs announcements void AC_AutoTune_Heli::do_gcs_announcements() { - const uint32_t now = AP_HAL::millis(); - if (now - announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) { + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - last_announce_ms < AUTOTUNE_ANNOUNCE_INTERVAL_MS) { return; } - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: %s %s", axis_string(), type_string()); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: %s %s", get_axis_name(), get_tune_type_name()); send_step_string(); switch (tune_type) { - case TuneType::RFF_UP: - case TuneType::RD_UP: - case TuneType::RP_UP: + case TuneType::RATE_FF_UP: + case TuneType::RATE_D_UP: + case TuneType::RATE_P_UP: case TuneType::MAX_GAINS: - case TuneType::SP_UP: + case TuneType::ANGLE_P_UP: case TuneType::TUNE_CHECK: if (is_equal(start_freq,stop_freq)) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Dwell"); @@ -330,7 +330,7 @@ void AC_AutoTune_Heli::do_gcs_announcements() break; } - announce_time = now; + last_announce_ms = now_ms; } // send post test updates to user @@ -368,23 +368,23 @@ void AC_AutoTune_Heli::do_post_test_gcs_announcements() { if (step == StepType::UPDATE_GAINS) { switch (tune_type) { - case TuneType::RFF_UP: - case TuneType::RP_UP: - case TuneType::RD_UP: - case TuneType::SP_UP: + case TuneType::RATE_FF_UP: + case TuneType::RATE_P_UP: + case TuneType::RATE_D_UP: + case TuneType::ANGLE_P_UP: case TuneType::MAX_GAINS: if (is_equal(start_freq,stop_freq)) { // announce results of dwell GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(curr_data.freq), (double)(curr_data.gain)); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: ph=%f", (double)(curr_data.phase)); - if (tune_type == TuneType::RP_UP) { + if (tune_type == TuneType::RATE_P_UP) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: rate_p=%f", (double)(tune_rp)); - } else if (tune_type == TuneType::RD_UP) { + } else if (tune_type == TuneType::RATE_D_UP) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: rate_d=%f", (double)(tune_rd)); - } else if (tune_type == TuneType::RFF_UP) { + } else if (tune_type == TuneType::RATE_FF_UP) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: rate_ff=%f", (double)(tune_rff)); - } else if (tune_type == TuneType::SP_UP) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: angle_p=%f tune_accel=%f max_accel=%f", (double)(tune_sp), (double)(tune_accel), (double)(test_accel_max)); + } else if (tune_type == TuneType::ANGLE_P_UP) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: angle_p=%f tune_accel=%f max_accel=%f", (double)(tune_sp), (double)(tune_accel), (double)(test_accel_max_cdss)); } } else { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep_tgt.maxgain.freq), (double)(sweep_tgt.maxgain.gain)); @@ -534,7 +534,7 @@ void AC_AutoTune_Heli::load_test_gains() rate_test_max = 0.0; accel_test_max = 0.0; } - if (tune_type == TuneType::SP_UP || tune_type == TuneType::TUNE_CHECK) { + if (tune_type == TuneType::ANGLE_P_UP || tune_type == TuneType::TUNE_CHECK) { rate_i = tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL; } else { // freeze integrator to hold trim by making i term small during rate controller tuning @@ -558,7 +558,7 @@ void AC_AutoTune_Heli::load_test_gains() rate_test_max = 0.0; accel_test_max = 0.0; } - if (tune_type == TuneType::SP_UP || tune_type == TuneType::TUNE_CHECK) { + if (tune_type == TuneType::ANGLE_P_UP || tune_type == TuneType::TUNE_CHECK) { rate_i = tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL; } else { // freeze integrator to hold trim by making i term small during rate controller tuning @@ -583,7 +583,7 @@ void AC_AutoTune_Heli::load_test_gains() rate_test_max = 0.0; accel_test_max = 0.0; } - if (tune_type == TuneType::SP_UP || tune_type == TuneType::TUNE_CHECK) { + if (tune_type == TuneType::ANGLE_P_UP || tune_type == TuneType::TUNE_CHECK) { rate_i = tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL; } else { // freeze integrator to hold trim by making i term small during rate controller tuning @@ -747,7 +747,7 @@ void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float am // initialize frequency response object if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) { - step_time_limit_ms = sweep_time_ms + 500; + step_timeout_ms = sweep_time_ms + 500; reset_sweep_variables(); curr_test.gain = 0.0f; curr_test.phase = 0.0f; @@ -755,9 +755,9 @@ void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float am } else { if (!is_zero(start_frq)) { // time limit set by adding the pre calc cycles with the dwell cycles. 500 ms added to account for settling with buffer. - step_time_limit_ms = (uint32_t) (2000 + ((float)num_dwell_cycles + pre_calc_cycles + 2.0f) * 1000.0f * M_2PI / start_frq); + step_timeout_ms = (uint32_t) (2000 + ((float)num_dwell_cycles + pre_calc_cycles + 2.0f) * 1000.0f * M_2PI / start_frq); } - chirp_input.init(0.001f * step_time_limit_ms, start_frq / M_2PI, stop_frq / M_2PI, 0.0f, 0.0001f * step_time_limit_ms, 0.0f); + chirp_input.init(0.001f * step_timeout_ms, start_frq / M_2PI, stop_frq / M_2PI, 0.0f, 0.0001f * step_timeout_ms, 0.0f); } freqresp_tgt.init(test_input_type, resp_type, num_dwell_cycles); @@ -796,7 +796,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) float gyro_reading = 0.0f; float command_reading = 0.0f; float tgt_rate_reading = 0.0f; - const uint32_t now = AP_HAL::millis(); + const uint32_t now_ms = AP_HAL::millis(); float target_angle_cd = 0.0f; float dwell_freq = test_start_freq; @@ -828,7 +828,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) tgt_att_limited = tgt_att_temp; } } - target_angle_cd = -chirp_input.update((now - dwell_start_time_ms) * 0.001, degrees(tgt_att_limited) * 100.0f); + target_angle_cd = -chirp_input.update((now_ms - dwell_start_time_ms) * 0.001, degrees(tgt_att_limited) * 100.0f); dwell_freq = chirp_input.get_frequency_rads(); const Vector2f att_fdbk { -5730.0f * vel_hold_gain * velocity_bf.y, @@ -839,7 +839,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) target_angle_cd = 0.0f; trim_yaw_tgt_reading_cd = (float)attitude_control->get_att_target_euler_cd().z; trim_yaw_heading_reading_cd = (float)ahrs_view->yaw_sensor; - dwell_start_time_ms = now; + dwell_start_time_ms = now_ms; filt_att_fdbk_from_velxy_cd.reset(Vector2f(0.0f,0.0f)); settle_time--; } @@ -918,7 +918,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) float dwell_gain_tgt = 0.0f; float dwell_phase_tgt = 0.0f; // wait for dwell to start before determining gain and phase - if ((float)(now - dwell_start_time_ms) > pre_calc_cycles * cycle_time_ms || (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP && settle_time == 0)) { + if ((float)(now_ms - dwell_start_time_ms) > pre_calc_cycles * cycle_time_ms || (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP && settle_time == 0)) { freqresp_mtr.update(command_out,command_out,rotation_rate, dwell_freq); freqresp_tgt.update(command_out,filt_target_rate,rotation_rate, dwell_freq); @@ -952,7 +952,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) curr_test_tgt.freq = freqresp_tgt.get_freq(); curr_test_tgt.gain = freqresp_tgt.get_gain(); curr_test_tgt.phase = freqresp_tgt.get_phase(); - if (test_calc_type == DRB) {test_accel_max = freqresp_tgt.get_accel_max();} + if (test_calc_type == DRB) {test_accel_max_cdss = freqresp_tgt.get_accel_max();} } // reset cycle_complete to allow indication of next cycle freqresp_tgt.reset_cycle_complete(); @@ -963,7 +963,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) } else { dwell_gain_tgt = freqresp_tgt.get_gain(); dwell_phase_tgt = freqresp_tgt.get_phase(); - if (test_calc_type == DRB) {test_accel_max = freqresp_tgt.get_accel_max();} + if (test_calc_type == DRB) {test_accel_max_cdss = freqresp_tgt.get_accel_max();} cycle_complete_tgt = true; } } @@ -988,7 +988,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) } // set sweep data if a frequency sweep is being conducted - if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP && (float)(now - dwell_start_time_ms) > 2.5f * cycle_time_ms) { + if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP && (float)(now_ms - dwell_start_time_ms) > 2.5f * cycle_time_ms) { // track sweep phase to prevent capturing 180 deg and 270 deg data after phase has wrapped. if (curr_test_tgt.phase > 180.0f && sweep_tgt.progress == 0) { sweep_tgt.progress = 1; @@ -1020,14 +1020,14 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) sweep_mtr.maxgain = curr_test_mtr; } - if (now - step_start_time_ms >= sweep_time_ms + 200) { + if (now_ms - step_start_time_ms >= sweep_time_ms + 200) { // we have passed the maximum stop time sweep_complete = true; step = StepType::UPDATE_GAINS; } } else { - if (now - step_start_time_ms >= step_time_limit_ms || (freqresp_tgt.is_cycle_complete() && freqresp_mtr.is_cycle_complete())) { - if (now - step_start_time_ms >= step_time_limit_ms) { + if (now_ms - step_start_time_ms >= step_timeout_ms || (freqresp_tgt.is_cycle_complete() && freqresp_mtr.is_cycle_complete())) { + if (now_ms - step_start_time_ms >= step_timeout_ms) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Step time limit exceeded"); } cycle_complete_tgt = false; @@ -1086,7 +1086,7 @@ void AC_AutoTune_Heli::updating_rate_ff_up_all(AxisType test_axis) case AxisType::YAW_D: updating_rate_ff_up(tune_yaw_rff, curr_data, next_test_freq); // TODO make FF updating routine determine when to set rff gain to zero based on A/C response - if (tune_yaw_rff <= AUTOTUNE_RFF_MIN && counter == AUTOTUNE_SUCCESS_COUNT) { + if (tune_yaw_rff <= AUTOTUNE_RFF_MIN && success_counter == AUTOTUNE_SUCCESS_COUNT) { tune_yaw_rff = 0.0f; } break; @@ -1155,10 +1155,10 @@ void AC_AutoTune_Heli::updating_max_gains_all(AxisType test_axis) case AxisType::YAW_D: updating_max_gains(curr_data, next_test_freq, max_rate_p, max_rate_d, tune_yaw_rp, tune_yaw_rd); // rate P and rate D can be non zero for yaw and need to be included in the max allowed gain - if (!is_zero(max_rate_p.max_allowed) && counter == AUTOTUNE_SUCCESS_COUNT) { + if (!is_zero(max_rate_p.max_allowed) && success_counter == AUTOTUNE_SUCCESS_COUNT) { max_rate_p.max_allowed += tune_yaw_rp; } - if (!is_zero(max_rate_d.max_allowed) && counter == AUTOTUNE_SUCCESS_COUNT) { + if (!is_zero(max_rate_d.max_allowed) && success_counter == AUTOTUNE_SUCCESS_COUNT) { max_rate_d.max_allowed += tune_yaw_rd; } break; @@ -1169,7 +1169,7 @@ void AC_AutoTune_Heli::updating_max_gains_all(AxisType test_axis) void AC_AutoTune_Heli::set_tuning_gains_with_backoff(AxisType test_axis) { switch (tune_type) { - case TuneType::RD_UP: + case TuneType::RATE_D_UP: switch (test_axis) { case AxisType::ROLL: tune_roll_rd = MAX(0.0f, tune_roll_rd * AUTOTUNE_RD_BACKOFF); @@ -1183,7 +1183,7 @@ void AC_AutoTune_Heli::set_tuning_gains_with_backoff(AxisType test_axis) break; } break; - case TuneType::RP_UP: + case TuneType::RATE_P_UP: switch (test_axis) { case AxisType::ROLL: tune_roll_rp = MAX(0.0f, tune_roll_rp * AUTOTUNE_RP_BACKOFF); @@ -1197,7 +1197,7 @@ void AC_AutoTune_Heli::set_tuning_gains_with_backoff(AxisType test_axis) break; } break; - case TuneType::SP_UP: + case TuneType::ANGLE_P_UP: switch (test_axis) { case AxisType::ROLL: tune_roll_sp = MAX(AUTOTUNE_SP_MIN, tune_roll_sp * AUTOTUNE_SP_BACKOFF); @@ -1211,7 +1211,7 @@ void AC_AutoTune_Heli::set_tuning_gains_with_backoff(AxisType test_axis) break; } break; - case TuneType::RFF_UP: + case TuneType::RATE_FF_UP: break; default: break; @@ -1247,7 +1247,7 @@ void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, sweep_info &test_data tune_ff *= 0.98; } } else if (test_data.gain >= tune_tgt - tune_tol && test_data.gain <= tune_tgt + tune_tol) { - counter = AUTOTUNE_SUCCESS_COUNT; + success_counter = AUTOTUNE_SUCCESS_COUNT; // reset next_freq for next test next_freq = 0.0f; tune_ff = constrain_float(tune_ff,0.0f,1.0f); @@ -1269,7 +1269,7 @@ void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, sweep_info &test_data, tune_p += 0.05f * max_gain_p.max_allowed; next_freq = data_at_ph161.freq; } else { - counter = AUTOTUNE_SUCCESS_COUNT; + success_counter = AUTOTUNE_SUCCESS_COUNT; // reset next_freq for next test next_freq = 0.0f; tune_p -= 0.05f * max_gain_p.max_allowed; @@ -1296,7 +1296,7 @@ void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, sweep_info &test_data, rd_prev_gain = data_at_ph161.gain; next_freq = data_at_ph161.freq; } else { - counter = AUTOTUNE_SUCCESS_COUNT; + success_counter = AUTOTUNE_SUCCESS_COUNT; // reset next freq and rd_prev_gain for next test next_freq = 0; rd_prev_gain = 0.0f; @@ -1331,7 +1331,7 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, sweep_info &test_data, } else if (test_data.gain > max_resp_gain && tune_p <= AUTOTUNE_SP_MIN) { // exceeded max response gain at the minimum allowable tuning gain. terminate testing. tune_p = AUTOTUNE_SP_MIN; - counter = AUTOTUNE_SUCCESS_COUNT; + success_counter = AUTOTUNE_SUCCESS_COUNT; LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } else if (test_data.gain > sp_prev_gain) { freq_max = test_data.freq; @@ -1378,7 +1378,7 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, sweep_info &test_data, next_freq = test_data.freq; if (tune_p >= AUTOTUNE_SP_MAX) { tune_p = AUTOTUNE_SP_MAX; - counter = AUTOTUNE_SUCCESS_COUNT; + success_counter = AUTOTUNE_SUCCESS_COUNT; LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } sp_prev_gain = test_data.gain; @@ -1390,10 +1390,10 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, sweep_info &test_data, float adj_factor = (max_resp_gain - test_data.gain) / (test_data.gain - sp_prev_gain); tune_p = tune_p + gain_incr * adj_factor; } - counter = AUTOTUNE_SUCCESS_COUNT; + success_counter = AUTOTUNE_SUCCESS_COUNT; } } - if (counter == AUTOTUNE_SUCCESS_COUNT) { + if (success_counter == AUTOTUNE_SUCCESS_COUNT) { next_freq = 0.0f; //initializes next test that uses dwell test sweep_complete = false; reset_sweep_variables(); @@ -1444,7 +1444,7 @@ void AC_AutoTune_Heli::updating_max_gains(sweep_info &test_data, float &next_fre } if (found_max_p && found_max_d) { - counter = AUTOTUNE_SUCCESS_COUNT; + success_counter = AUTOTUNE_SUCCESS_COUNT; // reset variables for next test next_freq = 0.0f; //initializes next test that uses dwell test GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Max rate P freq=%f gain=%f", (double)(max_rate_p.freq), (double)(max_rate_p.gain)); @@ -1532,14 +1532,14 @@ void AC_AutoTune_Heli::Log_AutoTune() switch (axis) { case AxisType::ROLL: - Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_roll_rff, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max); + Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_roll_rff, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max_cdss); break; case AxisType::PITCH: - Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_pitch_rff, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max); + Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_pitch_rff, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max_cdss); break; case AxisType::YAW: case AxisType::YAW_D: - Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_yaw_rff, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max); + Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_yaw_rff, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max_cdss); break; } } @@ -1547,7 +1547,7 @@ void AC_AutoTune_Heli::Log_AutoTune() // log autotune time history results for command, angular rate, and attitude void AC_AutoTune_Heli::Log_AutoTuneDetails() { - if (tune_type == TuneType::SP_UP || tune_type == TuneType::TUNE_CHECK) { + if (tune_type == TuneType::ANGLE_P_UP || tune_type == TuneType::TUNE_CHECK) { Log_Write_AutoTuneDetails(command_out, 0.0f, 0.0f, filt_target_rate, rotation_rate); } else { Log_Write_AutoTuneDetails(command_out, filt_target_rate, rotation_rate, 0.0f, 0.0f); @@ -1715,19 +1715,19 @@ void AC_AutoTune_Heli::set_tune_sequence() uint8_t seq_cnt = 0; if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_VFF) { - tune_seq[seq_cnt] = TuneType::RFF_UP; + tune_seq[seq_cnt] = TuneType::RATE_FF_UP; seq_cnt++; } if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_RATE_D) { tune_seq[seq_cnt] = TuneType::MAX_GAINS; seq_cnt++; - tune_seq[seq_cnt] = TuneType::RD_UP; + tune_seq[seq_cnt] = TuneType::RATE_D_UP; seq_cnt++; - tune_seq[seq_cnt] = TuneType::RP_UP; + tune_seq[seq_cnt] = TuneType::RATE_P_UP; seq_cnt++; } if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_ANGLE_P) { - tune_seq[seq_cnt] = TuneType::SP_UP; + tune_seq[seq_cnt] = TuneType::ANGLE_P_UP; seq_cnt++; } if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_MAX_GAIN && !(seq_bitmask & AUTOTUNE_SEQ_BITMASK_RATE_D)) { diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index 1562acc3416..80b8a3cc0a2 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -70,7 +70,7 @@ protected: // reset the update gain variables for heli void reset_update_gain_variables() override; - // initializes test + // Prepares all tuning state variables and target values for a new twitch test. void test_init() override; // runs test diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index 06f63cbc558..20ea46ccce0 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -130,14 +130,15 @@ AC_AutoTune_Multi::AC_AutoTune_Multi() void AC_AutoTune_Multi::do_gcs_announcements() { - const uint32_t now = AP_HAL::millis(); - if (now - announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) { + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - last_announce_ms < AUTOTUNE_ANNOUNCE_INTERVAL_MS) { return; } - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: %s %s %u%%", axis_string(), type_string(), (counter * (100/AUTOTUNE_SUCCESS_COUNT))); - announce_time = now; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: %s %s %u%%", get_axis_name(), get_tune_type_name(), (success_counter * (100/AUTOTUNE_SUCCESS_COUNT))); + last_announce_ms = now_ms; } +// Prepares all tuning state variables and target values for a new twitch test. void AC_AutoTune_Multi::test_init() { twitch_test_init(); @@ -531,11 +532,11 @@ void AC_AutoTune_Multi::report_axis_gains(const char* axis_string, float rate_P, GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: %s Angle P:%0.3f, Max Accel:%0.0f",axis_string,angle_P,max_accel); } -// twitching_test_rate - twitching tests -// update min and max and test for end conditions +// Measures peak angular rates and bounce-back behavior during rate tuning. +// Updates step timer and determines if tuning step is complete. void AC_AutoTune_Multi::twitching_test_rate(float angle, float rate, float rate_target_max, float &meas_rate_min, float &meas_rate_max, float &meas_angle_min) { - const uint32_t now = AP_HAL::millis(); + const uint32_t now_ms = AP_HAL::millis(); // capture maximum rate if (rate > meas_rate_max) { @@ -555,8 +556,8 @@ void AC_AutoTune_Multi::twitching_test_rate(float angle, float rate, float rate_ // calculate early stopping time based on the time it takes to get to 63.21% if (meas_rate_max < rate_target_max * 0.6321) { // the measurement not reached the 63.21% threshold yet - step_time_limit_ms = (now - step_start_time_ms) * 3; - step_time_limit_ms = MIN(step_time_limit_ms, AUTOTUNE_TESTING_STEP_TIMEOUT_MS); + step_timeout_ms = (now_ms - step_start_time_ms) * 3; + step_timeout_ms = MIN(step_timeout_ms, AUTOTUNE_TESTING_STEP_TIMEOUT_MS); } if (meas_rate_max > rate_target_max) { @@ -569,7 +570,7 @@ void AC_AutoTune_Multi::twitching_test_rate(float angle, float rate, float rate_ step = StepType::UPDATE_GAINS; } - if (now - step_start_time_ms >= step_time_limit_ms) { + if (now_ms - step_start_time_ms >= step_timeout_ms) { // we have passed the maximum stop time step = StepType::UPDATE_GAINS; } @@ -603,7 +604,7 @@ void AC_AutoTune_Multi::twitching_abort_rate(float angle, float rate, float angl // update min and max and test for end conditions void AC_AutoTune_Multi::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(); + const uint32_t now_ms = AP_HAL::millis(); // capture maximum angle if (angle > meas_angle_max) { @@ -634,8 +635,8 @@ void AC_AutoTune_Multi::twitching_test_angle(float angle, float rate, float angl // calculate early stopping time based on the time it takes to get to 63.21% if (meas_angle_max < angle_target_max * 0.6321) { // the measurement not reached the 63.21% threshold yet - step_time_limit_ms = (now - step_start_time_ms) * 3; - step_time_limit_ms = MIN(step_time_limit_ms, AUTOTUNE_TESTING_STEP_TIMEOUT_MS); + step_timeout_ms = (now_ms - step_start_time_ms) * 3; + step_timeout_ms = MIN(step_timeout_ms, AUTOTUNE_TESTING_STEP_TIMEOUT_MS); } if (meas_angle_max > angle_target_max) { @@ -648,7 +649,7 @@ void AC_AutoTune_Multi::twitching_test_angle(float angle, float rate, float angl step = StepType::UPDATE_GAINS; } - if (now - step_start_time_ms >= step_time_limit_ms) { + if (now_ms - step_start_time_ms >= step_timeout_ms) { // we have passed the maximum stop time step = StepType::UPDATE_GAINS; } @@ -758,9 +759,9 @@ void AC_AutoTune_Multi::updating_angle_p_down_all(AxisType test_axis) void AC_AutoTune_Multi::set_tuning_gains_with_backoff(AxisType test_axis) { switch (tune_type) { - case TuneType::RD_UP: + case TuneType::RATE_D_UP: break; - case TuneType::RD_DOWN: + case TuneType::RATE_D_DOWN: switch (test_axis) { case AxisType::ROLL: tune_roll_rd = MAX(min_d, tune_roll_rd * AUTOTUNE_RD_BACKOFF); @@ -780,7 +781,7 @@ void AC_AutoTune_Multi::set_tuning_gains_with_backoff(AxisType test_axis) break; } break; - case TuneType::RP_UP: + case TuneType::RATE_P_UP: switch (test_axis) { case AxisType::ROLL: tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RP_BACKOFF); @@ -794,26 +795,26 @@ void AC_AutoTune_Multi::set_tuning_gains_with_backoff(AxisType test_axis) break; } break; - case TuneType::SP_DOWN: + case TuneType::ANGLE_P_DOWN: break; - case TuneType::SP_UP: + case TuneType::ANGLE_P_UP: switch (test_axis) { case AxisType::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); + tune_roll_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max_cdss * AUTOTUNE_ACCEL_RP_BACKOFF); break; case AxisType::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); + tune_pitch_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max_cdss * AUTOTUNE_ACCEL_RP_BACKOFF); break; case AxisType::YAW: case AxisType::YAW_D: 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); + tune_yaw_accel = MAX(AUTOTUNE_Y_ACCEL_MIN, test_accel_max_cdss * AUTOTUNE_ACCEL_Y_BACKOFF); break; } break; - case TuneType::RFF_UP: + case TuneType::RATE_FF_UP: case TuneType::MAX_GAINS: case TuneType::TUNE_CHECK: // this should never happen @@ -839,7 +840,7 @@ void AC_AutoTune_Multi::updating_rate_d_up(float &tune_d, float tune_d_min, floa if (tune_d <= tune_d_min) { // We have reached minimum D gain so stop tuning tune_d = tune_d_min; - counter = AUTOTUNE_SUCCESS_COUNT; + success_counter = AUTOTUNE_SUCCESS_COUNT; LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); // This may be mean AGGR should be increased or MIN_D decreased GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Min Rate D limit reached"); @@ -859,19 +860,19 @@ void AC_AutoTune_Multi::updating_rate_d_up(float &tune_d, float tune_d_min, floa // 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++; + success_counter++; } else { if (ignore_next == false) { // bounce back is smaller than our threshold so decrement the success counter - if (counter > 0) { - counter--; + if (success_counter > 0) { + success_counter--; } // increase D gain (which should increase bounce back) tune_d += tune_d*tune_d_step_ratio * 2.0; // stop tuning if we hit maximum D if (tune_d >= tune_d_max) { tune_d = tune_d_max; - counter = AUTOTUNE_SUCCESS_COUNT; + success_counter = AUTOTUNE_SUCCESS_COUNT; LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } } else { @@ -896,7 +897,7 @@ void AC_AutoTune_Multi::updating_rate_d_down(float &tune_d, float tune_d_min, fl if (tune_d <= tune_d_min) { // We have reached minimum D so stop tuning tune_d = tune_d_min; - counter = AUTOTUNE_SUCCESS_COUNT; + success_counter = AUTOTUNE_SUCCESS_COUNT; LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); // This may be mean AGGR should be increased or MIN_D decreased GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Min Rate D limit reached"); @@ -915,7 +916,7 @@ void AC_AutoTune_Multi::updating_rate_d_down(float &tune_d, float tune_d_min, fl 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++; + success_counter++; } else { ignore_next = false; } @@ -923,15 +924,15 @@ void AC_AutoTune_Multi::updating_rate_d_down(float &tune_d, float tune_d_min, fl // 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--; + if (success_counter > 0) { + success_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; + success_counter = AUTOTUNE_SUCCESS_COUNT; LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); // This may be mean AGGR should be increased or MIN_D decreased GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Min Rate D limit reached"); @@ -948,13 +949,13 @@ void AC_AutoTune_Multi::updating_rate_p_up_d_down(float &tune_d, float tune_d_mi // 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++; + success_counter++; } else if ((meas_rate_max < rate_target) && (meas_rate_max > rate_target * (1.0 - 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--; + if (success_counter > 0) { + success_counter--; } - // decrease D gain (which should decrease bounce back) + // Reduce D gain if bounce-back exceeds threshold. tune_d -= tune_d * tune_d_step_ratio; // do not decrease the D term past the minimum if (tune_d <= tune_d_min) { @@ -978,15 +979,15 @@ void AC_AutoTune_Multi::updating_rate_p_up_d_down(float &tune_d, float tune_d_mi } else { if (ignore_next == false) { // if maximum measurement was lower than target so decrement the success counter - if (counter > 0) { - counter--; + if (success_counter > 0) { + success_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; + success_counter = AUTOTUNE_SUCCESS_COUNT; LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } } else { @@ -1002,7 +1003,7 @@ void AC_AutoTune_Multi::updating_angle_p_down(float &tune_p, float tune_p_min, f if (meas_angle_max < angle_target * (1 + 0.5 * aggressiveness)) { if (ignore_next == false) { // if maximum measurement was lower than target so increment the success counter - counter++; + success_counter++; } else { ignore_next = false; } @@ -1010,8 +1011,8 @@ void AC_AutoTune_Multi::updating_angle_p_down(float &tune_p, float tune_p_min, f // 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--; + if (success_counter > 0) { + success_counter--; } // decrease P gain (which should decrease the maximum) tune_p -= tune_p*tune_p_step_ratio; @@ -1035,19 +1036,19 @@ void AC_AutoTune_Multi::updating_angle_p_up(float &tune_p, float tune_p_max, flo // 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++; + success_counter++; } else { if (ignore_next == false) { // if maximum measurement was lower than target so decrement the success counter - if (counter > 0) { - counter--; + if (success_counter > 0) { + success_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; + success_counter = AUTOTUNE_SUCCESS_COUNT; LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } } else { @@ -1059,34 +1060,34 @@ void AC_AutoTune_Multi::updating_angle_p_up(float &tune_p, float tune_p_max, flo #if HAL_LOGGING_ENABLED void AC_AutoTune_Multi::Log_AutoTune() { - if ((tune_type == TuneType::SP_DOWN) || (tune_type == TuneType::SP_UP)) { + if ((tune_type == TuneType::ANGLE_P_DOWN) || (tune_type == TuneType::ANGLE_P_UP)) { switch (axis) { case AxisType::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); + 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_cdss); break; case AxisType::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); + 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_cdss); break; case AxisType::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); + 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_cdss); break; case AxisType::YAW_D: - Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max); + Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max_cdss); break; } } else { switch (axis) { case AxisType::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); + 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_cdss); break; case AxisType::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); + 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_cdss); break; case AxisType::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); + 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_cdss); break; case AxisType::YAW_D: - Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max); + Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max_cdss); break; } } @@ -1219,13 +1220,13 @@ void AC_AutoTune_Multi::twitch_test_init() break; } - if ((tune_type == TuneType::SP_DOWN) || (tune_type == TuneType::SP_UP)) { - // todo: consider if this should be done for other axis + if ((tune_type == TuneType::ANGLE_P_DOWN) || (tune_type == TuneType::ANGLE_P_UP)) { + // TODO: Evaluate whether initializing rotation_rate_filt with start_rate improves results for other axes rotation_rate_filt.reset(start_rate); } else { rotation_rate_filt.reset(0.0); } - twitch_first_iter = true; + angle_step_commanded = false; test_rate_max = 0.0; test_rate_min = 0.0; test_angle_max = 0.0; @@ -1233,15 +1234,16 @@ void AC_AutoTune_Multi::twitch_test_init() accel_measure_rate_max = 0.0; } -//run twitch test +// Executes a single twitch iteration for the specified axis and direction. +// Configures attitude or rate commands based on the current TuneType. void AC_AutoTune_Multi::twitch_test_run(AxisType test_axis, const float dir_sign) { // hold current attitude - if ((tune_type == TuneType::SP_DOWN) || (tune_type == TuneType::SP_UP)) { + if ((tune_type == TuneType::ANGLE_P_DOWN) || (tune_type == TuneType::ANGLE_P_UP)) { // step angle targets on first iteration - if (twitch_first_iter) { - twitch_first_iter = false; + if (!angle_step_commanded) { + angle_step_commanded = true; // Testing increasing stabilize P gain so will set lean angle target switch (test_axis) { case AxisType::ROLL: @@ -1303,8 +1305,8 @@ void AC_AutoTune_Multi::twitch_test_run(AxisType test_axis, const float dir_sign // Add filter to measurements float filter_value; switch (tune_type) { - case TuneType::SP_DOWN: - case TuneType::SP_UP: + case TuneType::ANGLE_P_DOWN: + case TuneType::ANGLE_P_UP: filter_value = dir_sign * (degrees(gyro_reading) * 100.0); break; default: @@ -1315,23 +1317,23 @@ void AC_AutoTune_Multi::twitch_test_run(AxisType test_axis, const float dir_sign AP::scheduler().get_loop_period_s()); switch (tune_type) { - case TuneType::RD_UP: - case TuneType::RD_DOWN: + case TuneType::RATE_D_UP: + case TuneType::RATE_D_DOWN: twitching_test_rate(lean_angle, rotation_rate, target_rate, test_rate_min, test_rate_max, test_angle_min); - twitching_measure_acceleration(test_accel_max, rotation_rate, accel_measure_rate_max); + twitching_measure_acceleration(test_accel_max_cdss, rotation_rate, accel_measure_rate_max); twitching_abort_rate(lean_angle, rotation_rate, angle_abort, test_rate_min, test_angle_min); break; - case TuneType::RP_UP: + case TuneType::RATE_P_UP: twitching_test_rate(lean_angle, rotation_rate, target_rate * (1 + 0.5 * aggressiveness), test_rate_min, test_rate_max, test_angle_min); - twitching_measure_acceleration(test_accel_max, rotation_rate, accel_measure_rate_max); + twitching_measure_acceleration(test_accel_max_cdss, rotation_rate, accel_measure_rate_max); twitching_abort_rate(lean_angle, rotation_rate, angle_abort, test_rate_min, test_angle_min); break; - case TuneType::SP_DOWN: - case TuneType::SP_UP: + case TuneType::ANGLE_P_DOWN: + case TuneType::ANGLE_P_UP: twitching_test_angle(lean_angle, rotation_rate, target_angle * (1 + 0.5 * aggressiveness), test_angle_min, test_angle_max, test_rate_min, test_rate_max); - twitching_measure_acceleration(test_accel_max, rotation_rate - dir_sign * start_rate, accel_measure_rate_max); + twitching_measure_acceleration(test_accel_max_cdss, rotation_rate - dir_sign * start_rate, accel_measure_rate_max); break; - case TuneType::RFF_UP: + case TuneType::RATE_FF_UP: case TuneType::MAX_GAINS: case TuneType::TUNE_CHECK: // this should never happen diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.h b/libraries/AC_AutoTune/AC_AutoTune_Multi.h index adf4d45775c..aa75b27d8b1 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.h @@ -13,8 +13,8 @@ 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 + Multirotor implementation of AutoTune. Based on the original ArduCopter + autotune code by Leonard Hall. */ #pragma once @@ -28,172 +28,192 @@ class AC_AutoTune_Multi : public AC_AutoTune { public: - // constructor + // Constructor AC_AutoTune_Multi(); - // save gained, called on disarm + // Saves tuned gains to EEPROM on disarm void save_tuning_gains() override; - // var_info for holding Parameter information + // Parameter group for AP_Param registration static const struct AP_Param::GroupInfo var_info[]; protected: // - // methods to load and save gains + // Gain management and initialization // - // backup original gains and prepare for start of tuning + // Backs up original gains and resets tuning state void backup_gains_and_initialise() override; - // switch to use original gains + // Loads original pre-tune gains void load_orig_gains() override; - // switch to gains found by last successful autotune + // Loads gains from the last successful autotune session void load_tuned_gains() override; - // load gains used between tests. called during testing mode's update-gains step to set gains ahead of return-to-level step + // Loads conservative gains used between twitch tests void load_intra_test_gains() override; - // load test gains + // Loads current test gains before executing a twitch void load_test_gains() override; - // reset the test variables for multi + // Resets vehicle-specific test state (not used in multirotor) void reset_vehicle_test_variables() override {}; - // reset the update gain variables for multi + // Resets vehicle-specific gain tracking state (not used in multirotor) void reset_update_gain_variables() override {}; + // Maximum/Minimum target angles for the current axis (in centidegrees) float target_angle_max_rp_cd() const override; - float target_angle_max_y_cd() const override; - float target_angle_min_rp_cd() const override; - float target_angle_min_y_cd() const override; + // Absolute and negative angle abort limits for twitch safety (in centidegrees) float angle_lim_max_rp_cd() const override; - float angle_lim_neg_rpy_cd() const override; + // Prepares all tuning state variables and target values for a new twitch test. void test_init() override; void test_run(AxisType test_axis, const float dir_sign) override; - // send intermittent updates to user on status of tune + // Sends regular status messages to the ground station void do_gcs_announcements() override; - // send post test updates to user + // (Unused) Placeholder for post-test announcements void do_post_test_gcs_announcements() override {}; - // report final gains for a given axis to GCS + // Reports final tuned gains to the ground station void report_final_gains(AxisType test_axis) const override; - // update gains for the rate P up tune type + // Update functions for each TuneType void updating_rate_p_up_all(AxisType test_axis) override; - - // update gains for the rate D up tune type void updating_rate_d_up_all(AxisType test_axis) override; - - // update gains for the rate D down tune type void updating_rate_d_down_all(AxisType test_axis) override; - - // update gains for the rate ff up tune type - void updating_rate_ff_up_all(AxisType test_axis) override { - // this should never happen - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - } - - // update gains for the angle P up tune type void updating_angle_p_up_all(AxisType test_axis) override; - - // update gains for the angle P down tune type void updating_angle_p_down_all(AxisType test_axis) override; - // update gains for the max gain tune type - void updating_max_gains_all(AxisType test_axis) override { - // this should never happen + // These tune types are not used in multirotors + void updating_rate_ff_up_all(AxisType) override { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + void updating_max_gains_all(AxisType) override { INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } - // set gains post tune for the tune type + // Applies gain margin (backoff) at the end of a TuneType void set_tuning_gains_with_backoff(AxisType test_axis) override; - // reverse direction for twitch test + // Reverses twitch direction for next test bool twitch_reverse_direction() override { return !positive_direction; } #if HAL_LOGGING_ENABLED void Log_AutoTune() override; void Log_AutoTuneDetails() override; void Log_AutoTuneSweep() override { - // this should never happen INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } - void Log_Write_AutoTune(AxisType axis, TuneType 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_AutoTune(AxisType axis, TuneType 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); #endif + // Defines the sequence of tuning steps (P, D, etc.) void set_tune_sequence() override { - tune_seq[0] = TuneType::RD_UP; - tune_seq[1] = TuneType::RD_DOWN; - tune_seq[2] = TuneType::RP_UP; - tune_seq[3] = TuneType::SP_DOWN; - tune_seq[4] = TuneType::SP_UP; + tune_seq[0] = TuneType::RATE_D_UP; + tune_seq[1] = TuneType::RATE_D_DOWN; + tune_seq[2] = TuneType::RATE_P_UP; + tune_seq[3] = TuneType::ANGLE_P_DOWN; + tune_seq[4] = TuneType::ANGLE_P_UP; tune_seq[5] = TuneType::TUNE_COMPLETE; } - // get_axis_bitmask accessor + // Axis tuning mask accessor uint8_t get_axis_bitmask() const override { return axis_bitmask; } - // get_testing_step_timeout_ms accessor + // Timeout for a single twitch test uint32_t get_testing_step_timeout_ms() const override; private: // twitch test functions for multicopter void twitch_test_init(); + + // Executes a single twitch iteration for the specified axis and direction. + // Configures attitude or rate commands based on the current TuneType. void twitch_test_run(AxisType test_axis, const float dir_sign); - void twitching_test_rate(float angle, float rate, float rate_target, float &meas_rate_min, float &meas_rate_max, float &meas_angle_min); - void twitching_abort_rate(float angle, float rate, float angle_max, float meas_rate_min, float angle_min); - 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); - // measure acceleration during twitch test + void twitching_test_rate(float angle, float rate, float rate_target, + float &meas_rate_min, float &meas_rate_max, float &meas_angle_min); + + void twitching_abort_rate(float angle, float rate, float angle_max, + float meas_rate_min, float angle_min); + + 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); + + // Calculates average acceleration during twitch void twitching_measure_acceleration(float &accel_average, float rate, float &rate_max) const; - // 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 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); +// Gain update routines - // 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 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); +// Increases D and adjusts P to produce controlled bounce-back behavior. +// Seeks to bring the response peak just below the target rate while inducing +// a small overshoot (bounce) for evaluating D influence. +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); - // 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 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, bool fail_min_d = true); +// Decreases D and adjusts P to eliminate bounce-back in the response. +// Aims to achieve a clean response just below the target rate with no overshoot. +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); - // 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 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); +// Increases P to ensure the target rate is reached quickly, while reducing D +// if bounce-back exceeds the aggressiveness threshold. +// Fails if D hits minimum and clean response is not achievable (optional). +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, + bool fail_min_d = true); - // 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 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); +// Decreases angle P gain to reduce overshoot until the target is no longer reached before timeout. +// Used to back off from aggressive behavior in angle control. +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); - // report gain formatting helper - void report_axis_gains(const char* axis_string, float rate_P, float rate_I, float rate_D, float angle_P, float max_accel) const; +// Increases angle P gain to ensure the target is reached within a reasonable time. +// Stops increasing once target response is consistently achieved without overshoot. +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); - // parameters - AP_Int8 axis_bitmask; // axes to be tuned - AP_Float aggressiveness; // aircraft response aggressiveness to be tuned - AP_Float min_d; // minimum rate d gain allowed during tuning - bool ignore_next; // ignore the results of the next test when true - float target_angle; // target angle for the test - float target_rate; // target rate for the test - float angle_abort; // Angle that test is aborted - float test_rate_min; // the minimum angular rate achieved during TESTING_RATE - float test_rate_max; // the maximum angular rate achieved during TESTING_RATE - float test_angle_min; // the minimum angle achieved during TESTING_ANGLE - float test_angle_max; // the maximum angle achieved during TESTING_ANGLE - float accel_measure_rate_max; // the maximum rate used to measure average acceleration during twitch + // Formats and sends gain reports + void report_axis_gains(const char* axis_string, float rate_P, float rate_I, + float rate_D, float angle_P, float max_accel) const; + + // Parameters + AP_Int8 axis_bitmask; // Axis enable mask + AP_Float aggressiveness; // Target overshoot ratio (D tuning sensitivity) + AP_Float min_d; // Minimum allowed D gain + bool ignore_next; // Skip next result (used for rate overshoot handling) + + // Measurement and target values for each test step + float target_angle; + float target_rate; + float angle_abort; + + float test_rate_min; + float test_rate_max; + float test_angle_min; + float test_angle_max; + + float accel_measure_rate_max; }; -#endif // AC_AUTOTUNE_ENABLED +#endif // AC_AUTOTUNE_ENABLED