Mavlink support

.gitmodules

@@ -6,3 +6,9 @@
 	path = radio/src/thirdparty/libACCESS
 	url = https://github.com/FrSkyRC/libACCESS.git
 	branch = master
+[submodule "radio/src/thirdparty/Mavlink/mavlink"]
+	path = radio/src/thirdparty/Mavlink/mavlink
+	url = https://github.com/mavlink/mavlink.git
+[submodule "radio/src/thirdparty/Mavlink/fastmavlink"]
+	path = radio/src/thirdparty/Mavlink/fastmavlink
+	url = https://github.com/olliw42/fastmavlink.git

radio/src/dataconstants.h

@@ -242,6 +242,9 @@ enum UartModes {
   UART_MODE_TELEMETRY,
   UART_MODE_SBUS_TRAINER,
   UART_MODE_LUA,
+#if defined(TELEMETRY_MAVLINK)
+  UART_MODE_MAVLINK,
+#endif
   UART_MODE_COUNT,
   UART_MODE_MAX = UART_MODE_COUNT-1
 };

radio/src/datastructs.h

@@ -651,6 +651,16 @@ PACK(struct ModelData {
 
   char modelRegistrationID[PXX2_LEN_REGISTRATION_ID];
 
+#if defined(TELEMETRY_MAVLINK)
+  // TODO: use a specific structure
+  uint8_t mavlinkRssi:1;
+  uint8_t mavlinkMimicSensors:3; // currently just off/on, but allow e.g. FrSky, CF, FrSky passthrough.
+  uint8_t mavlinkRcOverride:1;
+  uint8_t mavlinkSpare:2;
+  uint8_t mavlinkRssiScale;
+  // needs to adapt CHKSIZE below //if not all are use compiled optiomizes to lowest size, which may raise error
+#endif
+
   bool isTrainerTraineeEnable() const
   {
 #if defined(PCBNV14)
@@ -844,6 +854,14 @@ PACK(struct RadioData {
   GYRO_FIELDS
 
   NOBACKUP(int8_t   uartSampleMode:2); // See UartSampleModes
+
+#if defined(TELEMETRY_MAVLINK)
+  // TODO: use specific structure
+  uint8_t mavlinkBaudrate:3;
+  uint8_t mavlinkBaudrate2:3;
+  uint8_t mavlinkSpare:2;
+  // needs to adapt CHKSIZE below
+#endif
 });
 
 #undef SWITCHES_WARNING_DATA
@@ -965,9 +983,14 @@ static inline void check_struct()
   CHKSIZE(RadioData, 899);
   CHKSIZE(ModelData, 6604);
 #elif defined(PCBHORUS)
+#if defined(TELEMETRY_MAVLINK)
+  CHKSIZE(RadioData, 902+1);
+  CHKSIZE(ModelData, 11020+2);
+#else
   CHKSIZE(RadioData, 902);
   CHKSIZE(ModelData, 11020);
 #endif
+#endif
 
 #undef CHKSIZE
 }

radio/src/gui/colorlcd/model_setup.cpp

@@ -1218,6 +1218,30 @@ void ModelSetupPage::build(FormWindow * window)
     grid.addWindow(new TrainerModuleWindow(window, {0, grid.getWindowHeight(), LCD_W, 0}));
   }
 
+#if defined(TELEMETRY_MAVLINK)
+  // Mavlink
+  {
+    new Subtitle(window, grid.getLineSlot(), STR_MAVLINK);
+    grid.nextLine();
+
+    new StaticText(window, grid.getLabelSlot(true), STR_MAVLINK_RSSI);
+    new CheckBox(window, grid.getFieldSlot(), GET_SET_DEFAULT(g_model.mavlinkRssi));
+    grid.nextLine();
+
+    new StaticText(window, grid.getLabelSlot(true), STR_MAVLINK_RSSI_SCALE);
+    new NumberEdit(window, grid.getFieldSlot(2,0), 0, 255, GET_SET_DEFAULT(g_model.mavlinkRssiScale));
+    grid.nextLine();
+
+    new StaticText(window, grid.getLabelSlot(true), STR_MAVLINK_SENSOR_MIMICRY);
+    new CheckBox(window, grid.getFieldSlot(), GET_SET_DEFAULT(g_model.mavlinkMimicSensors));
+    grid.nextLine();
+
+    new StaticText(window, grid.getLabelSlot(true), STR_MAVLINK_RC_OVERRIDE);
+    new CheckBox(window, grid.getFieldSlot(), GET_SET_DEFAULT(g_model.mavlinkRcOverride));
+    grid.nextLine();
+  }
+#endif
+
   window->setInnerHeight(grid.getWindowHeight());
 }
 

radio/src/gui/colorlcd/radio_hardware.cpp

@@ -278,7 +278,12 @@ void RadioHardwarePage::build(FormWindow * window)
 
 #if defined(AUX_SERIAL)
   new StaticText(window, grid.getLabelSlot(), STR_AUX_SERIAL_MODE);
