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

libraries/AP_ESC_Telem/AP_ESC_Telem.cpp

@@ -15,15 +15,14 @@
 
 #include "AP_ESC_Telem.h"
 #include <AP_HAL/AP_HAL.h>
+#include <GCS_MAVLink/GCS.h>
+#include <AP_Logger/AP_Logger.h>
 
-#if HAL_MAX_CAN_PROTOCOL_DRIVERS
-  #include <AP_CANManager/AP_CANManager.h>
-  #include <AP_Common/AP_Common.h>
-  #include <AP_Vehicle/AP_Vehicle.h>
-  #include <AP_UAVCAN/AP_UAVCAN.h>
-  #include <AP_KDECAN/AP_KDECAN.h>
-  #include <AP_ToshibaCAN/AP_ToshibaCAN.h>
-#endif
+#if HAL_WITH_ESC_TELEM
+
+#include <AP_BoardConfig/AP_BoardConfig.h>
+
+//#define ESC_TELEM_DEBUG
 
 extern const AP_HAL::HAL& hal;
 
@@ -35,23 +34,329 @@ AP_ESC_Telem::AP_ESC_Telem()
     _singleton = this;
 }
 
+// return the average motor frequency in Hz for dynamic filtering
+float AP_ESC_Telem::get_average_motor_frequency_hz() const
+{
+    float motor_freq = 0.0f;
+    uint8_t valid_escs = 0;
+
+    // average the rpm of each motor and convert to Hz
+    for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
+        float rpm;
+        if (get_rpm(i, rpm)) {
+            motor_freq += rpm * (1.0f / 60.0f);
+            valid_escs++;
+        }
+    }
+    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;
+    }
+
+    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_id, uint32_t& usage_sec) const
-{
-#if HAL_MAX_CAN_PROTOCOL_DRIVERS
-    const uint8_t num_drivers = AP::can().get_num_drivers();
-    for (uint8_t i = 0; i < num_drivers; i++) {
-        if (AP::can().get_driver_type(i) == AP_CANManager::Driver_Type_ToshibaCAN) {
-            AP_ToshibaCAN *tcan = AP_ToshibaCAN::get_tcan(i);
-            if (tcan != nullptr) {
-                usage_sec = tcan->get_usage_seconds(esc_id);
-                return true;
+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;
             }
-            return true;
+            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
-    return false;
+}
+
+// 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;
@@ -72,3 +377,5 @@ AP_ESC_Telem &esc_telem()
 }
 
 };
+
+#endif

libraries/AP_ESC_Telem/AP_ESC_Telem.h

@@ -1,9 +1,17 @@
 #pragma once
 
 #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 {
 public:
+    friend class AP_ESC_Telem_Backend;
 
     AP_ESC_Telem();
 
@@ -13,15 +21,70 @@ public:
 
     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
-    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:
+    // 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);
 
-    static AP_ESC_Telem *_singleton;
+    // 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;
 };
 
 namespace AP {
     AP_ESC_Telem &esc_telem();
 };
+
+#endif

libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp

@@ -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

libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h

@@ -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

libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp

@@ -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

libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.h

@@ -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

libraries/AP_ESC_Telem/LogStructure.h

@@ -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-" },