diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index a1fdd777fd..0c30c38c26 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -74,16 +74,28 @@ uint8_t AP_ESC_Telem::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) con return MIN(valid_escs, nfreqs); } +// get mask of ESCs that sent valid telemetry data in the last +// ESC_TELEM_DATA_TIMEOUT_MS +uint16_t AP_ESC_Telem::get_active_esc_mask() const { + uint16_t ret = 0; + const uint32_t now = AP_HAL::millis(); + for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { + if (now - _telem_data[i].last_update_ms >= ESC_TELEM_DATA_TIMEOUT_MS) { + continue; + } + if (_telem_data[i].last_update_ms == 0) { + // have never seen telem from this ESC + continue; + } + ret |= (1U << i); + } + return ret; +} + // return number of active ESCs present uint8_t AP_ESC_Telem::get_num_active_escs() const { - uint8_t nmotors = 0; - uint32_t now = AP_HAL::millis(); - for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { - if (now - _telem_data[i].last_update_ms < ESC_TELEM_DATA_TIMEOUT_MS) { - nmotors++; - } - } - return nmotors; + uint16_t active = get_active_esc_mask(); + return __builtin_popcount(active); } // get an individual ESC's slewed rpm if available, returns true on success diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index b427031a0a..04726c1d0a 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -63,9 +63,13 @@ public: // return all of the motor frequencies in Hz for dynamic filtering uint8_t get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const; - // get the number of valid ESCs + // get the number of ESCs that sent valid telemetry data in the last ESC_TELEM_DATA_TIMEOUT_MS uint8_t get_num_active_escs() const; + // get mask of ESCs that sent valid telemetry data in the last + // ESC_TELEM_DATA_TIMEOUT_MS + uint16_t get_active_esc_mask() const; + // return the last time telemetry data was received in ms for the given ESC or 0 if never uint32_t get_last_telem_data_ms(uint8_t esc_index) const { if (esc_index >= ESC_TELEM_MAX_ESCS) {return 0;}