+#if defined(TELEMETRY_MAVLINK) && !defined(CLI) && !defined(DEBUG)
+  auto aux = new Choice(window, grid.getFieldSlot(2,0), STR_MAVLINK_AUX_SERIAL_MODES, 0, UART_MODE_MAX, GET_SET_DEFAULT(g_eeGeneral.auxSerialMode));
+  new Choice(window, grid.getFieldSlot(2,1), STR_MAVLINK_AUX_BAUDRATES, 0, 3, GET_SET_DEFAULT(g_eeGeneral.mavlinkBaudrate));
+#else
   auto aux = new Choice(window, grid.getFieldSlot(1,0), STR_AUX_SERIAL_MODES, 0, UART_MODE_MAX, GET_SET_DEFAULT(g_eeGeneral.auxSerialMode));
+#endif
   aux->setAvailableHandler([=](int value) {
       return isAuxModeAvailable;
   });
@@ -287,7 +292,12 @@ void RadioHardwarePage::build(FormWindow * window)
 
 #if defined(AUX2_SERIAL)
   new StaticText(window, grid.getLabelSlot(), STR_AUX2_SERIAL_MODE);
+#if defined(TELEMETRY_MAVLINK) && !defined(CLI) && !defined(DEBUG)
+  auto aux2 = new Choice(window, grid.getFieldSlot(2,0), STR_MAVLINK_AUX_SERIAL_MODES, 0, UART_MODE_MAX, GET_SET_DEFAULT(g_eeGeneral.aux2SerialMode));
+  new Choice(window, grid.getFieldSlot(2,1), STR_MAVLINK_AUX_BAUDRATES, 0, 3, GET_SET_DEFAULT(g_eeGeneral.mavlinkBaudrate2));
+#else
   auto aux2 = new Choice(window, grid.getFieldSlot(1,0), STR_AUX_SERIAL_MODES, 0, UART_MODE_MAX, GET_SET_DEFAULT(g_eeGeneral.aux2SerialMode));
+#endif
   aux2->setAvailableHandler([=](int value) {
       return isAux2ModeAvailable;
   });

radio/src/gui/gui_common.cpp

@@ -386,6 +386,11 @@ bool isAuxModeAvailable(int mode)
 #if defined(AUX2_SERIAL)
   if (mode == UART_MODE_SBUS_TRAINER)
     return g_eeGeneral.aux2SerialMode != UART_MODE_SBUS_TRAINER;
+#if defined(TELEMETRY_MAVLINK)
+  else
+  if (mode == UART_MODE_MAVLINK)
+    return true;
+#endif
 #if defined(RADIO_TX16S)
   else
     return (g_model.trainerData.mode != TRAINER_MODE_MASTER_BATTERY_COMPARTMENT || g_eeGeneral.aux2SerialMode == UART_MODE_SBUS_TRAINER);
@@ -399,6 +404,11 @@ bool isAux2ModeAvailable(int mode)
 #if defined(AUX_SERIAL)
   if (mode == UART_MODE_SBUS_TRAINER)
     return g_eeGeneral.auxSerialMode != UART_MODE_SBUS_TRAINER;
+#if defined(TELEMETRY_MAVLINK)
+  else
+  if (mode == UART_MODE_MAVLINK)
+    return true;
+#endif
 #if defined(RADIO_TX16S)
   else
     return (g_model.trainerData.mode != TRAINER_MODE_MASTER_BATTERY_COMPARTMENT || g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER);

radio/src/lua/api_mavlink.cpp

