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

AP_BLHeli: allow all motor frequencies to be returned

Browse Source 
cater for ESCs not returning telemetry all the time
slew rpm updates for the harmonic notch
This commit is contained in:
Andy Piper 2020-05-30 14:07:46 +01:00 committed by Andrew Tridgell
parent cb524b7d3d
commit 07ad4798dc
2 changed files with 30 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_BLHeli/AP_BLHeli.cpp
View File

@ -121,6 +121,8 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = {
AP_GROUPEND
};
#define RPM_SLEW_RATE 50
AP_BLHeli *AP_BLHeli::_singleton;
// constructor
@ -1337,7 +1339,9 @@ float AP_BLHeli::get_average_motor_frequency_hz() const
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;
// slew the update
const float slew = MIN(1.0f, (now - last_telem[i].timestamp_ms) * telem_rate / 1000.0f);
motor_freq += (prev_motor_rpm[i] + (last_telem[i].rpm - prev_motor_rpm[i]) * slew) / 60.0f;
}
}
if (valid_escs > 0) {
@ -1347,6 +1351,23 @@ float AP_BLHeli::get_average_motor_frequency_hz() const
return motor_freq;
}
// return all the motor frequencies in Hz for dynamic filtering
uint8_t AP_BLHeli::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const
{
const 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 < nfreqs; i++) {
if (last_telem[i].timestamp_ms && (now - last_telem[i].timestamp_ms < 1000)) {
// slew the update
const float slew = MIN(1.0f, (now - last_telem[i].timestamp_ms) * telem_rate / 1000.0f);
freqs[valid_escs++] = (prev_motor_rpm[i] + (last_telem[i].rpm - prev_motor_rpm[i]) * slew) / 60.0f;
}
}
return MIN(valid_escs, nfreqs);
}
/*
implement the 8 bit CRC used by the BLHeli ESC telemetry protocol
*/
@ -1391,6 +1412,8 @@ void AP_BLHeli::read_telemetry_packet(void)
td.consumption = (buf[5]<<8) | buf[6];
td.rpm = ((buf[7]<<8) | buf[8]) * 200 / motor_poles;
td.timestamp_ms = AP_HAL::millis();
// record the previous rpm so that we can slew to the new one
prev_motor_rpm[last_telem_esc] = last_telem[last_telem_esc].rpm;
last_telem[last_telem_esc] = td;
last_telem[last_telem_esc].count++;

6
libraries/AP_BLHeli/AP_BLHeli.h
View File

@ -29,6 +29,7 @@
#define HAVE_AP_BLHELI_SUPPORT
#include <AP_Param/AP_Param.h>
#include <Filter/LowPassFilter.h>
#include "msp_protocol.h"
#include "blheli_4way_protocol.h"
@ -59,6 +60,8 @@ public:
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;
// return all of the motor frequencies in Hz for dynamic filtering
uint8_t get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const;
static AP_BLHeli *get_singleton(void) {
return _singleton;
@ -224,6 +227,9 @@ private:
struct telem_data last_telem[max_motors];
// previous motor rpm so that changes can be slewed
float prev_motor_rpm[max_motors];
// have we initialised the interface?
bool initialised;