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

AC_Autotune: Add level time out.

Browse Source
This commit is contained in:
Leonard Hall 2018-12-21 00:27:30 +10:30 committed by Andrew Tridgell
parent db9ce0064b
commit b0f3674eee
2 changed files with 15 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -46,7 +46,8 @@
#define AUTOTUNE_LEVEL_ANGLE_CD 500 // angle which qualifies as level
#define AUTOTUNE_LEVEL_RATE_RP_CD 1000 // rate which qualifies as level for roll and pitch
#define AUTOTUNE_LEVEL_RATE_Y_CD 750 // rate which qualifies as level for yaw
#define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 500 // time we require the copter to be level
#define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 500 // time we require the aircraft to be level
#define AUTOTUNE_LEVEL_TIMEOUT_MS 2000 // time out for level
#define AUTOTUNE_RD_STEP 0.05f // minimum increment when increasing/decreasing Rate D term
#define AUTOTUNE_RP_STEP 0.05f // minimum increment when increasing/decreasing Rate P term
#define AUTOTUNE_SP_STEP 0.05f // minimum increment when increasing/decreasing Stab P term
@ -485,15 +486,15 @@ void AC_AutoTune::control_attitude()
// hold the copter level for 0.5 seconds before we begin a twitch
// reset counter if we are no longer level
if (!currently_level()) {
step_start_time = now;
step_start_time_ms = now;
}
// if we have been level for a sufficient amount of time (0.5 seconds) move onto tuning step
if (now - step_start_time >= AUTOTUNE_REQUIRED_LEVEL_TIME_MS) {
if ((AUTOTUNE_REQUIRED_LEVEL_TIME_MS < now - step_start_time_ms) || (AUTOTUNE_LEVEL_TIMEOUT_MS < now - step_time_limit_ms)) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Twitch");
// initiate variables for next step
step = TWITCHING;
step_start_time = now;
step_start_time_ms = now;
step_time_limit_ms = AUTOTUNE_TESTING_STEP_TIMEOUT_MS;
twitch_first_iter = true;
test_rate_max = 0.0f;
@ -860,7 +861,8 @@ void AC_AutoTune::control_attitude()
// reset testing step
step = WAITING_FOR_LEVEL;
step_start_time = now;
step_start_time_ms = now;
step_time_limit_ms = now;
break;
}
}
@ -882,7 +884,7 @@ void AC_AutoTune::backup_gains_and_initialise()
positive_direction = false;
step = WAITING_FOR_LEVEL;
step_start_time = AP_HAL::millis();
step_start_time_ms = AP_HAL::millis();
tune_type = RD_UP;
desired_yaw_cd = ahrs_view->yaw_sensor;
@ -1210,7 +1212,7 @@ void AC_AutoTune::twitching_test_rate(float rate, float rate_target_max, float &
// calculate early stopping time based on the time it takes to get to 75%
if (meas_rate_max < rate_target_max * 0.75f) {
// the measurement not reached the 75% threshold yet
step_time_limit_ms = (now - step_start_time) * 3.0f;
step_time_limit_ms = (now - step_start_time_ms) * 3.0f;
step_time_limit_ms = MIN(step_time_limit_ms, AUTOTUNE_TESTING_STEP_TIMEOUT_MS);
}
@ -1224,7 +1226,7 @@ void AC_AutoTune::twitching_test_rate(float rate, float rate_target_max, float &
step = UPDATE_GAINS;
}
if (now - step_start_time >= step_time_limit_ms) {
if (now - step_start_time_ms >= step_time_limit_ms) {
// we have passed the maximum stop time
step = UPDATE_GAINS;
}
@ -1265,7 +1267,7 @@ void AC_AutoTune::twitching_test_angle(float angle, float rate, float angle_targ
// calculate early stopping time based on the time it takes to get to 75%
if (meas_angle_max < angle_target_max * 0.75f) {
// the measurement not reached the 75% threshold yet
step_time_limit_ms = (now - step_start_time) * 3.0f;
step_time_limit_ms = (now - step_start_time_ms) * 3.0f;
step_time_limit_ms = MIN(step_time_limit_ms, AUTOTUNE_TESTING_STEP_TIMEOUT_MS);
}
@ -1279,7 +1281,7 @@ void AC_AutoTune::twitching_test_angle(float angle, float rate, float angle_targ
step = UPDATE_GAINS;
}
if (now - step_start_time >= step_time_limit_ms) {
if (now - step_start_time_ms >= step_time_limit_ms) {
// we have passed the maximum stop time
step = UPDATE_GAINS;
}
@ -1290,7 +1292,7 @@ void AC_AutoTune::twitching_measure_acceleration(float &rate_of_change, float ra
{
if (rate_measurement_max < rate_measurement) {
rate_measurement_max = rate_measurement;
rate_of_change = (1000.0f*rate_measurement_max)/(AP_HAL::millis() - step_start_time);
rate_of_change = (1000.0f*rate_measurement_max)/(AP_HAL::millis() - step_start_time_ms);
}
}

4
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -172,8 +172,8 @@ private:
float test_rate_max; // the maximum angular rate achieved during TESTING_RATE step
float test_angle_min; // the minimum angle achieved during TESTING_ANGLE step
float test_angle_max; // the maximum angle achieved during TESTING_ANGLE step
uint32_t step_start_time; // start time of current tuning step (used for timeout checks)
uint32_t step_time_limit_ms; // time limit of current tuning step
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
int8_t counter; // counter for tuning gains
float target_rate, start_rate; // target and start rate
float target_angle, start_angle; // target and start angles