@@ -0,0 +1,190 @@
+/*
+ * Copyright (C) EdgeTX
+ *
+ * (c) www.olliw.eu, OlliW, OlliW42
+ *
+ * Based on code named
+ *   opentx - http://github.com/opentx/opentx
+ *   th9x - http://code.google.com/p/th9x
+ *   er9x - http://code.google.com/p/er9x
+ *   gruvin9x - http://code.google.com/p/gruvin9x
+ *
+ * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <ctype.h>
+#include <stdio.h>
+#include "opentx.h"
+#include "lua_api.h"
+#include "telemetry/mavlink/mavlink_telem.h"
+#include "thirdparty/Mavlink/edgetx_lua_lib_constants.h"
+#include "thirdparty/Mavlink/edgetx_lua_lib_messages.h"
+
+
+static int luaMavlinkGetVersion(lua_State * L)
+{
+  lua_pushinteger(L, FASTMAVLINK_MAVLINK_VERSION); // this is the version reported also by the heartbeat
+  return 1;
+}
+
+static int luaMavlinkGetChannelStatus(lua_State * L)
+{
+  lua_createtable(L, 0, 6);
+  lua_pushtableinteger(L, "msg_rx_count", mavlinkTelem.msg_rx_count);
+  lua_pushtableinteger(L, "msg_rx_per_sec", mavlinkTelem.msg_rx_persec);
+  lua_pushtableinteger(L, "bytes_rx_per_sec", mavlinkTelem.bytes_rx_persec);
+  lua_pushtableinteger(L, "msg_tx_count", mavlinkTelem.msg_tx_count);
+  lua_pushtableinteger(L, "msg_tx_per_sec", mavlinkTelem.msg_tx_persec);
+  lua_pushtableinteger(L, "bytes_tx_per_sec", mavlinkTelem.bytes_tx_persec);
+  return 1;
+}
+
+//-- mavlink api --
+
+static int luaMavlinkGetSystemId(lua_State *L)
+{
+  lua_pushinteger(L, mavlinkTelem.systemSysId());
+  return 1;
+}
+
+static int luaMavlinkGetAutopilotIds(lua_State *L)
+{
+  lua_pushinteger(L, mavlinkTelem.systemSysId());
+  lua_pushinteger(L, mavlinkTelem.autopilotCompId());
+  return 1;
+}
+
+static int luaMavlinkGetCameraIds(lua_State *L)
+{
+  lua_pushinteger(L, mavlinkTelem.systemSysId());
+  lua_pushinteger(L, mavlinkTelem.cameraCompId());
+  return 1;
+}
+
+static int luaMavlinkGetGimbalIds(lua_State *L)
+{
+  lua_pushinteger(L, mavlinkTelem.systemSysId());
+  lua_pushinteger(L, mavlinkTelem.gimbalCompId());
+  return 1;
+}
+
+static int luaMavlinkGetGimbalManagerIds(lua_State *L)
+{
+  lua_pushinteger(L, mavlinkTelem.systemSysId());
+  lua_pushinteger(L, mavlinkTelem.gimbalManagerCompId());
+  return 1;
+}
+
+//-- mavlink api, messages --
+
+static int luaMavlinkInEnable(lua_State *L)
+{
+  bool flag = (luaL_checkinteger(L, 1) > 0);
+  mavlinkTelem.mavapiMsgInEnable(flag);
+  return 0;
+}
+
+static int luaMavlinkInCount(lua_State *L)
+{
+  lua_pushinteger(L, mavlinkTelem.mavapiMsgInCount());
+  return 1;
+}
+
+static int luaMavlinkGetMessage(lua_State *L)
+{
+  int msgid = luaL_checknumber(L, 1);
+
+  MavlinkTelem::MavMsg* mavmsg = mavlinkTelem.mavapiMsgInGet(msgid);
+  if (!mavmsg) {
+    lua_pushnil(L);
+  }
+  else {
+    luaMavlinkPushMavMsg(L, mavmsg);
+    mavmsg->updated = false;
+  }
+  return 1;
+}
+
+static int luaMavlinkGetMessageLast(lua_State *L)
+{
+  MavlinkTelem::MavMsg* mavmsg = mavlinkTelem.mavapiMsgInGetLast();
+  if (!mavmsg) {
+    lua_pushnil(L);
+  }
+  else {
+    luaMavlinkPushMavMsg(L, mavmsg);
+  }
+  return 1;
+}
+
+static int luaMavlinkOutEnable(lua_State *L)
+{
+  bool flag = (luaL_checkinteger(L, 1) > 0);
+  mavlinkTelem.mavapiMsgOutEnable(flag);
+  return 0;
+}
+
+static int luaMavlinkIsFree(lua_State *L)
+{
+  lua_pushboolean(L, (mavlinkTelem.mavapiMsgOutPtr() != NULL));
+  return 1;
+}
+
+static int luaMavlinkSendMessage(lua_State *L)
+{
+  fmav_message_t* msg_out = mavlinkTelem.mavapiMsgOutPtr();
+
+  if (!lua_istable(L, -1) || !msg_out) {
+    lua_pushnil(L);
+  }
+  else if (luaMavlinkCheckMsgOut(L, msg_out)) {
+    mavlinkTelem.mavapiMsgOutSet();
+    lua_pushboolean(L, true);
+  }
+  else {
+    lua_pushboolean(L, false);
+  }
+  return 1;
+}
+
+
+//------------------------------------------------------------
+// mavlink luaL and luaR arrays
+//------------------------------------------------------------
+
+const luaL_Reg mavlinkLib[] = {
+  { "getVersion", luaMavlinkGetVersion },
+  { "getChannelStatus", luaMavlinkGetChannelStatus },
+
+  { "getSystemId", luaMavlinkGetSystemId },
+  { "getAutopilotIds", luaMavlinkGetAutopilotIds },
+  { "getCameraIds", luaMavlinkGetCameraIds },
+  { "getGimbalIds", luaMavlinkGetGimbalIds },
+  { "getGimbalManagerIds", luaMavlinkGetGimbalManagerIds },
+
+  { "enableIn", luaMavlinkInEnable },
+  { "getInCount", luaMavlinkInCount },
+  { "getMessage", luaMavlinkGetMessage },
+  { "getMessageLast", luaMavlinkGetMessageLast },
+  { "enableOut", luaMavlinkOutEnable },
+  { "isFree", luaMavlinkIsFree },
+  { "sendMessage", luaMavlinkSendMessage },
+
+  { nullptr, nullptr }  /* sentinel */
+};
+
+const luaR_value_entry mavlinkConstants[] = {
+  MAVLINK_LIB_CONSTANTS
+
+  { nullptr, 0 }  /* sentinel */
+};
+