Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 21:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_BLHeli: add get_average_motor_frequency_hz() for dynamic filtering

Browse Source 
correctly calculate rpm from erpm
This commit is contained in:
Andy Piper 2019-10-12 18:30:05 +01:00 committed by Randy Mackay
parent bf12c686c7
commit 78d9330a4a
2 changed files with 23 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_BLHeli/AP_BLHeli.cpp
View File

@ -1296,6 +1296,26 @@ bool AP_BLHeli::get_telem_data(uint8_t esc_index, struct telem_data &td)
return true;
}
// return the average motor frequency in Hz for dynamic filtering
float AP_BLHeli::get_average_motor_frequency_hz() const
{
float motor_freq = 0.0f;
uint32_t now = AP_HAL::millis();
uint8_t valid_escs = 0;
// average the rpm of each motor as reported by BLHeli and convert to Hz
for (uint8_t i = 0; i < num_motors; i++) {
if (last_telem[i].timestamp_ms && (now - last_telem[i].timestamp_ms < 1000)) {
valid_escs++;
motor_freq += last_telem[i].rpm / 60.0f;
}
}
if (valid_escs) {
motor_freq /= valid_escs;
}
return motor_freq;
}
/*
implement the 8 bit CRC used by the BLHeli ESC telemetry protocol
*/
@ -1342,7 +1362,7 @@ void AP_BLHeli::read_telemetry_packet(void)
td.voltage = (buf[1]<<8) | buf[2];
td.current = (buf[3]<<8) | buf[4];
td.consumption = (buf[5]<<8) | buf[6];
td.rpm = ((buf[7]<<8) | buf[8]) * 100 * 2 / motor_poles;
td.rpm = ((buf[7]<<8) | buf[8]) * 200 / motor_poles;
td.timestamp_ms = AP_HAL::millis();
last_telem[last_telem_esc] = td;

2
libraries/AP_BLHeli/AP_BLHeli.h
View File

@ -57,6 +57,8 @@ public:
// get the most recent telemetry data packet for a motor
bool get_telem_data(uint8_t esc_index, struct telem_data &td);
// return the average motor frequency in Hz for dynamic filtering
float get_average_motor_frequency_hz() const;
static AP_BLHeli *get_singleton(void) {
return _singleton;