Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AutoTune: Add check for reverse response abort.

This commit is contained in:
Leonard Hall 2019-11-30 14:40:15 +10:30 committed by Andrew Tridgell
parent 1226291189
commit c6f0294b98
1 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -644,9 +644,6 @@ void AC_AutoTune::control_attitude()
twitching_test_rate(rotation_rate, target_rate, test_rate_min, test_rate_max); twitching_test_rate(rotation_rate, target_rate, test_rate_min, test_rate_max);
twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max); twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max);
twitching_abort_rate(lean_angle, rotation_rate, abort_angle, test_rate_min); twitching_abort_rate(lean_angle, rotation_rate, abort_angle, test_rate_min);
if (lean_angle >= target_angle) {
step = UPDATE_GAINS;
}
break; break;
case RP_UP: case RP_UP:
twitching_test_rate(rotation_rate, target_rate*(1+0.5f*aggressiveness), test_rate_min, test_rate_max); twitching_test_rate(rotation_rate, target_rate*(1+0.5f*aggressiveness), test_rate_min, test_rate_max);
@ -660,6 +657,11 @@ void AC_AutoTune::control_attitude()
break; break;
} }
// Check for failure causing reverse response
if (lean_angle <= -AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD) {
step = WAITING_FOR_LEVEL;
}
// log this iterations lean angle and rotation rate // log this iterations lean angle and rotation rate
Log_Write_AutoTuneDetails(lean_angle, rotation_rate); Log_Write_AutoTuneDetails(lean_angle, rotation_rate);
AP::logger().Write_Rate(ahrs_view, *motors, *attitude_control, *pos_control); AP::logger().Write_Rate(ahrs_view, *motors, *attitude_control, *pos_control);
@ -779,7 +781,7 @@ void AC_AutoTune::control_attitude()
counter = 0; counter = 0;
// reset scaling factor // reset scaling factor
step_scaler = 1; step_scaler = 1.0f;
// move to the next tuning type // move to the next tuning type
switch (tune_type) { switch (tune_type) {
@ -911,7 +913,7 @@ void AC_AutoTune::backup_gains_and_initialise()
step_start_time_ms = AP_HAL::millis(); step_start_time_ms = AP_HAL::millis();
level_start_time_ms = step_start_time_ms; level_start_time_ms = step_start_time_ms;
tune_type = RD_UP; tune_type = RD_UP;
step_scaler = 1; step_scaler = 1.0f;
desired_yaw_cd = ahrs_view->yaw_sensor; desired_yaw_cd = ahrs_view->yaw_sensor;
@ -1306,7 +1308,7 @@ void AC_AutoTune::twitching_test_rate(float rate, float rate_target_max, float &
void AC_AutoTune::twitching_abort_rate(float angle, float rate, float angle_max, float meas_rate_min) void AC_AutoTune::twitching_abort_rate(float angle, float rate, float angle_max, float meas_rate_min)
{ {
if (angle >= angle_max) { if (angle >= angle_max) {
if (is_equal(rate, meas_rate_min) && step_scaler > 0.5) { if (is_equal(rate, meas_rate_min) && step_scaler > 0.5f) {
// 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
// reduce the maximum target rate // reduce the maximum target rate
step_scaler *= 0.9f; step_scaler *= 0.9f;