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

AC_AutoTune: fix pilot testing bug

Browse Source
This commit is contained in:
Bill Geyer 2022-11-12 14:04:46 -05:00 committed by Randy Mackay
parent 3a4d2a9403
commit fc6a6ceaa8
1 changed files with 26 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -200,29 +200,32 @@ void AC_AutoTune::run()
const bool zero_rp_input = is_zero(target_roll_cd) && is_zero(target_pitch_cd); const bool zero_rp_input = is_zero(target_roll_cd) && is_zero(target_pitch_cd);
const uint32_t now = AP_HAL::millis(); const uint32_t now = AP_HAL::millis();
if (!zero_rp_input || !is_zero(target_yaw_rate_cds) || !is_zero(target_climb_rate_cms)) {
if (!pilot_override) { if (mode != SUCCESS) {
pilot_override = true; if (!zero_rp_input || !is_zero(target_yaw_rate_cds) || !is_zero(target_climb_rate_cms)) {
// set gains to their original values if (!pilot_override) {
load_gains(GAIN_ORIGINAL); pilot_override = true;
attitude_control->use_sqrt_controller(true); // set gains to their original values
} load_gains(GAIN_ORIGINAL);
// reset pilot override time attitude_control->use_sqrt_controller(true);
override_time = now; }
if (!zero_rp_input) { // reset pilot override time
// only reset position on roll or pitch input override_time = now;
have_position = false; if (!zero_rp_input) {
} // only reset position on roll or pitch input
} else if (pilot_override) { have_position = false;
// check if we should resume tuning after pilot's override }
if (now - override_time > AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS) { } else if (pilot_override) {
pilot_override = false; // turn off pilot override // check if we should resume tuning after pilot's override
// set gains to their intra-test values (which are very close to the original gains) if (now - override_time > AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS) {
// load_gains(GAIN_INTRA_TEST); //I think we should be keeping the originals here to let the I term settle quickly pilot_override = false; // turn off pilot override
step = WAITING_FOR_LEVEL; // set tuning step back from beginning // set gains to their intra-test values (which are very close to the original gains)
step_start_time_ms = now; // load_gains(GAIN_INTRA_TEST); //I think we should be keeping the originals here to let the I term settle quickly
level_start_time_ms = now; step = WAITING_FOR_LEVEL; // set tuning step back from beginning
desired_yaw_cd = ahrs_view->yaw_sensor; step_start_time_ms = now;
level_start_time_ms = now;
desired_yaw_cd = ahrs_view->yaw_sensor;
}
} }
} }
if (pilot_override) { if (pilot_override) {