From 69e076605e7391cca576581d7ebcdd6343915e08 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Mon, 1 Jan 2024 20:55:03 +0000
Subject: [PATCH] AP_DroneCAN: support streaming relay hardpoint command

---
 libraries/AP_DroneCAN/AP_DroneCAN.cpp | 52 +++++++++++++++++++++++++++
 libraries/AP_DroneCAN/AP_DroneCAN.h   | 16 +++++++++
 2 files changed, 68 insertions(+)

diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp
index 9f34e7f93b..cc6242e8da 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp
+++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp
@@ -55,6 +55,10 @@
 #include "AP_DroneCAN_serial.h"
 #endif
 
+#if AP_RELAY_DRONECAN_ENABLED
+#include <AP_Relay/AP_Relay.h>
+#endif
+
 extern const AP_HAL::HAL& hal;
 
 // setup default pool size
@@ -153,6 +157,16 @@ const AP_Param::GroupInfo AP_DroneCAN::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("ESC_RV", 9, AP_DroneCAN, _esc_rv, 0),
 
+#if AP_RELAY_DRONECAN_ENABLED
+    // @Param: RLY_RT
+    // @DisplayName: DroneCAN relay output rate
+    // @Description: Maximum transmit rate for relay outputs, note that this rate is per message each message does 1 relay, so if with more relays will take longer to update at the same rate, a extra message will be sent when a relay changes state
+    // @Range: 0 200
+    // @Units: Hz
+    // @User: Advanced
+    AP_GROUPINFO("RLY_RT", 23, AP_DroneCAN, _relay.rate_hz, 0),
+#endif
+
 #if AP_DRONECAN_SERIAL_ENABLED
     /*
       due to the parameter tree depth limitation we can't use a sub-table for the serial parameters
@@ -252,6 +266,8 @@ const AP_Param::GroupInfo AP_DroneCAN::var_info[] = {
 #endif
 #endif // AP_DRONECAN_SERIAL_ENABLED
 
+    // RLY_RT is index 23 but has to be above SER_EN so its not hidden
+
     AP_GROUPEND
 };
 
@@ -434,6 +450,11 @@ void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters)
     xacti_gnss_status.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
 #endif
 
+#if AP_RELAY_DRONECAN_ENABLED
+    relay_hardpoint.set_timeout_ms(20);
+    relay_hardpoint.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
+#endif
+
     param_save_client.set_timeout_ms(20);
     param_save_client.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
 
@@ -532,6 +553,10 @@ void AP_DroneCAN::loop(void)
 #if AP_DRONECAN_SERIAL_ENABLED
         serial.update();
 #endif
+
+#if AP_RELAY_DRONECAN_ENABLED
+        relay_hardpoint_send();
+#endif
     }
 }
 
@@ -1212,6 +1237,33 @@ void AP_DroneCAN::safety_state_send()
     }
 }
 
+// Send relay outputs with hardpoint msg
+#if AP_RELAY_DRONECAN_ENABLED
+void AP_DroneCAN::relay_hardpoint_send()
+{
+    const uint32_t now = AP_HAL::millis();
+    if ((_relay.rate_hz == 0) || ((now - _relay.last_send_ms) < uint32_t(1000 / _relay.rate_hz))) {
+        // Rate limit per user config
+        return;
+    }
+    _relay.last_send_ms = now;
+
+    AP_Relay *relay = AP::relay();
+    if (relay == nullptr) {
+        return;
+    }
+
+    uavcan_equipment_hardpoint_Command msg {};
+
+    // Relay will populate the next command to send and update the last index
+    // This will cycle through each relay in turn
+    if (relay->dronecan.populate_next_command(_relay.last_index, msg)) {
+        relay_hardpoint.broadcast(msg);
+    }
+
+}
+#endif // AP_RELAY_DRONECAN_ENABLED
+
 /*
   handle Button message
  */
diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h
index 61f0528bc9..8b37c2528b 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN.h
+++ b/libraries/AP_DroneCAN/AP_DroneCAN.h
@@ -35,6 +35,7 @@
 #include <canard.h>
 #include <dronecan_msgs.h>
 #include <AP_SerialManager/AP_SerialManager_config.h>
+#include <AP_Relay/AP_Relay_config.h>
 
 #ifndef DRONECAN_SRV_NUMBER
 #define DRONECAN_SRV_NUMBER NUM_SERVO_CHANNELS
@@ -165,6 +166,12 @@ public:
     Canard::Publisher<com_xacti_GimbalControlData> xacti_gimbal_control_data{canard_iface};
     Canard::Publisher<com_xacti_GnssStatus> xacti_gnss_status{canard_iface};
 
+#if AP_RELAY_DRONECAN_ENABLED
+    // Hardpoint for relay
+    // Needs to be public so relay can edge trigger as well as streaming
+    Canard::Publisher<uavcan_equipment_hardpoint_Command> relay_hardpoint{canard_iface};
+#endif
+
 private:
     void loop(void);
 
@@ -272,6 +279,15 @@ private:
     uint32_t _last_notify_state_ms;
     uavcan_protocol_NodeStatus node_status_msg;
 
+#if AP_RELAY_DRONECAN_ENABLED
+    void relay_hardpoint_send();
+    struct {
+        AP_Int16 rate_hz;
+        uint32_t last_send_ms;
+        uint8_t last_index;
+    } _relay;
+#endif
+
     CanardInterface canard_iface;
 
 #if AP_DRONECAN_SERIAL_ENABLED