AP_DroneCAN: ESC_STATUS_EXTENDED_HANDLER

ArduPilot · Jul 30, 2024 · 244fce6 · 244fce6
1 parent 23ae8e2
commit 244fce6
Show file tree

Hide file tree

Showing 2 changed files with 39 additions and 0 deletions.
diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp
@@ -1457,6 +1457,40 @@ void AP_DroneCAN::handle_ESC_status(const CanardRxTransfer& transfer, const uavc
 #endif
 }
 
+
+/*
+  handle ESC Extended status message
+ */
+void AP_DroneCAN::handle_ESC_extended_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg)
+{
+#if HAL_WITH_ESC_TELEM
+    const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER);
+    const uint8_t esc_index = msg.esc_index + esc_offset;
+
+    if (!is_esc_data_index_valid(esc_index)) {
+        return;
+    }
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - last_log_escx_ms < 100) {
+        return;
+    }
+    last_log_escx_ms = now_ms;
+    AP::logger().WriteStreaming("ESCX",
+                                "TimeUS,I,IThr,OThr,mTemp,mAng,stat",
+                                "s#%%O--",
+                                "F------",
+                                "QBBBhHI",
+                                AP_HAL::micros64(),
+                                esc_index,
+                                msg.input_pct,
+                                msg.output_pct,
+                                msg.motor_temperature_degC,
+                                msg.motor_angle,
+                                msg.status_flags);
+
+#endif
+}
+
 bool AP_DroneCAN::is_esc_data_index_valid(const uint8_t index) {
     if (index > DRONECAN_SRV_NUMBER) {
         // printf("DroneCAN: invalid esc index: %d. max index allowed: %d\n\r", index, DRONECAN_SRV_NUMBER);

diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h
@@ -255,6 +255,7 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
 
     // last log time
     uint32_t last_log_ms;
+    uint32_t last_log_escx_ms;
 
 #if AP_DRONECAN_SEND_GPS
     // send GNSS Fix and yaw, same thing AP_GPS_DroneCAN would receive
@@ -326,6 +327,9 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_esc_Status> esc_status_cb{this, &AP_DroneCAN::handle_ESC_status};
     Canard::Subscriber<uavcan_equipment_esc_Status> esc_status_listener{esc_status_cb, _driver_index};
 
+    Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_esc_StatusExtended> esc_status_extended_cb{this, &AP_DroneCAN::handle_ESC_extended_status};
+    Canard::Subscriber<uavcan_equipment_esc_StatusExtended> esc_status_extended_listener{esc_status_extended_cb, _driver_index};
+
     Canard::ObjCallback<AP_DroneCAN, uavcan_protocol_debug_LogMessage> debug_cb{this, &AP_DroneCAN::handle_debug};
     Canard::Subscriber<uavcan_protocol_debug_LogMessage> debug_listener{debug_cb, _driver_index};
 
@@ -387,6 +391,7 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     void handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg);
     void handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg);
     void handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg);
+    void handle_ESC_extended_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg);
     static bool is_esc_data_index_valid(const uint8_t index);
     void handle_debug(const CanardRxTransfer& transfer, const uavcan_protocol_debug_LogMessage& msg);
     void handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp);