aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AutoTune: add failure to level warning

Browse Source
This commit is contained in:
Randy Mackay 2021-02-01 11:41:27 +09:00
parent 4032ac9884
commit 25beab4c82
2 changed files with 12 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -52,7 +52,8 @@
#endif
#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 aircraft to be level
#define AUTOTUNE_LEVEL_TIMEOUT_MS 2000 // time out for level
#define AUTOTUNE_LEVEL_TIMEOUT_MS 2000 // time out for level (relaxes criteria)
#define AUTOTUNE_LEVEL_WARNING_INTERVAL_MS 5000 // level failure warning messages sent at this interval to users
#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
@ -439,12 +440,20 @@ bool AC_AutoTune::currently_level()
{
float threshold_mul = 1.0;
if (AP_HAL::millis() - level_start_time_ms > AUTOTUNE_LEVEL_TIMEOUT_MS) {
uint32_t now_ms = AP_HAL::millis();
if (now_ms - level_start_time_ms > AUTOTUNE_LEVEL_TIMEOUT_MS) {
// after a long wait we use looser threshold, to allow tuning
// with poor initial gains
threshold_mul *= 2;
}
// display warning if vehicle fails to level
if ((now_ms - level_start_time_ms > AUTOTUNE_LEVEL_WARNING_INTERVAL_MS) &&
(now_ms - level_fail_warning_time_ms > AUTOTUNE_LEVEL_WARNING_INTERVAL_MS)) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "AutoTune: failing to level, manual tune may be required");
level_fail_warning_time_ms = now_ms;
}
if (!check_level(LevelIssue::ANGLE_ROLL,
fabsf(ahrs_view->roll_sensor - roll_cd),
threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD)) {

1
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -179,6 +179,7 @@ private:
float test_angle_max; // the maximum angle achieved during TESTING_ANGLE step
uint32_t step_start_time_ms; // start time of current tuning step (used for timeout checks)
uint32_t level_start_time_ms; // start time of waiting for level
uint32_t level_fail_warning_time_ms; // last time level failure warning message was sent to GCS
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