AP_ESC_Telem: prevent possible overrun

This commit is contained in:
Pierre Kancir 2021-08-11 00:14:19 +02:00 committed by Randy Mackay
parent 2c62d32ca8
commit aad2733d06
1 changed files with 10 additions and 3 deletions

View File

@ -86,9 +86,13 @@ uint8_t AP_ESC_Telem::get_num_active_escs() const {
// get an individual ESC's slewed rpm if available, returns true on success // get an individual ESC's slewed rpm if available, returns true on success
bool AP_ESC_Telem::get_rpm(uint8_t esc_index, float& rpm) const bool AP_ESC_Telem::get_rpm(uint8_t esc_index, float& rpm) const
{ {
if (esc_index >= ESC_TELEM_MAX_ESCS) {
return false;
}
const volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[esc_index]; const volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[esc_index];
if (esc_index >= ESC_TELEM_MAX_ESCS || is_zero(rpmdata.update_rate_hz)) { if (is_zero(rpmdata.update_rate_hz)) {
return false; return false;
} }
@ -105,12 +109,15 @@ bool AP_ESC_Telem::get_rpm(uint8_t esc_index, float& rpm) const
// get an individual ESC's raw rpm if available, returns true on success // get an individual ESC's raw rpm if available, returns true on success
bool AP_ESC_Telem::get_raw_rpm(uint8_t esc_index, float& rpm) const bool AP_ESC_Telem::get_raw_rpm(uint8_t esc_index, float& rpm) const
{ {
if (esc_index >= ESC_TELEM_MAX_ESCS) {
return false;
}
const volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[esc_index]; const volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[esc_index];
const uint32_t now = AP_HAL::micros(); const uint32_t now = AP_HAL::micros();
if (esc_index >= ESC_TELEM_MAX_ESCS || now < rpmdata.last_update_us if (now < rpmdata.last_update_us || now - rpmdata.last_update_us > ESC_RPM_DATA_TIMEOUT_US) {
|| now - rpmdata.last_update_us > ESC_RPM_DATA_TIMEOUT_US) {
return false; return false;
} }