AP_Relay: support virtual DroneCAN pins using hardpoint msg

libraries/AP_Relay/AP_Relay.cpp

@@ -24,6 +24,11 @@
 #include <AR_Motors/AP_MotorsUGV.h>
 #endif
 
+#if AP_RELAY_DRONECAN_ENABLED
+#include <AP_DroneCAN/AP_DroneCAN.h>
+#include <AP_CANManager/AP_CANManager.h>
+#endif
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
   #define RELAY1_PIN_DEFAULT 13
 
@@ -327,7 +332,7 @@ void AP_Relay::init()
 
     // setup the actual default values of all the pins
     for (uint8_t instance = 0; instance < ARRAY_SIZE(_params); instance++) {
-        const int8_t pin = _params[instance].pin;
+        const int16_t pin = _params[instance].pin;
         if (pin == -1) {
             // no valid pin to set it on, skip it
             continue;
@@ -352,6 +357,12 @@ void AP_Relay::init()
             // this will need revisiting when we support inversion
             set_pin_by_instance(instance, false);
         }
+
+        // Make sure any DroneCAN pin is enabled for streaming
+#if AP_RELAY_DRONECAN_ENABLED
+        dronecan.enable_pin(pin);
+#endif
+
     }
 }
 
@@ -374,7 +385,7 @@ void AP_Relay::set(const AP_Relay_Params::FUNCTION function, const bool value) {
 // this is an internal helper, instance must have already been validated to be in range
 void AP_Relay::set_pin_by_instance(uint8_t instance, bool value)
 {
-    const int8_t pin = _params[instance].pin;
+    const int16_t pin = _params[instance].pin;
     if (pin == -1) {
         // no valid pin to set it on, skip it
         return;
@@ -386,11 +397,10 @@ void AP_Relay::set_pin_by_instance(uint8_t instance, bool value)
     }
 #endif
 
-    hal.gpio->pinMode(pin, HAL_GPIO_OUTPUT);
-    const bool initial_value = (bool)hal.gpio->read(pin);
+    const bool initial_value = get_pin(pin);
 
     if (initial_value != value) {
-        hal.gpio->write(pin, value);
+        set_pin(pin, value);
         AP::logger().Write("RELY", "TimeUS,Instance,State", "s#-", "F--", "QBB",
                             AP_HAL::micros64(),
                             instance,
@@ -433,7 +443,14 @@ bool AP_Relay::arming_checks(size_t buflen, char *buffer) const
             // User should set function to none to disable
             continue;
         }
-        if (!hal.gpio->valid_pin(pin)) {
+
+#if AP_RELAY_DRONECAN_ENABLED
+        const bool DroneCAN_pin = dronecan.valid_pin(pin);
+#else
+        const bool DroneCAN_pin = false;
+#endif
+
+        if (!DroneCAN_pin && !hal.gpio->valid_pin(pin)) {
             // Check GPIO pin is valid
             char param_name_buf[14];
             hal.util->snprintf(param_name_buf, ARRAY_SIZE(param_name_buf), "RELAY%u_PIN", unsigned(i+1));
@@ -456,16 +473,50 @@ bool AP_Relay::get(uint8_t instance) const
         return false;
     }
 
-    const int8_t pin = _params[instance].pin.get();
+    return get_pin(_params[instance].pin.get());
+}
 
+// Get relay state from pin number
+bool AP_Relay::get_pin(const int16_t pin) const
+{
     if (pin < 0) {
         // invalid pin
         return false;
     }
 
+#if AP_RELAY_DRONECAN_ENABLED
+    if (dronecan.valid_pin(pin)) {
+        // Virtual DroneCAN pin
+        return dronecan.get_pin(pin);
+    }
+#endif
+
+    // Real GPIO pin
+    hal.gpio->pinMode(pin, HAL_GPIO_OUTPUT);
     return (bool)hal.gpio->read(pin);
 }
 
+// Set relay state from pin number
+void AP_Relay::set_pin(const int16_t pin, const bool value)
+{
+    if (pin < 0) {
+        // invalid pin
+        return;
+    }
+
+#if AP_RELAY_DRONECAN_ENABLED
+    if (dronecan.valid_pin(pin)) {
+        // Virtual DroneCAN pin
+        dronecan.set_pin(pin, value);
+        return;
+    }
+#endif
+
+    // Real GPIO pin
+    hal.gpio->pinMode(pin, HAL_GPIO_OUTPUT);
+    hal.gpio->write(pin, value);
+}
+
 // see if the relay is enabled
 bool AP_Relay::enabled(uint8_t instance) const 
 {
@@ -484,6 +535,92 @@ bool AP_Relay::enabled(AP_Relay_Params::FUNCTION function) const
     return false;
 }
 
+#if AP_RELAY_DRONECAN_ENABLED
+// Return true if pin number is a virtual DroneCAN pin
+bool AP_Relay::DroneCAN::valid_pin(int16_t pin) const
+{
+    switch(pin) {
+        case (int16_t)AP_Relay_Params::VIRTUAL_PINS::DroneCAN_0 ... (int16_t)AP_Relay_Params::VIRTUAL_PINS::DroneCAN_15:
+            return true;
+        default:
+            return false;
+    }
+}
+
+// Enable streaming of pin number
+void AP_Relay::DroneCAN::enable_pin(int16_t pin)
+{
+    if (!valid_pin(pin)) {
+        return;
+    }
+
+    const uint8_t index = hardpoint_index(pin);
+    state[index].enabled = true;
+}
+
+// Get the hardpoint index of given pin number
+uint8_t AP_Relay::DroneCAN::hardpoint_index(const int16_t pin) const
+{
+    return pin - (int16_t)AP_Relay_Params::VIRTUAL_PINS::DroneCAN_0;
+}
+
+// Set DroneCAN relay state from pin number
+void AP_Relay::DroneCAN::set_pin(const int16_t pin, const bool value)
+{
+    const uint8_t index = hardpoint_index(pin);
+
+    // Set pin and ensure enabled for streaming
+    state[index].enabled = true;
+    state[index].value = value;
+
+    // Broadcast msg on all channels
+    // Just a single send, rely on streaming to fill in any lost packet
+
+    uavcan_equipment_hardpoint_Command msg {};
+    msg.hardpoint_id = index;
+    msg.command = state[index].value;
+
+    uint8_t can_num_drivers = AP::can().get_num_drivers();
+    for (uint8_t i = 0; i < can_num_drivers; i++) {
+        auto *ap_dronecan = AP_DroneCAN::get_dronecan(i);
+        if (ap_dronecan != nullptr) {
+            ap_dronecan->relay_hardpoint.broadcast(msg);
+        }
+    }
+
+}
+
+// Get relay state from pin number, this relies on a cached value, assume remote pin is in sync
+bool AP_Relay::DroneCAN::get_pin(const int16_t pin) const
+{
+    const uint8_t index = hardpoint_index(pin);
+    return state[index].value;
+}
+
+// Populate message and update index with the sent command
+// This will allow the caller to cycle through each enabled pin
+bool AP_Relay::DroneCAN::populate_next_command(uint8_t &index, uavcan_equipment_hardpoint_Command &msg) const
+{
+    // Find the next enabled index
+    for (uint8_t i = 0; i < ARRAY_SIZE(state); i++) {
+        // Look for next index, wrapping back to 0 as needed
+        const uint8_t new_index = (index + 1 + i) % ARRAY_SIZE(state);
+        if (!state[new_index].enabled) {
+            // This index is not being used
+            continue;
+        }
+
+        // Update command and index then return
+        msg.hardpoint_id = new_index;
+        msg.command = state[new_index].value;
+        index = new_index;
+        return true;
+    }
+
+    return false;
+}
+#endif // AP_RELAY_DRONECAN_ENABLED
+
 #if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED
 // this method may only return false if there is no space in the
 // supplied link for the message.

libraries/AP_Relay/AP_Relay.h

@@ -25,9 +25,17 @@
   #error There must be at least one relay instance if using AP_Relay
 #endif
 
+#if AP_RELAY_DRONECAN_ENABLED
+#include <AP_DroneCAN/AP_DroneCAN.h>
+#endif
+
 /// @class	AP_Relay
 /// @brief	Class to manage the ArduPilot relay
 class AP_Relay {
+#if AP_RELAY_DRONECAN_ENABLED
+    // Allow DroneCAN to directly access private DroneCAN state
+    friend class AP_DroneCAN;
+#endif
 public:
     AP_Relay();
 
@@ -80,6 +88,50 @@ private:
     void convert_params();
 
     void set_pin_by_instance(uint8_t instance, bool value);
+
+    // Set relay state from pin number
+    void set_pin(const int16_t pin, const bool value);
+
+    // Get relay state from pin number
+    bool get_pin(const int16_t pin) const;
+
+#if HAL_ENABLE_DRONECAN_DRIVERS
+    // Virtual DroneCAN pins
+    class DroneCAN {
+    public:
+        // Return true if pin number is a virtual DroneCAN pin
+        bool valid_pin(int16_t pin) const;
+
+        // Enable streaming of pin number
+        void enable_pin(int16_t pin);
+
+        // Populate message and update index with the sent command
+        bool populate_next_command(uint8_t &index, uavcan_equipment_hardpoint_Command &msg) const;
+
+        // Set DroneCAN relay state from pin number
+        void set_pin(const int16_t pin, const bool value);
+
+        // Get relay state from pin number
+        bool get_pin(const int16_t pin) const;
+
+    private:
+
+        // Get the hardpoint index of given pin number
+        uint8_t hardpoint_index(const int16_t pin) const;
+
+        // Send DroneCAN hardpoint message for given index on all interfaces
+        void send_index(const uint8_t index);
+
+        static constexpr uint8_t num_pins = (int16_t)AP_Relay_Params::VIRTUAL_PINS::DroneCAN_15 - (int16_t)AP_Relay_Params::VIRTUAL_PINS::DroneCAN_0;
+
+        struct {
+            bool value;
+            bool enabled;
+        } state[num_pins];
+
+    } dronecan;
+#endif // HAL_ENABLE_DRONECAN_DRIVERS
+
 };
 
 namespace AP {

libraries/AP_Relay/AP_Relay_Params.cpp

@@ -23,6 +23,22 @@ const AP_Param::GroupInfo AP_Relay_Params::var_info[] = {
     // @DisplayName: Relay pin
     // @Description: Digital pin number for relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot.
     // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5,62:BBBMini Pin P8.13,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8
+    // @Values: 1000: DroneCAN Hardpoint ID 0
+    // @Values: 1001: DroneCAN Hardpoint ID 1
+    // @Values: 1002: DroneCAN Hardpoint ID 2
+    // @Values: 1003: DroneCAN Hardpoint ID 3
+    // @Values: 1004: DroneCAN Hardpoint ID 4
+    // @Values: 1005: DroneCAN Hardpoint ID 5
+    // @Values: 1006: DroneCAN Hardpoint ID 6
+    // @Values: 1007: DroneCAN Hardpoint ID 7
+    // @Values: 1008: DroneCAN Hardpoint ID 8
+    // @Values: 1009: DroneCAN Hardpoint ID 9
+    // @Values: 1010: DroneCAN Hardpoint ID 10
+    // @Values: 1011: DroneCAN Hardpoint ID 11
+    // @Values: 1012: DroneCAN Hardpoint ID 12
+    // @Values: 1013: DroneCAN Hardpoint ID 13
+    // @Values: 1014: DroneCAN Hardpoint ID 14
+    // @Values: 1015: DroneCAN Hardpoint ID 15
     // @User: Standard
     AP_GROUPINFO("PIN", 2, AP_Relay_Params, pin, -1),
 

libraries/AP_Relay/AP_Relay_Params.h

@@ -32,6 +32,26 @@ public:
         NUM_FUNCTIONS // must be the last entry
     };
 
+    // Pins that do not go via GPIO
+    enum class VIRTUAL_PINS {
+        DroneCAN_0  = 1000,
+        DroneCAN_1  = 1001,
+        DroneCAN_2  = 1002,
+        DroneCAN_3  = 1003,
+        DroneCAN_4  = 1004,
+        DroneCAN_5  = 1005,
+        DroneCAN_6  = 1006,
+        DroneCAN_7  = 1007,
+        DroneCAN_8  = 1008,
+        DroneCAN_9  = 1009,
+        DroneCAN_10 = 1010,
+        DroneCAN_11 = 1011,
+        DroneCAN_12 = 1012,
+        DroneCAN_13 = 1013,
+        DroneCAN_14 = 1014,
+        DroneCAN_15 = 1015,
+    };
+
     AP_Enum<FUNCTION> function;            // relay function
     AP_Int16 pin;                          // gpio pin number
     AP_Enum<DefaultState> default_state;  // default state

libraries/AP_Relay/AP_Relay_config.h

@@ -5,3 +5,5 @@
 #ifndef AP_RELAY_ENABLED
 #define AP_RELAY_ENABLED 1
 #endif
+
+#define AP_RELAY_DRONECAN_ENABLED AP_RELAY_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS && !defined(HAL_BUILD_AP_PERIPH)