AP_UAVCAN: add handler for esc.StatusExtended

SW-187
CarbonixUAV · May 27, 2024 · 0d402de · 0d402de
1 parent f82ab94
commit 0d402de
Show file tree

Hide file tree

Showing 2 changed files with 38 additions and 0 deletions.
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp
@@ -35,6 +35,7 @@
 
 #include <uavcan/equipment/esc/RawCommand.hpp>
 #include <uavcan/equipment/esc/Status.hpp>
+#include <uavcan/equipment/esc/StatusExtended.hpp>
 #include <uavcan/equipment/indication/LightsCommand.hpp>
 #include <uavcan/equipment/indication/SingleLightCommand.hpp>
 #include <uavcan/equipment/indication/BeepCommand.hpp>
@@ -204,6 +205,10 @@ static uavcan::Subscriber<uavcan::equipment::device::Temperature, DeviceTemperat
 UC_REGISTRY_BINDER(ESCStatusCb, uavcan::equipment::esc::Status);
 static uavcan::Subscriber<uavcan::equipment::esc::Status, ESCStatusCb> *esc_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
 
+// handler Extended ESC status
+UC_REGISTRY_BINDER(ExtESCStatusCb, uavcan::equipment::esc::StatusExtended);
+static uavcan::Subscriber<uavcan::equipment::esc::StatusExtended, ExtESCStatusCb> *extesc_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
+
 // handler DEBUG
 UC_REGISTRY_BINDER(DebugCb, uavcan::protocol::debug::LogMessage);
 static uavcan::Subscriber<uavcan::protocol::debug::LogMessage, DebugCb> *debug_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS];
@@ -432,6 +437,11 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
         esc_status_listener[driver_index]->start(ESCStatusCb(this, &handle_ESC_status));
     }
 
+    extesc_status_listener[driver_index] = new uavcan::Subscriber<uavcan::equipment::esc::StatusExtended, ExtESCStatusCb>(*_node);
+    if (extesc_status_listener[driver_index]) {
+        extesc_status_listener[driver_index]->start(ExtESCStatusCb(this, &handle_ExtESC_status));
+    }
+
     debug_listener[driver_index] = new uavcan::Subscriber<uavcan::protocol::debug::LogMessage, DebugCb>(*_node);
     if (debug_listener[driver_index]) {
         debug_listener[driver_index]->start(DebugCb(this, &handle_debug));
@@ -1073,6 +1083,32 @@ void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const E
 #endif
 }
 
+/*
+  handle ESC status extended message
+ */
+void AP_UAVCAN::handle_ExtESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ExtESCStatusCb &cb)
+{
+#if HAL_WITH_ESC_TELEM
+    const uint8_t esc_offset = constrain_int16(ap_uavcan->_esc_offset.get(), 0, UAVCAN_SRV_NUMBER);
+    const uint8_t esc_index = cb.msg->esc_index + esc_offset;
+
+    if (!is_esc_data_index_valid(esc_index)) {
+        return;
+    }
+
+    TelemetryData telemetryData {
+        .input_duty = cb.msg->input_pct,
+        .output_duty = cb.msg->output_pct,
+        .flags = (uint8_t)cb.msg->status_flags,
+    };
+
+    ap_uavcan->update_telem_data(esc_index, telemetryData,
+        AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY
+        |AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY
+        |AP_ESC_Telem_Backend::TelemetryType::FLAGS);
+#endif
+}
+
 bool AP_UAVCAN::is_esc_data_index_valid(const uint8_t index) {
     if (index > UAVCAN_SRV_NUMBER) {
         // printf("UAVCAN: invalid esc index: %d. max index allowed: %d\n\r", index, UAVCAN_SRV_NUMBER);

diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h
@@ -51,6 +51,7 @@ class ActuatorStatusCb;
 class PowerCktStatusCb;
 class DeviceTemperatureCb;
 class ESCStatusCb;
+class ExtESCStatusCb;
 class DebugCb;
 class ParamGetSetCb;
 class ParamExecuteOpcodeCb;
@@ -346,6 +347,7 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     static void handle_power_cktstatus(AP_UAVCAN* ap_uavcan, uint8_t node_id, const PowerCktStatusCb &cb);
     static void handle_device_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DeviceTemperatureCb &cb);
     static void handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb);
+    static void handle_ExtESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ExtESCStatusCb &cb);
     static bool is_esc_data_index_valid(const uint8_t index);
     static void handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugCb &cb);
     static void handle_param_get_set_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamGetSetCb &cb);