From 22071ba230742d96122884aba6e0bcc25be08003 Mon Sep 17 00:00:00 2001
From: Brian Webb <webbbn@gmail.com>
Date: Sun, 4 Jun 2017 08:28:25 -0700
Subject: [PATCH] LP-526 Adds support for the MSP_SERVO command to the MSP
 module.

---
 flight/modules/UAVOMSPBridge/UAVOMSPBridge.c | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c
index 6114a963c..841276623 100644
--- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c
+++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c
@@ -49,6 +49,7 @@
 #include "airspeedstate.h"
 #include "actuatorsettings.h"
 #include "actuatordesired.h"
+#include "actuatorcommand.h"
 #include "flightstatus.h"
 #include "systemstats.h"
 #include "systemalarms.h"
@@ -665,6 +666,20 @@ static void msp_send_channels(struct msp_bridge *m)
     msp_send(m, MSP_RC, data.buf, sizeof(data));
 }
 
+static void msp_send_servo(struct msp_bridge *m)
+{
+    ActuatorCommandData ac;
+
+    ActuatorCommandGet(&ac);
+
+    // Only the first 8 channels are supported.
+    // Channels are 16 bits (2 bytes).
+    uint8_t channels[16];
+    memcpy(channels, (uint8_t *)ac.Channel, 16);
+
+    msp_send(m, MSP_SERVO, channels, sizeof(channels));
+}
+
 static void msp_send_boxids(struct msp_bridge *m) // This is actually sending a map of MSP_STATUS.flag bits to BOX ids.
 {
     msp_send(m, MSP_BOXIDS, msp_boxes, sizeof(msp_boxes));
@@ -871,6 +886,9 @@ static msp_state msp_state_checksum(struct msp_bridge *m, uint8_t b)
     case MSP_RC:
         msp_send_channels(m);
         break;
+    case MSP_SERVO:
+        msp_send_servo(m);
+        break;
     case MSP_BOXIDS:
         msp_send_boxids(m);
         break;