Basic implementation of RC over MAVLink

src/main/rx/mavlink.c

@@ -40,12 +40,59 @@ FILE_COMPILE_FOR_SPEED
 #include "rx/rx.h"
 #include "rx/mavlink.h"
 
-#define MAVLINK_MAX_CHANNEL 18
+#define MAVLINK_CHANNEL_COUNT 18
+static uint16_t mavlinkChannelData[MAVLINK_CHANNEL_COUNT];
+
+void mavlinkRxHandleMessage(const mavlink_rc_channels_override_t *msg) {
+    mavlinkChannelData[0] = msg->chan1_raw;
+    mavlinkChannelData[1] = msg->chan2_raw;
+    mavlinkChannelData[2] = msg->chan3_raw;
+    mavlinkChannelData[3] = msg->chan4_raw;
+    mavlinkChannelData[4] = msg->chan5_raw;
+    mavlinkChannelData[5] = msg->chan6_raw;
+    mavlinkChannelData[6] = msg->chan7_raw;
+    mavlinkChannelData[7] = msg->chan8_raw;
+    mavlinkChannelData[8] = msg->chan9_raw;
+    mavlinkChannelData[9] = msg->chan10_raw;
+    mavlinkChannelData[10] = msg->chan11_raw;
+    mavlinkChannelData[11] = msg->chan12_raw;
+    mavlinkChannelData[12] = msg->chan13_raw;
+    mavlinkChannelData[13] = msg->chan14_raw;
+    mavlinkChannelData[14] = msg->chan15_raw;
+    mavlinkChannelData[15] = msg->chan16_raw;
+    mavlinkChannelData[16] = msg->chan17_raw;
+    mavlinkChannelData[17] = msg->chan18_raw;
+}
+
+static uint8_t mavlinkFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    return RX_FRAME_COMPLETE;
+}
+
+static uint16_t mavlinkReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
+{
+    UNUSED(rxRuntimeConfig);
+    // MAVLink values are sent as PWM values in microseconds so no conversion is needed
+    return mavlinkChannelData[chan];
+}
 
 bool mavlinkRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 {
-    // TODO
+    // TODO failsafe
-    return false;
+
+    UNUSED(rxConfig);
+
+    rxRuntimeConfig->channelData = mavlinkChannelData;
+    rxRuntimeConfig->channelCount = MAVLINK_CHANNEL_COUNT;
+    rxRuntimeConfig->rxRefreshRate = 11000;
+    rxRuntimeConfig->rcReadRawFn = mavlinkReadRawRC;
+    rxRuntimeConfig->rcFrameStatusFn = mavlinkFrameStatus;
+
+    for (int ii = 0; ii < MAVLINK_CHANNEL_COUNT; ++ii) {
+        mavlinkChannelData[ii] = (16 * PWM_RANGE_MIDDLE) / 10 - 1408;
+    }
+
+    return true;
 }
 
 #endif

src/main/rx/mavlink.h

@@ -17,4 +17,7 @@
 
 #pragma once
 
+#include "common/mavlink.h"
+
+void mavlinkRxHandleMessage(const mavlink_rc_channels_override_t *msg);
 bool mavlinkRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);

src/main/telemetry/mavlink.c

@@ -67,6 +67,7 @@
 #include "navigation/navigation_private.h"
 
 #include "rx/rx.h"
+#include "rx/mavlink.h"
 
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
@@ -86,13 +87,7 @@
 
 #include "scheduler/scheduler.h"
 
-// mavlink library uses unnamed unions that's causes GCC to complain if -Wpedantic is used
-// until this is resolved in mavlink library - ignore -Wpedantic for mavlink code
-// TODO check if this is resolved in V2 library
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wpedantic"
 #include "common/mavlink.h"
-#pragma GCC diagnostic pop
 
 #define TELEMETRY_MAVLINK_PORT_MODE     MODE_RXTX
 #define TELEMETRY_MAVLINK_MAXRATE       50
@@ -1048,7 +1043,17 @@ static bool handleIncoming_MISSION_REQUEST(void)
 }
 
 static bool handleIncoming_RC_CHANNELS_OVERRIDE(void) {
+    mavlink_rc_channels_override_t msg;
+    mavlink_msg_rc_channels_override_decode(&mavRecvMsg, &msg);
 
+    // Check if this message is for us
+    if (msg.target_system == mavSystemId) {
+        mavlinkRxHandleMessage(&msg);
+        // TODO do we need to send an ack?
+        return true;
+    }
+
+    return false;
 }
 
 static bool processMAVLinkIncomingTelemetry(void)