mirror of https://github.com/ArduPilot/ardupilot
AP_BLHeli: generalise ESC telemetry to allow harmonic notch handling with other ESCs
move part of frequency calculation into AP_ESC_Telem_Backend factor out duplicate code into AP_ESC_Telem_Backend remove send_esc_telemetry_mavlink() log telemetry data in frontend remove datastructures and API obsoleted by AP_ESC_Telem* (<amilcar.lucas@iav.de>) record volts, amps and consumption as floats fix wrong motor channel in telemtry update
This commit is contained in:
Andy Piper
Andrew Tridgell
parent
d2f78c69be
commit
6d50549476
|
|
@ -32,6 +32,7 @@
|
|||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||
#include <AP_ESC_Telem/AP_ESC_Telem.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
|
@ -1352,6 +1353,7 @@ void AP_BLHeli::update(void)
|
|||
#ifdef HAL_WITH_BIDIR_DSHOT
|
||||
// possibly enable bi-directional dshot
|
||||
hal.rcout->set_bidir_dshot_mask(channel_bidir_dshot_mask.get() & mask);
|
||||
hal.rcout->set_motor_poles(motor_poles);
|
||||
#endif
|
||||
// add motors from channel mask
|
||||
for (uint8_t i=0; i<16 && num_motors < max_motors; i++) {
|
||||
|
|
@ -1375,70 +1377,6 @@ void AP_BLHeli::update(void)
|
|||
|
||||
}
|
||||
|
||||
// get the most recent telemetry data packet for a motor
|
||||
bool AP_BLHeli::get_telem_data(uint8_t esc_index, struct telem_data &td)
|
||||
{
|
||||
if (esc_index >= max_motors) {
|
||||
return false;
|
||||
}
|
||||
if (last_telem[esc_index].timestamp_ms == 0) {
|
||||
return false;
|
||||
}
|
||||
td = last_telem[esc_index];
|
||||
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;
|
||||
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++) {
|
||||
if (has_bidir_dshot(i)) {
|
||||
uint16_t erpm = hal.rcout->get_erpm(motor_map[i]);
|
||||
if (erpm > 0 && erpm < 0xFFFF) {
|
||||
valid_escs++;
|
||||
motor_freq += (erpm * 200 / motor_poles) / 60.f;
|
||||
}
|
||||
} else if (last_telem[i].timestamp_ms && (now - last_telem[i].timestamp_ms < 1000)) {
|
||||
valid_escs++;
|
||||
// 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) {
|
||||
motor_freq /= valid_escs;
|
||||
}
|
||||
|
||||
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 (has_bidir_dshot(i)) {
|
||||
uint16_t erpm = hal.rcout->get_erpm(motor_map[i]);
|
||||
if (erpm > 0 && erpm < 0xFFFF) {
|
||||
freqs[valid_escs++] = (erpm * 200 / motor_poles) / 60.f;
|
||||
}
|
||||
} else 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
|
||||
*/
|
||||
|
|
@ -1476,68 +1414,41 @@ void AP_BLHeli::read_telemetry_packet(void)
|
|||
debug("Bad CRC on %u", last_telem_esc);
|
||||
return;
|
||||
}
|
||||
struct telem_data td;
|
||||
td.temperature = int8_t(buf[0]);
|
||||
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]) * 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++;
|
||||
received_telem_data = true;
|
||||
|
||||
AP_Logger *logger = AP_Logger::get_singleton();
|
||||
|
||||
uint16_t trpm = td.rpm;
|
||||
float terr = 0.0f;
|
||||
|
||||
uint16_t new_rpm = ((buf[7]<<8) | buf[8]) * 200 / motor_poles;
|
||||
const uint8_t motor_idx = motor_map[last_telem_esc];
|
||||
// we have received valid data, mark the ESC as now active
|
||||
hal.rcout->set_active_escs_mask(1<<motor_idx);
|
||||
update_rpm(motor_idx, new_rpm);
|
||||
|
||||
if (logger && logger->logging_enabled()
|
||||
// log at 10Hz
|
||||
&& td.timestamp_ms - last_log_ms[last_telem_esc] > 100) {
|
||||
TelemetryData t {
|
||||
.temperature_cdeg = int16_t(buf[0] * 100),
|
||||
.voltage = float(uint16_t((buf[1]<<8) | buf[2])) * 0.01,
|
||||
.current = float(uint16_t((buf[3]<<8) | buf[4])) * 0.01,
|
||||
.consumption_mah = float(uint16_t((buf[5]<<8) | buf[6])),
|
||||
};
|
||||
|
||||
if (has_bidir_dshot(last_telem_esc)) {
|
||||
const uint16_t erpm = hal.rcout->get_erpm(motor_idx);
|
||||
if (erpm != 0xFFFF) { // don't log invalid values
|
||||
trpm = erpm * 200 / motor_poles;
|
||||
}
|
||||
terr = hal.rcout->get_erpm_error_rate(motor_idx);
|
||||
}
|
||||
|
||||
logger->Write_ESC(uint8_t(last_telem_esc),
|
||||
AP_HAL::micros64(),
|
||||
trpm*100U,
|
||||
td.voltage,
|
||||
td.current,
|
||||
td.temperature * 100U,
|
||||
td.consumption,
|
||||
0,
|
||||
terr);
|
||||
last_log_ms[last_telem_esc] = td.timestamp_ms;
|
||||
}
|
||||
update_telem_data(motor_idx, t,
|
||||
AP_ESC_Telem_Backend::TelemetryType::CURRENT
|
||||
| AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
|
||||
| AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION
|
||||
| AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
|
||||
|
||||
if (debug_level >= 2) {
|
||||
uint16_t trpm = new_rpm;
|
||||
if (has_bidir_dshot(last_telem_esc)) {
|
||||
trpm = hal.rcout->get_erpm(motor_idx);
|
||||
if (trpm != 0xFFFF) {
|
||||
trpm = trpm * 200 / motor_poles;
|
||||
}
|
||||
terr = hal.rcout->get_erpm_error_rate(motor_idx);
|
||||
}
|
||||
hal.console->printf("ESC[%u] T=%u V=%u C=%u con=%u RPM=%u e=%.1f t=%u\n",
|
||||
hal.console->printf("ESC[%u] T=%u V=%f C=%f con=%f RPM=%u e=%.1f t=%u\n",
|
||||
last_telem_esc,
|
||||
td.temperature,
|
||||
td.voltage,
|
||||
td.current,
|
||||
td.consumption,
|
||||
trpm, terr, (unsigned)AP_HAL::millis());
|
||||
t.temperature_cdeg,
|
||||
t.voltage,
|
||||
t.current,
|
||||
t.consumption_mah,
|
||||
trpm, hal.rcout->get_erpm_error_rate(motor_idx), (unsigned)AP_HAL::millis());
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1546,32 +1457,19 @@ void AP_BLHeli::read_telemetry_packet(void)
|
|||
*/
|
||||
void AP_BLHeli::log_bidir_telemetry(void)
|
||||
{
|
||||
AP_Logger *logger = AP_Logger::get_singleton();
|
||||
|
||||
uint32_t now = AP_HAL::millis();
|
||||
|
||||
if (logger && logger->logging_enabled()
|
||||
// log at 10Hz
|
||||
&& now - last_log_ms[last_telem_esc] > 100) {
|
||||
|
||||
if (debug_level >= 2 && now - last_log_ms[last_telem_esc] > 100) {
|
||||
if (has_bidir_dshot(last_telem_esc)) {
|
||||
const uint8_t motor_idx = motor_map[last_telem_esc];
|
||||
uint16_t trpm = hal.rcout->get_erpm(motor_idx);
|
||||
const float terr = hal.rcout->get_erpm_error_rate(motor_idx);
|
||||
if (trpm != 0xFFFF) { // don't log invalid values as they are never used
|
||||
trpm = trpm * 200 / motor_poles;
|
||||
|
||||
logger->Write_ESC(uint8_t(last_telem_esc),
|
||||
AP_HAL::micros64(),
|
||||
trpm*100U,
|
||||
0, 0, 0, 0,
|
||||
terr);
|
||||
last_log_ms[last_telem_esc] = now;
|
||||
}
|
||||
|
||||
if (debug_level >= 2) {
|
||||
hal.console->printf("ESC[%u] RPM=%u e=%.1f t=%u\n", last_telem_esc, trpm, terr, (unsigned)AP_HAL::millis());
|
||||
}
|
||||
last_log_ms[last_telem_esc] = now;
|
||||
hal.console->printf("ESC[%u] RPM=%u e=%.1f t=%u\n", last_telem_esc, trpm, terr, (unsigned)AP_HAL::millis());
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1642,49 +1540,4 @@ void AP_BLHeli::update_telemetry(void)
|
|||
}
|
||||
}
|
||||
|
||||
/*
|
||||
send ESC telemetry messages over MAVLink
|
||||
*/
|
||||
void AP_BLHeli::send_esc_telemetry_mavlink(uint8_t mav_chan)
|
||||
{
|
||||
if (num_motors == 0) {
|
||||
return;
|
||||
}
|
||||
uint8_t temperature[4] {};
|
||||
uint16_t voltage[4] {};
|
||||
uint16_t current[4] {};
|
||||
uint16_t totalcurrent[4] {};
|
||||
uint16_t rpm[4] {};
|
||||
uint16_t count[4] {};
|
||||
uint32_t now = AP_HAL::millis();
|
||||
for (uint8_t i=0; i<num_motors; i++) {
|
||||
uint8_t idx = i % 4;
|
||||
if (last_telem[i].timestamp_ms && (now - last_telem[i].timestamp_ms < 1000)) {
|
||||
temperature[idx] = last_telem[i].temperature;
|
||||
voltage[idx] = last_telem[i].voltage;
|
||||
current[idx] = last_telem[i].current;
|
||||
totalcurrent[idx] = last_telem[i].consumption;
|
||||
rpm[idx] = last_telem[i].rpm;
|
||||
count[idx] = last_telem[i].count;
|
||||
} else {
|
||||
temperature[idx] = 0;
|
||||
voltage[idx] = 0;
|
||||
current[idx] = 0;
|
||||
totalcurrent[idx] = 0;
|
||||
rpm[idx] = 0;
|
||||
count[idx] = 0;
|
||||
}
|
||||
if (i % 4 == 3 || i == num_motors - 1) {
|
||||
if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t)mav_chan, ESC_TELEMETRY_1_TO_4)) {
|
||||
return;
|
||||
}
|
||||
if (i < 4) {
|
||||
mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t)mav_chan, temperature, voltage, current, totalcurrent, rpm, count);
|
||||
} else {
|
||||
mavlink_msg_esc_telemetry_5_to_8_send((mavlink_channel_t)mav_chan, temperature, voltage, current, totalcurrent, rpm, count);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif // HAVE_AP_BLHELI_SUPPORT
|
||||
|
|
|
|||
|
|
@ -27,6 +27,11 @@
|
|||
#if HAL_SUPPORT_RCOUT_SERIAL
|
||||
|
||||
#define HAVE_AP_BLHELI_SUPPORT
|
||||
#ifndef HAL_WITH_ESC_TELEM
|
||||
#define HAL_WITH_ESC_TELEM TRUE
|
||||
#endif
|
||||
|
||||
#include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
|
||||
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <Filter/LowPassFilter.h>
|
||||
|
|
@ -35,7 +40,7 @@
|
|||
|
||||
#define AP_BLHELI_MAX_ESCS 8
|
||||
|
||||
class AP_BLHeli {
|
||||
class AP_BLHeli : public AP_ESC_Telem_Backend {
|
||||
|
||||
public:
|
||||
AP_BLHeli();
|
||||
|
|
@ -46,30 +51,6 @@ public:
|
|||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
struct telem_data {
|
||||
int8_t temperature; // degrees C, negative values allowed
|
||||
uint16_t voltage; // volts * 100
|
||||
uint16_t current; // amps * 100
|
||||
uint16_t consumption;// mAh
|
||||
uint16_t rpm; // eRPM
|
||||
uint16_t count;
|
||||
uint32_t timestamp_ms;
|
||||
};
|
||||
|
||||
// number of ESCs configured as BLHeli in channel mask
|
||||
uint8_t get_num_motors(void) { return num_motors;};
|
||||
// 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;
|
||||
// return all of the motor frequencies in Hz for dynamic filtering
|
||||
uint8_t get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const;
|
||||
|
||||
// return true if we have received any telemetry data
|
||||
bool have_telem_data(void) const {
|
||||
return received_telem_data;
|
||||
}
|
||||
|
||||
bool has_bidir_dshot(uint8_t esc_index) const {
|
||||
return channel_bidir_dshot_mask.get() & (1U << motor_map[esc_index]);
|
||||
}
|
||||
|
|
@ -79,9 +60,6 @@ public:
|
|||
static AP_BLHeli *get_singleton(void) {
|
||||
return _singleton;
|
||||
}
|
||||
|
||||
// send ESC telemetry messages over MAVLink
|
||||
void send_esc_telemetry_mavlink(uint8_t mav_chan);
|
||||
|
||||
private:
|
||||
static AP_BLHeli *_singleton;
|
||||
|
|
@ -236,20 +214,14 @@ private:
|
|||
|
||||
AP_HAL::UARTDriver *uart;
|
||||
AP_HAL::UARTDriver *debug_uart;
|
||||
AP_HAL::UARTDriver *telem_uart;
|
||||
|
||||
AP_HAL::UARTDriver *telem_uart;
|
||||
|
||||
static const uint8_t max_motors = AP_BLHELI_MAX_ESCS;
|
||||
uint8_t num_motors;
|
||||
|
||||
struct telem_data last_telem[max_motors];
|
||||
uint32_t received_telem_data;
|
||||
|
||||
// last log output to avoid beat frequencies
|
||||
uint32_t last_log_ms[max_motors];
|
||||
|
||||
// previous motor rpm so that changes can be slewed
|
||||
float prev_motor_rpm[max_motors];
|
||||
|
||||
// have we initialised the interface?
|
||||
bool initialised;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue