zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
zr-v5.1
Andy Piper 4 years ago committed by Andrew Tridgell
parent
d2f78c69be
commit
6d50549476
2 changed files with 33 additions and 208 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 197
      libraries/AP_BLHeli/AP_BLHeli.cpp
  2. 44
      libraries/AP_BLHeli/AP_BLHeli.h

197
libraries/AP_BLHeli/AP_BLHeli.cpp
Unescape View File

@ -32,6 +32,7 @@ @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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

44
libraries/AP_BLHeli/AP_BLHeli.h
Unescape View File

@ -27,6 +27,11 @@ @@ -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 @@ @@ -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: @@ -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: @@ -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: @@ -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;

Write Preview
Loading…
Cancel
Save