Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: Rate limit blocking accel calibrations

Browse Source 
This allows us to drop any queued commands that may have arrived while
we were calibrating. Rather then entering a second and unexpected
calibration.
This commit is contained in:
Michael du Breuil 2022-09-06 16:45:58 -07:00 committed by Peter Barker
parent dd89d68107
commit 5282dba42a
2 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/GCS_MAVLink/GCS.h
View File

@ -659,6 +659,8 @@ private:
void log_mavlink_stats(); 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); 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 // send a (textual) message to the GCS that a received message has

14
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -4130,21 +4130,35 @@ MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_comm
#endif #endif
#if HAL_INS_ENABLED #if HAL_INS_ENABLED
const uint32_t now = AP_HAL::millis();
if (is_equal(packet.param5,2.0f)) { 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()) { if (!calibrate_gyros()) {
last_accel_cal_ms = AP_HAL::millis();
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} }
Vector3f trim_rad = AP::ahrs().get_trim(); Vector3f trim_rad = AP::ahrs().get_trim();
if (!AP::ins().calibrate_trim(trim_rad)) { if (!AP::ins().calibrate_trim(trim_rad)) {
last_accel_cal_ms = AP_HAL::millis();
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
} }
// reset ahrs's trim to suggested values from calibration routine // reset ahrs's trim to suggested values from calibration routine
AP::ahrs().set_trim(trim_rad); AP::ahrs().set_trim(trim_rad);
last_accel_cal_ms = AP_HAL::millis();
return MAV_RESULT_ACCEPTED; return MAV_RESULT_ACCEPTED;
} }
if (is_equal(packet.param5,4.0f)) { if (is_equal(packet.param5,4.0f)) {
if ((now - last_accel_cal_ms) < 5000) {
return MAV_RESULT_TEMPORARILY_REJECTED;
}
// simple accel calibration // simple accel calibration
last_accel_cal_ms = AP_HAL::millis();
return AP::ins().simple_accel_cal(); return AP::ins().simple_accel_cal();
} }