From 412cd4f19aa2f56196fb0f7596981f2a3a8b1792 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 19 Jan 2023 10:07:20 +1100 Subject: [PATCH] AP_InertialSensor: cleanup NAMED_VALUE_FLOAT for fifo error --- .../AP_InertialSensor_Invensense.cpp | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index b3a69e2908..0f6e5e3070 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -458,14 +458,11 @@ bool AP_InertialSensor_Invensense::update() /* front end */ // check if we have reported in the last 1 seconds or // fast_reset_count changed #if HAL_GCS_ENABLED && BOARD_FLASH_SIZE > 1024 - if (AP_HAL::millis() - last_fast_reset_count_report_ms > 1000) { - last_fast_reset_count_report_ms = AP_HAL::millis(); - char param_name[sizeof("IMUx_RST")]; - if (_gyro_instance <= 9) { - snprintf(param_name, sizeof(param_name), "IMU%u_RST", _gyro_instance); - } else { - snprintf(param_name, sizeof(param_name), "IMUx_RST"); - } + const uint32_t now = AP_HAL::millis(); + if (now - last_fast_reset_count_report_ms > 5000U) { + last_fast_reset_count_report_ms = now; + char param_name[sizeof("IMUxx_RST")]; + snprintf(param_name, sizeof(param_name), "IMU%u_RST", MIN(_gyro_instance,9)); gcs().send_named_float(param_name, fast_reset_count); } #endif