Tools: AP_Periph: add support for relay via incoming hardpoint command

Tools/AP_Periph/AP_Periph.cpp

@@ -285,6 +285,10 @@ void AP_Periph_FW::init()
     notify.init();
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_RELAY
+    relay.init();
+#endif
+
 #if AP_SCRIPTING_ENABLED
     scripting.init();
 #endif

Tools/AP_Periph/AP_Periph.h

@@ -41,6 +41,16 @@
 #include <SITL/SITL.h>
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_RELAY
+#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
+    #error "Relay and PWM_HARDPOINT both use hardpoint message"
+#endif
+#include <AP_Relay/AP_Relay.h>
+#if !AP_RELAY_ENABLED
+    #error "HAL_PERIPH_ENABLE_RELAY requires AP_RELAY_ENABLED"
+#endif
+#endif
+
 #include <AP_NMEA_Output/AP_NMEA_Output.h>
 #if HAL_NMEA_OUTPUT_ENABLED && !(HAL_GCS_ENABLED && defined(HAL_PERIPH_ENABLE_GPS))
     // Needs SerialManager + (AHRS or GPS)
@@ -383,6 +393,11 @@ public:
 #if HAL_GCS_ENABLED
     GCS_Periph _gcs;
 #endif
+
+#ifdef HAL_PERIPH_ENABLE_RELAY
+    AP_Relay relay;
+#endif
+
     // setup the var_info table
     AP_Param param_loader{var_info};
 
@@ -482,6 +497,7 @@ public:
     void handle_beep_command(CanardInstance* canard_instance, CanardRxTransfer* transfer);
     void handle_lightscommand(CanardInstance* canard_instance, CanardRxTransfer* transfer);
     void handle_notify_state(CanardInstance* canard_instance, CanardRxTransfer* transfer);
+    void handle_hardpoint_command(CanardInstance* canard_instance, CanardRxTransfer* transfer);
 
     void process1HzTasks(uint64_t timestamp_usec);
     void processTx(void);

Tools/AP_Periph/Parameters.cpp

@@ -644,6 +644,12 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GOBJECT(rtc,                   "RTC",    AP_RTC),
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_RELAY
+    // @Group: RELAY
+    // @Path: ../libraries/AP_Relay/AP_Relay.cpp
+    GOBJECT(relay,                 "RELAY", AP_Relay),
+#endif
+
     AP_VAREND
 };
 

Tools/AP_Periph/Parameters.h

@@ -89,6 +89,7 @@ public:
         k_param_can_terminate1,
         k_param_can_terminate2,
         k_param_serial_options,
+        k_param_relay,
     };
 
     AP_Int16 format_version;

Tools/AP_Periph/can.cpp

@@ -848,6 +848,13 @@ void AP_Periph_FW::onTransferReceived(CanardInstance* canard_instance,
         handle_notify_state(canard_instance, transfer);
         break;
 #endif
+
+#ifdef HAL_PERIPH_ENABLE_RELAY
+    case UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID:
+        handle_hardpoint_command(canard_instance, transfer);
+        break;
+#endif
+
     }
 }
 
@@ -956,6 +963,11 @@ bool AP_Periph_FW::shouldAcceptTransfer(const CanardInstance* canard_instance,
     case ARDUPILOT_INDICATION_NOTIFYSTATE_ID:
         *out_data_type_signature = ARDUPILOT_INDICATION_NOTIFYSTATE_SIGNATURE;
         return true;
+#endif
+#ifdef HAL_PERIPH_ENABLE_RELAY
+    case UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID:
+        *out_data_type_signature = UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE;
+        return true;
 #endif
     default:
         break;

Tools/AP_Periph/relay.cpp

@@ -0,0 +1,38 @@
+#include "AP_Periph.h"
+
+#ifdef HAL_PERIPH_ENABLE_RELAY
+
+#include <dronecan_msgs.h>
+
+void AP_Periph_FW::handle_hardpoint_command(CanardInstance* canard_instance, CanardRxTransfer* transfer)
+{
+    uavcan_equipment_hardpoint_Command cmd {};
+    if (uavcan_equipment_hardpoint_Command_decode(transfer, &cmd)) {
+        // Failed to decode
+        return;
+    }
+
+    // Command must be 0 or 1, other values may be supported in the future
+    // rejecting them now ensures no change in behaviour
+    if ((cmd.command != 0) && (cmd.command != 1)) {
+        return;
+    }
+
+    // Translate hardpoint ID to relay function
+    AP_Relay_Params::FUNCTION fun;
+    switch (cmd.hardpoint_id) {
+        case 0 ... 15:
+            // 0 to 15 are continuous
+            fun = AP_Relay_Params::FUNCTION(cmd.hardpoint_id + (uint8_t)AP_Relay_Params::FUNCTION::DroneCAN_HARDPOINT_0);
+            break;
+
+        default:
+            // ID not supported
+            return;
+    }
+
+    // Set relay
+    relay.set(fun, cmd.command);
+
+}
+#endif

Tools/AP_Periph/wscript

@@ -81,6 +81,7 @@ def build(bld):
                      'AP_RobotisServo',
                      'AP_FETtecOneWire',
                      'GCS_MAVLink',
+                     'AP_Relay'
                      ]
 
     bld.ap_stlib(