AP_UAVCAN: add esc telemetry updates

remove send_esc_telemetry_mavlink()
remove datastructures and semaphore obsoleted by AP_ESC_Telem* (<amilcar.lucas@iav.de>)
record volts, amps and consumption as floats

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -137,9 +137,6 @@ static uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb> *esc_stat
 UC_REGISTRY_BINDER(DebugCb, uavcan::protocol::debug::LogMessage);
 static uavcan::Subscriber<uavcan::protocol::debug::LogMessage, DebugCb> *debug_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
 
-AP_UAVCAN::esc_data AP_UAVCAN::_escs_data[];
-HAL_Semaphore AP_UAVCAN::_telem_sem;
-
 
 AP_UAVCAN::AP_UAVCAN() :
     _node_allocator()
@@ -349,79 +346,6 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
 }
 #pragma GCC diagnostic pop
 
-// send ESC telemetry messages over MAVLink
-void AP_UAVCAN::send_esc_telemetry_mavlink(uint8_t mav_chan)
-{
-#ifndef HAL_NO_GCS
-    static const uint8_t MAV_ESC_GROUPS = 3;
-    static const uint8_t MAV_ESC_PER_GROUP = 4;
-
-    for (uint8_t i = 0; i < MAV_ESC_GROUPS; i++) {
-
-        // arrays to hold output
-        uint8_t temperature[MAV_ESC_PER_GROUP] {};
-        uint16_t voltage[MAV_ESC_PER_GROUP] {};
-        uint16_t current[MAV_ESC_PER_GROUP] {};
-        uint16_t current_tot[MAV_ESC_PER_GROUP] {};
-        uint16_t rpm[MAV_ESC_PER_GROUP] {};
-        uint16_t count[MAV_ESC_PER_GROUP] {};
-
-        // if at least one of the ESCs in the group is availabe, the group
-        // is considered to be available too, and will be sent over MAVlink
-        bool group_available = false;
-
-        // fill in output arrays of ESCs sensors with available data.
-        for (uint8_t j = 0; j < MAV_ESC_PER_GROUP; j++) {
-            const uint8_t esc_idx = i * MAV_ESC_PER_GROUP + j;
-            
-            if (!is_esc_data_index_valid(esc_idx)) {
-                return;
-            }
-
-            WITH_SEMAPHORE(_telem_sem);
-            
-            if (!_escs_data[esc_idx].available) {
-                continue;
-            } 
-
-            _escs_data[esc_idx].available = false;
-
-            temperature[j] = _escs_data[esc_idx].temp;
-            voltage[j] = _escs_data[esc_idx].voltage;
-            current[j] = _escs_data[esc_idx].current;
-            current_tot[j] = 0; // currently not implemented
-            rpm[j] = _escs_data[esc_idx].rpm;
-            count[j] = _escs_data[esc_idx].count;
-
-            group_available = true;
-        }
-
-        if (!group_available) {
-            continue;
-        }
-
-        if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t) mav_chan, ESC_TELEMETRY_1_TO_4)) {
-            return;
-        }
-
-        // 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;
-        }
-    }
-#endif // HAL_NO_GCS
-}
-
 void AP_UAVCAN::loop(void)
 {
     while (true) {
@@ -867,30 +791,21 @@ void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const E
 {
     const uint8_t esc_index = cb.msg->esc_index;
 
-    // log as CESC message
-    AP::logger().Write_ESCStatus(AP_HAL::native_micros64(),
-                                 cb.msg->esc_index,
-                                 cb.msg->error_count,
-                                 cb.msg->voltage,
-                                 cb.msg->current,
-                                 cb.msg->temperature - C_TO_KELVIN,
-                                 cb.msg->rpm,
-                                 cb.msg->power_rating_pct);
-
-    WITH_SEMAPHORE(_telem_sem);
-
     if (!is_esc_data_index_valid(esc_index)) {
         return;
     }
 
-    esc_data &esc = _escs_data[esc_index];
-    esc.available = true;
-    esc.temp = (cb.msg->temperature - C_TO_KELVIN);
-    esc.voltage = cb.msg->voltage*100;
-    esc.current = cb.msg->current*100;
-    esc.rpm = cb.msg->rpm;
-    esc.count++;
+    TelemetryData t {
+        .temperature_cdeg = int16_t((cb.msg->temperature - C_TO_KELVIN) * 100),
+        .voltage = cb.msg->voltage,
+        .current = cb.msg->current,
+    };
 
+    ap_uavcan->update_rpm(esc_index, cb.msg->rpm);
+    ap_uavcan->update_telem_data(esc_index, t,
+        AP_ESC_Telem_Backend::TelemetryType::CURRENT
+            | AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
+            | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
 }
 
 bool AP_UAVCAN::is_esc_data_index_valid(const uint8_t index) {

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -27,6 +27,7 @@
 #include <AP_CANManager/AP_CANDriver.h>
 #include <AP_HAL/Semaphores.h>
 #include <AP_Param/AP_Param.h>
+#include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
 
 #include <uavcan/helpers/heap_based_pool_allocator.hpp>
 
@@ -71,7 +72,7 @@ class DebugCb;
 
 /*
     Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
-    the Callback will invoke registery to register the node as separate backend.
+    the Callback will invoke registry to register the node as separate backend.
 */
 #define UC_REGISTRY_BINDER(ClassName_, DataType_) \
     class ClassName_ : public AP_UAVCAN::RegistryBinder<DataType_> { \
@@ -84,7 +85,7 @@ class DebugCb;
             DISABLE_W_CAST_FUNCTION_TYPE_POP \
     }
 
-class AP_UAVCAN : public AP_CANDriver {
+class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
 public:
     AP_UAVCAN();
     ~AP_UAVCAN();
@@ -97,9 +98,6 @@ public:
     void init(uint8_t driver_index, bool enable_filters) override;
     bool add_interface(AP_HAL::CANIface* can_iface) override;
     
-    // send ESC telemetry messages over MAVLink
-    void send_esc_telemetry_mavlink(uint8_t mav_chan);
-    
     uavcan::Node<0>* get_node() { return _node; }
     uint8_t get_driver_index() const { return _driver_index; }
 
@@ -125,7 +123,7 @@ public:
 
     public:
         RegistryBinder() :
-        	_uc(),
+            _uc(),
             _ffunc(),
             msg() {}
 
@@ -226,19 +224,6 @@ private:
 
     static HAL_Semaphore _telem_sem;
 
-    struct esc_data {
-        uint8_t temp;
-        uint16_t voltage;
-        uint16_t current;
-        uint16_t total_current;
-        uint16_t rpm;
-        uint16_t count; //count of telemetry packets received (wraps at 65535).
-        bool available;
-    };
-
-    static esc_data _escs_data[UAVCAN_SRV_NUMBER];
-
-    
     // safety status send state
     uint32_t _last_safety_state_ms;