GCS_MAVLink: move mavlink support for servo/relay up

libraries/GCS_MAVLink/GCS.h

@@ -16,6 +16,7 @@
 #include <AP_Avoidance/AP_Avoidance.h>
 #include <AP_HAL/utility/RingBuffer.h>
 #include <AP_Frsky_Telem/AP_Frsky_Telem.h>
+#include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
 
 // check if a message will fit in the payload space available
 #define HAVE_PAYLOAD_SPACE(chan, id) (comm_get_txspace(chan) >= GCS_MAVLINK::packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN)
@@ -219,6 +220,7 @@ protected:
     virtual bool accept_packet(const mavlink_status_t &status, mavlink_message_t &msg) { return true; }
     virtual AP_Mission *get_mission() = 0;
     virtual AP_Rally *get_rally() const = 0;
+    virtual AP_ServoRelayEvents *get_servorelayevents() const = 0;
 
     bool            waypoint_receiving; // currently receiving
     // the following two variables are only here because of Tracker
@@ -281,6 +283,8 @@ private:
 
     virtual void        handleMessage(mavlink_message_t * msg) = 0;
 
+    MAV_RESULT handle_servorelay_message(mavlink_command_long_t &packet);
+
     /// The stream we are communicating over
     AP_HAL::UARTDriver *_port;
 

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1840,6 +1840,18 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_message(mavlink_command_long_t &pack
     MAV_RESULT result = MAV_RESULT_FAILED;
 
     switch (packet.command) {
+
+    case MAV_CMD_DO_SET_SERVO:
+        /* fall through */
+    case MAV_CMD_DO_REPEAT_SERVO:
+        /* fall through */
+    case MAV_CMD_DO_SET_RELAY:
+        /* fall through */
+    case MAV_CMD_DO_REPEAT_RELAY:
+        /* fall through */
+        result = handle_servorelay_message(packet);
+        break;
+
     default:
         result = MAV_RESULT_UNSUPPORTED;
     }

libraries/GCS_MAVLink/GCS_Dummy.h

@@ -16,6 +16,7 @@ protected:
 
     AP_Mission *get_mission() override { return nullptr; }
     AP_Rally *get_rally() const override { return nullptr; };
+    AP_ServoRelayEvents *get_servorelayevents() const override { return nullptr; }
 
     uint8_t sysid_my_gcs() const override { return 1; }
 

libraries/GCS_MAVLink/GCS_ServoRelay.cpp

@@ -0,0 +1,45 @@
+#include "GCS.h"
+
+#include "AP_ServoRelayEvents/AP_ServoRelayEvents.h"
+
+MAV_RESULT GCS_MAVLINK::handle_servorelay_message(mavlink_command_long_t &packet)
+{
+    AP_ServoRelayEvents *handler = get_servorelayevents();
+    if (handler == nullptr) {
+        return MAV_RESULT_UNSUPPORTED;
+    }
+
+    MAV_RESULT result = MAV_RESULT_FAILED;
+
+    switch (packet.command) {
+    case MAV_CMD_DO_SET_SERVO:
+        if (handler->do_set_servo(packet.param1, packet.param2)) {
+            result = MAV_RESULT_ACCEPTED;
+        }
+        break;
+
+    case MAV_CMD_DO_REPEAT_SERVO:
+        if (handler->do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4 * 1000)) {
+            result = MAV_RESULT_ACCEPTED;
+        }
+        break;
+
+    case MAV_CMD_DO_SET_RELAY:
+        if (handler->do_set_relay(packet.param1, packet.param2)) {
+            result = MAV_RESULT_ACCEPTED;
+        }
+        break;
+
+    case MAV_CMD_DO_REPEAT_RELAY:
+        if (handler->do_repeat_relay(packet.param1, packet.param2, packet.param3 * 1000)) {
+            result = MAV_RESULT_ACCEPTED;
+        }
+        break;
+
+    default:
+        result = MAV_RESULT_UNSUPPORTED;
+        break;
+    }
+
+    return result;
+}

libraries/GCS_MAVLink/examples/routing/routing.cpp

@@ -23,6 +23,7 @@ protected:
     uint32_t telem_delay() const override { return 0; }
     AP_Mission *get_mission() override { return nullptr; }
     AP_Rally *get_rally() const override { return nullptr; }
+    AP_ServoRelayEvents *get_servorelayevents() const override { return nullptr; }
     uint8_t sysid_my_gcs() const override { return 1; }
 
 private: