diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index de5e4d3d72..a1fdd777fd 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -89,9 +89,13 @@ uint8_t AP_ESC_Telem::get_num_active_escs() const { // 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 { + if (esc_index >= ESC_TELEM_MAX_ESCS) { + return false; + } + 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; } @@ -108,12 +112,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 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 uint32_t now = AP_HAL::micros(); - if (esc_index >= ESC_TELEM_MAX_ESCS || now < rpmdata.last_update_us - || now - rpmdata.last_update_us > ESC_RPM_DATA_TIMEOUT_US) { + if (now < rpmdata.last_update_us || now - rpmdata.last_update_us > ESC_RPM_DATA_TIMEOUT_US) { return false; }