Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_ESC_Telem: implement get_active_esc_mask() 

Co-authored-by: Peter Barker <pb-gh@barker.dropbear.id.au>
This commit is contained in:
Dr.-Ing. Amilcar do Carmo Lucas 2021-06-22 19:26:49 +02:00 committed by Peter Barker
parent 2251e442cc
commit 72a69ec274
2 changed files with 25 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_ESC_Telem/AP_ESC_Telem.cpp
View File

@ -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

6
libraries/AP_ESC_Telem/AP_ESC_Telem.h
View File

@ -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;}