Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -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:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Updating Gains");
return;
case ABORT:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Aborting Test");
return;
case TESTING:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Testing");
return;
@ -527,7 +530,9 @@ void AC_AutoTune::control_attitude()
}
}
}
FALLTHROUGH;
case ABORT:
if (axis == YAW || axis == YAW_D) {
// 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);
@ -585,6 +590,7 @@ void AC_AutoTune::backup_gains_and_initialise()
*/
void AC_AutoTune::load_gains(enum GainType gain_type)
{
// todo: add previous setting so gains are not loaded on each loop.
switch (gain_type) {
case GAIN_ORIGINAL:
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
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
};
// 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
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 (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
@ -587,10 +586,7 @@ void AC_AutoTune_Multi::twitching_abort_rate(float angle, float rate, float angl
LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
}
// ignore result and start test again
step = WAITING_FOR_LEVEL;
positive_direction = twitch_reverse_direction();
step_start_time_ms = now;
level_start_time_ms = now;
step = ABORT;
} else {
step = UPDATE_GAINS;
}