From 5282dba42a70b229f277b40fbce0ff21faff9ab6 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 6 Sep 2022 16:45:58 -0700 Subject: [PATCH] GCS_MAVLink: Rate limit blocking accel calibrations This allows us to drop any queued commands that may have arrived while we were calibrating. Rather then entering a second and unexpected calibration. --- libraries/GCS_MAVLink/GCS.h | 2 ++ libraries/GCS_MAVLink/GCS_Common.cpp | 14 ++++++++++++++ 2 files changed, 16 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 886d1f72a4..21dbca0de7 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -659,6 +659,8 @@ private: void log_mavlink_stats(); + uint32_t last_accel_cal_ms; // used to rate limit accel cals for bad links + MAV_RESULT _set_mode_common(const MAV_MODE base_mode, const uint32_t custom_mode); // send a (textual) message to the GCS that a received message has diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index cfe2bec419..814dfd403e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4130,21 +4130,35 @@ MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_comm #endif #if HAL_INS_ENABLED + const uint32_t now = AP_HAL::millis(); if (is_equal(packet.param5,2.0f)) { + // reject any time we've done a calibration recently + if ((now - last_accel_cal_ms) < 5000) { + return MAV_RESULT_TEMPORARILY_REJECTED; + } + if (!calibrate_gyros()) { + last_accel_cal_ms = AP_HAL::millis(); return MAV_RESULT_FAILED; } Vector3f trim_rad = AP::ahrs().get_trim(); if (!AP::ins().calibrate_trim(trim_rad)) { + last_accel_cal_ms = AP_HAL::millis(); return MAV_RESULT_FAILED; } // reset ahrs's trim to suggested values from calibration routine AP::ahrs().set_trim(trim_rad); + last_accel_cal_ms = AP_HAL::millis(); return MAV_RESULT_ACCEPTED; } if (is_equal(packet.param5,4.0f)) { + if ((now - last_accel_cal_ms) < 5000) { + return MAV_RESULT_TEMPORARILY_REJECTED; + } + // simple accel calibration + last_accel_cal_ms = AP_HAL::millis(); return AP::ins().simple_accel_cal(); }