From 4407b1ada0a5459d540fb2db14891e510eb51d4f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 29 Nov 2019 09:46:02 +0900 Subject: [PATCH] AP_NMEA_Output: 10hz rate limiting uses uint32_t --- libraries/AP_NMEA_Output/AP_NMEA_Output.cpp | 6 +++--- libraries/AP_NMEA_Output/AP_NMEA_Output.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp b/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp index 55c4379702..2cf816c7ee 100644 --- a/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp +++ b/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp @@ -67,13 +67,13 @@ uint8_t AP_NMEA_Output::_nmea_checksum(const char *str) void AP_NMEA_Output::update() { - const uint16_t now = AP_HAL::millis16(); + const uint32_t now_ms = AP_HAL::millis(); // only send at 10Hz - if (uint16_t(now - _last_run) < 100) { + if ((now_ms - _last_run_ms) < 100) { return; } - _last_run = now; + _last_run_ms = now_ms; // get time and date uint64_t time_usec; diff --git a/libraries/AP_NMEA_Output/AP_NMEA_Output.h b/libraries/AP_NMEA_Output/AP_NMEA_Output.h index e53ec9f4c3..afd0192e29 100644 --- a/libraries/AP_NMEA_Output/AP_NMEA_Output.h +++ b/libraries/AP_NMEA_Output/AP_NMEA_Output.h @@ -47,7 +47,7 @@ private: uint8_t _num_outputs; AP_HAL::UARTDriver* _uart[SERIALMANAGER_NUM_PORTS]; - uint16_t _last_run; + uint32_t _last_run_ms; }; #endif // !HAL_MINIMIZE_FEATURES && AP_AHRS_NAVEKF_AVAILABLE