mirror of https://github.com/ArduPilot/ardupilot
AP_ESC_Telem: generalise ESC telemetry to allow harmonic notch handling with other ESCs
refactor to capture and output slewed rpm values enable with HAL_WITH_ESC_TELEM move notch calculation to front end refactor telemetry data into frontend cope with blended data add mavlink send function log telemetry data in frontend add SITL ESC telemetry record volts, amps and consumption as floats report telemetry transmission errors disable ESC Telemetry inclusion when there is no need for it move ESC_Telem logging to the AP_ESC_Telem class (by amilcar.lucas@iav.de) various cleanups (by amilcar.lucas@iav.de) add support for raw ESC rpm check RPM validity for mavlink output Use const when applicable
This commit is contained in:
Andy Piper
Andrew Tridgell
parent
2df7b5453e
commit
8df4e0f127
|
|
@ -15,15 +15,14 @@
|
||||||
|
|
||||||
#include "AP_ESC_Telem.h"
|
#include "AP_ESC_Telem.h"
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
#include <GCS_MAVLink/GCS.h>
|
||||||
|
#include <AP_Logger/AP_Logger.h>
|
||||||
|
|
||||||
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
|
#if HAL_WITH_ESC_TELEM
|
||||||
#include <AP_CANManager/AP_CANManager.h>
|
|
||||||
#include <AP_Common/AP_Common.h>
|
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||||
#include <AP_Vehicle/AP_Vehicle.h>
|
|
||||||
#include <AP_UAVCAN/AP_UAVCAN.h>
|
//#define ESC_TELEM_DEBUG
|
||||||
#include <AP_KDECAN/AP_KDECAN.h>
|
|
||||||
#include <AP_ToshibaCAN/AP_ToshibaCAN.h>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
|
|
@ -35,23 +34,329 @@ AP_ESC_Telem::AP_ESC_Telem()
|
||||||
_singleton = this;
|
_singleton = this;
|
||||||
}
|
}
|
||||||
|
|
||||||
// get an individual ESC's usage time in seconds if available, returns true on success
|
// return the average motor frequency in Hz for dynamic filtering
|
||||||
bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_id, uint32_t& usage_sec) const
|
float AP_ESC_Telem::get_average_motor_frequency_hz() const
|
||||||
{
|
{
|
||||||
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
|
float motor_freq = 0.0f;
|
||||||
const uint8_t num_drivers = AP::can().get_num_drivers();
|
uint8_t valid_escs = 0;
|
||||||
for (uint8_t i = 0; i < num_drivers; i++) {
|
|
||||||
if (AP::can().get_driver_type(i) == AP_CANManager::Driver_Type_ToshibaCAN) {
|
// average the rpm of each motor and convert to Hz
|
||||||
AP_ToshibaCAN *tcan = AP_ToshibaCAN::get_tcan(i);
|
for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
|
||||||
if (tcan != nullptr) {
|
float rpm;
|
||||||
usage_sec = tcan->get_usage_seconds(esc_id);
|
if (get_rpm(i, rpm)) {
|
||||||
return true;
|
motor_freq += rpm * (1.0f / 60.0f);
|
||||||
}
|
valid_escs++;
|
||||||
return true;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
if (valid_escs > 0) {
|
||||||
|
motor_freq /= valid_escs;
|
||||||
|
}
|
||||||
|
|
||||||
|
return motor_freq;
|
||||||
|
}
|
||||||
|
|
||||||
|
// return all the motor frequencies in Hz for dynamic filtering
|
||||||
|
uint8_t AP_ESC_Telem::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const
|
||||||
|
{
|
||||||
|
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 < ESC_TELEM_MAX_ESCS && i < nfreqs; i++) {
|
||||||
|
float rpm;
|
||||||
|
if (get_rpm(i, rpm)) {
|
||||||
|
freqs[valid_escs++] = rpm * (1.0f / 60.0f);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return MIN(valid_escs, nfreqs);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get an individual ESC's slewed rpm if available, returns true on success
|
||||||
|
bool AP_ESC_Telem::get_rpm(uint8_t esc_index, float& rpm) const
|
||||||
|
{
|
||||||
|
const volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[esc_index];
|
||||||
|
|
||||||
|
if (esc_index > ESC_TELEM_MAX_ESCS || is_zero(rpmdata.update_rate_hz)) {
|
||||||
return false;
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
const uint32_t now = AP_HAL::micros();
|
||||||
|
if (rpmdata.last_update_us > 0 && (now >= rpmdata.last_update_us)
|
||||||
|
&& (now - rpmdata.last_update_us < ESC_RPM_DATA_TIMEOUT_US)) {
|
||||||
|
const float slew = MIN(1.0f, (now - rpmdata.last_update_us) * rpmdata.update_rate_hz * (1.0f / 1e6f));
|
||||||
|
rpm = (rpmdata.prev_rpm + (rpmdata.rpm - rpmdata.prev_rpm) * slew);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get an individual ESC's raw rpm if available, returns true on success
|
||||||
|
bool AP_ESC_Telem::get_raw_rpm(uint8_t esc_index, float& rpm) const
|
||||||
|
{
|
||||||
|
const volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[esc_index];
|
||||||
|
|
||||||
|
const uint32_t now = AP_HAL::micros();
|
||||||
|
|
||||||
|
if (esc_index >= ESC_TELEM_MAX_ESCS || now < rpmdata.last_update_us
|
||||||
|
|| now - rpmdata.last_update_us > ESC_RPM_DATA_TIMEOUT_US) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
rpm = rpmdata.rpm;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get an individual ESC's temperature in centi-degrees if available, returns true on success
|
||||||
|
bool AP_ESC_Telem::get_temperature(uint8_t esc_index, int16_t& temp) const
|
||||||
|
{
|
||||||
|
if (esc_index >= ESC_TELEM_MAX_ESCS
|
||||||
|
|| AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
|
||||||
|
|| !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
temp = _telem_data[esc_index].temperature_cdeg;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get an individual motor's temperature in centi-degrees if available, returns true on success
|
||||||
|
bool AP_ESC_Telem::get_motor_temperature(uint8_t esc_index, int16_t& temp) const
|
||||||
|
{
|
||||||
|
if (esc_index >= ESC_TELEM_MAX_ESCS
|
||||||
|
|| AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
|
||||||
|
|| !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
temp = _telem_data[esc_index].motor_temp_cdeg;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get an individual ESC's current in Ampere if available, returns true on success
|
||||||
|
bool AP_ESC_Telem::get_current(uint8_t esc_index, float& amps) const
|
||||||
|
{
|
||||||
|
if (esc_index >= ESC_TELEM_MAX_ESCS
|
||||||
|
|| AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
|
||||||
|
|| !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::CURRENT)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
amps = _telem_data[esc_index].current;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get an individual ESC's voltage in Volt if available, returns true on success
|
||||||
|
bool AP_ESC_Telem::get_voltage(uint8_t esc_index, float& volts) const
|
||||||
|
{
|
||||||
|
if (esc_index >= ESC_TELEM_MAX_ESCS
|
||||||
|
|| AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
|
||||||
|
|| !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
volts = _telem_data[esc_index].voltage;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get an individual ESC's energy consumption in milli-Ampere.hour if available, returns true on success
|
||||||
|
bool AP_ESC_Telem::get_consumption_mah(uint8_t esc_index, float& consumption_mah) const
|
||||||
|
{
|
||||||
|
if (esc_index >= ESC_TELEM_MAX_ESCS
|
||||||
|
|| AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
|
||||||
|
|| !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
consumption_mah = _telem_data[esc_index].consumption_mah;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get an individual ESC's usage time in seconds if available, returns true on success
|
||||||
|
bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const
|
||||||
|
{
|
||||||
|
if (esc_index >= ESC_TELEM_MAX_ESCS
|
||||||
|
|| AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS
|
||||||
|
|| !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::USAGE)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
usage_s = _telem_data[esc_index].usage_s;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// send ESC telemetry messages over MAVLink
|
||||||
|
void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan)
|
||||||
|
{
|
||||||
|
static_assert(ESC_TELEM_MAX_ESCS <= 12, "AP_ESC_Telem::send_esc_telemetry_mavlink() only supports up-to 12 motors");
|
||||||
|
|
||||||
|
uint32_t now = AP_HAL::millis();
|
||||||
|
uint32_t now_us = AP_HAL::micros();
|
||||||
|
// loop through 3 groups of 4 ESCs
|
||||||
|
for (uint8_t i = 0; i < 3; i++) {
|
||||||
|
|
||||||
|
// return if no space in output buffer to send mavlink messages
|
||||||
|
if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t)mav_chan, ESC_TELEMETRY_1_TO_4)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
#define ESC_DATA_STALE(idx) \
|
||||||
|
(now - _telem_data[idx].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS \
|
||||||
|
&& now_us - _rpm_data[idx].last_update_us > ESC_RPM_DATA_TIMEOUT_US)
|
||||||
|
|
||||||
|
// skip this group of ESCs if no data to send
|
||||||
|
if (ESC_DATA_STALE(i * 4) && ESC_DATA_STALE(i * 4 + 1) && ESC_DATA_STALE(i * 4 + 2) && ESC_DATA_STALE(i * 4 + 3)) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
// arrays to hold output
|
||||||
|
uint8_t temperature[4] {};
|
||||||
|
uint16_t voltage[4] {};
|
||||||
|
uint16_t current[4] {};
|
||||||
|
uint16_t current_tot[4] {};
|
||||||
|
uint16_t rpm[4] {};
|
||||||
|
uint16_t count[4] {};
|
||||||
|
|
||||||
|
// fill in output arrays
|
||||||
|
for (uint8_t j = 0; j < 4; j++) {
|
||||||
|
const uint8_t esc_id = i * 4 + j;
|
||||||
|
temperature[j] = _telem_data[esc_id].temperature_cdeg / 100;
|
||||||
|
voltage[j] = constrain_float(_telem_data[esc_id].voltage * 100.0f, 0, UINT16_MAX);
|
||||||
|
current[j] = constrain_float(_telem_data[esc_id].current * 100.0f, 0, UINT16_MAX);
|
||||||
|
current_tot[j] = constrain_float(_telem_data[esc_id].consumption_mah, 0, UINT16_MAX);
|
||||||
|
float rpmf = 0.0f;
|
||||||
|
if (get_rpm(esc_id, rpmf)) {
|
||||||
|
rpm[j] = constrain_float(rpmf, 0, UINT16_MAX);
|
||||||
|
} else {
|
||||||
|
rpm[j] = 0;
|
||||||
|
}
|
||||||
|
count[j] = _telem_data[esc_id].count;
|
||||||
|
}
|
||||||
|
|
||||||
|
// send messages
|
||||||
|
switch (i) {
|
||||||
|
case 0:
|
||||||
|
mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t)mav_chan, temperature, voltage, current, current_tot, rpm, count);
|
||||||
|
break;
|
||||||
|
case 1:
|
||||||
|
mavlink_msg_esc_telemetry_5_to_8_send((mavlink_channel_t)mav_chan, temperature, voltage, current, current_tot, rpm, count);
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
mavlink_msg_esc_telemetry_9_to_12_send((mavlink_channel_t)mav_chan, temperature, voltage, current, current_tot, rpm, count);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// record an update to the telemetry data together with timestamp
|
||||||
|
// this should be called by backends when new telemetry values are available
|
||||||
|
void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem_Backend::TelemetryData& new_data, const uint16_t data_mask)
|
||||||
|
{
|
||||||
|
// rpm and telemetry data are not protected by a semaphore even though updated from different threads
|
||||||
|
// all data is per-ESC and only written from the update thread and read by the user thread
|
||||||
|
// each element is a primitive type and the timestamp is only updated at the end, thus a caller
|
||||||
|
// can only get slightly more up-to-date information that perhaps they were expecting or might
|
||||||
|
// read data that has just gone stale - both of these are safe and avoid the overhead of locking
|
||||||
|
|
||||||
|
if (esc_index > ESC_TELEM_MAX_ESCS) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE) {
|
||||||
|
_telem_data[esc_index].temperature_cdeg = new_data.temperature_cdeg;
|
||||||
|
}
|
||||||
|
if (data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE) {
|
||||||
|
_telem_data[esc_index].motor_temp_cdeg = new_data.motor_temp_cdeg;
|
||||||
|
}
|
||||||
|
if (data_mask & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE) {
|
||||||
|
_telem_data[esc_index].voltage = new_data.voltage;
|
||||||
|
}
|
||||||
|
if (data_mask & AP_ESC_Telem_Backend::TelemetryType::CURRENT) {
|
||||||
|
_telem_data[esc_index].current = new_data.current;
|
||||||
|
}
|
||||||
|
if (data_mask & AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION) {
|
||||||
|
_telem_data[esc_index].consumption_mah = new_data.consumption_mah;
|
||||||
|
}
|
||||||
|
if (data_mask & AP_ESC_Telem_Backend::TelemetryType::USAGE) {
|
||||||
|
_telem_data[esc_index].usage_s = new_data.usage_s;
|
||||||
|
}
|
||||||
|
|
||||||
|
_telem_data[esc_index].count++;
|
||||||
|
_telem_data[esc_index].types |= data_mask;
|
||||||
|
_telem_data[esc_index].last_update_ms = AP_HAL::millis();
|
||||||
|
}
|
||||||
|
|
||||||
|
// record an update to the RPM together with timestamp, this allows the notch values to be slewed
|
||||||
|
// this should be called by backends when new telemetry values are available
|
||||||
|
void AP_ESC_Telem::update_rpm(const uint8_t esc_index, const uint16_t new_rpm, const float error_rate)
|
||||||
|
{
|
||||||
|
if (esc_index > ESC_TELEM_MAX_ESCS) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
const uint32_t now = AP_HAL::micros();
|
||||||
|
volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[esc_index];
|
||||||
|
|
||||||
|
rpmdata.prev_rpm = rpmdata.rpm;
|
||||||
|
rpmdata.rpm = new_rpm;
|
||||||
|
if (now > rpmdata.last_update_us) { // cope with wrapping
|
||||||
|
rpmdata.update_rate_hz = 1.0e6f / (now - rpmdata.last_update_us);
|
||||||
|
}
|
||||||
|
rpmdata.last_update_us = now;
|
||||||
|
rpmdata.error_rate = error_rate;
|
||||||
|
|
||||||
|
#ifdef ESC_TELEM_DEBUG
|
||||||
|
hal.console->printf("RPM: rate=%.1fhz, rpm=%d)\n", rpmdata.update_rate_hz, new_rpm);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
// log ESC telemetry at 10Hz
|
||||||
|
void AP_ESC_Telem::update()
|
||||||
|
{
|
||||||
|
AP_Logger *logger = AP_Logger::get_singleton();
|
||||||
|
|
||||||
|
// Push received telemtry data into the logging system
|
||||||
|
if (logger && logger->logging_enabled()) {
|
||||||
|
|
||||||
|
for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
|
||||||
|
if (_telem_data[i].last_update_ms != _last_telem_log_ms[i]
|
||||||
|
|| _rpm_data[i].last_update_us != _last_rpm_log_us[i]) {
|
||||||
|
|
||||||
|
float rpm = 0.0f;
|
||||||
|
get_rpm(i, rpm);
|
||||||
|
|
||||||
|
// Write ESC status messages
|
||||||
|
// id starts from 0
|
||||||
|
// rpm is eRPM (rpm * 100)
|
||||||
|
// voltage is in Volt
|
||||||
|
// current is in Ampere
|
||||||
|
// esc_temp is in centi-degrees Celsius
|
||||||
|
// current_tot is in mili-Ampere hours
|
||||||
|
// motor_temp is in centi-degrees Celsius
|
||||||
|
// error_rate is in percentage
|
||||||
|
const struct log_Esc pkt{
|
||||||
|
LOG_PACKET_HEADER_INIT(uint8_t(LOG_ESC_MSG)),
|
||||||
|
time_us : AP_HAL::micros64(),
|
||||||
|
instance : i,
|
||||||
|
rpm : (int32_t) rpm * 100,
|
||||||
|
voltage : _telem_data[i].voltage,
|
||||||
|
current : _telem_data[i].current,
|
||||||
|
esc_temp : _telem_data[i].temperature_cdeg,
|
||||||
|
current_tot : _telem_data[i].consumption_mah,
|
||||||
|
motor_temp : _telem_data[i].motor_temp_cdeg,
|
||||||
|
error_rate : _rpm_data[i].error_rate
|
||||||
|
};
|
||||||
|
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
||||||
|
_last_telem_log_ms[i] = _telem_data[i].last_update_ms;
|
||||||
|
_last_rpm_log_us[i] = _rpm_data[i].last_update_us;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
AP_ESC_Telem *AP_ESC_Telem::_singleton = nullptr;
|
AP_ESC_Telem *AP_ESC_Telem::_singleton = nullptr;
|
||||||
|
|
@ -72,3 +377,5 @@ AP_ESC_Telem &esc_telem()
|
||||||
}
|
}
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
|
||||||
|
|
@ -1,9 +1,17 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
#include "AP_ESC_Telem_Backend.h"
|
||||||
|
|
||||||
|
#if HAL_WITH_ESC_TELEM
|
||||||
|
|
||||||
|
#define ESC_TELEM_MAX_ESCS 12
|
||||||
|
#define ESC_TELEM_DATA_TIMEOUT_MS 5000UL
|
||||||
|
#define ESC_RPM_DATA_TIMEOUT_US 1000000UL
|
||||||
|
|
||||||
class AP_ESC_Telem {
|
class AP_ESC_Telem {
|
||||||
public:
|
public:
|
||||||
|
friend class AP_ESC_Telem_Backend;
|
||||||
|
|
||||||
AP_ESC_Telem();
|
AP_ESC_Telem();
|
||||||
|
|
||||||
|
|
@ -13,15 +21,70 @@ public:
|
||||||
|
|
||||||
static AP_ESC_Telem *get_singleton();
|
static AP_ESC_Telem *get_singleton();
|
||||||
|
|
||||||
|
// get an individual ESC's slewed rpm if available, returns true on success
|
||||||
|
bool get_rpm(uint8_t esc_index, float& rpm) const;
|
||||||
|
|
||||||
|
// get an individual ESC's raw rpm if available
|
||||||
|
bool get_raw_rpm(uint8_t esc_index, float& rpm) const;
|
||||||
|
|
||||||
|
// get an individual ESC's temperature in centi-degrees if available, returns true on success
|
||||||
|
bool get_temperature(uint8_t esc_index, int16_t& temp) const;
|
||||||
|
|
||||||
|
// get an individual motor's temperature in centi-degrees if available, returns true on success
|
||||||
|
bool get_motor_temperature(uint8_t esc_index, int16_t& temp) const;
|
||||||
|
|
||||||
|
// get an individual ESC's current in Ampere if available, returns true on success
|
||||||
|
bool get_current(uint8_t esc_index, float& amps) const;
|
||||||
|
|
||||||
// get an individual ESC's usage time in seconds if available, returns true on success
|
// get an individual ESC's usage time in seconds if available, returns true on success
|
||||||
bool get_usage_seconds(uint8_t esc_id, uint32_t& usage_sec) const;
|
bool get_usage_seconds(uint8_t esc_index, uint32_t& usage_sec) const;
|
||||||
|
|
||||||
|
// get an individual ESC's voltage in Volt if available, returns true on success
|
||||||
|
bool get_voltage(uint8_t esc_index, float& volts) const;
|
||||||
|
|
||||||
|
// get an individual ESC's consumption in milli-Ampere.hour if available, returns true on success
|
||||||
|
bool get_consumption_mah(uint8_t esc_index, float& consumption_mah) const;
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
|
||||||
|
// get the number of valid ESCs
|
||||||
|
uint8_t get_num_active_escs() 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;
|
||||||
|
return _telem_data[esc_index].last_update_ms;
|
||||||
|
}
|
||||||
|
|
||||||
|
// send telemetry data to mavlink
|
||||||
|
void send_esc_telemetry_mavlink(uint8_t mav_chan);
|
||||||
|
|
||||||
|
// udpate at 10Hz to log telemetry
|
||||||
|
void update();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
// callback to update the rpm in the frontend, should be called by the driver when new data is available
|
||||||
|
void update_rpm(const uint8_t esc_index, const uint16_t new_rpm, const float error_rate);
|
||||||
|
// callback to update the data in the frontend, should be called by the driver when new data is available
|
||||||
|
void update_telem_data(const uint8_t esc_index, const AP_ESC_Telem_Backend::TelemetryData& new_data, const uint16_t data_mask);
|
||||||
|
|
||||||
|
// rpm data
|
||||||
|
volatile AP_ESC_Telem_Backend::RpmData _rpm_data[ESC_TELEM_MAX_ESCS];
|
||||||
|
// telemetry data
|
||||||
|
volatile AP_ESC_Telem_Backend::TelemetryData _telem_data[ESC_TELEM_MAX_ESCS];
|
||||||
|
|
||||||
|
uint32_t _last_telem_log_ms[ESC_TELEM_MAX_ESCS];
|
||||||
|
uint32_t _last_rpm_log_us[ESC_TELEM_MAX_ESCS];
|
||||||
|
|
||||||
static AP_ESC_Telem *_singleton;
|
static AP_ESC_Telem *_singleton;
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
namespace AP {
|
namespace AP {
|
||||||
AP_ESC_Telem &esc_telem();
|
AP_ESC_Telem &esc_telem();
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,43 @@
|
||||||
|
/*
|
||||||
|
This program is free software: you can redistribute it and/or modify
|
||||||
|
it under the terms of the GNU General Public License as published by
|
||||||
|
the Free Software Foundation, either version 3 of the License, or
|
||||||
|
(at your option) any later version.
|
||||||
|
|
||||||
|
This program is distributed in the hope that it will be useful,
|
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
GNU General Public License for more details.
|
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License
|
||||||
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "AP_ESC_Telem_Backend.h"
|
||||||
|
#include "AP_ESC_Telem.h"
|
||||||
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
|
#if HAL_WITH_ESC_TELEM
|
||||||
|
|
||||||
|
#include <AP_Math/AP_Math.h>
|
||||||
|
|
||||||
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
|
AP_ESC_Telem_Backend::AP_ESC_Telem_Backend() {
|
||||||
|
_frontend = AP_ESC_Telem::_singleton;
|
||||||
|
if (_frontend == nullptr) {
|
||||||
|
AP_HAL::panic("No ESC frontend");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// callback to update the rpm in the frontend, should be called by the driver when new data is available
|
||||||
|
void AP_ESC_Telem_Backend::update_rpm(const uint8_t esc_index, const uint16_t new_rpm, const float error_rate) {
|
||||||
|
_frontend->update_rpm(esc_index, new_rpm, error_rate);
|
||||||
|
}
|
||||||
|
|
||||||
|
// callback to update the data in the frontend, should be called by the driver when new data is available
|
||||||
|
void AP_ESC_Telem_Backend::update_telem_data(const uint8_t esc_index, const TelemetryData& new_data, const uint16_t data_present_mask) {
|
||||||
|
_frontend->update_telem_data(esc_index, new_data, data_present_mask);
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,63 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
|
#ifndef HAL_WITH_ESC_TELEM
|
||||||
|
#define HAL_WITH_ESC_TELEM HAL_SUPPORT_RCOUT_SERIAL || HAL_MAX_CAN_PROTOCOL_DRIVERS
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if HAL_WITH_ESC_TELEM
|
||||||
|
|
||||||
|
class AP_ESC_Telem;
|
||||||
|
|
||||||
|
class AP_ESC_Telem_Backend {
|
||||||
|
public:
|
||||||
|
|
||||||
|
struct TelemetryData {
|
||||||
|
int16_t temperature_cdeg; // centi-degrees C, negative values allowed
|
||||||
|
float voltage; // Volt
|
||||||
|
float current; // Ampere
|
||||||
|
float consumption_mah; // milli-Ampere.hours
|
||||||
|
uint32_t usage_s; // usage seconds
|
||||||
|
int16_t motor_temp_cdeg; // centi-degrees C, negative values allowed
|
||||||
|
uint32_t last_update_ms; // last update time in miliseconds, determines whether active
|
||||||
|
uint16_t types; // telemetry types present
|
||||||
|
uint16_t count; // number of times updated
|
||||||
|
};
|
||||||
|
|
||||||
|
struct RpmData {
|
||||||
|
float rpm; // rpm
|
||||||
|
float prev_rpm; // previous rpm
|
||||||
|
float error_rate; // error rate in percent
|
||||||
|
uint32_t last_update_us; // last update time, determines whether active
|
||||||
|
float update_rate_hz;
|
||||||
|
};
|
||||||
|
|
||||||
|
enum TelemetryType {
|
||||||
|
TEMPERATURE = 1 << 0,
|
||||||
|
MOTOR_TEMPERATURE = 1 << 1,
|
||||||
|
VOLTAGE = 1 << 2,
|
||||||
|
CURRENT = 1 << 3,
|
||||||
|
CONSUMPTION = 1 << 4,
|
||||||
|
USAGE = 1 << 5
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
AP_ESC_Telem_Backend();
|
||||||
|
|
||||||
|
/* Do not allow copies */
|
||||||
|
AP_ESC_Telem_Backend(const AP_ESC_Telem_Backend &other) = delete;
|
||||||
|
AP_ESC_Telem_Backend &operator=(const AP_ESC_Telem_Backend&) = delete;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
// callback to update the rpm in the frontend, should be called by the driver when new data is available
|
||||||
|
void update_rpm(const uint8_t esc_index, const uint16_t new_rpm, const float error_rate = 0.0f);
|
||||||
|
|
||||||
|
// callback to update the data in the frontend, should be called by the driver when new data is available
|
||||||
|
void update_telem_data(const uint8_t esc_index, const TelemetryData& new_data, const uint16_t data_present_mask);
|
||||||
|
|
||||||
|
private:
|
||||||
|
AP_ESC_Telem* _frontend;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,48 @@
|
||||||
|
/*
|
||||||
|
This program is free software: you can redistribute it and/or modify
|
||||||
|
it under the terms of the GNU General Public License as published by
|
||||||
|
the Free Software Foundation, either version 3 of the License, or
|
||||||
|
(at your option) any later version.
|
||||||
|
|
||||||
|
This program is distributed in the hope that it will be useful,
|
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
GNU General Public License for more details.
|
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License
|
||||||
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "AP_ESC_Telem_SITL.h"
|
||||||
|
#include "AP_ESC_Telem.h"
|
||||||
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
#include <SITL/SITL.h>
|
||||||
|
|
||||||
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
|
|
||||||
|
#include <AP_Math/AP_Math.h>
|
||||||
|
|
||||||
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
|
AP_ESC_Telem_SITL::AP_ESC_Telem_SITL()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void AP_ESC_Telem_SITL::update()
|
||||||
|
{
|
||||||
|
SITL::SITL* sitl = AP::sitl();
|
||||||
|
|
||||||
|
if (!sitl) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (is_zero(sitl->throttle)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
|
||||||
|
update_rpm(i, sitl->state.rpm[sitl->state.vtol_motor_start+i]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,20 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
#include "AP_ESC_Telem_Backend.h"
|
||||||
|
#include <SITL/SITL.h>
|
||||||
|
|
||||||
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
|
|
||||||
|
class AP_ESC_Telem_SITL : public AP_ESC_Telem_Backend {
|
||||||
|
public:
|
||||||
|
AP_ESC_Telem_SITL();
|
||||||
|
|
||||||
|
void update();
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
private:
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,34 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <AP_Logger/LogStructure.h>
|
||||||
|
|
||||||
|
#define LOG_IDS_FROM_ESC_TELEM \
|
||||||
|
LOG_ESC_MSG
|
||||||
|
|
||||||
|
// @LoggerMessage: ESC
|
||||||
|
// @Description: Feedback received from ESCs
|
||||||
|
// @Field: TimeUS: microseconds since system startup
|
||||||
|
// @Field: Instance: ESC instance number
|
||||||
|
// @Field: RPM: reported motor rotation rate
|
||||||
|
// @Field: Volt: Perceived input voltage for the ESC
|
||||||
|
// @Field: Curr: Perceived current through the ESC
|
||||||
|
// @Field: Temp: ESC temperature in centi-degrees C
|
||||||
|
// @Field: CTot: current consumed total mAh
|
||||||
|
// @Field: MotTemp: measured motor temperature in centi-degrees C
|
||||||
|
// @Field: Err: error rate
|
||||||
|
struct PACKED log_Esc {
|
||||||
|
LOG_PACKET_HEADER;
|
||||||
|
uint64_t time_us;
|
||||||
|
uint8_t instance;
|
||||||
|
int32_t rpm;
|
||||||
|
float voltage;
|
||||||
|
float current;
|
||||||
|
int16_t esc_temp;
|
||||||
|
float current_tot;
|
||||||
|
int16_t motor_temp;
|
||||||
|
float error_rate;
|
||||||
|
};
|
||||||
|
|
||||||
|
#define LOG_STRUCTURE_FROM_ESC_TELEM \
|
||||||
|
{ LOG_ESC_MSG, sizeof(log_Esc), \
|
||||||
|
"ESC", "QBeffcfcf", "TimeUS,Instance,RPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qvAOaO%", "F-B--BCB-" },
|
||||||
Loading…
Reference in New Issue