Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Autotune: Add ABORT state for consistency between tests

Browse Source
This commit is contained in:
Leonard Hall 2024-07-21 22:41:41 +09:30 committed by Peter Barker
parent bea2c5b59b
commit bb1758ecb1
3 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -134,6 +134,9 @@ void AC_AutoTune::send_step_string()
case UPDATE_GAINS: case UPDATE_GAINS:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Updating Gains"); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Updating Gains");
return; return;
case ABORT:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Aborting Test");
return;
case TESTING: case TESTING:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Testing"); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Testing");
return; return;
@ -527,7 +530,9 @@ void AC_AutoTune::control_attitude()
} }
} }
} }
FALLTHROUGH;
case ABORT:
if (axis == YAW || axis == YAW_D) { if (axis == YAW || axis == YAW_D) {
// todo: check to make sure we need this // todo: check to make sure we need this
attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs_view->yaw_sensor, false); attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs_view->yaw_sensor, false);
@ -585,6 +590,7 @@ void AC_AutoTune::backup_gains_and_initialise()
*/ */
void AC_AutoTune::load_gains(enum GainType gain_type) void AC_AutoTune::load_gains(enum GainType gain_type)
{ {
// todo: add previous setting so gains are not loaded on each loop.
switch (gain_type) { switch (gain_type) {
case GAIN_ORIGINAL: case GAIN_ORIGINAL:
load_orig_gains(); load_orig_gains();

1
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -198,6 +198,7 @@ protected:
WAITING_FOR_LEVEL = 0, // autotune is waiting for vehicle to return to level before beginning the next twitch 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 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 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
}; };
// mini steps performed while in Tuning mode, Testing step // mini steps performed while in Tuning mode, Testing step

6
libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View File

@ -573,7 +573,6 @@ void AC_AutoTune_Multi::twitching_test_rate(float angle, float rate, float rate_
// update min and max and test for end conditions // update min and max and test for end conditions
void AC_AutoTune_Multi::twitching_abort_rate(float angle, float rate, float angle_max, float meas_rate_min, float angle_min) void AC_AutoTune_Multi::twitching_abort_rate(float angle, float rate, float angle_max, float meas_rate_min, float angle_min)
{ {
const uint32_t now = AP_HAL::millis();
if (angle >= angle_max) { if (angle >= angle_max) {
if (is_equal(rate, meas_rate_min) || (angle_min > 0.95 * angle_max)) { if (is_equal(rate, meas_rate_min) || (angle_min > 0.95 * angle_max)) {
// we have reached the angle limit before completing the measurement of maximum and minimum // we have reached the angle limit before completing the measurement of maximum and minimum
@ -587,10 +586,7 @@ void AC_AutoTune_Multi::twitching_abort_rate(float angle, float rate, float angl
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED); LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
} }
// ignore result and start test again // ignore result and start test again
step = WAITING_FOR_LEVEL; step = ABORT;
positive_direction = twitch_reverse_direction();
step_start_time_ms = now;
level_start_time_ms = now;
} else { } else {
step = UPDATE_GAINS; step = UPDATE_GAINS;
} }