From 20ad0ff1b50cae47cc967bd76f55cbd2056c764d Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 29 May 2021 16:14:26 +0200 Subject: [PATCH 01/16] add mavlink, fastmavlink submodules --- .gitmodules | 6 ++++++ radio/src/thirdparty/Mavlink/fastmavlink | 1 + radio/src/thirdparty/Mavlink/mavlink | 1 + 3 files changed, 8 insertions(+) create mode 160000 radio/src/thirdparty/Mavlink/fastmavlink create mode 160000 radio/src/thirdparty/Mavlink/mavlink diff --git a/.gitmodules b/.gitmodules index 9ce4543c5fa..5b10173a780 100644 --- a/.gitmodules +++ b/.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 diff --git a/radio/src/thirdparty/Mavlink/fastmavlink b/radio/src/thirdparty/Mavlink/fastmavlink new file mode 160000 index 00000000000..be8b000a3b3 --- /dev/null +++ b/radio/src/thirdparty/Mavlink/fastmavlink @@ -0,0 +1 @@ +Subproject commit be8b000a3b392ee097991f205effdca1bfc1ed7b diff --git a/radio/src/thirdparty/Mavlink/mavlink b/radio/src/thirdparty/Mavlink/mavlink new file mode 160000 index 00000000000..37a6e90d04d --- /dev/null +++ b/radio/src/thirdparty/Mavlink/mavlink @@ -0,0 +1 @@ +Subproject commit 37a6e90d04d8c0554f99eacb28e17d9a8d5912bf From 4bde990e798a69fe59bcbb0076861e6caea969ae Mon Sep 17 00:00:00 2001 From: olliw42 Date: Sat, 29 May 2021 16:38:50 +0200 Subject: [PATCH 02/16] recreate PR MAVLink for EdgeTx II #36 up to this point --- radio/src/dataconstants.h | 5 + radio/src/datastructs.h | 33 + radio/src/gui/colorlcd/model_setup.cpp | 26 + radio/src/gui/colorlcd/radio_hardware.cpp | 26 + radio/src/gui/gui_common.cpp | 14 + radio/src/lua/api_mavlink.cpp | 224 +++ radio/src/lua/api_mavsdk.cpp | 1609 +++++++++++++++++ radio/src/main.cpp | 19 + radio/src/opentx.cpp | 7 + radio/src/opentx.h | 8 + radio/src/options.h | 3 + .../common/arm/stm32/aux_serial_driver.cpp | 76 +- .../arm/stm32/bootloader/CMakeLists.txt | 1 + .../targets/common/arm/stm32/usb_driver.cpp | 3 + .../src/targets/common/arm/stm32/usb_driver.h | 3 + .../src/targets/common/arm/stm32/usbd_cdc.cpp | 8 +- .../src/targets/common/arm/stm32/usbd_desc.c | 12 + radio/src/targets/horus/CMakeLists.txt | 69 + radio/src/targets/horus/board.h | 23 +- radio/src/targets/horus/hal.h | 43 + radio/src/targets/horus/telemetry_driver.cpp | 78 + radio/src/tasks.cpp | 12 + radio/src/tasks.h | 10 + radio/src/telemetry/mavlink/mavlink_telem.cpp | 667 +++++++ radio/src/telemetry/mavlink/mavlink_telem.h | 892 +++++++++ .../mavlink/mavlink_telem_autopilot.cpp | 1378 ++++++++++++++ .../mavlink/mavlink_telem_camera.cpp | 268 +++ .../mavlink/mavlink_telem_gimbal.cpp | 612 +++++++ .../mavlink/mavlink_telem_interface.cpp | 495 +++++ .../mavlink/mavlink_telem_mavapi.cpp | 139 ++ .../telemetry/mavlink/mavlink_telem_qshot.cpp | 117 ++ radio/src/telemetry/telemetry.cpp | 13 +- radio/src/telemetry/telemetry.h | 5 +- radio/src/thirdparty/Lua/src/lauxlib.h | 6 + radio/src/thirdparty/Lua/src/linit.c | 6 + radio/src/thirdparty/Lua/src/lrotable.h | 6 + radio/src/thirdparty/Mavlink/.gitignore | 2 + radio/src/thirdparty/Mavlink/README.md | 7 + radio/src/thirdparty/Mavlink/edgetx.xml | 8 + .../Mavlink/fmav_generate_c_library.py | 54 + .../Mavlink/fmav_generate_lua_lib.py | 284 +++ radio/src/translations.cpp | 14 + radio/src/translations.h | 14 + radio/src/translations/untranslated.h | 14 + 44 files changed, 7305 insertions(+), 8 deletions(-) create mode 100644 radio/src/lua/api_mavlink.cpp create mode 100644 radio/src/lua/api_mavsdk.cpp create mode 100644 radio/src/telemetry/mavlink/mavlink_telem.cpp create mode 100644 radio/src/telemetry/mavlink/mavlink_telem.h create mode 100644 radio/src/telemetry/mavlink/mavlink_telem_autopilot.cpp create mode 100644 radio/src/telemetry/mavlink/mavlink_telem_camera.cpp create mode 100644 radio/src/telemetry/mavlink/mavlink_telem_gimbal.cpp create mode 100644 radio/src/telemetry/mavlink/mavlink_telem_interface.cpp create mode 100644 radio/src/telemetry/mavlink/mavlink_telem_mavapi.cpp create mode 100644 radio/src/telemetry/mavlink/mavlink_telem_qshot.cpp create mode 100644 radio/src/thirdparty/Mavlink/.gitignore create mode 100644 radio/src/thirdparty/Mavlink/README.md create mode 100644 radio/src/thirdparty/Mavlink/edgetx.xml create mode 100644 radio/src/thirdparty/Mavlink/fmav_generate_c_library.py create mode 100644 radio/src/thirdparty/Mavlink/fmav_generate_lua_lib.py diff --git a/radio/src/dataconstants.h b/radio/src/dataconstants.h index 86070e89b2d..7ead996792d 100644 --- a/radio/src/dataconstants.h +++ b/radio/src/dataconstants.h @@ -242,6 +242,11 @@ enum UartModes { UART_MODE_TELEMETRY, UART_MODE_SBUS_TRAINER, UART_MODE_LUA, +//OW +#if defined(TELEMETRY_MAVLINK) + UART_MODE_MAVLINK, +#endif +//OWEND UART_MODE_COUNT, UART_MODE_MAX = UART_MODE_COUNT-1 }; diff --git a/radio/src/datastructs.h b/radio/src/datastructs.h index 557a9f3cd36..6125bd644eb 100644 --- a/radio/src/datastructs.h +++ b/radio/src/datastructs.h @@ -651,6 +651,20 @@ PACK(struct ModelData { char modelRegistrationID[PXX2_LEN_REGISTRATION_ID]; +//OW +#if defined(TELEMETRY_MAVLINK) + uint16_t _mavlinkEnabled:1; // currently not used + uint16_t mavlinkRssi:1; + uint16_t _mavlinkDummy:2; // currently not used + uint16_t mavlinkMimicSensors:3; // currently just off/on, but allow e.g. FrSky, CF, FrSky passthrough. + uint16_t mavlinkRcOverride:1; + uint16_t _mavlinkGpsIcon:1; // currently not used + uint8_t mavlinkRssiScale; + uint8_t _mavlinkDummy2; // currently not used + // needs to adapt CHKSIZE below //if not all are use compiled optiomizes to lowest size, which may raise error +#endif +//OWEND + bool isTrainerTraineeEnable() const { #if defined(PCBNV14) @@ -844,6 +858,16 @@ PACK(struct RadioData { GYRO_FIELDS NOBACKUP(int8_t uartSampleMode:2); // See UartSampleModes + +//OW +#if defined(TELEMETRY_MAVLINK) + uint16_t mavlinkBaudrate:3; + uint16_t mavlinkBaudrate2:3; + uint16_t mavlinkExternal:2; + uint16_t _mavlinkDummy:8; // currently not used + // needs to adapt CHKSIZE below +#endif +//OWEND }); #undef SWITCHES_WARNING_DATA @@ -965,9 +989,18 @@ static inline void check_struct() CHKSIZE(RadioData, 899); CHKSIZE(ModelData, 6604); #elif defined(PCBHORUS) +//OW +// CHKSIZE(RadioData, 901); +// CHKSIZE(ModelData, 11020); +#if defined(TELEMETRY_MAVLINK) + CHKSIZE(RadioData, 902+2); + CHKSIZE(ModelData, 11020+4); +#else CHKSIZE(RadioData, 902); CHKSIZE(ModelData, 11020); #endif +//OWEND +#endif #undef CHKSIZE } diff --git a/radio/src/gui/colorlcd/model_setup.cpp b/radio/src/gui/colorlcd/model_setup.cpp index b7817d2c7e1..8fd316263cd 100644 --- a/radio/src/gui/colorlcd/model_setup.cpp +++ b/radio/src/gui/colorlcd/model_setup.cpp @@ -1218,6 +1218,32 @@ void ModelSetupPage::build(FormWindow * window) grid.addWindow(new TrainerModuleWindow(window, {0, grid.getWindowHeight(), LCD_W, 0})); } +//OW +#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 +//OWEND + window->setInnerHeight(grid.getWindowHeight()); } diff --git a/radio/src/gui/colorlcd/radio_hardware.cpp b/radio/src/gui/colorlcd/radio_hardware.cpp index b5c2648468a..fd86e77dc6a 100644 --- a/radio/src/gui/colorlcd/radio_hardware.cpp +++ b/radio/src/gui/colorlcd/radio_hardware.cpp @@ -278,7 +278,16 @@ void RadioHardwarePage::build(FormWindow * window) #if defined(AUX_SERIAL) new StaticText(window, grid.getLabelSlot(), STR_AUX_SERIAL_MODE); +//OW +#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 +//OWEND auto aux = new Choice(window, grid.getFieldSlot(1,0), STR_AUX_SERIAL_MODES, 0, UART_MODE_MAX, GET_SET_DEFAULT(g_eeGeneral.auxSerialMode)); +//OW +#endif +//OWEND aux->setAvailableHandler([=](int value) { return isAuxModeAvailable; }); @@ -287,7 +296,16 @@ void RadioHardwarePage::build(FormWindow * window) #if defined(AUX2_SERIAL) new StaticText(window, grid.getLabelSlot(), STR_AUX2_SERIAL_MODE); +//OW +#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 +//OWEND auto aux2 = new Choice(window, grid.getFieldSlot(1,0), STR_AUX_SERIAL_MODES, 0, UART_MODE_MAX, GET_SET_DEFAULT(g_eeGeneral.aux2SerialMode)); +//OW +#endif +//OWEND aux2->setAvailableHandler([=](int value) { return isAux2ModeAvailable; }); @@ -299,6 +317,14 @@ void RadioHardwarePage::build(FormWindow * window) grid.nextLine(); #endif +//OW +#if defined(TELEMETRY_MAVLINK) + new StaticText(window, grid.getLabelSlot(), STR_MAVLINK_EXTERNAL); + new CheckBox(window, grid.getFieldSlot(), GET_SET_DEFAULT(g_eeGeneral.mavlinkExternal)); + grid.nextLine(); +#endif +//OWEND + // ADC filter new StaticText(window, grid.getLabelSlot(), STR_JITTER_FILTER); new CheckBox(window, grid.getFieldSlot(1,0), GET_SET_INVERTED(g_eeGeneral.jitterFilter)); diff --git a/radio/src/gui/gui_common.cpp b/radio/src/gui/gui_common.cpp index d243d154f86..67351bb6f9d 100644 --- a/radio/src/gui/gui_common.cpp +++ b/radio/src/gui/gui_common.cpp @@ -386,6 +386,13 @@ bool isAuxModeAvailable(int mode) #if defined(AUX2_SERIAL) if (mode == UART_MODE_SBUS_TRAINER) return g_eeGeneral.aux2SerialMode != UART_MODE_SBUS_TRAINER; +//OW +#if defined(TELEMETRY_MAVLINK) + else + if (mode == UART_MODE_MAVLINK) + return true; +#endif +//OWEND #if defined(RADIO_TX16S) else return (g_model.trainerData.mode != TRAINER_MODE_MASTER_BATTERY_COMPARTMENT || g_eeGeneral.aux2SerialMode == UART_MODE_SBUS_TRAINER); @@ -399,6 +406,13 @@ bool isAux2ModeAvailable(int mode) #if defined(AUX_SERIAL) if (mode == UART_MODE_SBUS_TRAINER) return g_eeGeneral.auxSerialMode != UART_MODE_SBUS_TRAINER; +//OW +#if defined(TELEMETRY_MAVLINK) + else + if (mode == UART_MODE_MAVLINK) + return true; +#endif +//OWEND #if defined(RADIO_TX16S) else return (g_model.trainerData.mode != TRAINER_MODE_MASTER_BATTERY_COMPARTMENT || g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER); diff --git a/radio/src/lua/api_mavlink.cpp b/radio/src/lua/api_mavlink.cpp new file mode 100644 index 00000000000..fea8774605c --- /dev/null +++ b/radio/src/lua/api_mavlink.cpp @@ -0,0 +1,224 @@ +/* + * (c) www.olliw.eu, OlliW, OlliW42 + */ + +#include +#include +#include "opentx.h" +#include "lua_api.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; +} + +//-- some statistics -- + +extern int _end; +extern int _heap_end; +extern unsigned char *heap; + +static int luaMavlinkGetMemUsed(lua_State * L) +{ + uint32_t s = luaGetMemUsed(lsScripts); +#if defined(COLORLCD) + uint32_t w = luaGetMemUsed(lsWidgets); + uint32_t e = luaExtraMemoryUsage; +#else + uint32_t w = 0; + uint32_t e = 0; +#endif + lua_createtable(L, 0, 6); + lua_pushtableinteger(L, "scripts", s); + lua_pushtableinteger(L, "widgets", w); + lua_pushtableinteger(L, "extra", e); + lua_pushtableinteger(L, "total", s+w+e); + lua_pushtableinteger(L, "heap_used", (int)(heap - (unsigned char *)&_end)); + lua_pushtableinteger(L, "heap_free", (int)((unsigned char *)&_heap_end - heap)); + return 1; +} + +static int luaMavlinkGetStackUsed(lua_State * L) +{ + lua_createtable(L, 0, 10); + lua_pushtableinteger(L, "main_available", stackAvailable()*4); + lua_pushtableinteger(L, "main_size", stackSize()*4); + lua_pushtableinteger(L, "menus_available", menusStack.available()*4); + lua_pushtableinteger(L, "menus_size", menusStack.size()); + lua_pushtableinteger(L, "mixer_available", mixerStack.available()*4); + lua_pushtableinteger(L, "mixer_size", mixerStack.size()); + lua_pushtableinteger(L, "audio_available", audioStack.available()*4); + lua_pushtableinteger(L, "audio_size", audioStack.size()); + lua_pushtableinteger(L, "mavlink_available", mavlinkStack.available()*4); + lua_pushtableinteger(L, "mavlink_size", mavlinkStack.size()); + return 1; +} + +static int luaMavlinkGetTaskStats(lua_State *L) +{ + lua_newtable(L); + lua_pushtableinteger(L, "time", mavlinkTaskRunTime()); + lua_pushtableinteger(L, "max", mavlinkTaskRunTimeMax()); + lua_pushtableinteger(L, "loop", mavlinkTaskLoop()); + 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 }, + { "getMemUsed", luaMavlinkGetMemUsed }, + { "getStackUsed", luaMavlinkGetStackUsed }, + { "getTaskStats", luaMavlinkGetTaskStats }, + + { "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 */ +}; + diff --git a/radio/src/lua/api_mavsdk.cpp b/radio/src/lua/api_mavsdk.cpp new file mode 100644 index 00000000000..aa27cabab02 --- /dev/null +++ b/radio/src/lua/api_mavsdk.cpp @@ -0,0 +1,1609 @@ +/* + * (c) www.olliw.eu, OlliW, OlliW42 + */ + +#include +#include +#include "opentx.h" +#include "lua_api.h" + + +constexpr float FPI = 3.141592653589793f; +constexpr float FDEGTORAD = FPI/180.0f; +constexpr float FRADTODEG = 180.0f/FPI; + +void u8toBCDstr(uint8_t n, char* s) +{ + if (n >= 100) { + for (*s = '0'; n >= 100; n -= 100) (*s)++; + s++; + } + if (n >= 10) { + for (*s = '0'; n >= 10; n -= 10) (*s)++; + s++; + } + *s = '0' + n; + s++; + *s = '\0'; +} + +// -- GIMBAL -- + +static int luaMavsdkGimbalIsReceiving(lua_State *L) +{ + bool flag = (mavlinkTelem.gimbal.is_receiving > 0); + lua_pushboolean(L, flag); + return 1; +} + +static int luaMavsdkGimbalIsInitialized(lua_State *L) +{ + bool flag = (mavlinkTelem.gimbal.is_receiving > 0) && mavlinkTelem.gimbal.is_initialized; + lua_pushboolean(L, flag); + return 1; +} + +static int luaMavsdkGimbalGetInfo(lua_State *L) +{ + lua_newtable(L); + lua_pushtableinteger(L, "compid", mavlinkTelem.gimbal.compid); + + lua_pushtablestring(L, "vendor_name", mavlinkTelem.gimbaldeviceInfo.vendor_name); + lua_pushtablestring(L, "model_name", mavlinkTelem.gimbaldeviceInfo.model_name); + lua_pushtablestring(L, "custom_name", mavlinkTelem.gimbaldeviceInfo.custom_name); + char s[32], ss[20]; + s[0] = '\0'; + if (mavlinkTelem.gimbaldeviceInfo.firmware_version) { + u8toBCDstr((mavlinkTelem.gimbaldeviceInfo.firmware_version >> 0) & 0xFF, ss); strcat(s, ss); strcat(s, "."); + u8toBCDstr((mavlinkTelem.gimbaldeviceInfo.firmware_version >> 8) & 0xFF, ss); strcat(s, ss); strcat(s, "."); + u8toBCDstr((mavlinkTelem.gimbaldeviceInfo.firmware_version >> 16) & 0xFF, ss); strcat(s, ss); strcat(s, "."); + u8toBCDstr((mavlinkTelem.gimbaldeviceInfo.firmware_version >> 24) & 0xFF, ss); strcat(s, ss); + } + lua_pushtablestring(L, "firmware_version", s); + s[0] = '\0'; + if (mavlinkTelem.gimbaldeviceInfo.hardware_version) { + u8toBCDstr((mavlinkTelem.gimbaldeviceInfo.hardware_version >> 0) & 0xFF, ss); strcat(s, ss); strcat(s, "."); + u8toBCDstr((mavlinkTelem.gimbaldeviceInfo.hardware_version >> 8) & 0xFF, ss); strcat(s, ss); strcat(s, "."); + u8toBCDstr((mavlinkTelem.gimbaldeviceInfo.hardware_version >> 16) & 0xFF, ss); strcat(s, ss); strcat(s, "."); + u8toBCDstr((mavlinkTelem.gimbaldeviceInfo.hardware_version >> 24) & 0xFF, ss); strcat(s, ss); + } + lua_pushtablestring(L, "hardware_version", s); + lua_pushtableinteger(L, "capability_flags", mavlinkTelem.gimbaldeviceInfo.cap_flags); + return 1; +} + +static int luaMavsdkGimbalGetStatus(lua_State *L) +{ + lua_newtable(L); + lua_pushtablenumber(L, "system_status", mavlinkTelem.gimbal.system_status); + lua_pushtablenumber(L, "custom_mode", mavlinkTelem.gimbal.custom_mode); + lua_pushtableboolean(L, "is_armed", mavlinkTelem.gimbal.is_armed); + lua_pushtableboolean(L, "prearm_ok", mavlinkTelem.gimbal.prearm_ok); + return 1; +} + +static int luaMavsdkGimbalGetAttRollDeg(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.gimbalAtt.roll_deg); + return 1; +} + +static int luaMavsdkGimbalGetAttPitchDeg(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.gimbalAtt.pitch_deg); + return 1; +} + +static int luaMavsdkGimbalGetAttYawDeg(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.gimbalAtt.yaw_deg_relative); + return 1; +} + +// gimbal protocol v1 +// can also be used for gimbal protocol v2, if _gimbal_protocol_v2 is set accordingly + +static int luaMavsdkGimbalSendNeutralMode(lua_State *L) +{ + mavlinkTelem.sendGimbalTargetingMode(MAV_MOUNT_MODE_NEUTRAL); + return 0; +} + +static int luaMavsdkGimbalSendMavlinkTargetingMode(lua_State *L) +{ + mavlinkTelem.sendGimbalTargetingMode(MAV_MOUNT_MODE_MAVLINK_TARGETING); + return 0; +} + +static int luaMavsdkGimbalSendRcTargetingMode(lua_State *L) +{ + mavlinkTelem.sendGimbalTargetingMode(MAV_MOUNT_MODE_RC_TARGETING); + return 0; +} + +static int luaMavsdkGimbalSendGpsPointMode(lua_State *L) +{ + mavlinkTelem.sendGimbalTargetingMode(MAV_MOUNT_MODE_GPS_POINT); + return 0; +} + +static int luaMavsdkGimbalSendSysIdTargetingMode(lua_State *L) +{ + mavlinkTelem.sendGimbalTargetingMode(MAV_MOUNT_MODE_SYSID_TARGET); + return 0; +} + +static int luaMavsdkGimbalSendPitchYawDeg(lua_State *L) +{ + float pitch = luaL_checknumber(L, 1); + float yaw = luaL_checknumber(L, 2); + mavlinkTelem.sendGimbalPitchYawDeg(pitch, yaw); + return 0; +} + +// -- GIMBAL CLIENT -- gimbal protocol v2 + +static int luaMavsdkIsGimbalProtocolV2(lua_State *L) +{ + bool flag = mavlinkTelem.isStorm32GimbalProtocolV2(); + lua_pushboolean(L, flag); + return 1; +} + +static int luaMavsdkSetGimbalProtocolV2(lua_State *L) +{ + bool flag = (luaL_checknumber(L, 1) > 0); + mavlinkTelem.setStorm32GimbalProtocolV2(flag); + return 0; +} + +static int luaMavsdkGimbalClientIsReceiving(lua_State *L) +{ + bool flag = (mavlinkTelem.gimbalmanager.is_receiving > 0); + lua_pushboolean(L, flag); + return 1; +} + +static int luaMavsdkGimbalClientIsInitialized(lua_State *L) +{ + bool flag = (mavlinkTelem.gimbalmanager.is_receiving > 0) && mavlinkTelem.gimbalmanager.is_initialized; + lua_pushboolean(L, flag); + return 1; +} + +static int luaMavsdkGimbalClientGetInfo(lua_State *L) +{ + lua_newtable(L); + lua_pushtableinteger(L, "gimbal_manager_id", mavlinkTelem.gimbalmanager.compid); + lua_pushtableinteger(L, "gimbal_id", mavlinkTelem.gimbal.compid); + lua_pushtableinteger(L, "device_capability_flags", mavlinkTelem.gimbalmanagerInfo.device_cap_flags); + lua_pushtableinteger(L, "manager_capability_flags", mavlinkTelem.gimbalmanagerInfo.manager_cap_flags); + return 1; +} + +static int luaMavsdkGimbalClientGetStatus(lua_State *L) +{ + lua_newtable(L); + lua_pushtableinteger(L, "supervisor", mavlinkTelem.gimbalmanagerStatus.supervisor); + lua_pushtableinteger(L, "device_flags", mavlinkTelem.gimbalmanagerStatus.device_flags); + lua_pushtableinteger(L, "manager_flags", mavlinkTelem.gimbalmanagerStatus.manager_flags); + lua_pushtableinteger(L, "profile", mavlinkTelem.gimbalmanagerStatus.profile); + return 1; +} + +static int luaMavsdkGimbalClientSetRetract(lua_State *L) +{ + bool enable = (luaL_checkinteger(L, 1) > 0); + mavlinkTelem.setStorm32GimbalClientRetract(enable); + return 0; +} + +static int luaMavsdkGimbalClientSetNeutral(lua_State *L) +{ + bool enable = (luaL_checkinteger(L, 1) > 0); + mavlinkTelem.setStorm32GimbalClientNeutral(enable); + return 0; +} + +static int luaMavsdkGimbalClientSetLock(lua_State *L) +{ + bool roll_lock = (luaL_checkinteger(L, 1) > 0); + bool pitch_lock = (luaL_checkinteger(L, 1) > 0); + bool yaw_lock = (luaL_checkinteger(L, 1) > 0); + mavlinkTelem.setStorm32GimbalClientLock(roll_lock, pitch_lock, yaw_lock); + return 0; +} + +static int luaMavsdkGimbalClientSetFlags(lua_State *L) +{ + uint16_t manager_flags = luaL_checkinteger(L, 1); + mavlinkTelem.setStorm32GimbalClientFlags(manager_flags); + return 0; +} + +static int luaMavsdkGimbalClientSendPitchYawDeg(lua_State *L) +{ + float pitch = luaL_checknumber(L, 1); + float yaw = luaL_checknumber(L, 2); + mavlinkTelem.sendStorm32GimbalManagerPitchYawDeg(pitch, yaw); + return 0; +} + +static int luaMavsdkGimbalClientSendControlPitchYawDeg(lua_State *L) +{ + float pitch = luaL_checknumber(L, 1); + float yaw = luaL_checknumber(L, 2); + mavlinkTelem.sendStorm32GimbalManagerControlPitchYawDeg(pitch, yaw); + return 0; +} + +static int luaMavsdkGimbalClientSendCmdPitchYawDeg(lua_State *L) +{ + float pitch = luaL_checknumber(L, 1); + float yaw = luaL_checknumber(L, 2); + mavlinkTelem.sendStorm32GimbalManagerCmdPitchYawDeg(pitch, yaw); + return 0; +} + +static int luaMavsdkGimbalDeviceSendPitchYawDeg(lua_State *L) +{ + float pitch = luaL_checknumber(L, 1); + float yaw = luaL_checknumber(L, 2); + mavlinkTelem.sendStorm32GimbalDevicePitchYawDeg(pitch, yaw); + return 0; +} + +// -- QSHOT -- + +static int luaMavsdkQShotSendCmdConfigure(lua_State *L) +{ + uint8_t mode = luaL_checkinteger(L, 1); + uint8_t shot_state = luaL_checkinteger(L, 2); + mavlinkTelem.sendQShotCmdConfigure(mode, shot_state); + return 0; +} + +static int luaMavsdkQShotSendStatus(lua_State *L) +{ + uint8_t mode = luaL_checkinteger(L, 1); + uint8_t shot_state = luaL_checkinteger(L, 2); + mavlinkTelem.sendQShotStatus(mode, shot_state); + return 0; +} + +static int luaMavsdkQShotGetStatus(lua_State *L) +{ + lua_newtable(L); + lua_pushtableinteger(L, "mode", mavlinkTelem.qshot.mode); + lua_pushtableinteger(L, "shot_state", mavlinkTelem.qshot.shot_state); + return 1; +} + +static int luaMavsdkQShotButtonState(lua_State *L) +{ + uint8_t button_state = luaL_checkinteger(L, 1); + mavlinkTelem.sendQShotButtonState(button_state); + return 0; +} + +// -- CAMERA -- + +static int luaMavsdkCameraIsReceiving(lua_State *L) +{ + bool flag = (mavlinkTelem.camera.is_receiving > 0); + lua_pushboolean(L, flag); + return 1; +} + +static int luaMavsdkCameraIsInitialized(lua_State *L) +{ + bool flag = (mavlinkTelem.camera.is_receiving > 0) && mavlinkTelem.camera.is_initialized; + lua_pushboolean(L, flag); + return 1; +} + +static int luaMavsdkCameraGetInfo(lua_State *L) +{ + lua_newtable(L); + lua_pushtableinteger(L, "compid", mavlinkTelem.camera.compid); + lua_pushtableinteger(L, "flags", mavlinkTelem.cameraInfo.flags); + lua_pushtableboolean(L, "has_video", mavlinkTelem.cameraInfo.has_video); + lua_pushtableboolean(L, "has_photo", mavlinkTelem.cameraInfo.has_photo); + lua_pushtableboolean(L, "has_modes", mavlinkTelem.cameraInfo.has_modes); + if (!isnan(mavlinkTelem.cameraInfo.total_capacity_MiB)) { + lua_pushtablenumber(L, "total_capacity", mavlinkTelem.cameraInfo.total_capacity_MiB); + } + else { + lua_pushtablenil(L, "total_capacity"); + } + lua_pushtablestring(L, "vendor_name", mavlinkTelem.cameraInfo.vendor_name); + lua_pushtablestring(L, "model_name", mavlinkTelem.cameraInfo.model_name); + char s[32], ss[20]; s[0] = '\0'; + if (mavlinkTelem.cameraInfo.firmware_version) { + u8toBCDstr((mavlinkTelem.cameraInfo.firmware_version >> 0) & 0xFF, ss); strcat(s, ss); strcat(s, "."); + u8toBCDstr((mavlinkTelem.cameraInfo.firmware_version >> 8) & 0xFF, ss); strcat(s, ss); strcat(s, "."); + u8toBCDstr((mavlinkTelem.cameraInfo.firmware_version >> 16) & 0xFF, ss); strcat(s, ss); strcat(s, "."); + u8toBCDstr((mavlinkTelem.cameraInfo.firmware_version >> 24) & 0xFF, ss); strcat(s, ss); + } + lua_pushtablestring(L, "firmware_version", s); + return 1; +} + +static int luaMavsdkCameraGetStatus(lua_State *L) +{ + lua_newtable(L); + lua_pushtableinteger(L, "system_status", mavlinkTelem.camera.system_status); + lua_pushtableinteger(L, "mode", mavlinkTelem.cameraStatus.mode); + lua_pushtableboolean(L, "video_on", mavlinkTelem.cameraStatus.video_on); + lua_pushtableboolean(L, "photo_on", mavlinkTelem.cameraStatus.photo_on); + if (!isnan(mavlinkTelem.cameraStatus.available_capacity_MiB)) { + lua_pushtablenumber(L, "available_capacity", mavlinkTelem.cameraStatus.available_capacity_MiB); + } + else { + lua_pushtablenil(L, "available_capacity"); + } + if (!isnan(mavlinkTelem.cameraStatus.battery_voltage_V)) { + lua_pushtablenumber(L, "battery_voltage", mavlinkTelem.cameraStatus.battery_voltage_V); + } + else { + lua_pushtablenil(L, "battery_voltage"); + } + if (mavlinkTelem.cameraStatus.battery_remaining_pct >= 0) { + lua_pushtableinteger(L, "battery_remainingpct", mavlinkTelem.cameraStatus.battery_remaining_pct); + } + else { + lua_pushtablenil(L, "battery_remainingpct"); + } + return 1; +} + +static int luaMavsdkCameraSendVideoMode(lua_State *L) +{ + mavlinkTelem.sendCameraSetVideoMode(); + return 0; +} + +static int luaMavsdkCameraSendPhotoMode(lua_State *L) +{ + mavlinkTelem.sendCameraSetPhotoMode(); + return 0; +} + +static int luaMavsdkCameraStartVideo(lua_State *L) +{ + mavlinkTelem.sendCameraStartVideo(); + return 0; +} + +static int luaMavsdkCameraStopVideo(lua_State *L) +{ + mavlinkTelem.sendCameraStopVideo(); + return 0; +} + +static int luaMavsdkCameraTakePhoto(lua_State *L) +{ + mavlinkTelem.sendCameraTakePhoto(); + return 0; +} + +// -- MAVSDK GENERAL -- + +static int luaMavsdkMavTelemIsEnabled(lua_State *L) +{ + bool flag = (g_eeGeneral.auxSerialMode == UART_MODE_MAVLINK) || (g_eeGeneral.aux2SerialMode == UART_MODE_MAVLINK) || (g_eeGeneral.mavlinkExternal == 1); + lua_pushboolean(L, flag); + return 1; +} + +static int luaMavsdkMavTelemVersion(lua_State *L) +{ + lua_pushstring(L, MAVLINKTELEMVERSIONSTR); + return 1; +} + +static int luaMavsdkIsReceiving(lua_State *L) +{ + bool flag = mavlinkTelem.isReceiving(); + lua_pushboolean(L, flag); + return 1; +} + +static int luaMavsdkIsInitialized(lua_State *L) +{ + bool flag = (mavlinkTelem.autopilot.is_receiving > 0) && mavlinkTelem.autopilot.is_initialized; + lua_pushboolean(L, flag); + return 1; +} + +static int luaMavsdkGetAutopilotType(lua_State *L) +{ + int nbr = mavlinkTelem.autopilottype; + lua_pushnumber(L, nbr); + return 1; +} + +static int luaMavsdkGetVehicleType(lua_State *L) +{ + int nbr = mavlinkTelem.vehicletype; + lua_pushnumber(L, nbr); + return 1; +} + +static int luaMavsdkGetFlightMode(lua_State *L) +{ + int nbr = mavlinkTelem.flightmode; + lua_pushnumber(L, nbr); + return 1; +} + +typedef enum MAVSDK_VEHICLECLASS { + MAVSDK_VEHICLECLASS_GENERIC = 0, + MAVSDK_VEHICLECLASS_PLANE, + MAVSDK_VEHICLECLASS_COPTER, + MAVSDK_VEHICLECLASS_ROVER, + MAVSDK_VEHICLECLASS_BOAT, + MAVSDK_VEHICLECLASS_SUB, + MAVSDK_VEHICLECLASS_ENUM_END +} MAVSDK_VEHICLECLASS; + +static int luaMavsdkGetVehicleClass(lua_State *L) +{ +int nbr; + + switch (mavlinkTelem.vehicletype) { + case MAV_TYPE_FIXED_WING: + case MAV_TYPE_FLAPPING_WING: + case MAV_TYPE_VTOL_DUOROTOR: + case MAV_TYPE_VTOL_QUADROTOR: + case MAV_TYPE_VTOL_TILTROTOR: + case MAV_TYPE_VTOL_RESERVED2: + case MAV_TYPE_VTOL_RESERVED3: + case MAV_TYPE_VTOL_RESERVED4: + case MAV_TYPE_VTOL_RESERVED5: + case MAV_TYPE_PARAFOIL: + nbr = MAVSDK_VEHICLECLASS_PLANE; + break; + case MAV_TYPE_QUADROTOR: + case MAV_TYPE_COAXIAL: + case MAV_TYPE_HELICOPTER: + case MAV_TYPE_HEXAROTOR: + case MAV_TYPE_OCTOROTOR: + case MAV_TYPE_TRICOPTER: + case MAV_TYPE_DODECAROTOR: + nbr = MAVSDK_VEHICLECLASS_COPTER; + break; + case MAV_TYPE_GROUND_ROVER: + nbr = MAVSDK_VEHICLECLASS_ROVER; + break; + case MAV_TYPE_SURFACE_BOAT: + nbr = MAVSDK_VEHICLECLASS_BOAT; + break; + case MAV_TYPE_SUBMARINE: + nbr = MAVSDK_VEHICLECLASS_SUB; + break; + default: + nbr = MAVSDK_VEHICLECLASS_GENERIC; + } + lua_pushnumber(L, nbr); + return 1; +} + +static int luaMavsdkGetSystemStatus(lua_State *L) +{ + int nbr = mavlinkTelem.autopilot.system_status; + lua_pushnumber(L, nbr); + return 1; +} + +static int luaMavsdkIsArmed(lua_State *L) +{ + bool flag = mavlinkTelem.autopilot.is_armed; + lua_pushboolean(L, flag); + return 1; +} + +// -- MAVSDK RADIO -- + +static int luaMavsdkGetRadioRssi(lua_State *L) +{ + uint8_t rssi = UINT8_MAX; + if (mavlinkTelem.radio.is_receiving) { + rssi = mavlinkTelem.radio.rssi; + } + else if (mavlinkTelem.radio.is_receiving65) { + rssi = mavlinkTelem.radio.rssi65; + } + else if (mavlinkTelem.radio.is_receiving35) { + rssi = mavlinkTelem.radio.rssi35; + } + if (rssi != UINT8_MAX) { + lua_pushinteger(L, rssi); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetRadioRssiScaled(lua_State *L) +{ + if (mavlinkTelem.radio.is_receiving || mavlinkTelem.radio.is_receiving65 || mavlinkTelem.radio.is_receiving35) { + lua_pushinteger(L, mavlinkTelem.radio.rssi_scaled); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetRadioRemoteRssi(lua_State *L) +{ + if (mavlinkTelem.radio.is_receiving) { + lua_pushinteger(L, mavlinkTelem.radio.remrssi); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetRadioNoise(lua_State *L) +{ + if (mavlinkTelem.radio.is_receiving) { + lua_pushinteger(L, mavlinkTelem.radio.noise); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetRadioRemoteNoise(lua_State *L) +{ + if (mavlinkTelem.radio.is_receiving) { + lua_pushinteger(L, mavlinkTelem.radio.remnoise); + } + else { + lua_pushnil(L); + } + return 1; +} + +// -- MAVSDK SYSTEM STATUS -- + +static int luaMavsdkGetSystemStatusSensors(lua_State *L) +{ + if (mavlinkTelem.sysstatus.received) { + lua_newtable(L); + lua_pushtablenumber(L, "present", mavlinkTelem.sysstatus.sensors_present); + lua_pushtablenumber(L, "enabled", mavlinkTelem.sysstatus.sensors_enabled); + lua_pushtablenumber(L, "health", mavlinkTelem.sysstatus.sensors_health); + } + else { + lua_pushnil(L); + } + return 1; +} + +// -- MAVSDK ATTITUDE -- + +static int luaMavsdkGetAttRollDeg(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.att.roll_rad * FRADTODEG); + return 1; +} + +static int luaMavsdkGetAttPitchDeg(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.att.pitch_rad * FRADTODEG); + return 1; +} + +static int luaMavsdkGetAttYawDeg(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.att.yaw_rad * FRADTODEG); + return 1; +} + +// -- MAVSDK GPS -- + +static int luaMavsdkGetGps1Status(lua_State *L) +{ +/* what method is better ?? + lua_createtable(L, 0, 4); + lua_pushtablenumber(L, "fix", mavlinkTelem.gps_fix); + lua_pushtablenumber(L, "hdop", mavlinkTelem.gps_hdop * 0.01); + lua_pushtablenumber(L, "vdop", mavlinkTelem.gps_vdop * 0.01); + lua_pushtablenumber(L, "sat", mavlinkTelem.gps_sat); +*/ + lua_newtable(L); + lua_pushtablenumber(L, "fix", mavlinkTelem.gps1.fix); + lua_pushtablenumber(L, "hdop", mavlinkTelem.gps1.hdop * 0.01f); + lua_pushtablenumber(L, "vdop", mavlinkTelem.gps1.vdop * 0.01f); + lua_pushtablenumber(L, "sat", mavlinkTelem.gps1.sat); + return 1; +} + +static int luaMavsdkGetGps1Fix(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.gps1.fix); + return 1; +} + +static int luaMavsdkGetGps1HDop(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.gps1.hdop * 0.01f); + return 1; +} + +static int luaMavsdkGetGps1VDop(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.gps1.vdop * 0.01f); + return 1; +} + +static int luaMavsdkGetGps1Sat(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.gps1.sat); // UINT8_MAX if not known, but we don't do a nil here + return 1; +} + +static int luaMavsdkGetGps1LatLonInt(lua_State *L) +{ + lua_newtable(L); + lua_pushtableinteger(L, "lat", mavlinkTelem.gps1.lat); + lua_pushtableinteger(L, "lon", mavlinkTelem.gps1.lon); + return 1; +} + +static int luaMavsdkGetGps1AltitudeMsl(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.gps1.alt_mm * 0.001f); + return 1; +} + +static int luaMavsdkGetGps1Speed(lua_State *L) +{ + if (mavlinkTelem.gps1.vel_cmps < UINT16_MAX) { + lua_pushnumber(L, mavlinkTelem.gps1.vel_cmps * 0.01f); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetGps1CourseOverGroundDeg(lua_State *L) +{ + if (mavlinkTelem.gps1.cog_cdeg < UINT16_MAX) { + lua_pushnumber(L, mavlinkTelem.gps1.cog_cdeg * 0.01f); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetGps2Status(lua_State *L) +{ + lua_newtable(L); + lua_pushtablenumber(L, "fix", mavlinkTelem.gps2.fix); + lua_pushtablenumber(L, "hdop", mavlinkTelem.gps2.hdop * 0.01f); + lua_pushtablenumber(L, "vdop", mavlinkTelem.gps2.vdop * 0.01f); + lua_pushtablenumber(L, "sat", mavlinkTelem.gps2.sat); + return 1; +} + +static int luaMavsdkGetGps2Fix(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.gps2.fix); + return 1; +} + +static int luaMavsdkGetGps2HDop(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.gps2.hdop * 0.01f); + return 1; +} + +static int luaMavsdkGetGps2VDop(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.gps2.vdop * 0.01f); + return 1; +} + +static int luaMavsdkGetGps2Sat(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.gps2.sat); + return 1; +} + +static int luaMavsdkGetGps2LatLonInt(lua_State *L) +{ + lua_newtable(L); + lua_pushtableinteger(L, "lat", mavlinkTelem.gps2.lat); + lua_pushtableinteger(L, "lon", mavlinkTelem.gps2.lon); + return 1; +} + +static int luaMavsdkGetGps2AltitudeMsl(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.gps2.alt_mm * 0.001f); + return 1; +} + +static int luaMavsdkGetGps2Speed(lua_State *L) +{ + if (mavlinkTelem.gps2.vel_cmps < UINT16_MAX) { + lua_pushnumber(L, mavlinkTelem.gps2.vel_cmps * 0.01f); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetGps2CourseOverGroundDeg(lua_State *L) +{ + if (mavlinkTelem.gps2.cog_cdeg < UINT16_MAX) { + lua_pushnumber(L, mavlinkTelem.gps2.cog_cdeg * 0.01f); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkIsGps1Available(lua_State *L) +{ + bool flag = mavlinkTelem.gps_instancemask & 0x01; + lua_pushboolean(L, flag); + return 1; +} + +static int luaMavsdkIsGps2Available(lua_State *L) +{ + bool flag = mavlinkTelem.gps_instancemask & 0x02; + lua_pushboolean(L, flag); + return 1; +} + +static int luaMavsdkGetGpsCount(lua_State *L) +{ + uint16_t cnt = 0, mask = mavlinkTelem.gps_instancemask; + for (uint8_t i = 0; i < 8; i++) { + if (mask & 0x01) cnt++; + mask >>= 1; + } + lua_pushinteger(L, cnt); + return 1; +} + +// -- MAVSDK POSITION -- + +static int luaMavsdkGetPositionLatLonInt(lua_State *L) +{ + lua_newtable(L); + lua_pushtableinteger(L, "lat", mavlinkTelem.gposition.lat); + lua_pushtableinteger(L, "lon", mavlinkTelem.gposition.lon); + return 1; +} + +static int luaMavsdkGetPositionAltitudeMsl(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.gposition.alt_mm * 0.001f); + return 1; +} + +static int luaMavsdkGetPositionAltitudeRelative(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.gposition.relative_alt_mm * 0.001f); + return 1; +} + +static int luaMavsdkGetPositionHeadingDeg(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.gposition.hdg_cdeg * 0.01f); + return 1; +} + +static int luaMavsdkGetPositionSpeedNed(lua_State *L) +{ + lua_newtable(L); + lua_pushtablenumber(L, "vx", mavlinkTelem.gposition.vx_cmps * 0.01f); + lua_pushtablenumber(L, "vy", mavlinkTelem.gposition.vy_cmps * 0.01f); + lua_pushtablenumber(L, "vz", mavlinkTelem.gposition.vz_cmps * 0.01f); + return 1; +} + +// -- MAVSDK VFR -- + +static int luaMavsdkGetVfrAirSpeed(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.vfr.airspd_mps); + return 1; +} + +static int luaMavsdkGetVfrGroundSpeed(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.vfr.groundspd_mps); + return 1; +} + +static int luaMavsdkGetVfrAltitudeMsl(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.vfr.alt_m); + return 1; +} + +static int luaMavsdkGetVfrClimbRate(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.vfr.climbrate_mps); + return 1; +} + +static int luaMavsdkGetVfrHeadingDeg(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.vfr.heading_deg); + return 1; +} + +static int luaMavsdkGetVfrThrottle(lua_State *L) +{ + lua_pushinteger(L, mavlinkTelem.vfr.thro_pct); + return 1; +} + +// -- MAVSDK BATTERY -- + +static int luaMavsdkGetBat1ChargeConsumed(lua_State *L) +{ + if (mavlinkTelem.bat1.charge_consumed_mAh != -1) { + lua_pushnumber(L, mavlinkTelem.bat1.charge_consumed_mAh); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetBat1EnergyConsumed(lua_State *L) +{ + if (mavlinkTelem.bat1.energy_consumed_hJ != -1) { + lua_pushnumber(L, mavlinkTelem.bat1.energy_consumed_hJ * 100.0f); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetBat1Temperature(lua_State *L) +{ + if (mavlinkTelem.bat1.temperature_cC < INT16_MAX) { + lua_pushnumber(L, mavlinkTelem.bat1.temperature_cC * 0.01f); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetBat1Voltage(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.bat1.voltage_mV * 0.001f); + return 1; +} + +static int luaMavsdkGetBat1Current(lua_State *L) +{ + if (mavlinkTelem.bat1.current_cA != -1) { + lua_pushnumber(L, mavlinkTelem.bat1.current_cA * 0.01f); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetBat1Remaining(lua_State *L) +{ + if (mavlinkTelem.bat1.remaining_pct != -1) { + lua_pushinteger(L, mavlinkTelem.bat1.remaining_pct); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetBat1TimeRemaining(lua_State *L) +{ + if (mavlinkTelem.bat1.time_remaining != 0) { + lua_pushinteger(L, mavlinkTelem.bat1.time_remaining); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetBat1ChargeState(lua_State *L) +{ + if (mavlinkTelem.bat1.charge_state != MAV_BATTERY_CHARGE_STATE_UNDEFINED) { + lua_pushinteger(L, mavlinkTelem.bat1.charge_state); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetBat1FaultBitMask(lua_State *L) +{ + if (mavlinkTelem.bat1.charge_state == MAV_BATTERY_CHARGE_STATE_FAILED || + mavlinkTelem.bat1.charge_state == MAV_BATTERY_CHARGE_STATE_UNHEALTHY) { + lua_pushinteger(L, mavlinkTelem.bat1.fault_bitmask); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetBat1CellCount(lua_State *L) +{ + if (mavlinkTelem.bat1.cellcount < 0) { + lua_pushnil(L); + } + else { + lua_pushinteger(L, mavlinkTelem.bat1.cellcount); + } + return 1; +} + +static int luaMavsdkGetBat2ChargeConsumed(lua_State *L) +{ + if (mavlinkTelem.bat2.charge_consumed_mAh != -1) { + lua_pushnumber(L, mavlinkTelem.bat2.charge_consumed_mAh); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetBat2EnergyConsumed(lua_State *L) +{ + if (mavlinkTelem.bat2.energy_consumed_hJ != -1) { + lua_pushnumber(L, mavlinkTelem.bat2.energy_consumed_hJ * 100.0f); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetBat2Temperature(lua_State *L) +{ + if (mavlinkTelem.bat2.temperature_cC < INT16_MAX) { + lua_pushnumber(L, mavlinkTelem.bat2.temperature_cC * 0.01f); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetBat2Voltage(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.bat2.voltage_mV * 0.001f); + return 1; +} + +static int luaMavsdkGetBat2Current(lua_State *L) +{ + if (mavlinkTelem.bat2.current_cA != -1) { + lua_pushnumber(L, mavlinkTelem.bat2.current_cA * 0.01f); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetBat2Remaining(lua_State *L) +{ + if (mavlinkTelem.bat2.remaining_pct != -1) { + lua_pushinteger(L, mavlinkTelem.bat2.remaining_pct); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetBat2TimeRemaining(lua_State *L) +{ + if (mavlinkTelem.bat2.time_remaining != 0) { + lua_pushinteger(L, mavlinkTelem.bat2.time_remaining); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetBat2ChargeState(lua_State *L) +{ + if (mavlinkTelem.bat2.charge_state != MAV_BATTERY_CHARGE_STATE_UNDEFINED) { + lua_pushinteger(L, mavlinkTelem.bat2.charge_state); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetBat2FaultBitMask(lua_State *L) +{ + if (mavlinkTelem.bat2.charge_state == MAV_BATTERY_CHARGE_STATE_FAILED || + mavlinkTelem.bat2.charge_state == MAV_BATTERY_CHARGE_STATE_UNHEALTHY) { + lua_pushinteger(L, mavlinkTelem.bat2.fault_bitmask); + } + else { + lua_pushnil(L); + } + return 1; +} + +static int luaMavsdkGetBat2CellCount(lua_State *L) +{ + if (mavlinkTelem.bat2.cellcount < 0) { + lua_pushnil(L); + } + else { + lua_pushinteger(L, mavlinkTelem.bat2.cellcount); + } + return 1; +} + +static int luaMavsdkIsBat1Available(lua_State *L) +{ + bool flag = mavlinkTelem.bat_instancemask & 0x01; + lua_pushboolean(L, flag); + return 1; +} + +static int luaMavsdkIsBat2Available(lua_State *L) +{ + bool flag = mavlinkTelem.bat_instancemask & 0x02; + lua_pushboolean(L, flag); + return 1; +} + +static int luaMavsdkGetBatCount(lua_State *L) +{ + uint16_t cnt = 0, mask = mavlinkTelem.bat_instancemask; + for (uint8_t i = 0; i < 8; i++) { + if (mask & 0x01) cnt++; + mask >>= 1; + } + lua_pushinteger(L, cnt); + return 1; +} + +static int luaMavsdkGetBat1Capacity(lua_State *L) +{ + if (mavlinkTelem.param.BATT_CAPACITY < 0) { + lua_pushnil(L); + } + else { + lua_pushnumber(L, mavlinkTelem.param.BATT_CAPACITY); + } + return 1; +} + +static int luaMavsdkGetBat2Capacity(lua_State *L) +{ + if (mavlinkTelem.param.BATT2_CAPACITY < 0) { + lua_pushnil(L); + } + else { + lua_pushnumber(L, mavlinkTelem.param.BATT2_CAPACITY); + } + return 1; +} + +// -- MAVSDK ARDUPILOT -- + +static int luaMavsdkApIsFlying(lua_State *L) +{ + lua_pushboolean(L, !mavlinkTelem.autopilot.is_standby); + return 1; +} + +static int luaMavsdkApIsFailsafe(lua_State *L) +{ + lua_pushboolean(L, mavlinkTelem.autopilot.is_critical); + return 1; +} + +static int luaMavsdkApPositionOk(lua_State *L) +{ + lua_pushboolean(L, mavlinkTelem.apPositionOk()); + return 1; +} + +static int luaMavsdkApSetFlightMode(lua_State *L) +{ + int32_t ap_flight_mode = luaL_checkinteger(L, 1); + mavlinkTelem.apSetFlightMode(ap_flight_mode); + return 0; +} + +static int luaMavsdkApRequestBanner(lua_State *L) +{ + mavlinkTelem.apRequestBanner(); + return 0; +} + +static int luaMavsdkApArm(lua_State *L) +{ + int32_t arm = luaL_checkinteger(L, 1); + mavlinkTelem.apArm(arm > 0); + return 0; +} + +static int luaMavsdkApCopterTakeOff(lua_State *L) +{ + float alt = luaL_checknumber(L, 1); + mavlinkTelem.apCopterTakeOff(alt); + return 0; +} + +static int luaMavsdkApLand(lua_State *L) +{ + mavlinkTelem.apLand(); + return 0; +} + +static int luaMavsdkApSetGroundSpeed(lua_State *L) +{ + float speed = luaL_checknumber(L, 1); + mavlinkTelem.apSetGroundSpeed(speed); + return 0; +} + +static int luaMavsdkApSimpleGotoPosIntAltRel(lua_State *L) +{ + int32_t lat = luaL_checkinteger(L, 1); + int32_t lon = luaL_checkinteger(L, 2); + float alt = luaL_checknumber(L, 3); + mavlinkTelem.apSimpleGotoPosAlt(lat, lon, alt); + return 0; +} + +static int luaMavsdkApGotoPosIntAltRel(lua_State *L) +{ + int32_t lat = luaL_checkinteger(L, 1); + int32_t lon = luaL_checkinteger(L, 2); + float alt = luaL_checknumber(L, 3); + mavlinkTelem.apGotoPosAltYawDeg(lat, lon, alt, NAN); + return 0; +} + +static int luaMavsdkApGotoPosIntAltRelYawDeg(lua_State *L) +{ + int32_t lat = luaL_checkinteger(L, 1); + int32_t lon = luaL_checkinteger(L, 2); + float alt = luaL_checknumber(L, 3); + float yaw = luaL_checknumber(L, 4); + mavlinkTelem.apGotoPosAltYawDeg(lat, lon, alt, yaw); + return 0; +} + +static int luaMavsdkApGotoPosIntAltRelVel(lua_State *L) +{ + int32_t lat = luaL_checkinteger(L, 1); + int32_t lon = luaL_checkinteger(L, 2); + float alt = luaL_checknumber(L, 3); + float vx = luaL_checknumber(L, 4); + float vy = luaL_checknumber(L, 5); + float vz = luaL_checknumber(L, 6); + mavlinkTelem.apGotoPosAltVel(lat, lon, alt, vx, vy, vz); + return 0; +} + +static int luaMavsdkApSetYawDeg(lua_State *L) +{ + float yaw = luaL_checknumber(L, 1); + int32_t relative = luaL_optunsigned(L, 2, 0); + mavlinkTelem.apSetYawDeg(yaw, (relative) ? true : false); + return 0; +} + +static int luaMavsdkApCopterFlyClick(lua_State *L) +{ + mavlinkTelem.apCopterFlyClick(); + return 0; +} + +static int luaMavsdkApCopterFlyHold(lua_State *L) +{ + float alt = luaL_checknumber(L, 1); + mavlinkTelem.apCopterFlyHold(alt); + return 0; +} + +static int luaMavsdkApCopterFlyPause(lua_State *L) +{ + mavlinkTelem.apCopterFlyPause(); + return 0; +} + +static int luaMavsdkApGetRangefinder(lua_State *L) +{ + lua_pushnumber(L, mavlinkTelem.rangefinder.distance); + return 1; +} + +static int luaMavsdkApGetArmingCheck(lua_State *L) +{ + if (mavlinkTelem.param.ARMING_CHECK < 0) { + lua_pushnil(L); + } + else { + lua_pushnumber(L, mavlinkTelem.param.ARMING_CHECK); + } + return 1; +} + + +// -- MAVSDK STATUSTEXT -- + +static int luaMavsdkIsStatusTextAvailable(lua_State *L) +{ + lua_pushboolean(L, !mavlinkTelem.statustext.fifo.isEmpty()); + return 1; +} + +static int luaMavsdkGetStatusText(lua_State *L) +{ + char text[FASTMAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; //mavlink string is not necessarily \0 terminated + fmav_statustext_t payload; + if (!mavlinkTelem.statustext.fifo.pop(payload)) { //should not happen, use isStatusTextAvailable() to check beforehand + text[0] = '\0'; + payload.severity = MAV_SEVERITY_INFO; + lua_pushnil(L); + lua_pushnil(L); + } + else { + memcpy(text, payload.text, FASTMAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); + text[FASTMAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = '\0'; + lua_pushinteger(L, payload.severity); + lua_pushstring(L, text); + } + return 2; +} + +// -- MAVSDK MISSION -- + +static int luaMavsdkGetNavControllerOutput(lua_State *L) +{ + lua_newtable(L); + lua_pushtablenumber(L, "nav_bearing", (float)mavlinkTelem.navControllerOutput.nav_bearing); + lua_pushtablenumber(L, "target_bearing", (float)mavlinkTelem.navControllerOutput.target_bearing); + lua_pushtablenumber(L, "wp_dist", (float)mavlinkTelem.navControllerOutput.wp_dist); + return 1; +} + +static int luaMavsdkGetMission(lua_State *L) +{ + lua_newtable(L); + lua_pushtableinteger(L, "count", mavlinkTelem.mission.count); + lua_pushtableinteger(L, "current_seq", mavlinkTelem.mission.seq_current); + return 1; +} + +static int luaMavsdkGetMissionItem(lua_State *L) +{ + lua_newtable(L); + lua_pushtableinteger(L, "seq", mavlinkTelem.missionItem.seq); + lua_pushtableinteger(L, "command", mavlinkTelem.missionItem.command); + lua_pushtableinteger(L, "frame", mavlinkTelem.missionItem.frame); + bool is_global = false; + switch (mavlinkTelem.missionItem.frame) { + case MAV_FRAME_GLOBAL_INT: + case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT: + //these should not occur, but play it safe and handle them anyway + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_GLOBAL_TERRAIN_ALT: + is_global = true; + break; + } + lua_pushtableboolean(L, "is_global", is_global); + if (is_global) { + lua_pushtableinteger(L, "lat", mavlinkTelem.missionItem.x); + lua_pushtableinteger(L, "lon", mavlinkTelem.missionItem.y); + lua_pushtablenumber(L, "alt", mavlinkTelem.missionItem.z); + } + else { + lua_pushtablenumber(L, "x", mavlinkTelem.missionItem.x * 0.0001f); + lua_pushtablenumber(L, "y", mavlinkTelem.missionItem.y * 0.0001f); + lua_pushtablenumber(L, "z", mavlinkTelem.missionItem.z); + } + return 1; +} + +// -- Fake RSSI -- + +static int luaMavsdkOptionIsRssiEnabled(lua_State *L) +{ + lua_pushboolean(L, g_model.mavlinkRssi); + return 1; +} + +static int luaMavsdkOptionEnableRssi(lua_State *L) +{ + bool enable = (luaL_checkinteger(L, 1) > 0); + g_model.mavlinkRssi = (enable) ? 1 : 0; + return 0; +} + +static int luaMavsdkOptionGetRssiScale(lua_State *L) +{ + lua_pushinteger(L, g_model.mavlinkRssiScale); + return 1; +} + +static int luaMavsdkOptionSetRssiScale(lua_State *L) +{ + int32_t scale = luaL_checkinteger(L, 1); + if (scale < 0) scale = 0; + if (scale > 255) scale = 255; + g_model.mavlinkRssiScale = scale; + return 0; +} + +static int luaMavsdkRadioDisableRssiVoice(lua_State *L) +{ + bool disable = (luaL_checkinteger(L, 1) > 0); + mavlinkTelem.radio.rssi_voice_disabled = disable; + return 0; +} + + +//------------------------------------------------------------ +// mavsdk luaL and luaR arrays +//------------------------------------------------------------ +// I believe the names can't be longer than 32 chars + +const luaL_Reg mavsdkLib[] = { + { "mavtelemIsEnabled", luaMavsdkMavTelemIsEnabled }, + { "getVersion", luaMavsdkMavTelemVersion }, + + { "isReceiving", luaMavsdkIsReceiving }, + { "isInitialized", luaMavsdkIsInitialized }, + { "getAutopilotType", luaMavsdkGetAutopilotType }, + { "getVehicleType", luaMavsdkGetVehicleType }, + { "getFlightMode", luaMavsdkGetFlightMode }, + { "getVehicleClass", luaMavsdkGetVehicleClass }, + { "getSystemStatus", luaMavsdkGetSystemStatus }, + { "isArmed", luaMavsdkIsArmed }, + + { "gimbalIsReceiving", luaMavsdkGimbalIsReceiving }, + { "gimbalIsInitialized", luaMavsdkGimbalIsInitialized }, + { "gimbalGetInfo", luaMavsdkGimbalGetInfo }, + { "gimbalGetStatus", luaMavsdkGimbalGetStatus }, + { "gimbalGetAttRollDeg", luaMavsdkGimbalGetAttRollDeg }, + { "gimbalGetAttPitchDeg", luaMavsdkGimbalGetAttPitchDeg }, + { "gimbalGetAttYawDeg", luaMavsdkGimbalGetAttYawDeg }, + // gimbal protocol v1 + { "gimbalSendNeutralMode", luaMavsdkGimbalSendNeutralMode }, + { "gimbalSendMavlinkTargetingMode", luaMavsdkGimbalSendMavlinkTargetingMode }, + { "gimbalSendRcTargetingMode", luaMavsdkGimbalSendRcTargetingMode }, + { "gimbalSendGpsPointMode", luaMavsdkGimbalSendGpsPointMode }, + { "gimbalSendSysIdTargetingMode", luaMavsdkGimbalSendSysIdTargetingMode }, + { "gimbalSendPitchYawDeg", luaMavsdkGimbalSendPitchYawDeg }, + // gimbal protocol v2 + { "gimbalIsProtocolV2", luaMavsdkIsGimbalProtocolV2 }, + { "gimbalSetProtocolV2", luaMavsdkSetGimbalProtocolV2 }, + { "gimbalClientIsReceiving", luaMavsdkGimbalClientIsReceiving }, + { "gimbalClientIsInitialized", luaMavsdkGimbalClientIsInitialized }, + { "gimbalClientGetInfo", luaMavsdkGimbalClientGetInfo }, + { "gimbalClientGetStatus", luaMavsdkGimbalClientGetStatus }, + { "gimbalClientSetRetract", luaMavsdkGimbalClientSetRetract }, + { "gimbalClientSetNeutral", luaMavsdkGimbalClientSetNeutral }, + { "gimbalClientSetLock", luaMavsdkGimbalClientSetLock }, + { "gimbalClientSetFlags", luaMavsdkGimbalClientSetFlags }, + { "gimbalClientSendPitchYawDeg", luaMavsdkGimbalClientSendPitchYawDeg }, + //for testing only + { "gimbalClientSendControlPitchYawDeg", luaMavsdkGimbalClientSendControlPitchYawDeg }, + { "gimbalClientSendCmdPitchYawDeg", luaMavsdkGimbalClientSendCmdPitchYawDeg }, + { "gimbalDeviceSendPitchYawDeg", luaMavsdkGimbalDeviceSendPitchYawDeg }, + + { "qshotSendCmdConfigure", luaMavsdkQShotSendCmdConfigure }, + { "qshotSendStatus", luaMavsdkQShotSendStatus }, + { "qshotGetStatus", luaMavsdkQShotGetStatus }, + { "qshotButtonState", luaMavsdkQShotButtonState }, + + { "cameraIsReceiving", luaMavsdkCameraIsReceiving }, + { "cameraIsInitialized", luaMavsdkCameraIsInitialized }, + { "cameraGetInfo", luaMavsdkCameraGetInfo }, + { "cameraGetStatus", luaMavsdkCameraGetStatus }, + { "cameraSendVideoMode", luaMavsdkCameraSendVideoMode }, + { "cameraSendPhotoMode", luaMavsdkCameraSendPhotoMode }, + { "cameraStartVideo", luaMavsdkCameraStartVideo }, + { "cameraStopVideo", luaMavsdkCameraStopVideo }, + { "cameraTakePhoto", luaMavsdkCameraTakePhoto }, + + { "getRadioRssi", luaMavsdkGetRadioRssi }, + { "getRadioRemoteRssi", luaMavsdkGetRadioRemoteRssi }, + { "getRadioNoise", luaMavsdkGetRadioNoise }, + { "getRadioRemoteNoise", luaMavsdkGetRadioRemoteNoise }, + { "getRadioRssiScaled", luaMavsdkGetRadioRssiScaled }, + + { "getSystemStatusSensors", luaMavsdkGetSystemStatusSensors }, + + { "getAttRollDeg", luaMavsdkGetAttRollDeg }, + { "getAttPitchDeg", luaMavsdkGetAttPitchDeg }, + { "getAttYawDeg", luaMavsdkGetAttYawDeg }, + + { "getGpsStatus", luaMavsdkGetGps1Status }, + { "getGpsFix", luaMavsdkGetGps1Fix }, + { "getGpsHDop", luaMavsdkGetGps1HDop }, + { "getGpsVDop", luaMavsdkGetGps1VDop }, + { "getGpsSat", luaMavsdkGetGps1Sat }, + { "getGpsLatLonInt", luaMavsdkGetGps1LatLonInt }, + { "getGpsAltitudeMsl", luaMavsdkGetGps1AltitudeMsl }, + { "getGpsSpeed", luaMavsdkGetGps1Speed }, + { "getGpsCourseOverGroundDeg", luaMavsdkGetGps1CourseOverGroundDeg }, + + { "getGps2Status", luaMavsdkGetGps2Status }, + { "getGps2Fix", luaMavsdkGetGps2Fix }, + { "getGps2HDop", luaMavsdkGetGps2HDop }, + { "getGps2VDop", luaMavsdkGetGps2VDop }, + { "getGps2Sat", luaMavsdkGetGps2Sat }, + { "getGps2LatLonInt", luaMavsdkGetGps2LatLonInt }, + { "getGps2AltitudeMsl", luaMavsdkGetGps2AltitudeMsl }, + { "getGps2Speed", luaMavsdkGetGps2Speed }, + { "getGps2CourseOverGroundDeg", luaMavsdkGetGps2CourseOverGroundDeg }, + + { "isGpsAvailable", luaMavsdkIsGps1Available }, + { "isGps2Available", luaMavsdkIsGps2Available }, + { "getGpsCount", luaMavsdkGetGpsCount }, + + { "getPositionLatLonInt", luaMavsdkGetPositionLatLonInt }, + { "getPositionAltitudeMsl", luaMavsdkGetPositionAltitudeMsl }, + { "getPositionAltitudeRelative", luaMavsdkGetPositionAltitudeRelative }, + { "getPositionHeadingDeg", luaMavsdkGetPositionHeadingDeg }, + { "getPositionSpeedNed", luaMavsdkGetPositionSpeedNed }, + + { "getVfrAirSpeed", luaMavsdkGetVfrAirSpeed }, + { "getVfrGroundSpeed", luaMavsdkGetVfrGroundSpeed }, + { "getVfrAltitudeMsl", luaMavsdkGetVfrAltitudeMsl }, + { "getVfrClimbRate", luaMavsdkGetVfrClimbRate }, + { "getVfrHeadingDeg", luaMavsdkGetVfrHeadingDeg }, + { "getVfrThrottle", luaMavsdkGetVfrThrottle }, + + { "getBatChargeConsumed", luaMavsdkGetBat1ChargeConsumed }, + { "getBatEnergyConsumed", luaMavsdkGetBat1EnergyConsumed }, + { "getBatTemperature", luaMavsdkGetBat1Temperature }, + { "getBatVoltage", luaMavsdkGetBat1Voltage }, + { "getBatCurrent", luaMavsdkGetBat1Current }, + { "getBatRemaining", luaMavsdkGetBat1Remaining }, + { "getBatTimeRemaining", luaMavsdkGetBat1TimeRemaining }, + { "getBatChargeState", luaMavsdkGetBat1ChargeState }, + { "getBatFaultBitMask", luaMavsdkGetBat1FaultBitMask }, + { "getBatCellCount", luaMavsdkGetBat1CellCount }, + + { "getBat2ChargeConsumed", luaMavsdkGetBat2ChargeConsumed }, + { "getBat2EnergyConsumed", luaMavsdkGetBat2EnergyConsumed }, + { "getBat2Temperature", luaMavsdkGetBat2Temperature }, + { "getBat2Voltage", luaMavsdkGetBat2Voltage }, + { "getBat2Current", luaMavsdkGetBat2Current }, + { "getBat2Remaining", luaMavsdkGetBat2Remaining }, + { "getBat2TimeRemaining", luaMavsdkGetBat2TimeRemaining }, + { "getBat2ChargeState", luaMavsdkGetBat2ChargeState }, + { "getBat2FaultBitMask", luaMavsdkGetBat2FaultBitMask }, + { "getBat2CellCount", luaMavsdkGetBat2CellCount }, + + { "isBatAvailable", luaMavsdkIsBat1Available }, + { "isBat2Available", luaMavsdkIsBat2Available }, + { "getBatCount", luaMavsdkGetBatCount }, + + { "getBatCapacity", luaMavsdkGetBat1Capacity }, + { "getBat2Capacity", luaMavsdkGetBat2Capacity }, + + { "isStatusTextAvailable", luaMavsdkIsStatusTextAvailable }, + { "getStatusText", luaMavsdkGetStatusText }, + + { "getNavController", luaMavsdkGetNavControllerOutput }, + { "getMission", luaMavsdkGetMission }, + { "getMissionItem", luaMavsdkGetMissionItem }, + + { "apIsFlying", luaMavsdkApIsFlying }, + { "apIsFailsafe", luaMavsdkApIsFailsafe }, + { "apPositionOk", luaMavsdkApPositionOk }, + { "apSetFlightMode", luaMavsdkApSetFlightMode }, + { "apRequestBanner", luaMavsdkApRequestBanner }, + { "apArm", luaMavsdkApArm }, + { "apCopterTakeOff", luaMavsdkApCopterTakeOff }, + { "apLand", luaMavsdkApLand }, + { "apSetGroundSpeed", luaMavsdkApSetGroundSpeed }, + { "apSimpleGotoPosIntAltRel", luaMavsdkApSimpleGotoPosIntAltRel }, + { "apGotoPosIntAltRel", luaMavsdkApGotoPosIntAltRel }, + { "apGotoPosIntAltRelYawDeg", luaMavsdkApGotoPosIntAltRelYawDeg }, + { "apGotoPosIntAltRelVel", luaMavsdkApGotoPosIntAltRelVel }, + { "apSetYawDeg", luaMavsdkApSetYawDeg }, + { "apCopterFlyClick", luaMavsdkApCopterFlyClick }, + { "apCopterFlyHold", luaMavsdkApCopterFlyHold }, + { "apCopterFlyPause", luaMavsdkApCopterFlyPause }, + { "apGetRangefinder", luaMavsdkApGetRangefinder }, + { "apGetArmingCheck", luaMavsdkApGetArmingCheck }, + + { "optionIsRssiEnabled", luaMavsdkOptionIsRssiEnabled }, + { "optionEnableRssi", luaMavsdkOptionEnableRssi }, + { "optionGetRssiScale", luaMavsdkOptionGetRssiScale }, + { "optionSetRssiScale", luaMavsdkOptionSetRssiScale }, + { "radioDisableRssiVoice", luaMavsdkRadioDisableRssiVoice }, + + { NULL, NULL } /* sentinel */ +}; + +const luaR_value_entry mavsdkConstants[] = { + { "VEHICLECLASS_GENERIC", MAVSDK_VEHICLECLASS_GENERIC }, + { "VEHICLECLASS_PLANE", MAVSDK_VEHICLECLASS_PLANE }, + { "VEHICLECLASS_COPTER", MAVSDK_VEHICLECLASS_COPTER }, + { "VEHICLECLASS_ROVER", MAVSDK_VEHICLECLASS_ROVER }, + { "VEHICLECLASS_BOAT", MAVSDK_VEHICLECLASS_BOAT }, + { "VEHICLECLASS_SUB", MAVSDK_VEHICLECLASS_SUB }, + + { "GDFLAGS_RETRACT", MAV_STORM32_GIMBAL_DEVICE_FLAGS_RETRACT }, + { "GDFLAGS_NEUTRAL", MAV_STORM32_GIMBAL_DEVICE_FLAGS_NEUTRAL }, + { "GDFLAGS_ROLL_LOCK", MAV_STORM32_GIMBAL_DEVICE_FLAGS_ROLL_LOCK }, + { "GDFLAGS_PITCH_LOCK", MAV_STORM32_GIMBAL_DEVICE_FLAGS_PITCH_LOCK }, + { "GDFLAGS_YAW_LOCK", MAV_STORM32_GIMBAL_DEVICE_FLAGS_YAW_LOCK }, + { "GDFLAGS_CAN_ACCEPT_YAW_ABSOLUTE", MAV_STORM32_GIMBAL_DEVICE_FLAGS_CAN_ACCEPT_YAW_ABSOLUTE }, + { "GDFLAGS_YAW_ABSOLUTE", MAV_STORM32_GIMBAL_DEVICE_FLAGS_YAW_ABSOLUTE }, + { "GDFLAGS_RC_EXCLUSIVE", MAV_STORM32_GIMBAL_DEVICE_FLAGS_RC_EXCLUSIVE }, + { "GDFLAGS_RC_MIXED", MAV_STORM32_GIMBAL_DEVICE_FLAGS_RC_MIXED }, + + { "GMFLAGS_NONE", MAV_STORM32_GIMBAL_MANAGER_FLAGS_NONE }, + { "GMFLAGS_RC_ACTIVE", MAV_STORM32_GIMBAL_MANAGER_FLAGS_RC_ACTIVE }, + { "GMFLAGS_ONBOARD_ACTIVE", MAV_STORM32_GIMBAL_MANAGER_FLAGS_CLIENT_ONBOARD_ACTIVE }, + { "GMFLAGS_AUTOPILOT_ACTIVE", MAV_STORM32_GIMBAL_MANAGER_FLAGS_CLIENT_AUTOPILOT_ACTIVE }, + { "GMFLAGS_GCS_ACTIVE", MAV_STORM32_GIMBAL_MANAGER_FLAGS_CLIENT_GCS_ACTIVE }, + { "GMFLAGS_CAMERA_ACTIVE", MAV_STORM32_GIMBAL_MANAGER_FLAGS_CLIENT_CAMERA_ACTIVE }, + { "GMFLAGS_GCS2_ACTIVE", MAV_STORM32_GIMBAL_MANAGER_FLAGS_CLIENT_GCS2_ACTIVE }, + { "GMFLAGS_CAMERA2_ACTIVE", MAV_STORM32_GIMBAL_MANAGER_FLAGS_CLIENT_CAMERA2_ACTIVE }, + { "GMFLAGS_CLIENT_ACTIVE", MAV_STORM32_GIMBAL_MANAGER_FLAGS_CLIENT_CUSTOM_ACTIVE }, + { "GMFLAGS_CLIENT2_ACTIVE", MAV_STORM32_GIMBAL_MANAGER_FLAGS_CLIENT_CUSTOM2_ACTIVE }, + { "GMFLAGS_SET_SUPERVISON", MAV_STORM32_GIMBAL_MANAGER_FLAGS_SET_SUPERVISON }, + { "GMFLAGS_SET_RELEASE", MAV_STORM32_GIMBAL_MANAGER_FLAGS_SET_RELEASE }, + + { "GMCLIENT_ONBOARD", MAV_STORM32_GIMBAL_MANAGER_CLIENT_ONBOARD }, + { "GMCLIENT_AUTOPILOT", MAV_STORM32_GIMBAL_MANAGER_CLIENT_AUTOPILOT }, + { "GMCLIENT_GCS", MAV_STORM32_GIMBAL_MANAGER_CLIENT_GCS }, + { "GMCLIENT_CAMERA", MAV_STORM32_GIMBAL_MANAGER_CLIENT_CAMERA }, + { "GMCLIENT_GCS2", MAV_STORM32_GIMBAL_MANAGER_CLIENT_GCS2 }, + { "GMCLIENT_CAMERA2", MAV_STORM32_GIMBAL_MANAGER_CLIENT_CAMERA2 }, + { "GMCLIENT_CUSTOM", MAV_STORM32_GIMBAL_MANAGER_CLIENT_CUSTOM }, + { "GMCLIENT_CUSTOM2", MAV_STORM32_GIMBAL_MANAGER_CLIENT_CUSTOM2 }, + + { "QSHOT_MODE_UNDEFINED", MAV_QSHOT_MODE_UNDEFINED }, + { "QSHOT_MODE_DEFAULT", MAV_QSHOT_MODE_DEFAULT }, + { "QSHOT_MODE_RETRACT", MAV_QSHOT_MODE_GIMBAL_RETRACT }, + { "QSHOT_MODE_NEUTRAL", MAV_QSHOT_MODE_GIMBAL_NEUTRAL }, + { "QSHOT_MODE_RC_CONTROL", MAV_QSHOT_MODE_GIMBAL_RC_CONTROL }, + { "QSHOT_MODE_POI", MAV_QSHOT_MODE_POI_TARGETING }, + { "QSHOT_MODE_SYSID", MAV_QSHOT_MODE_SYSID_TARGETING }, + { "QSHOT_MODE_CABLECAM", MAV_QSHOT_MODE_CABLECAM_2POINT }, + + { nullptr, 0 } /* sentinel */ +}; + diff --git a/radio/src/main.cpp b/radio/src/main.cpp index 24dd9406603..10a2cb81de7 100644 --- a/radio/src/main.cpp +++ b/radio/src/main.cpp @@ -70,6 +70,13 @@ void openUsbMenu() TRACE("USB mass storage"); setSelectedUsbMode(USB_MASS_STORAGE_MODE); }); +//OW +#if defined(TELEMETRY_MAVLINK_USB_SERIAL) + _usbMenu->addLine(STR_USB_MAVLINK, [] { + setSelectedUsbMode(USB_MAVLINK_MODE); + }); +#endif +//OWEND #if defined(DEBUG) _usbMenu->addLine(STR_USB_SERIAL, [] { TRACE("USB serial"); @@ -88,6 +95,13 @@ void onUSBConnectMenu(const char *result) else if (result == STR_USB_JOYSTICK) { setSelectedUsbMode(USB_JOYSTICK_MODE); } +//OW +#if defined(TELEMETRY_MAVLINK_USB_SERIAL) + else if (result == STR_USB_MAVLINK) { + setSelectedUsbMode(USB_MAVLINK_MODE); + } +#endif +//OWEND else if (result == STR_USB_SERIAL) { setSelectedUsbMode(USB_SERIAL_MODE); } @@ -101,6 +115,11 @@ void openUsbMenu() if (popupMenuHandler != onUSBConnectMenu) { POPUP_MENU_ADD_ITEM(STR_USB_JOYSTICK); POPUP_MENU_ADD_ITEM(STR_USB_MASS_STORAGE); +//OW +#if defined(TELEMETRY_MAVLINK_USB_SERIAL) + POPUP_MENU_ADD_ITEM(USB_MAVLINK_MODE); +#endif +//OWEND #if defined(DEBUG) POPUP_MENU_ADD_ITEM(STR_USB_SERIAL); #endif diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp index e934bcdcb6b..7090cb24b7b 100644 --- a/radio/src/opentx.cpp +++ b/radio/src/opentx.cpp @@ -227,6 +227,13 @@ void per10ms() outputTelemetryBuffer.per10ms(); +//OW +#if defined(TELEMETRY_MAVLINK) + mavlinkTelem.tick10ms(); + //XX checkEventLockTmo(); +#endif +//OWEND + heartbeat |= HEART_TIMER_10MS; } diff --git a/radio/src/opentx.h b/radio/src/opentx.h index cc9f4aff84b..7216b121a04 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -18,6 +18,8 @@ * GNU General Public License for more details. */ +//OW TELEMETRY_MAVLINK + #pragma once #include @@ -1015,6 +1017,12 @@ constexpr uint8_t SD_SCREEN_FILE_LENGTH = 64; #include "bluetooth.h" #endif +//OW +#if defined(TELEMETRY_MAVLINK) +#include "telemetry/mavlink/mavlink_telem.h" +#endif +//OWEND + constexpr uint8_t TEXT_FILENAME_MAXLEN = 40; union ReusableBuffer diff --git a/radio/src/options.h b/radio/src/options.h index 9918baa6070..995e8ac447b 100644 --- a/radio/src/options.h +++ b/radio/src/options.h @@ -103,6 +103,9 @@ static const char * const options[] = { #endif #if defined(BIND_KEY) "bindkey", +#endif +#if defined(TELEMETRY_MAVLINK) + "mavlink", #endif nullptr //sentinel }; diff --git a/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp b/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp index a04c16ed86e..ba461cec022 100644 --- a/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp +++ b/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp @@ -23,8 +23,12 @@ #if defined(AUX_SERIAL) uint8_t auxSerialMode = UART_MODE_COUNT; // Prevent debug output before port is setup +//OW +//Fifo auxSerialTxFifo; +#if !defined(TELEMETRY_MAVLINK) Fifo auxSerialTxFifo; - +#endif +//OWEND #if defined(AUX_SERIAL_DMA_Stream_RX) AuxSerialRxFifo auxSerialRxFifo __DMA (AUX_SERIAL_DMA_Stream_RX); #else @@ -153,6 +157,18 @@ void auxSerialInit(unsigned int mode, unsigned int protocol) case UART_MODE_LUA: auxSerialSetup(LUA_DEFAULT_BAUDRATE, false); AUX_SERIAL_POWER_ON(); +//OW + break; + +#if defined(TELEMETRY_MAVLINK) + case UART_MODE_MAVLINK: + auxSerialSetup(mavlinkTelemAuxBaudrate(), false); + AUX_SERIAL_POWER_ON(); + auxSerialTxFifo.clear(); + mavlinkTelemAuxSerialRxFifo.clear(); + break; +#endif +//OWEND } } @@ -181,6 +197,15 @@ void auxSerialStop() #endif USART_DeInit(AUX_SERIAL_USART); + +//OW +#if defined(TELEMETRY_MAVLINK) + if (auxSerialMode == UART_MODE_MAVLINK) { + auxSerialTxFifo.clear(); + mavlinkTelemAuxSerialRxFifo.clear(); + } +#endif +//OWEND } uint8_t auxSerialTracesEnabled() @@ -206,6 +231,18 @@ extern "C" void AUX_SERIAL_USART_IRQHandler(void) USART_ITConfig(AUX_SERIAL_USART, USART_IT_TXE, DISABLE); } } +//OW +#if defined(TELEMETRY_MAVLINK) + if (auxSerialMode == UART_MODE_MAVLINK) { + if (USART_GetITStatus(AUX_SERIAL_USART, USART_IT_RXNE) != RESET) { + USART_ClearITPendingBit(AUX_SERIAL_USART, USART_IT_RXNE); + uint8_t c = USART_ReceiveData(AUX_SERIAL_USART); + mavlinkTelemAuxSerialRxFifo.push(c); + } + return; + } +#endif +//OWEND #if defined(CLI) if (getSelectedUsbMode() != USB_SERIAL_MODE) { @@ -243,7 +280,12 @@ extern "C" void AUX_SERIAL_USART_IRQHandler(void) #if defined(AUX2_SERIAL) uint8_t aux2SerialMode = UART_MODE_COUNT; // Prevent debug output before port is setup +//OW +//Fifo aux2SerialTxFifo; +#if !defined(TELEMETRY_MAVLINK) Fifo aux2SerialTxFifo; +#endif +//OWEND AuxSerialRxFifo aux2SerialRxFifo __DMA (AUX2_SERIAL_DMA_Stream_RX); void aux2SerialSetup(unsigned int baudrate, bool dma, uint16_t length, uint16_t parity, uint16_t stop) @@ -351,6 +393,18 @@ void aux2SerialInit(unsigned int mode, unsigned int protocol) case UART_MODE_LUA: aux2SerialSetup(LUA_DEFAULT_BAUDRATE, false); AUX2_SERIAL_POWER_ON(); +//OW + break; + +#if defined(TELEMETRY_MAVLINK) + case UART_MODE_MAVLINK: + aux2SerialSetup(mavlinkTelemAux2Baudrate(), false); + AUX2_SERIAL_POWER_ON(); + aux2SerialTxFifo.clear(); + mavlinkTelemAux2SerialRxFifo.clear(); + break; +#endif +//OWEND } } @@ -376,6 +430,14 @@ void aux2SerialStop() { DMA_DeInit(AUX2_SERIAL_DMA_Stream_RX); USART_DeInit(AUX2_SERIAL_USART); +//OW +#if defined(TELEMETRY_MAVLINK) + if (aux2SerialMode == UART_MODE_MAVLINK) { + aux2SerialTxFifo.clear(); + mavlinkTelemAux2SerialRxFifo.clear(); + } +#endif +//OWEND } uint8_t aux2SerialTracesEnabled() @@ -401,6 +463,18 @@ extern "C" void AUX2_SERIAL_USART_IRQHandler(void) USART_ITConfig(AUX2_SERIAL_USART, USART_IT_TXE, DISABLE); } } +//OW +#if defined(TELEMETRY_MAVLINK) + if (aux2SerialMode == UART_MODE_MAVLINK) { + if (USART_GetITStatus(AUX2_SERIAL_USART, USART_IT_RXNE) != RESET) { + USART_ClearITPendingBit(AUX2_SERIAL_USART, USART_IT_RXNE); + uint8_t c = USART_ReceiveData(AUX2_SERIAL_USART); + mavlinkTelemAux2SerialRxFifo.push(c); + } + return; + } +#endif +//OWEND #if defined(CLI) if (getSelectedUsbMode() != USB_SERIAL_MODE) { diff --git a/radio/src/targets/common/arm/stm32/bootloader/CMakeLists.txt b/radio/src/targets/common/arm/stm32/bootloader/CMakeLists.txt index 62e9868ff97..a160a94fc61 100644 --- a/radio/src/targets/common/arm/stm32/bootloader/CMakeLists.txt +++ b/radio/src/targets/common/arm/stm32/bootloader/CMakeLists.txt @@ -154,6 +154,7 @@ remove_definitions(-DCLI) remove_definitions(-DSEMIHOSTING) remove_definitions(-DUSB_SERIAL) remove_definitions(-DWATCHDOG) +remove_definitions(-DTELEMETRY_MAVLINK) add_definitions(-DBOOT) diff --git a/radio/src/targets/common/arm/stm32/usb_driver.cpp b/radio/src/targets/common/arm/stm32/usb_driver.cpp index 7e2c5abf1dd..7ba7ceb5911 100644 --- a/radio/src/targets/common/arm/stm32/usb_driver.cpp +++ b/radio/src/targets/common/arm/stm32/usb_driver.cpp @@ -79,6 +79,9 @@ void usbStart() #endif #if defined(USB_SERIAL) case USB_SERIAL_MODE: +//OW + case USB_MAVLINK_MODE: +//OWEND // initialize USB as CDC device (virtual serial port) USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb); break; diff --git a/radio/src/targets/common/arm/stm32/usb_driver.h b/radio/src/targets/common/arm/stm32/usb_driver.h index 350ceace0d2..5855ae41c86 100644 --- a/radio/src/targets/common/arm/stm32/usb_driver.h +++ b/radio/src/targets/common/arm/stm32/usb_driver.h @@ -29,6 +29,9 @@ enum usbMode { USB_JOYSTICK_MODE, USB_MASS_STORAGE_MODE, USB_SERIAL_MODE, +//OW + USB_MAVLINK_MODE, +//OWEND #if defined(USB_SERIAL) USB_MAX_MODE=USB_SERIAL_MODE #else diff --git a/radio/src/targets/common/arm/stm32/usbd_cdc.cpp b/radio/src/targets/common/arm/stm32/usbd_cdc.cpp index 4d9409ec89c..f7783ca73ef 100644 --- a/radio/src/targets/common/arm/stm32/usbd_cdc.cpp +++ b/radio/src/targets/common/arm/stm32/usbd_cdc.cpp @@ -230,7 +230,13 @@ static uint16_t VCP_DataRx (uint8_t* Buf, uint32_t Len) } } #endif - +//OW +#if defined(TELEMETRY_MAVLINK) && defined(USB_SERIAL) + for (uint32_t i = 0; i < Len; i++) { + mavlinkTelemUsbRxFifo.push(Buf[i]); + } +#endif +//OWEND return USBD_OK; } diff --git a/radio/src/targets/common/arm/stm32/usbd_desc.c b/radio/src/targets/common/arm/stm32/usbd_desc.c index 47e322a34c6..84499a50d95 100644 --- a/radio/src/targets/common/arm/stm32/usbd_desc.c +++ b/radio/src/targets/common/arm/stm32/usbd_desc.c @@ -140,6 +140,9 @@ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length) break; case USB_SERIAL_MODE: +//OW + case USB_MAVLINK_MODE: +//OWEND vid = USBD_CDC_VID; pid = USBD_CDC_PID; break; @@ -211,6 +214,9 @@ uint8_t * USBD_USR_ProductStrDescriptor( uint8_t speed , uint16_t *length) USBD_GetString ((uint8_t*)USBD_HID_PRODUCT_FS_STRING, USBD_StrDesc, length); break; case USB_SERIAL_MODE: +//OW + case USB_MAVLINK_MODE: +//OWEND USBD_GetString ((uint8_t*)USBD_CDC_PRODUCT_FS_STRING, USBD_StrDesc, length); break; case USB_MASS_STORAGE_MODE: @@ -261,6 +267,9 @@ uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length) USBD_GetString ((uint8_t*)USBD_HID_CONFIGURATION_FS_STRING, USBD_StrDesc, length); break; case USB_SERIAL_MODE: +//OW + case USB_MAVLINK_MODE: +//OWEND USBD_GetString ((uint8_t*)USBD_CDC_CONFIGURATION_FS_STRING, USBD_StrDesc, length); break; case USB_MASS_STORAGE_MODE: @@ -285,6 +294,9 @@ uint8_t * USBD_USR_InterfaceStrDescriptor( uint8_t speed , uint16_t *length) USBD_GetString ((uint8_t*)USBD_HID_INTERFACE_FS_STRING, USBD_StrDesc, length); break; case USB_SERIAL_MODE: +//OW + case USB_MAVLINK_MODE: +//OWEND USBD_GetString ((uint8_t*)USBD_CDC_INTERFACE_FS_STRING, USBD_StrDesc, length); break; case USB_MASS_STORAGE_MODE: diff --git a/radio/src/targets/horus/CMakeLists.txt b/radio/src/targets/horus/CMakeLists.txt index bfbb3153e47..17144a392f2 100644 --- a/radio/src/targets/horus/CMakeLists.txt +++ b/radio/src/targets/horus/CMakeLists.txt @@ -47,6 +47,38 @@ endif() add_definitions(-DPCBFRSKY) +# //OW +if (TELEMETRY_MAVLINK) + if (DEBUG OR CLI) + #set(TELEMETRY_MAVLINK NO) + message(FATAL_ERROR "don't compile with DEBUG or CLI") + endif() +endif() +if (TELEMETRY_MAVLINK) + add_definitions(-DTELEMETRY_MAVLINK) + if (USB_SERIAL AND NOT DEBUG AND NOT CLI) + add_definitions(-DTELEMETRY_MAVLINK_USB_SERIAL) + endif() + # generate required header files from fastmavlink submodule + execute_process( + COMMAND ${PYTHON_EXECUTABLE} fmav_generate_c_library.py + WORKING_DIRECTORY ${RADIO_DIRECTORY}/src/${THIRDPARTY_DIR}/Mavlink + RESULT_VARIABLE FMAV_GENERATE_CLIB_RESULT + ) + if (NOT FMAV_GENERATE_CLIB_RESULT EQUAL "0") + message(FATAL_ERROR "fmav_generate_c_library.py could not be run: ${FMAV_GENERATE_CLIB_RESULT}") + endif() + execute_process( + COMMAND ${PYTHON_EXECUTABLE} fmav_generate_lua_lib.py + WORKING_DIRECTORY ${RADIO_DIRECTORY}/src/${THIRDPARTY_DIR}/Mavlink + RESULT_VARIABLE FMAV_GENERATE_LUALIB_RESULT + ) + if (NOT FMAV_GENERATE_LUALIB_RESULT EQUAL "0") + message(FATAL_ERROR "fmav_generate_lua_lib.py could not be run: ${FMAV_GENERATE_LUALIB_RESULT}") + endif() +endif() +# //OWEND + if (PCB STREQUAL X10) set(PCBREV "STD" CACHE STRING "PCB Revision") add_definitions(-DPCBX10) @@ -81,6 +113,16 @@ if (PCB STREQUAL X10) option(INTERNAL_MODULE_MULTI "Support for MULTI internal module" OFF) option(BLUETOOTH "Support for bluetooth module" OFF) add_definitions(-DMANUFACTURER_JUMPER) +# //OW + if (TELEMETRY_MAVLINK) + option(INTERNAL_GPS "Support for internal ublox GPS" OFF) + set(AUX_SERIAL ON) + if (NOT BLUETOOTH AND NOT INTERNAL_GPS) + set(AUX2_SERIAL ON) + endif() + set(INTERNAL_GPS_BAUDRATE "9600" CACHE STRING "Baud rate for internal GPS") + endif() +# //OWEND elseif (PCBREV STREQUAL TX16S) set(FLAVOUR tx16s) set(LUA_EXPORT lua_export_t16) @@ -108,6 +150,16 @@ if (PCB STREQUAL X10) option(BLUETOOTH "Support for bluetooth module" OFF) set(HARDWARE_TOUCH YES) add_definitions(-DMANUFACTURER_JUMPER) +# //OW + if (TELEMETRY_MAVLINK) + option(INTERNAL_GPS "Support for internal ublox GPS" OFF) + set(AUX_SERIAL ON) + if (NOT BLUETOOTH AND NOT INTERNAL_GPS) + set(AUX2_SERIAL ON) + endif() + set(INTERNAL_GPS_BAUDRATE "9600" CACHE STRING "Baud rate for internal GPS") + endif() +# //OWEND else() set(FLAVOUR x10) option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module" ON) @@ -271,6 +323,23 @@ if (SBUS_TRAINER) ) endif() +# //OW +if (TELEMETRY_MAVLINK) + set(SRC ${SRC} + telemetry/mavlink/mavlink_telem.cpp + telemetry/mavlink/mavlink_telem_interface.cpp + telemetry/mavlink/mavlink_telem_mavapi.cpp + telemetry/mavlink/mavlink_telem_autopilot.cpp + telemetry/mavlink/mavlink_telem_camera.cpp + telemetry/mavlink/mavlink_telem_gimbal.cpp + telemetry/mavlink/mavlink_telem_qshot.cpp + ) + if (NOT LUA STREQUAL NO) + set(SRC ${SRC} lua/api_mavlink.cpp lua/api_mavsdk.cpp) + endif() +endif() +# //OWEND + set(FIRMWARE_TARGET_SRC ${FIRMWARE_TARGET_SRC} ${LCD_DRIVER} diff --git a/radio/src/targets/horus/board.h b/radio/src/targets/horus/board.h index b248791a1b2..daf9e5d8a18 100644 --- a/radio/src/targets/horus/board.h +++ b/radio/src/targets/horus/board.h @@ -153,10 +153,25 @@ void SDRAM_Init(); #define INTERNAL_MODULE_OFF() GPIO_ResetBits(INTMODULE_PWR_GPIO, INTMODULE_PWR_GPIO_PIN) #endif -#define EXTERNAL_MODULE_ON() GPIO_SetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) -#define EXTERNAL_MODULE_OFF() GPIO_ResetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) -#define IS_INTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(INTMODULE_PWR_GPIO, INTMODULE_PWR_GPIO_PIN) == Bit_SET) -#define IS_EXTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) == Bit_SET) +//OW +//#define EXTERNAL_MODULE_ON() GPIO_SetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) +//#define EXTERNAL_MODULE_OFF() GPIO_ResetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) +//#define IS_INTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(INTMODULE_PWR_GPIO, INTMODULE_PWR_GPIO_PIN) == Bit_SET) +//#define IS_EXTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) == Bit_SET) +#if defined(TELEMETRY_MAVLINK) + void extmoduleOn(void); + void extmoduleOff(void); + #define EXTERNAL_MODULE_ON() extmoduleOn() + #define EXTERNAL_MODULE_OFF() extmoduleOff() + #define IS_INTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(INTMODULE_PWR_GPIO, INTMODULE_PWR_GPIO_PIN) == Bit_SET) + #define IS_EXTERNAL_MODULE_ON() (g_eeGeneral.mavlinkExternal != 1 && GPIO_ReadInputDataBit(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) == Bit_SET) +#else + #define EXTERNAL_MODULE_ON() GPIO_SetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) + #define EXTERNAL_MODULE_OFF() GPIO_ResetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) + #define IS_INTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(INTMODULE_PWR_GPIO, INTMODULE_PWR_GPIO_PIN) == Bit_SET) + #define IS_EXTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) == Bit_SET) +#endif +//OWEND #if !defined(PXX2) #define IS_PXX2_INTERNAL_ENABLED() (false) diff --git a/radio/src/targets/horus/hal.h b/radio/src/targets/horus/hal.h index 752ed29d4b9..6c0f969a9b9 100644 --- a/radio/src/targets/horus/hal.h +++ b/radio/src/targets/horus/hal.h @@ -479,6 +479,49 @@ #define AUX2_SERIAL_RCC_APB2Periph 0 #endif +//OW +#if (defined(RADIO_T16) || defined(RADIO_T18)) && defined(AUX_SERIAL) + #undef AUX_SERIAL_RCC_AHB1Periph + #undef AUX_SERIAL_RCC_APB1Periph + #undef AUX_SERIAL_RCC_APB2Periph + + #define AUX_SERIAL_RCC_AHB1Periph (RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_DMA1) + #define AUX_SERIAL_RCC_APB1Periph RCC_APB1Periph_USART3 + #define AUX_SERIAL_RCC_APB2Periph 0 + #define AUX_SERIAL_GPIO GPIOB + #define AUX_SERIAL_GPIO_PIN_TX GPIO_Pin_10 // PB.10 + #define AUX_SERIAL_GPIO_PIN_RX GPIO_Pin_11 // PB.11 + #define AUX_SERIAL_GPIO_PinSource_TX GPIO_PinSource10 + #define AUX_SERIAL_GPIO_PinSource_RX GPIO_PinSource11 + #define AUX_SERIAL_GPIO_AF GPIO_AF_USART3 + #define AUX_SERIAL_USART USART3 + #define AUX_SERIAL_USART_IRQHandler USART3_IRQHandler + #define AUX_SERIAL_USART_IRQn USART3_IRQn + #define AUX_SERIAL_DMA_Stream_RX DMA1_Stream1 + #define AUX_SERIAL_DMA_Channel_RX DMA_Channel_4 +#endif +#if (defined(RADIO_T16) || defined(RADIO_T18)) && defined(AUX2_SERIAL) + #undef AUX2_SERIAL_PWR_GPIO + #undef AUX2_SERIAL_PWR_GPIO_PIN +#endif +#if (defined(RADIO_T16) || defined(RADIO_T18)) && defined(INTERNAL_GPS) + #define GPS_RCC_AHB1Periph (RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOG) + #define GPS_RCC_APB1Periph 0 + #define GPS_RCC_APB2Periph RCC_APB2Periph_USART6 + #define GPS_USART USART6 + #define GPS_GPIO_AF GPIO_AF_USART6 + #define GPS_USART_IRQn USART6_IRQn + #define GPS_USART_IRQHandler USART6_IRQHandler + #define GPS_UART_GPIO GPIOG + #define GPS_TX_GPIO_PIN GPIO_Pin_14 // PG.14 + #define GPS_RX_GPIO_PIN GPIO_Pin_9 // PG.09 + #define GPS_TX_GPIO_PinSource GPIO_PinSource14 + #define GPS_RX_GPIO_PinSource GPIO_PinSource9 + #define GPS_PWR_GPIO GPIOB + #define GPS_PWR_GPIO_PIN GPIO_Pin_0 // PB.00 +#endif +//OWEND + // Telemetry #define TELEMETRY_RCC_AHB1Periph (RCC_AHB1Periph_GPIOD | RCC_AHB1Periph_DMA1) #define TELEMETRY_RCC_APB1Periph RCC_APB1Periph_USART2 diff --git a/radio/src/targets/horus/telemetry_driver.cpp b/radio/src/targets/horus/telemetry_driver.cpp index 165e65933f2..93a04492325 100644 --- a/radio/src/targets/horus/telemetry_driver.cpp +++ b/radio/src/targets/horus/telemetry_driver.cpp @@ -28,6 +28,11 @@ DMAFifo telemetryDMAFifo __DMA (TELEMETRY_DMA_Stream_RX); uint8_t telemetryFifoMode; #endif +//OW +extern Fifo mavlinkTelemExternalTxFifo_frame; +extern Fifo mavlinkTelemExternalRxFifo; +//OWEND + static void telemetryInitDirPin() { GPIO_InitTypeDef GPIO_InitStructure; @@ -42,6 +47,11 @@ static void telemetryInitDirPin() void telemetryPortInit(uint32_t baudrate, uint8_t mode) { +//OW +#if defined(TELEMETRY_MAVLINK) + if (g_eeGeneral.mavlinkExternal == 1) return; +#endif +//OWEND if (baudrate == 0) { USART_DeInit(TELEMETRY_USART); return; @@ -144,6 +154,11 @@ static uint16_t probeTimeFromStartBit; void telemetryPortInvertedInit(uint32_t baudrate) { +//OW +#if defined(TELEMETRY_MAVLINK) + if (g_eeGeneral.mavlinkExternal == 1) return; +#endif +//OWEND if (baudrate == 0) { NVIC_DisableIRQ(TELEMETRY_EXTI_IRQn); NVIC_DisableIRQ(TELEMETRY_TIMER_IRQn); @@ -241,6 +256,11 @@ void telemetryPortInvertedRxBit() void telemetryPortSetDirectionOutput() { +//OW +#if defined(TELEMETRY_MAVLINK) + if (g_eeGeneral.mavlinkExternal == 1) return; +#endif +//OWEND #if defined(GHOST) && SPORT_MAX_BAUDRATE < 400000 if (isModuleGhost(EXTERNAL_MODULE)) { TELEMETRY_USART->BRR = BRR_400K; @@ -257,6 +277,11 @@ void sportWaitTransmissionComplete() void telemetryPortSetDirectionInput() { +//OW +#if defined(TELEMETRY_MAVLINK) + if (g_eeGeneral.mavlinkExternal == 1) return; +#endif +//OWEND sportWaitTransmissionComplete(); #if defined(GHOST) && SPORT_MAX_BAUDRATE < 400000 if (isModuleGhost(EXTERNAL_MODULE) && g_eeGeneral.telemetryBaudrate == GHST_TELEMETRY_RATE_115K) { @@ -269,6 +294,11 @@ void telemetryPortSetDirectionInput() void sportSendByte(uint8_t byte) { +//OW +#if defined(TELEMETRY_MAVLINK) + if (g_eeGeneral.mavlinkExternal == 1) return; +#endif +//OWEND telemetryPortSetDirectionOutput(); while (!(TELEMETRY_USART->SR & USART_SR_TXE)); @@ -277,6 +307,11 @@ void sportSendByte(uint8_t byte) void sportSendByteLoop(uint8_t byte) { +//OW +#if defined(TELEMETRY_MAVLINK) + if (g_eeGeneral.mavlinkExternal == 1) return; +#endif +//OWEND telemetryPortSetDirectionOutput(); outputTelemetryBuffer.data[0] = byte; @@ -305,6 +340,11 @@ void sportSendByteLoop(uint8_t byte) void sportSendBuffer(const uint8_t * buffer, uint32_t count) { +//OW +#if defined(TELEMETRY_MAVLINK) + if (g_eeGeneral.mavlinkExternal == 1) return; +#endif +//OWEND telemetryPortSetDirectionOutput(); DMA_InitTypeDef DMA_InitStructure; @@ -350,6 +390,44 @@ extern "C" void TELEMETRY_DMA_TX_IRQHandler(void) extern "C" void TELEMETRY_USART_IRQHandler(void) { DEBUG_INTERRUPT(INT_TELEM_USART); + +//OW +#if defined(TELEMETRY_MAVLINK) + if (g_eeGeneral.mavlinkExternal == 1) { + + if (USART_GetITStatus(TELEMETRY_USART, USART_IT_TXE) != RESET) { + uint8_t txchar; + if (mavlinkTelemExternalTxFifo_frame.pop(txchar)) { + USART_SendData(TELEMETRY_USART, txchar); + } + else { + USART_ITConfig(TELEMETRY_USART, USART_IT_TXE, DISABLE); + // the uart send register is double buffered + // hence, the last byte is still send when we disable TXE + // hence, we can't switch to output here since this would kill the last byte + // so we enable TC interrupt and do it when + USART_ClearITPendingBit(TELEMETRY_USART, USART_IT_TC); + USART_ITConfig(TELEMETRY_USART, USART_IT_TC, ENABLE); + } + } + else if (USART_GetITStatus(TELEMETRY_USART, USART_IT_TC) != RESET) { + USART_ITConfig(TELEMETRY_USART, USART_IT_TC, DISABLE); + USART_ClearITPendingBit(TELEMETRY_USART, USART_IT_TC); + TELEMETRY_DIR_GPIO->BSRRH = TELEMETRY_DIR_GPIO_PIN; // output disable + TELEMETRY_USART->CR1 |= USART_CR1_RE; // turn on receiver + } + + if (USART_GetITStatus(TELEMETRY_USART, USART_IT_RXNE) != RESET) { + USART_ClearITPendingBit(TELEMETRY_USART, USART_IT_RXNE); + uint8_t c = USART_ReceiveData(TELEMETRY_USART); + mavlinkTelemExternalRxFifo.push(c); + } + + return; + } +#endif +//OWEND + uint32_t status = TELEMETRY_USART->SR; if ((status & USART_SR_TC) && (TELEMETRY_USART->CR1 & USART_CR1_TCIE)) { diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp index 6411827ad78..57777f6b064 100644 --- a/radio/src/tasks.cpp +++ b/radio/src/tasks.cpp @@ -41,6 +41,11 @@ void stackPaint() #if defined(CLI) cliStack.paint(); #endif +//OW +#if defined(TELEMETRY_MAVLINK) + mavlinkStack.paint(); +#endif +//OWEND } volatile uint16_t timeForcePowerOffPressed = 0; @@ -327,5 +332,12 @@ void tasksStart() #if !defined(SIMU) RTOS_CREATE_TASK(audioTaskId, audioTask, "audio", audioStack, AUDIO_STACK_SIZE, AUDIO_TASK_PRIO); #endif + +//OW +#if defined(TELEMETRY_MAVLINK) + mavlinkStart(); +#endif +//OWEND + RTOS_START(); } diff --git a/radio/src/tasks.h b/radio/src/tasks.h index 6f761d84394..87f2990151e 100644 --- a/radio/src/tasks.h +++ b/radio/src/tasks.h @@ -49,6 +49,16 @@ extern RTOS_DEFINE_STACK(audioStack, AUDIO_STACK_SIZE); extern RTOS_MUTEX_HANDLE mixerMutex; +//OW +#if defined(TELEMETRY_MAVLINK) +#define MAVLINK_STACK_SIZE 400 //consumes 4x +#define MAVLINK_TASK_PRIO 8 + +extern RTOS_TASK_HANDLE mavlinkTaskId; +extern RTOS_DEFINE_STACK(mavlinkStack, MAVLINK_STACK_SIZE); +#endif +//OWEND + void stackPaint(); void tasksStart(); diff --git a/radio/src/telemetry/mavlink/mavlink_telem.cpp b/radio/src/telemetry/mavlink/mavlink_telem.cpp new file mode 100644 index 00000000000..0dc0e9bdefd --- /dev/null +++ b/radio/src/telemetry/mavlink/mavlink_telem.cpp @@ -0,0 +1,667 @@ +/* + * (c) www.olliw.eu, OlliW, OlliW42 + */ + +#include "opentx.h" + +MAVLINK_RAM_SECTION MavlinkTelem mavlinkTelem; + +// -- TASK handlers -- +// tasks can be set directly with SETTASK() +// some tasks don't need immediate execution, or need reliable request +// this is what these handlers are for +// they push the task to a fifo, and also allow to set number of retries and retry rates + +void MavlinkTelem::push_task(uint8_t idx, uint32_t task) +{ + struct Task t = {.task = task, .idx = idx}; + _taskFifo.push(t); +} + +void MavlinkTelem::pop_and_set_task(void) +{ + struct Task t; + if (_taskFifo.pop(t)) SETTASK(t.idx, t.task); +} + +// -- REQUEST handlers -- + +void MavlinkTelem::set_request(uint8_t idx, uint32_t task, uint8_t retry, tmr10ms_t rate) +{ + push_task(idx, task); + + _request_is_waiting[idx] |= task; + + if (retry == 0) return; // well, if there would be another pending we would not kill it + + int8_t empty_i = -1; + + // first check if request is already pending, at the same time find free slot, to avoid having to loop twice + for (uint16_t i = 0; i < REQUESTLIST_MAX; i++) { + //TODO: should we modify the retry & rate of the pending task? + if ((_requestList[i].idx == idx) && (_requestList[i].task == task)) return; // already pending, we can get out of here + if ((empty_i < 0) && !_requestList[i].task) empty_i = i; // free slot + } + + // if not already pending, add it + if (empty_i < 0) return; // no free slot + + _requestList[empty_i].task = task; + _requestList[empty_i].idx = idx; + _requestList[empty_i].retry = retry; + _requestList[empty_i].tlast = get_tmr10ms(); + _requestList[empty_i].trate = rate; +} + +void MavlinkTelem::clear_request(uint8_t idx, uint32_t task) +{ + for (uint16_t i = 0; i < REQUESTLIST_MAX; i++) { + if ((_requestList[i].idx == idx) && (_requestList[i].task == task)) { + _requestList[i].task = 0; + _request_is_waiting[idx] &=~ task; + } + } +} + +// what happens if a clear never comes? +// well, this is what retry = UINT8_MAX says, right + +void MavlinkTelem::do_requests(void) +{ + tmr10ms_t tnow = get_tmr10ms(); + + for (uint16_t i = 0; i < TASKIDX_MAX; i++) _request_is_waiting[i] = 0; + + for (uint16_t i = 0; i < REQUESTLIST_MAX; i++) { + if (!_requestList[i].task) continue; + + _request_is_waiting[_requestList[i].idx] |= _requestList[i].task; + + if ((tnow - _requestList[i].tlast) >= _requestList[i].trate) { + push_task(_requestList[i].idx, _requestList[i].task); + _requestList[i].tlast = get_tmr10ms(); + if (_requestList[i].retry < UINT8_MAX) { + if (_requestList[i].retry) _requestList[i].retry--; + if (!_requestList[i].retry) _requestList[i].task = 0; // clear request + } + } + } + + if ((tnow - _taskFifo_tlast) > 6) { // 60 ms decimation + _taskFifo_tlast = tnow; + // change this, so that it skips tasks with 0, this would allow an easy means to clear tasks also in the Fifo + if (!_taskFifo.isEmpty()) pop_and_set_task(); + } +} + +// -- Generate MAVLink messages -- +// these should never be called directly, should only by called by the task handler + +void MavlinkTelem::_generateCmdLong( + uint8_t tsystem, uint8_t tcomponent, uint16_t cmd, + float p1, float p2, float p3, float p4, float p5, float p6, float p7) +{ + fmav_msg_command_long_pack( + &_msg_out, _my_sysid, _my_compid, + tsystem, tcomponent, cmd, 0, p1, p2, p3, p4, p5, p6, p7, + &_status_out + ); + _msg_out_available = true; +} + +void MavlinkTelem::generateHeartbeat(uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) +{ + fmav_msg_heartbeat_pack( + &_msg_out, _my_sysid, _my_compid, + MAV_TYPE_GCS, MAV_AUTOPILOT_INVALID, base_mode, custom_mode, system_status, + &_status_out + ); + _msg_out_available = true; +} + +void MavlinkTelem::generateParamRequestList(uint8_t tsystem, uint8_t tcomponent) +{ + fmav_msg_param_request_list_pack( + &_msg_out, _my_sysid, _my_compid, + tsystem, tcomponent, + &_status_out + ); + _msg_out_available = true; +} + +void MavlinkTelem::generateParamRequestRead(uint8_t tsystem, uint8_t tcomponent, const char* param_name) +{ +char param_id[16]; + + strncpy(param_id, param_name, 16); + fmav_msg_param_request_read_pack( + &_msg_out, _my_sysid, _my_compid, + tsystem, tcomponent, param_id, -1, + &_status_out + ); + _msg_out_available = true; +} + +void MavlinkTelem::generateRequestDataStream( + uint8_t tsystem, uint8_t tcomponent, uint8_t data_stream, uint16_t rate_hz, uint8_t startstop) +{ + fmav_msg_request_data_stream_pack( + &_msg_out, _my_sysid, _my_compid, + tsystem, tcomponent, data_stream, rate_hz, startstop, + &_status_out + ); + _msg_out_available = true; +} + +// ArduPilot: ignores param7 +void MavlinkTelem::generateCmdSetMessageInterval(uint8_t tsystem, uint8_t tcomponent, uint8_t msgid, int32_t period_us, uint8_t startstop) +{ + _generateCmdLong(tsystem, tcomponent, MAV_CMD_SET_MESSAGE_INTERVAL, msgid, (startstop) ? period_us : -1.0f); +} + +// -- Main message handler for incoming MAVLink messages -- + +void MavlinkTelem::handleMessage(void) +{ + if (_msg.sysid == 0) return; //this can't be anything meaningful + + // autodetect sys id, and handle autopilot connecting + if (!isSystemIdValid() || (autopilot.compid == 0)) { + if (_msg.msgid == FASTMAVLINK_MSG_ID_HEARTBEAT) { + fmav_heartbeat_t payload; + fmav_msg_heartbeat_decode(&payload, &_msg); + if ((_msg.compid == MAV_COMP_ID_AUTOPILOT1) || (payload.autopilot != MAV_AUTOPILOT_INVALID)) { + _sysid = _msg.sysid; + autopilottype = payload.autopilot; + vehicletype = payload.type; + _resetAutopilot(); + autopilot.compid = _msg.compid; + autopilot.requests_triggered = 1; // we need to postpone and schedule them + } + } + if (!isSystemIdValid()) return; + } + + msg_rx_count++; + _msg_rx_persec_cnt++; + _bytes_rx_persec_cnt += fmav_msg_frame_len(&_msg); + + // discoverers + // somewhat inefficient, lots of heartbeat decodes, we probably want a separate heartbeat handler + + if ((camera.compid == 0) && (_msg.msgid == FASTMAVLINK_MSG_ID_HEARTBEAT)) { + fmav_heartbeat_t payload; + fmav_msg_heartbeat_decode(&payload, &_msg); + if ( (payload.autopilot == MAV_AUTOPILOT_INVALID) && + ( (payload.type == MAV_TYPE_CAMERA) || + ((_msg.compid >= MAV_COMP_ID_CAMERA) && (_msg.compid <= MAV_COMP_ID_CAMERA6)) ) ) { + _resetCamera(); + camera.compid = _msg.compid; + camera.requests_triggered = 1; //we schedule them + } + } + + if ((gimbal.compid == 0) && (_msg.msgid == FASTMAVLINK_MSG_ID_HEARTBEAT)) { + fmav_heartbeat_t payload; + fmav_msg_heartbeat_decode(&payload, &_msg); + if ( (payload.autopilot == MAV_AUTOPILOT_INVALID) && + ( (payload.type == MAV_TYPE_GIMBAL) || + ((_msg.compid == MAV_COMP_ID_GIMBAL) || + ((_msg.compid >= MAV_COMP_ID_GIMBAL2) && (_msg.compid <= MAV_COMP_ID_GIMBAL6))) ) ) { + _resetGimbalAndGimbalClient(); + gimbal.compid = _msg.compid; + gimbal.is_initialized = true; //no startup requests, so true + } + } + + if ((gimbalmanager.compid == 0) && (gimbal.compid > 0) && (_msg.msgid == FASTMAVLINK_MSG_ID_STORM32_GIMBAL_MANAGER_STATUS)) { + fmav_storm32_gimbal_manager_status_t payload; + fmav_msg_storm32_gimbal_manager_status_decode(&payload, &_msg); + if (payload.gimbal_id == gimbal.compid) { //this is the gimbal's gimbal manager + _resetGimbalClient(); + gimbalmanager.compid = _msg.compid; + gimbalmanagerOut.device_flags = payload.device_flags; + gimbalmanagerOut.manager_flags = payload.manager_flags; + gimbalmanager.requests_triggered = 1; //we schedule them + } + } + + // reset receiving timeout, but ignore RADIO_STATUS + if (_msg.msgid != FASTMAVLINK_MSG_ID_RADIO_STATUS) { + _is_receiving = MAVLINK_TELEM_RECEIVING_TIMEOUT; + } + + // MAVLINK API + mavapiHandleMessage(&_msg); + + // MAVSDK + // also try to convert the MAVLink messages to FrSky sensors + + // RADIO_STATUS is somewhat tricky, this may need doing it better if there are more sources of it + // SiK comes as vehicle 51, comp 68! + // it must NOT be rated as _is_recieving! + if (_msg.msgid == FASTMAVLINK_MSG_ID_RADIO_STATUS) { + fmav_radio_status_t payload; + fmav_msg_radio_status_decode(&payload, &_msg); + radio.rssi = payload.rssi; + radio.remrssi = payload.remrssi; + radio.noise = payload.noise; + radio.remnoise = payload.remnoise; + radio.is_receiving = MAVLINK_TELEM_RADIO_RECEIVING_TIMEOUT; + telemetrySetRssiValue(radio.rssi); + return; + } + + //we handle all qshot wherever they come from + handleMessageQShot(); + + if (_msg.sysid != _sysid) return; //this is not from our system + + // handle messages coming from autopilot + if (autopilot.compid && (_msg.compid == autopilot.compid)) { + handleMessageAutopilot(); + } + if (camera.compid && (_msg.compid == camera.compid)) { + handleMessageCamera(); + } + if (gimbal.compid && (_msg.compid == gimbal.compid)) { + handleMessageGimbal(); + } + if (gimbalmanager.compid && (_msg.compid == gimbalmanager.compid)) { + handleMessageGimbalClient(); + } +} + +// -- Main task handler -- + +void MavlinkTelem::doTask(void) +{ + tmr10ms_t tnow = get_tmr10ms(); + + bool tick_1Hz = false; + + if ((tnow - _my_heartbeat_tlast) > 100) { //1 sec + _my_heartbeat_tlast = tnow; + SETTASK(TASK_ME, TASK_SENDMYHEARTBEAT); + + msg_rx_persec = _msg_rx_persec_cnt; + bytes_rx_persec = _bytes_rx_persec_cnt; + _msg_rx_persec_cnt = 0; + _bytes_rx_persec_cnt = 0; + + msg_tx_persec = _msg_tx_persec_cnt; + bytes_tx_persec = _bytes_tx_persec_cnt; + _msg_tx_persec_cnt = 0; + _bytes_tx_persec_cnt = 0; + + tick_1Hz = true; + } + + if (!isSystemIdValid()) return; + + // trigger startup requests + + // we need to wait until at least one heartbeat was send out before requesting data streams + if (autopilot.compid && autopilot.requests_triggered) { + if (tick_1Hz) autopilot.requests_triggered++; + if (autopilot.requests_triggered > 3) { // wait for 3 heartbeats + autopilot.requests_triggered = 0; + setAutopilotStartupRequests(); + } + } + + // we wait until at least one heartbeat was send out, and autopilot requests have been done + if (camera.compid && camera.requests_triggered && !autopilot.requests_triggered) { + if (tick_1Hz) camera.requests_triggered++; + if (camera.requests_triggered > 1) { // wait for the next heartbeat + camera.requests_triggered = 0; + setCameraStartupRequests(); + } + } + + // we wait until at least one heartbeat was send out, and autopilot requests have been done + if (gimbal.compid && gimbal.requests_triggered && !autopilot.requests_triggered) { + if (tick_1Hz) gimbal.requests_triggered++; + if (gimbal.requests_triggered > 1) { // wait for the next heartbeat + gimbal.requests_triggered = 0; + setGimbalStartupRequests(); + } + } + if (gimbalmanager.compid && gimbalmanager.requests_triggered && !autopilot.requests_triggered) { + if (tick_1Hz) gimbalmanager.requests_triggered++; + if (gimbalmanager.requests_triggered > 1) { // wait for the next heartbeat + gimbalmanager.requests_triggered = 0; + setGimbalClientStartupRequests(); + } + } + + if (!autopilot.is_initialized) autopilot.is_initialized = (autopilot.requests_waiting_mask == 0); + + if (!camera.is_initialized) camera.is_initialized = (camera.requests_waiting_mask == 0); + + if (!gimbal.is_initialized) gimbal.is_initialized = (gimbal.requests_waiting_mask == 0); + + if (!gimbalmanager.is_initialized) gimbalmanager.is_initialized = (gimbalmanager.requests_waiting_mask == 0); + + // handle pending requests + do_requests(); + + // do rc override + // ArduPilot has a DAMED BUG!!! + // per MAVLink spec 0 and UNIT16_MAX should not be considered for channels >= 8, but it doesn't do it for 0 + // but we can hope that it handles 0 for the higher channels + if (g_model.mavlinkRcOverride && param.SYSID_MYGCS >= 0) { + if ((tnow - _rcoverride_tlast) >= 5) { //50 ms + _rcoverride_tlast = tnow; + for (uint8_t i = 0; i < 8; i++) { + /* would this be the right way to figure out which output is actually active ?? + MixData * md; + if (i < MAX_MIXERS && (md=mixAddress(i))->srcRaw && md->destCh == i) { + int value = channelOutputs[i] + 2 * PPM_CH_CENTER(i) - 2 * PPM_CENTER; + _tovr_chan_raw[i] = value; + } + else { + _tovr_chan_raw[i] = UINT16_MAX; + }*/ + // the first four channels may not be ordered like with transmitter!! + int value = channelOutputs[i]/2 + PPM_CH_CENTER(i); + _tovr_chan_raw[i] = value; + } + for (uint8_t i = 8; i < 18; i++) { + _tovr_chan_raw[i] = 0; + } + SETTASK(TASK_AUTOPILOT, TASK_SENDMSG_RC_CHANNELS_OVERRIDE); + } + } + + // handle pending tasks + // do only one task and hence one msg_out per loop + if (!_msg_out_available && TASK_IS_PENDING()) { + //other TASKS + if (doTaskAutopilot()) return; + if (doTaskGimbalAndGimbalClient()) return; + if (doTaskCamera()) return; + + //TASK_ME + if (_task[TASK_ME] & TASK_SENDMYHEARTBEAT) { + RESETTASK(TASK_ME, TASK_SENDMYHEARTBEAT); + uint8_t base_mode = MAV_MODE_PREFLIGHT | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | MAV_MODE_FLAG_SAFETY_ARMED; + uint8_t system_status = MAV_STATE_UNINIT | MAV_STATE_ACTIVE; + uint32_t custom_mode = 0; + generateHeartbeat(base_mode, custom_mode, system_status); + return; // do only one per loop + } + if (_task[TASK_ME] & TASK_SENDMSG_MAVLINK_API) { + RESETTASK(TASK_ME, TASK_SENDMSG_MAVLINK_API); + mavapiGenerateMessage(); + return; // do only one per loop + } + + //other TASKS low priority + if (doTaskAutopilotLowPriority()) return; + if (doTaskCameraLowPriority()) return; + if (doTaskQShot()) return; + } +} + +// -- Wakeup call from OpenTx -- +// this is the main entry point + +// ourself = link 0 +// serial1 = link 1 +// serial2 = link 2 +// usb = link 3 + +void MavlinkTelem::wakeup() +{ + // track configuration changes + bool aux1_enabled = (g_eeGeneral.auxSerialMode == UART_MODE_MAVLINK); + bool aux2_enabled = (g_eeGeneral.aux2SerialMode == UART_MODE_MAVLINK); +#if defined(TELEMETRY_MAVLINK_USB_SERIAL) + bool usb_enabled = (getSelectedUsbMode() == USB_MAVLINK_MODE); +#else + bool usb_enabled = false; +#endif + bool external_enabled = (g_eeGeneral.mavlinkExternal == 1) && !s_pulses_paused; + + if ((_aux1_enabled != aux1_enabled) || (_aux2_enabled != aux2_enabled) || + (_aux1_baudrate != g_eeGeneral.mavlinkBaudrate) || (_aux2_baudrate != g_eeGeneral.mavlinkBaudrate2) || + (_external_enabled != external_enabled)) { + _aux1_enabled = aux1_enabled; + _aux2_enabled = aux2_enabled; + _aux1_baudrate = g_eeGeneral.mavlinkBaudrate; + _aux2_baudrate = g_eeGeneral.mavlinkBaudrate2; + _external_enabled = external_enabled; + mavlinkTelemExternal_init(external_enabled); + map_serials(); + _reset(); + } + + if (_usb_enabled != usb_enabled) { + _usb_enabled = usb_enabled; + fmav_router_clearout_link(3); + } + + if (external_enabled) mavlinkTelemExternal_wakeup(); + + // skip out if not one of the serial1, serial2 is enabled + if (!serial1_enabled && !serial1_enabled) return; + + // look for incoming messages on all channels + // only do one at a time + #define INCc(x,p) {x++; if(x >= p) x = 0;} + + INCc(_scheduled_serial, 3); + uint8_t currently_scheduled_serial = _scheduled_serial; + + uint32_t available = 0; + switch (currently_scheduled_serial) { + case 0: available = mavlinkTelem1Available(); break; + case 1: available = mavlinkTelem2Available(); break; + case 2: available = mavlinkTelem3Available(); break; + } + if (available > 128) available = 128; // 128 = 22 ms @ 57600 bps + + uint8_t c; + fmav_result_t result; + + // read serial1 + if (currently_scheduled_serial == 0) { + for (uint32_t i = 0; i < available; i++) { + if (!mavlinkTelem1Getc(&c)) break; + if (fmav_parse_and_check_to_frame_buf(&result, _buf1, &_status1, c)) { + fmav_router_handle_message(1, &result); + if (fmav_router_send_to_link(1)) {} // WE DO NOT REFLECT, SO THIS MUST NEVER HAPPEN !! + if (fmav_router_send_to_link(2)) { mavlinkTelem2PutBuf(_buf1, result.frame_len); } + if (fmav_router_send_to_link(3)) { mavlinkTelem3PutBuf(_buf1, result.frame_len); } + if (result.res == FASTMAVLINK_PARSE_RESULT_OK && fmav_router_send_to_link(0)) { + fmav_frame_buf_to_msg(&_msg, &result, _buf1); + handleMessage(); // checks _msg, and puts any result into a task queue + } + } + } + } + + // read serial2 + if (currently_scheduled_serial == 1) { + for (uint32_t i = 0; i < available; i++) { + if (!mavlinkTelem2Getc(&c)) break; + if (fmav_parse_and_check_to_frame_buf(&result, _buf2, &_status2, c)) { + fmav_router_handle_message(2, &result); + if (fmav_router_send_to_link(1)) { mavlinkTelem1PutBuf(_buf2, result.frame_len); } + if (fmav_router_send_to_link(2)) {} // WE DO NOT REFLECT, SO THIS MUST NEVER HAPPEN !! + if (fmav_router_send_to_link(3)) { mavlinkTelem3PutBuf(_buf2, result.frame_len); } + if (result.res == FASTMAVLINK_PARSE_RESULT_OK && fmav_router_send_to_link(0)) { + fmav_frame_buf_to_msg(&_msg, &result, _buf2); + handleMessage(); // checks _msg, and puts any result into a task queue + } + } + } + } + + // read serial3 = usb + if (currently_scheduled_serial == 2) { + for (uint32_t i = 0; i < available; i++) { + if (!mavlinkTelem3Getc(&c)) break; + if (fmav_parse_and_check_to_frame_buf(&result, _buf3, &_status3, c)) { + fmav_router_handle_message(3, &result); + if (fmav_router_send_to_link(1)) { mavlinkTelem1PutBuf(_buf3, result.frame_len); } + if (fmav_router_send_to_link(2)) { mavlinkTelem2PutBuf(_buf3, result.frame_len); } + if (fmav_router_send_to_link(3)) {} // WE DO NOT REFLECT, SO THIS MUST NEVER HAPPEN !! + if (result.res == FASTMAVLINK_PARSE_RESULT_OK && fmav_router_send_to_link(0)) { + fmav_frame_buf_to_msg(&_msg, &result, _buf3); + handleMessage(); // checks _msg, and puts any result into a task queue + } + } + } + } + + // do tasks + doTask(); // checks task queue _msg, and puts one result into _msg_out + + // send out pending message + if (_msg_out_available) { + fmav_router_handle_message_by_msg(0, &_msg_out); + if (fmav_router_send_to_link(1) || fmav_router_send_to_link(2) || fmav_router_send_to_link(3)) { + uint16_t count = fmav_msg_to_frame_buf(_buf_out, &_msg_out); + // check that message can be send to all enabled serials + if ((!serial1_enabled || mavlinkTelem1HasSpace(count)) && + (!serial2_enabled || mavlinkTelem2HasSpace(count)) && + (!_usb_enabled || mavlinkTelem3HasSpace(count))) { + if (serial1_enabled && fmav_router_send_to_link(1)) mavlinkTelem1PutBuf(_buf_out, count); + if (serial2_enabled && fmav_router_send_to_link(2)) mavlinkTelem2PutBuf(_buf_out, count); + if (_usb_enabled && fmav_router_send_to_link(3)) mavlinkTelem3PutBuf(_buf_out, count); + _msg_out_available = false; + msg_tx_count++; + _msg_tx_persec_cnt++; + _bytes_tx_persec_cnt += count; + } + } else { + _msg_out_available = false; // message is targeted at unknown component + } + } +} + +// -- 10 ms tick -- + +void MavlinkTelem::tick10ms() +{ + #define check(x,y) if(x){ (x)--; if(!(x)){ (y); }} + + check(_is_receiving, _reset()); + + check(radio.is_receiving, _resetRadio()); + check(radio.is_receiving65, _resetRadio65()); + check(radio.is_receiving35, _resetRadio35()); + + check(autopilot.is_receiving, _resetAutopilot()); + check(gimbal.is_receiving, _resetGimbalAndGimbalClient()); + check(gimbalmanager.is_receiving, _resetGimbalClient()); + check(camera.is_receiving, _resetCamera()); + + // keep 10us timer updated + time10us(); +} + +// -- Resets -- + +void MavlinkTelem::_resetRadio(void) +{ + radio.is_receiving = 0; + + radio.rssi = UINT8_MAX; + radio.remrssi = UINT8_MAX; + radio.noise = 0; + radio.remnoise = 0; + + telemetryResetRssiValue(); +} + +void MavlinkTelem::_resetRadio65(void) +{ + radio.is_receiving65 = 0; + radio.rssi65 = UINT8_MAX; + + telemetryResetRssiValue(); +} + +void MavlinkTelem::_resetRadio35(void) +{ + radio.is_receiving35 = 0; + radio.rssi35 = UINT8_MAX; + + telemetryResetRssiValue(); +} + +void MavlinkTelem::_reset(void) +{ +#if defined(CLI) || defined(DEBUG) +#define UART_MODE_NONE_OR_DEBUG UART_MODE_DEBUG +#else +#define UART_MODE_NONE_OR_DEBUG UART_MODE_NONE +#endif +#if !defined(AUX_SERIAL) + if (g_eeGeneral.auxSerialMode == UART_MODE_MAVLINK) g_eeGeneral.auxSerialMode = UART_MODE_NONE_OR_DEBUG; +#endif +#if !defined(AUX2_SERIAL) + if (g_eeGeneral.aux2SerialMode == UART_MODE_MAVLINK) g_eeGeneral.aux2SerialMode = UART_MODE_NONE_OR_DEBUG; +#endif + if (g_eeGeneral.mavlinkExternal > 1) g_eeGeneral.mavlinkExternal = 0; + + _my_sysid = MAVLINK_TELEM_MY_SYSID; + _my_compid = MAVLINK_TELEM_MY_COMPID; + + _sysid = 0; + autopilottype = MAV_AUTOPILOT_GENERIC; //TODO: shouldn't these be in _resetAutopilot() ?? + vehicletype = MAV_TYPE_GENERIC; + flightmode = 0; + + for (uint16_t i = 0; i < TASKIDX_MAX; i++) _task[i] = 0; + _taskFifo.clear(); + _taskFifo_tlast = 0; + for (uint16_t i = 0; i < REQUESTLIST_MAX; i++) _requestList[i].task = 0; + + _resetRadio(); + _resetRadio65(); + _resetRadio35(); + radio.rssi_scaled = 0; + radio.rssi_voice_disabled = false; + + _resetAutopilot(); + _resetGimbalAndGimbalClient(); + _resetCamera(); + + _resetQShot(); + + fmav_status_reset(&_status1); + fmav_status_reset(&_status2); + fmav_status_reset(&_status3); + fmav_status_reset(&_status_out); + fmav_router_reset(); + fmav_router_add_ourself(MAVLINK_TELEM_MY_SYSID, MAVLINK_TELEM_MY_COMPID); + + msg_rx_count = 0; + msg_rx_persec = 0; + bytes_rx_persec = 0; + _msg_rx_persec_cnt = 0; + _bytes_rx_persec_cnt = 0; + + msg_tx_count = 0; + msg_tx_persec = 0; + bytes_tx_persec = 0; + _msg_tx_persec_cnt = 0; + _bytes_tx_persec_cnt = 0; + + mavapiInit(); +} + + +void MavlinkTelem::_init(void) +{ + fmav_router_init(); + fmav_router_set_link_properties_all( + FASTMAVLINK_ROUTER_LINK_PROPERTY_FLAG_ALWAYS_SEND_HEARTBEAT | + FASTMAVLINK_ROUTER_LINK_PROPERTY_FLAG_DISCOVER_BY_HEARTBEAT + ); + _reset(); +} diff --git a/radio/src/telemetry/mavlink/mavlink_telem.h b/radio/src/telemetry/mavlink/mavlink_telem.h new file mode 100644 index 00000000000..7c7c656fe25 --- /dev/null +++ b/radio/src/telemetry/mavlink/mavlink_telem.h @@ -0,0 +1,892 @@ +/* + * (c) www.olliw.eu, OlliW, OlliW42 + */ + +#define MAVLINKTELEMVERSIONSTR "v27" //OW + +#define MAVLINK_RAM_SECTION __attribute__((section (".ram"))) + +// -- CoOS RTOS mavlink task handlers -- + +void mavlinkStart(); +uint16_t mavlinkTaskRunTime(void); +uint16_t mavlinkTaskRunTimeMax(void); +uint16_t mavlinkTaskLoop(void); + +// -- SERIAL and USB CDC handlers -- + +uint32_t mavlinkTelemAuxBaudrate(void); +uint32_t mavlinkTelemAux2Baudrate(void); + +#if defined(AUX_SERIAL) +extern Fifo auxSerialTxFifo; +extern Fifo mavlinkTelemAuxSerialRxFifo; +#endif + +#if defined(AUX2_SERIAL) +extern Fifo aux2SerialTxFifo; +extern Fifo mavlinkTelemAux2SerialRxFifo; +#endif + +#if defined(TELEMETRY_MAVLINK) && defined(USB_SERIAL) +extern Fifo mavlinkTelemUsbRxFifo; +#endif + +//TODO: since we want to allow only 2 channels max, this is memory waste, +//i.e., we should just have 2 instead of three Fifos. +extern Fifo mavlinkTelemExternalTxFifo_frame; +extern Fifo mavlinkTelemExternalRxFifo; + +void mavlinkTelemExternal_init(bool flag); +void mavlinkTelemExternal_wakeup(void); + +uint32_t mavlinkTelem1Available(void); +uint8_t mavlinkTelem1Getc(uint8_t *c); +bool mavlinkTelem1HasSpace(uint16_t count); +bool mavlinkTelem1PutBuf(const uint8_t *buf, const uint16_t count); + +uint32_t mavlinkTelem2Available(void); +uint8_t mavlinkTelem2Getc(uint8_t *c); +bool mavlinkTelem2HasSpace(uint16_t count); +bool mavlinkTelem2PutBuf(const uint8_t *buf, const uint16_t count); + +uint32_t mavlinkTelem3Available(void); +uint8_t mavlinkTelem3Getc(uint8_t *c); +bool mavlinkTelem3HasSpace(uint16_t count); +bool mavlinkTelem3PutBuf(const uint8_t *buf, const uint16_t count); + +// -- fastMavlink -- + +#define FASTMAVLINK_RAM_SECTION static MAVLINK_RAM_SECTION + +#define FASTMAVLINK_ROUTER_LINKS_MAX 4 +#define FASTMAVLINK_ROUTER_COMPONENTS_MAX 12 + +#include "thirdparty/Mavlink/out/edgetx/edgetx.h" +#include "thirdparty/Mavlink/out/lib/fastmavlink_router.h" + +// -- main Mavlink stuff -- + +#define MAVLINK_TELEM_MY_SYSID 254 //MissionPlanner is 255, QGroundControl is 255 +#define MAVLINK_TELEM_MY_COMPID (MAV_COMP_ID_MISSIONPLANNER + 4) //191 is companion, 194 is free + +// tick10ms() is called every 10 ms from 10ms ISR +// if this is changed, timing needs to be adapted !! +#define MAVLINK_TELEM_RECEIVING_TIMEOUT 330 // 3.3 secs +#define MAVLINK_TELEM_RADIO_RECEIVING_TIMEOUT 330 // 3.3 secs +#define MAVLINK_TELEM_GIMBALMANAGER_RECEIVING_TIMEOUT 330 // 3.3 secs ATTENTION: a GM may emit at slow rate + +//COMMENT: +// except of where noted, functions/structs use units of the MAVLink message +// the mavsdk caller/setter functions however use native units, and deg, whenever possible + +class MavlinkTelem +{ + public: + MavlinkTelem() { _init(); } // constructor + + void wakeup(); + void tick10ms(); + + // GENERATE MAVLink messages + + void _generateCmdLong(uint8_t tsystem, uint8_t tcomponent, uint16_t cmd, float p1=0.0f, float p2=0.0f, float p3=0.0f, float p4=0.0f, float p5=0.0f, float p6=0.0f, float p7=0.0f); + void generateHeartbeat(uint8_t base_mode, uint32_t custom_mode, uint8_t system_status); + void generateParamRequestList(uint8_t tsystem, uint8_t tcomponent); + void generateParamRequestRead(uint8_t tsystem, uint8_t tcomponent, const char* param_name); + // autopilot + void generateRequestDataStream(uint8_t tsystem, uint8_t tcomponent, uint8_t data_stream, uint16_t rate_hz, uint8_t startstop); + void generateCmdSetMessageInterval(uint8_t tsystem, uint8_t tcomponent, uint8_t msgid, int32_t period_us, uint8_t startstop); + void generateCmdDoSetMode(uint8_t tsystem, uint8_t tcomponent, MAV_MODE base_mode, uint32_t custom_mode); + void generateCmdNavTakeoff(uint8_t tsystem, uint8_t tcomponent, float alt_m, bool hor_nav_by_pilot); + void generateCmdDoChangeSpeed(uint8_t tsystem, uint8_t tcomponent, float speed_mps, uint16_t speed_type, bool relative); + void generateMissionItemInt(uint8_t tsystem, uint8_t tcomponent, uint8_t frame, uint16_t cmd, uint8_t current, int32_t lat, int32_t lon, float alt_m); + void generateSetPositionTargetGlobalInt(uint8_t tsystem, uint8_t tcomponent, uint8_t frame, uint16_t type_mask, int32_t lat, int32_t lon, float alt, float vx, float vy, float vz, float yaw_rad, float yaw_rad_rate); + void generateCmdConditionYaw(uint8_t tsystem, uint8_t tcomponent, float yaw_deg, float yaw_deg_rate, int8_t dir, bool rel); + void generateRcChannelsOverride(uint8_t sysid, uint8_t tsystem, uint8_t tcomponent, uint16_t* chan_raw); + void generateMissionRequestList(uint8_t tsystem, uint8_t tcomponent, uint8_t mission_type); + void generateMissionRequestInt(uint8_t tsystem, uint8_t tcomponent, uint16_t seq, uint8_t mission_type); + // camera + void generateCmdRequestCameraInformation(uint8_t tsystem, uint8_t tcomponent); + void generateCmdRequestCameraSettings(uint8_t tsystem, uint8_t tcomponent); + void generateCmdRequestStorageInformation(uint8_t tsystem, uint8_t tcomponent); + void generateCmdRequestCameraCapturesStatus(uint8_t tsystem, uint8_t tcomponent); + void generateCmdSetCameraMode(uint8_t tsystem, uint8_t tcomponent, uint8_t mode); + void generateCmdImageStartCapture(uint8_t tsystem, uint8_t tcomponent); + void generateCmdVideoStartCapture(uint8_t tsystem, uint8_t tcomponent); + void generateCmdVideoStopCapture(uint8_t tsystem, uint8_t tcomponent); + // gimbal & gimbalmanager + void generateCmdDoMountConfigure(uint8_t tsystem, uint8_t tcomponent, uint8_t mode); + void generateCmdDoMountControl(uint8_t tsystem, uint8_t tcomponent, float pitch_deg, float yaw_deg, uint8_t mode); + void generateCmdRequestGimbalDeviceInformation(uint8_t tsystem, uint8_t tcomponent); + void generateStorm32GimbalDeviceControl(uint8_t tsystem, uint8_t tcomponent, float pitch_deg, float yaw_deg, uint16_t flags); + void generateCmdRequestStorm32GimbalManagerInformation(uint8_t tsystem, uint8_t tcomponent); + void generateStorm32GimbalManagerControl(uint8_t tsystem, uint8_t tcomponent, uint8_t gimbal_id, float pitch_deg, float yaw_deg, uint16_t device_flags, uint16_t manager_flags); + void generateStorm32GimbalManagerControlPitchYaw(uint8_t tsystem, uint8_t tcomponent, uint8_t gimbal_id, float pitch_deg, float yaw_deg, uint16_t device_flags, uint16_t manager_flags); + void generateCmdStorm32DoGimbalManagerControlPitchYaw(uint8_t tsystem, uint8_t tcomponent, uint8_t gimbal_id, float pitch_deg, float yaw_deg, uint16_t device_flags, uint16_t manager_flags); + void generateCmdDoQShotConfigure(uint8_t tsystem, uint8_t tcomponent, uint8_t mode, uint8_t shot_state); + void generateQShotStatus(uint8_t mode, uint8_t shot_state); + void generateButtonChange(uint8_t button_state); + + // TASK AND MESSAGE HANDLERS + + bool isSystemIdValid(void) + { + return (_sysid > 0); + } + + bool doTaskAutopilot(void); + bool doTaskAutopilotLowPriority(void); + bool doTaskCamera(void); + bool doTaskCameraLowPriority(void); + bool doTaskGimbalAndGimbalClient(void); + bool doTaskQShot(void); + void doTask(void); + + void handleMessageAutopilot(void); + void handleMessageCamera(void); + void handleMessageGimbal(void); + void handleMessageGimbalClient(void); + void handleMessageQShot(void); + void handleMessage(void); + + void setAutopilotStartupRequests(void); + void setCameraStartupRequests(void); + void setGimbalStartupRequests(void); + void setGimbalClientStartupRequests(void); + + // TASKS + + #define TASKIDX_MAX 8 + + #define SETTASK(idx,x) {_task[idx] |= (x);} + #define RESETTASK(idx,x) {_task[idx] &=~ (x);} + bool TASK_IS_PENDING() {for(uint16_t i=0; i 0) return true;} return false;} + + // REQUESTS + + #define REQUESTLIST_MAX 32 + + // Times + + uint32_t time_boot_ms(void) { return get_tmr10ms()*10; } + + uint16_t _t2MHz_last = 0; + uint64_t _t10us_last = 0; + + uint32_t time10us(void) { // ca 11.9h, should be sufficient + uint16_t t2MHz_now = getTmr2MHz(); + _t10us_last += (t2MHz_now - _t2MHz_last); + _t2MHz_last = t2MHz_now; + return _t10us_last/20; + } + + // MAVLINK API + // in the receive list we need to differentiate not only by msgid, but also by sysis-compid + // if two components send the same message at (too) high rate considering only msgid leads to message loss + + #define MAVMSGLIST_MAX 64 + + struct MavMsg { + uint32_t msgid; + uint8_t sysid; + uint8_t compid; + uint8_t target_sysid; + uint8_t target_compid; + void* payload_ptr; + uint32_t timestamp; // used only for finding latest + bool updated; + }; + + MavMsg* _mavapi_rx_list[MAVMSGLIST_MAX] = { NULL }; // list of pointers into MavMsg structs + bool _mavapi_rx_enabled = false; + + uint8_t _mavapiMsgInFindOrAdd(uint32_t msgid); + void mavapiHandleMessage(fmav_message_t* msg); + void mavapiMsgInEnable(bool flag); + uint8_t mavapiMsgInCount(void); + MavMsg* mavapiMsgInGet(uint32_t msgid); + MavMsg* mavapiMsgInGetLast(void); + + // opentx's FiFo isn't ideal as it can hold only N-1 elements, which for + // big elements is a huge waste of mem. It is thread safe though. + // we go with it despite it's cost + // it doesn't really allow us to work on pointers, so we redo what we need + + #define MAVOUTFIFO_MAX 4 + fmav_message_t* _mavapiMsgOutFifo = NULL; // we allocate it only then it is really needed + volatile uint32_t _wi = 0; + volatile uint32_t _ri = 0; + bool _mavapi_tx_enabled = false; + + void mavapiMsgOutEnable(bool flag); + fmav_message_t* mavapiMsgOutPtr(void); + void mavapiMsgOutSet(void); + void mavapiGenerateMessage(void); + + void mavapiInit(void){}; + + // MAVSDK GENERAL + + bool isReceiving(void) + { + return (_is_receiving > 0); + } + + struct Radio { + uint16_t is_receiving; // RADIO_STATUS has priority + uint8_t rssi; + uint8_t remrssi; + uint8_t noise; + uint8_t remnoise; + uint16_t is_receiving65; // msg 65 has priority over 35 + uint8_t rssi65; + uint16_t is_receiving35; // msg 35 is last resort + uint8_t rssi35; + uint8_t rssi_scaled; + bool rssi_voice_disabled; + }; + struct Radio radio; + + void telemetrySetValue(uint16_t id, uint8_t subId, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec); + void telemetrySetRssiValue(uint8_t rssi); + void telemetryResetRssiValue(void); + bool telemetryVoiceEnabled(void); + + struct Comp { // not all fields are relevant for/used by all components + uint8_t compid; + uint16_t is_receiving; + //heartbeat + uint8_t system_status; + uint32_t custom_mode; + bool is_armed; + bool is_standby; + bool is_critical; + bool prearm_ok; + uint8_t updated; + //for initializing, if it expects some required messages to be received + //requests_waiting_mask is used to determine if component is initialized + uint8_t requests_triggered; + uint8_t requests_waiting_mask; + bool is_initialized; + }; + struct Comp autopilot; + struct Comp gimbal; + struct Comp camera; + struct Comp gimbalmanager; // it's not exactly a component, can be the autopilot or the companion or the gimbal + + // MAVSDK AUTOPILOT + + uint8_t autopilottype = MAV_AUTOPILOT_GENERIC; + uint8_t vehicletype = MAV_TYPE_GENERIC; + uint8_t flightmode = 0; + + struct SysStatus { + uint32_t sensors_present; // MAV_SYS_STATUS_SENSOR + uint32_t sensors_enabled; // MAV_SYS_STATUS_SENSOR + uint32_t sensors_health; // MAV_SYS_STATUS_SENSOR + uint8_t updated; + bool received; + }; + struct SysStatus sysstatus; + + struct Att { + float roll_rad; // rad + float pitch_rad; // rad + float yaw_rad; // rad + uint8_t updated; + }; + struct Att att; + + struct Gps { + uint8_t fix; + uint8_t sat; // UINT8_MAX if unknown + uint16_t hdop; // UINT16_MAX if unknown + uint16_t vdop; // UINT16_MAX if unknown + int32_t lat; // (WGS84), in degrees * 1E7 + int32_t lon; // (WGS84), in degrees * 1E7 + int32_t alt_mm; // (AMSL, NOT WGS84), in meters * 1000 + uint16_t vel_cmps; // m/s * 100, UINT16_MAX if unknown + uint16_t cog_cdeg; // degrees * 100, 0.0..359.99 degrees, UINT16_MAX if unknown + uint8_t updated; + }; + struct Gps gps1; + struct Gps gps2; + uint8_t gps_instancemask; + + struct GlobalPositionInt { + int32_t lat; // in degrees * 1E7 + int32_t lon; // in degrees * 1E7 + int32_t alt_mm; // (MSL), in mm + int32_t relative_alt_mm; // in mm + int16_t vx_cmps; // (Latitude, positive north), in cm/s + int16_t vy_cmps; // (Longitude, positive east), in cm/s + int16_t vz_cmps; // (Altitude, positive down), in cm/s + uint16_t hdg_cdeg; // degrees * 100, 0.0..359.99 degrees, UINT16_MAX if unknown + uint8_t updated; + }; + struct GlobalPositionInt gposition; + + struct Vfr { + float airspd_mps; // m/s + float groundspd_mps; // m/s + float alt_m; // (MSL), m ?? is this really MSL ?? it can't I think, appears to be above home + float climbrate_mps; // m/s + int16_t heading_deg; // degrees (0..360, 0=north) + uint16_t thro_pct; // percent, 0 to 100 + uint8_t updated; + }; + struct Vfr vfr; + + struct Bat { + int32_t charge_consumed_mAh; // mAh, -1 if not known + int32_t energy_consumed_hJ; // 0.1 kJ, -1 if not known + int16_t temperature_cC; // centi-degrees C�, INT16_MAX if not known + uint32_t voltage_mV; // mV + int16_t current_cA; // 10*mA, -1 if not known + int8_t remaining_pct; // (0%: 0, 100%: 100), -1 if not known + int32_t time_remaining; // 0 if not known + uint8_t charge_state; // 0 if not known + uint32_t fault_bitmask; + int8_t cellcount; // -1 if not known + uint8_t updated; + }; + struct Bat bat1; + struct Bat bat2; + uint8_t bat_instancemask; + + struct StatusText { + Fifo fifo; + uint8_t updated; + }; + struct StatusText statustext; + + enum MavApEkfFlags { + MAVAP_EKF_ATTITUDE = 1, + MAVAP_EKF_VELOCITY_HORIZ = 2, + MAVAP_EKF_VELOCITY_VERT = 4, + MAVAP_EKF_POS_HORIZ_REL = 8, + MAVAP_EKF_POS_HORIZ_ABS = 16, + MAVAP_EKF_POS_VERT_ABS = 32, + MAVAP_EKF_POS_VERT_AGL = 64, + MAVAP_EKF_CONST_POS_MODE = 128, + MAVAP_EKF_PRED_POS_HORIZ_REL = 256, + MAVAP_EKF_PRED_POS_HORIZ_ABS = 512, + }; + + struct Ekf { + //comment: we don't really need the other fields in the EKF message + uint16_t flags; + uint8_t updated; + }; + struct Ekf ekf; + + struct NavControllerOutput { + //comment: we don't really need the other fields + int16_t nav_bearing; // Current desired heading in degrees + int16_t target_bearing; // Bearing to current MISSION/target in degrees + uint16_t wp_dist; // Distance to active MISSION in meters + uint8_t updated; + }; + struct NavControllerOutput navControllerOutput; + + struct Mission { + uint16_t count; // from MISSION_COUNT + uint16_t seq_current; // from MISSION_CURRENT + uint8_t updated; + }; + struct Mission mission; + + struct MissionItem { // the INT version of it + //comment: we don't really need the other fields + uint16_t seq; // Waypoint ID (sequence number). Starts at zero. Increases monotonically for each waypoint, no gaps in the sequence (0,1,2,3,4). + uint8_t frame; // The coordinate system of the waypoint. + uint16_t command; // The scheduled action for the waypnt. + int32_t x; // PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + int32_t y; // PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 + float z; // PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + uint8_t updated; + }; + struct MissionItem missionItem; + + // AP specific + struct Rangefinder { + float distance; + uint8_t updated; + }; + struct Rangefinder rangefinder; + + // this is very flight stack dependent + struct Parameters { + int16_t number; // we use -1 to indicate it wasn't obtained + int32_t BATT_CAPACITY; // type int32 //we use -1 to indicate it wasn't obtained + int32_t BATT2_CAPACITY; // type int32 //we use -1 to indicate it wasn't obtained + float WPNAV_SPEED; // type = float //we use NAN to indicate it wasn't obtained + float WPNAV_ACCEL; // type = float //we use NAN to indicate it wasn't obtained + float WPNAV_ACCEL_Z; // type = float //we use NAN to indicate it wasn't obtained + int16_t SYSID_MYGCS; // we use -1 to indicate it wasn't obtained + int32_t ARMING_CHECK; // type int32 //we use -1 to indicate it wasn't obtained + }; + struct Parameters param; + + // AP: not armed -> filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs + // armed -> filt_status.flags.horiz_pos_abs && !filt_status.flags.const_pos_mode + bool apPositionOk(void) + { + return (ekf.flags & MAVAP_EKF_POS_HORIZ_ABS) && (ekf.flags & MAVAP_EKF_VELOCITY_HORIZ); + } + + // some tasks need some additional data + char _prr_param_id[16+1]; + + uint8_t _tcsm_base_mode; + uint32_t _tcsm_custom_mode; + float _tcnt_alt_m; + float _tccs_speed_mps; + uint8_t _tccs_speed_type; + uint8_t _tmii_frame; + uint16_t _tmii_cmd; + uint8_t _tmii_current; + int32_t _tmii_lat, _tmii_lon; + float _tmii_alt_m; + uint8_t _t_coordinate_frame; + uint16_t _t_type_mask; + int32_t _t_lat, _t_lon; + float _t_alt, _t_vx, _t_vy, _t_vz, _t_yaw_rad, _t_yaw_rad_rate; + float _tccy_yaw_deg; + int8_t _tccy_dir; + bool _tccy_relative; + float _tact_takeoff_alt_m; + float _tacf_takeoff_alt_m; + uint16_t _tovr_chan_raw[18]; + uint16_t _tmri_seq, _tmri_missiontype; + + // convenience task wrapper + void setTaskParamRequestList(void) + { + SETTASK(TASK_AUTOPILOT, TASK_SENDMSG_PARAM_REQUEST_LIST); + } + void setTaskParamRequestRead(const char* pname) + { + strncpy(_prr_param_id, pname, 16); + SETTASK(TASK_AUTOPILOT, TASK_SENDMSG_PARAM_REQUEST_READ); + } + void requestMissionRequestInt(uint16_t seq) + { + _tmri_seq = seq; _tmri_missiontype = MAV_MISSION_TYPE_MISSION; + set_request(TASK_AUTOPILOT, TASK_SENDMSG_MISSION_REQUEST_INT, 10, 473); + } + + void apSetFlightMode(uint32_t ap_flight_mode); + void apSetGroundSpeed(float speed); + void apSimpleGotoPosAlt(int32_t lat, int32_t lon, float alt); + void apGotoPosAltYawDeg(int32_t lat, int32_t lon, float alt, float yaw); + void apGotoPosAltVel(int32_t lat, int32_t lon, float alt, float vx, float vy, float vz); + void apSetYawDeg(float yaw, bool relative); // note, we can enter negative yaw here, sign determines direction + + void apRequestBanner(void) + { + SETTASK(TASK_AP, TASK_AP_REQUESTBANNER); + } + void apArm(bool arm) + { + SETTASK(TASK_AP, (arm) ? TASK_AP_ARM : TASK_AP_DISARM); + } + void apCopterTakeOff(float alt) + { + _tact_takeoff_alt_m = alt; + SETTASK(TASK_AP, TASK_AP_COPTER_TAKEOFF); + } + void apLand(void) + { + SETTASK(TASK_AP, TASK_AP_LAND); + } + void apCopterFlyClick(void) + { + SETTASK(TASK_AP, TASK_AP_COPTER_FLYCLICK); + } + void apCopterFlyHold(float alt) + { + _tacf_takeoff_alt_m = alt; + SETTASK(TASK_AP, TASK_AP_COPTER_FLYHOLD); + } + void apCopterFlyPause(void) + { + SETTASK(TASK_AP, TASK_AP_COPTER_FLYPAUSE); + } + + // MAVSDK CAMERA + + struct CameraInfo { + char vendor_name[32+1]; + char model_name[32+1]; + uint32_t firmware_version; + uint32_t flags; + bool has_video; + bool has_photo; + bool has_modes; + float total_capacity_MiB; // NAN if not known + }; + struct CameraInfo cameraInfo; // Info: static data + + struct CameraStatus { + uint8_t mode; + bool video_on; + bool photo_on; + float available_capacity_MiB; // NAN if not known + uint32_t recording_time_ms; + float battery_voltage_V; // NAN if not known + int8_t battery_remaining_pct; // (0%: 0, 100%: 100), -1 if not known + }; + struct CameraStatus cameraStatus; // Status: variable data + + // convenience task wrapper + void sendCameraSetVideoMode(void) + { + SETTASK(TASK_CAMERA, TASK_SENDCMD_SET_CAMERA_VIDEO_MODE); + } + void sendCameraSetPhotoMode(void) + { + SETTASK(TASK_CAMERA, TASK_SENDCMD_SET_CAMERA_PHOTO_MODE); + } + void sendCameraStartVideo(void) + { + SETTASK(TASK_CAMERA, TASK_SENDCMD_VIDEO_START_CAPTURE); + } + void sendCameraStopVideo(void) + { + SETTASK(TASK_CAMERA, TASK_SENDCMD_VIDEO_STOP_CAPTURE); + } + void sendCameraTakePhoto(void) + { + SETTASK(TASK_CAMERA, TASK_SENDCMD_IMAGE_START_CAPTURE); + } + + // MAVSDK GIMBAL & GIMBAL CLIENT + + struct GimbalAtt { + float roll_deg; + float pitch_deg; + float yaw_deg_relative; + float yaw_deg_absolute; + uint8_t updated; + }; + struct GimbalAtt gimbalAtt; + + // some tasks need some additional data + uint8_t _t_gimbal_mode; + float _t_gimbal_pitch_deg, _t_gimbal_yaw_deg; + + // convenience task wrapper + void sendGimbalTargetingMode(uint8_t mode); + void sendGimbalPitchYawDeg(float pitch, float yaw); + + // MAVSDK GIMBAL CLIENT only + + struct GimbalDeviceInfo { + char vendor_name[32+1]; + char model_name[32+1]; + char custom_name[32+1]; + uint32_t firmware_version; + uint32_t hardware_version; + uint32_t cap_flags; + uint8_t updated; + }; + struct GimbalDeviceInfo gimbaldeviceInfo; + + struct Storm32GimbalManagerInfo { + uint32_t device_cap_flags; + uint32_t manager_cap_flags; + uint8_t updated; + }; + struct Storm32GimbalManagerInfo gimbalmanagerInfo; + + struct Storm32GimbalManagerStatus { + uint8_t supervisor; + uint16_t device_flags; + uint16_t manager_flags; + uint8_t profile; + uint8_t updated; + }; + struct Storm32GimbalManagerStatus gimbalmanagerStatus; + + // some tasks need some additional data + float _t_storm32GD_pitch_deg, _t_storm32GD_yaw_deg; + uint16_t _t_storm32GD_flags; + float _t_storm32GM_pitch_deg, _t_storm32GM_yaw_deg; + uint16_t _t_storm32GM_gdflags, _t_storm32GM_gmflags; + float _t_storm32GM_control_pitch_deg, _t_storm32GM_control_yaw_deg; + uint16_t _t_storm32GM_control_gdflags, _t_storm32GM_control_gmflags; + float _t_storm32GM_cmd_pitch_deg, _t_storm32GM_cmd_yaw_deg; + uint16_t _t_storm32GM_cmd_gdflags, _t_storm32GM_cmd_gmflags; + + // convenience task wrapper + void sendStorm32GimbalDevicePitchYawDeg(float pitch, float yaw); + void sendStorm32GimbalManagerPitchYawDeg(float pitch, float yaw); + void sendStorm32GimbalManagerControlPitchYawDeg(float pitch, float yaw); + void sendStorm32GimbalManagerCmdPitchYawDeg(float pitch, float yaw); + + struct GimbalManagerOut { // collective structure to handle gimbalmanager outgoing flags + uint8_t mount_mode; // copy of the mount_mode + uint16_t device_flags; + uint16_t manager_flags; + }; + struct GimbalManagerOut gimbalmanagerOut; + + void _refreshGimbalClientFlags(uint16_t* device_flags, uint16_t* manager_flags); + void setStorm32GimbalClientRetract(bool enable); + void setStorm32GimbalClientNeutral(bool enable); + void setStorm32GimbalClientLock(bool roll_lock, bool pitch_lock, bool yaw_lock); + void setStorm32GimbalClientFlags(uint16_t manager_flags); + + // gimbal protocol v1 vs v2 + void setStorm32GimbalProtocolV2(bool flag) + { + _storm32_gimbal_protocol_v2 = flag; + } + bool isStorm32GimbalProtocolV2(void) + { + return _storm32_gimbal_protocol_v2; + } + + // MAVSDK QSHOT + struct QShot { + uint8_t mode; + uint8_t shot_state; + }; + struct QShot qshot; + + // some tasks need some additional data + uint8_t _t_qshot_mode, _t_qshot_shot_state; + uint8_t _t_qshot_cmd_mode, _t_qshot_cmd_shot_state; + uint8_t _t_qshot_button_state; + + // convenience task wrapper + void sendQShotCmdConfigure(uint8_t mode, uint8_t shot_state); + void sendQShotStatus(uint8_t mode, uint8_t shot_state); + void sendQShotButtonState(uint8_t button_state); + + // SOME more MAVLink stuff + + const uint8_t mySysId(void) { return _my_sysid; } + const uint8_t myCompId(void) { return _my_compid; } + const uint8_t systemSysId(void) { return _sysid; } + const uint8_t autopilotCompId(void) { return autopilot.compid; } + const uint8_t cameraCompId(void) { return camera.compid; } + const uint8_t gimbalCompId(void) { return gimbal.compid; } + const uint8_t gimbalManagerCompId(void) { return gimbalmanager.compid; } + + const fmav_status_t* getChannelStatusOut(void) + { + return &_status_out; + } + + uint32_t msg_rx_count; + uint32_t msg_rx_persec; + uint32_t bytes_rx_persec; + uint32_t msg_tx_count; + uint32_t msg_tx_persec; + uint32_t bytes_tx_persec; + + // PROTECTED FIELDS and METHODS + protected: + + void _reset(void); + void _resetRadio(void); + void _resetRadio65(void); + void _resetRadio35(void); + void _resetAutopilot(void); + void _resetCamera(void); + void _resetGimbalAndGimbalClient(void); + void _resetGimbalClient(void); + void _resetQShot(void); + + void _init(void); + + uint8_t _my_sysid = MAVLINK_TELEM_MY_SYSID; + uint8_t _my_compid = MAVLINK_TELEM_MY_COMPID; + tmr10ms_t _my_heartbeat_tlast; + tmr10ms_t _rcoverride_tlast; + + uint8_t _sysid = 0; // is autodetected by inspecting the autopilot heartbeat + + uint16_t _is_receiving = 0; // is set by any arbitrary incoming MAVLink message + + // TASKS + + enum TaskIdxEnum { + TASK_ME = 0, + TASK_AUTOPILOT, // autopilot, non-specific + TASK_AP, // autopilot, ardupilot-specific + TASK_GIMBAL, // gimbal and gimbal client + TASK_CAMERA, // camera + }; + + enum TaskMaskEnum { + // me + TASK_SENDMYHEARTBEAT = 0x00000001, + + TASK_SENDCMD_DO_QSHOT_CONFIGFURE = 0x00000002, + TASK_SENDMSG_QSHOT_STATUS = 0x00000004, + TASK_SENDMSG_QSHOT_BUTTON_STATE = 0x00000008, + + TASK_SENDMSG_MAVLINK_API = 0x00000010, + + // autopilot + TASK_SENDREQUESTDATASTREAM_RAW_SENSORS = 0x00000001, // group 1 + TASK_SENDREQUESTDATASTREAM_EXTENDED_STATUS = 0x00000002, // group 2 + TASK_SENDREQUESTDATASTREAM_RC_CHANNELS = 0x00000004, // group 3 + TASK_SENDREQUESTDATASTREAM_RAW_CONTROLLER = 0x00000008, // group 4 + TASK_SENDREQUESTDATASTREAM_POSITION = 0x00000010, // group 6 + TASK_SENDREQUESTDATASTREAM_EXTRA1 = 0x00000020, // group 10 + TASK_SENDREQUESTDATASTREAM_EXTRA2 = 0x00000040, // group 11 + TASK_SENDREQUESTDATASTREAM_EXTRA3 = 0x00000080, // group 12 + TASK_SENDCMD_REQUEST_ATTITUDE = 0x00000100, + TASK_SENDCMD_REQUEST_GLOBAL_POSITION_INT = 0x00000200, + TASK_SENDMSG_PARAM_REQUEST_LIST = 0x00001000, + TASK_SENDMSG_PARAM_REQUEST_READ = 0x00002000, + TASK_SENDMSG_MISSION_REQUEST_LIST = 0x00004000, + TASK_SENDMSG_MISSION_REQUEST_INT = 0x00008000, + + TASK_SENDCMD_DO_SET_MODE = 0x00100000, + TASK_SENDCMD_NAV_TAKEOFF = 0x00200000, // simple_takeoff() + TASK_SENDCMD_DO_CHANGE_SPEED = 0x00400000, // groundspeed(), airspeed() + TASK_SENDMSG_MISSION_ITEM_INT = 0x00800000, // simple_goto() + TASK_SENDMSG_SET_POSITION_TARGET_GLOBAL_INT = 0x01000000, + TASK_SENDCMD_CONDITION_YAW = 0x02000000, + TASK_SENDMSG_RC_CHANNELS_OVERRIDE = 0x04000000, + // ap + TASK_AP_REQUESTBANNER = 0x00000001, + TASK_AP_ARM = 0x00000002, + TASK_AP_DISARM = 0x00000004, + TASK_AP_COPTER_TAKEOFF = 0x00000008, + TASK_AP_LAND = 0x00000010, + TASK_AP_COPTER_FLYCLICK = 0x00000020, + TASK_AP_COPTER_FLYHOLD = 0x00000040, + TASK_AP_COPTER_FLYPAUSE = 0x00000080, + + TASK_AP_REQUESTPARAM_BATT_CAPACITY = 0x00010000, + TASK_AP_REQUESTPARAM_BATT2_CAPACITY = 0x00020000, + TASK_AP_REQUESTPARAM_WPNAV_SPEED = 0x00040000, + TASK_AP_REQUESTPARAM_WPNAV_ACCEL = 0x00080000, + TASK_AP_REQUESTPARAM_WPNAV_ACCEL_Z = 0x00100000, + TASK_AP_REQUESTPARAM_SYSID_MYGCS = 0x00200000, + TASK_AP_REQUESTPARAM_ARMING_CHECK = 0x00400000, + // camera + TASK_SENDREQUEST_CAMERA_INFORMATION = 0x00000001, + TASK_SENDREQUEST_CAMERA_SETTINGS = 0x00000002, + TASK_SENDREQUEST_STORAGE_INFORMATION = 0x00000004, + TASK_SENDREQUEST_CAMERA_CAPTURE_STATUS = 0x00000008, + TASK_SENDCMD_SET_CAMERA_VIDEO_MODE = 0x00000010, + TASK_SENDCMD_SET_CAMERA_PHOTO_MODE = 0x00000020, + TASK_SENDCMD_VIDEO_START_CAPTURE = 0x00000040, + TASK_SENDCMD_VIDEO_STOP_CAPTURE = 0x00000080, + TASK_SENDCMD_IMAGE_START_CAPTURE = 0x00000100, + // gimbal & gimbal client + TASK_SENDCMD_DO_MOUNT_CONFIGURE = 0x00000001, // this goes to the autopilot + TASK_SENDCMD_DO_MOUNT_CONTROL = 0x00000002, // this goes to the autopilot + + TASK_SENDREQUEST_GIMBAL_DEVICE_INFORMATION = 0x00000008, // this goes to the gimbal device + + TASK_SENDMSG_GIMBAL_DEVICE_CONTROL = 0x00000010, // this goes to the storm32 gimbal device + TASK_SENDREQUEST_GIMBAL_MANAGER_INFORMATION = 0x00000020, // this goes to the storm32 gimbal manager + TASK_SENDMSG_GIMBAL_MANAGER_CONTROL = 0x00000040, // this goes to the storm32 gimbal manager + TASK_SENDMSG_GIMBAL_MANAGER_CONTROL_PITCHYAW = 0x00000080, // this goes to the storm32 gimbal manager + TASK_SENDCMD_DO_GIMBAL_MANAGER_CONTROL_PITCHYAW = 0x00000100, // this goes to the storm32 gimbal manager + }; + + uint32_t _task[TASKIDX_MAX]; + + struct Task { + uint32_t task; + uint8_t idx; + }; + + Fifo _taskFifo; // the fifo is to further rate limit the execution of tasks + tmr10ms_t _taskFifo_tlast; + + void push_task(uint8_t idx, uint32_t task); + void pop_and_set_task(void); + + // REQUESTS + + enum AutopilotRequestWaitingFlags { + AUTOPILOT_REQUESTWAITING_GPS_RAW_INT = 0x01, + AUTOPILOT_REQUESTWAITING_GLOBAL_POSITION_INT = 0x02, + AUTOPILOT_REQUESTWAITING_ATTITUDE = 0x04, + AUTOPILOT_REQUESTWAITING_VFR_HUD = 0x08, + AUTOPILOT_REQUESTWAITING_EKF_STATUS_REPORT = 0x10, + AUTOPILOT_REQUESTWAITING_ALL = 0x0F, + }; + + enum CameraRequestWaitingFlags { + CAMERA_REQUESTWAITING_CAMERA_INFORMATION = 0x01, + CAMERA_REQUESTWAITING_CAMERA_SETTINGS = 0x02, + CAMERA_REQUESTWAITING_CAMERA_CAPTURE_STATUS = 0x04, + CAMERA_REQUESTWAITING_ALL = 0x07, + }; + + enum GimbalRequestWaitingFlags { + GIMBAL_REQUESTWAITING_GIMBAL_DEVICE_INFORMATION = 0x01, + GIMBAL_REQUESTWAITING_GIMBAL_MANAGER_INFORMATION = 0x02, + GIMBAL_REQUESTWAITING_ALL = 0x01, + GIMBALCLIENT_REQUESTWAITING_ALL = 0x03, + }; + + struct Request { + uint32_t task; + uint8_t idx; + uint8_t retry; // UINT8_MAX means request it for ever + tmr10ms_t tlast; + tmr10ms_t trate; + }; + + struct Request _requestList[REQUESTLIST_MAX]; // 0 in the task field indicates that the slot is free and unused + uint32_t _request_is_waiting[TASKIDX_MAX]; + + void set_request(uint8_t idx, uint32_t task, uint8_t retry, tmr10ms_t rate = 102); + void clear_request(uint8_t idx, uint32_t task); + void do_requests(void); + + // STUFF + + bool _storm32_gimbal_protocol_v2 = false; + + // MORE MAVLINK STUFF + + uint32_t _msg_rx_persec_cnt; + uint32_t _bytes_rx_persec_cnt; + uint32_t _msg_tx_persec_cnt; + uint32_t _bytes_tx_persec_cnt; + + uint16_t _scheduled_serial = 0; + + fmav_status_t _status1, _status2, _status3; + uint8_t _buf1[296], _buf2[296], _buf3[296]; + fmav_message_t _msg; + + fmav_status_t _status_out; + fmav_message_t _msg_out; // size is 292 bytes + uint8_t _buf_out[296]; // only needs to hold one MAVLink message, which is 280 max + bool _msg_out_available = false; + + // SERIALS STUFF + + bool _aux1_enabled = false; + uint32_t _aux1_baudrate = UINT32_MAX; // to enforce change + bool _aux2_enabled = false; + uint32_t _aux2_baudrate = UINT32_MAX; // to enforce change + bool _usb_enabled = false; + bool _external_enabled = false; + + void map_serials(void); + + public: + // map of aux1, aux2, external onto serial1, serial2 + bool serial1_enabled = false; + bool serial2_enabled = false; + bool serial1_isexternal = false; + bool serial2_isexternal = false; +}; + +extern MavlinkTelem mavlinkTelem; diff --git a/radio/src/telemetry/mavlink/mavlink_telem_autopilot.cpp b/radio/src/telemetry/mavlink/mavlink_telem_autopilot.cpp new file mode 100644 index 00000000000..57b8d5265f3 --- /dev/null +++ b/radio/src/telemetry/mavlink/mavlink_telem_autopilot.cpp @@ -0,0 +1,1378 @@ +/* + * (c) www.olliw.eu, OlliW, OlliW42 + */ + +#include "opentx.h" + +constexpr float FPI = 3.141592653589793f; +constexpr float FDEGTORAD = FPI/180.0f; +constexpr float FRADTODEG = 180.0f/FPI; + +#define INCU8(x) if ((x) < UINT8_MAX) { (x)++; } + +// -- Rate defines -- + +#define DATA_STREAM_RAW_SENSORS_RATE 2 // not used + +// MAVLINK_MSG_ID_GPS_RAW_INT +// MAVLINK_MSG_ID_GPS2_RAW +// MAVLINK_MSG_ID_SYS_STATUS +// MSG_NAV_CONTROLLER_OUTPUT +// MSG_CURRENT_WAYPOINT // MISSION_CURRENT +#define DATA_STREAM_EXTENDED_STATUS_RATE 2 + +// MAVLINK_MSG_ID_GLOBAL_POSITION_INT +#define DATA_STREAM_POSITION_RATE 4 // not used + +// MAVLINK_MSG_ID_ATTITUDE +#define DATA_STREAM_EXTRA1_RATE 4 // not used + +// MAVLINK_MSG_ID_VFR_HUD +#define DATA_STREAM_EXTRA2_RATE 2 + +// MAVLINK_MSG_ID_BATTERY_STATUS (only if batt monitor configured) +// MAVLINK_MSG_ID_BATTERY2 (only if batt2 monitor configured) +// MAVLINK_MSG_ID_EKF_STATUS_REPORT +#define DATA_STREAM_EXTRA3_RATE 2 + +// MSG_SERVO_OUTPUT_RAW +// MSG_RC_CHANNELS +// MSG_RC_CHANNELS_RAW (only sent on a mavlink1 connection) +#define DATA_STREAM_RC_CHANNELS_RATE 1 + +// call at high rates of 10 Hz, or 5 Hz +// MAVLINK_MSG_ID_ATTITUDE +#define MSG_ID_ATTITUDE_RATE 100000 // 100 ms = 10 Hz + +// call at high rates of 10 Hz, or 5 Hz +// MAVLINK_MSG_ID_GLOBAL_POSITION_INT +#define MSG_ID_GLOBAL_POSITION_INT_RATE 200000 // 200 ms = 5 Hz + +// -- Generate MAVLink messages -- +// these should never be called directly, should only by called by the task handler + +//ArduPilot: +// base_mode must have MAV_MODE_FLAG_CUSTOM_MODE_ENABLED bit set, +// custom_mode then determines the mode it will switch to +// usage of this cmd is thus very likely very flightstack dependent!! +void MavlinkTelem::generateCmdDoSetMode(uint8_t tsystem, uint8_t tcomponent, MAV_MODE base_mode, uint32_t custom_mode) +{ + _generateCmdLong(tsystem, tcomponent, MAV_CMD_DO_SET_MODE, base_mode, custom_mode); +} + +//ArduPilot: supports a param3 which is not in the specs, ignores param1,4,5,6 +// param3 = horizontal navigation by pilot acceptable +void MavlinkTelem::generateCmdNavTakeoff(uint8_t tsystem, uint8_t tcomponent, float alt_m, bool hor_nav_by_pilot) +{ + _generateCmdLong(tsystem, tcomponent, MAV_CMD_NAV_TAKEOFF, 0,0, (hor_nav_by_pilot) ? 1.0f : 0.0f, 0,0,0, alt_m); +} + +// speed type 0=Airspeed, 1=Ground Speed, 2=Climb Speed, 3=Descent Speed +//ArduPilot: ignores param3 = Throttle and param4 = Relative +void MavlinkTelem::generateCmdDoChangeSpeed(uint8_t tsystem, uint8_t tcomponent, float speed_mps, uint16_t speed_type, bool relative) +{ + _generateCmdLong(tsystem, tcomponent, MAV_CMD_DO_CHANGE_SPEED, speed_type, speed_mps, -1, (relative) ? 1.0f : 0.0f); +} + +//ArduPilot: current = 2 or 3 +// current = 2 is a flag to tell this is a "guided mode" waypoint and not for the mission +// current = 3 is a flag to tell this is a alt change +void MavlinkTelem::generateMissionItemInt(uint8_t tsystem, uint8_t tcomponent, + uint8_t frame, uint16_t cmd, uint8_t current, int32_t lat, int32_t lon, float alt_m) +{ + fmav_msg_mission_item_int_pack( + &_msg_out, _my_sysid, _my_compid, + tsystem, tcomponent, + 0, frame, cmd, current, 0, 0.0f, 0.0f, 0.0f, 0.0f, lat, lon, alt_m, MAV_MISSION_TYPE_MISSION, + &_status_out + ); + _msg_out_available = true; +} + +void MavlinkTelem::generateSetPositionTargetGlobalInt(uint8_t tsystem, uint8_t tcomponent, + uint8_t frame, uint16_t type_mask, + int32_t lat, int32_t lon, float alt, float vx, float vy, float vz, float yaw_rad, float yaw_rad_rate) +{ + fmav_msg_set_position_target_global_int_pack( + &_msg_out, _my_sysid, _my_compid, + time_boot_ms(), + tsystem, tcomponent, + frame, type_mask, + lat, lon, alt, vx, vy, vz, 0.0f, 0.0f, 0.0f, yaw_rad, yaw_rad_rate, // alt in m, v in m/s, yaw in rad + &_status_out + ); + _msg_out_available = true; +} + +// yaw must be in range 0..360 +void MavlinkTelem::generateCmdConditionYaw(uint8_t tsystem, uint8_t tcomponent, float yaw_deg, float yaw_deg_rate, int8_t dir, bool rel) +{ + _generateCmdLong(tsystem, tcomponent, MAV_CMD_CONDITION_YAW, yaw_deg, yaw_deg_rate, (dir>0)?1.0f:-1.0f, (rel)?1.0f:0.0f); +} + +void MavlinkTelem::generateRcChannelsOverride(uint8_t sysid, uint8_t tsystem, uint8_t tcomponent, uint16_t* chan_raw) +{ + fmav_msg_rc_channels_override_pack( + &_msg_out, _my_sysid, _my_compid, + tsystem, tcomponent, + chan_raw[0], chan_raw[1], chan_raw[2], chan_raw[3], chan_raw[4], chan_raw[5], chan_raw[6], chan_raw[7], + chan_raw[8], chan_raw[9], chan_raw[10], chan_raw[11], chan_raw[12], chan_raw[13], chan_raw[14], chan_raw[15], + chan_raw[16], chan_raw[17], + &_status_out + ); + _msg_out_available = true; +} + +void MavlinkTelem::generateMissionRequestList(uint8_t tsystem, uint8_t tcomponent, uint8_t mission_type) +{ + fmav_msg_mission_request_list_pack( + &_msg_out, _my_sysid, _my_compid, + tsystem, tcomponent, + mission_type, + &_status_out + ); + _msg_out_available = true; +} + +void MavlinkTelem::generateMissionRequestInt(uint8_t tsystem, uint8_t tcomponent, uint16_t seq, uint8_t mission_type) +{ + fmav_msg_mission_request_int_pack( + &_msg_out, _my_sysid, _my_compid, + tsystem, tcomponent, + seq, mission_type, + &_status_out + ); + _msg_out_available = true; +} + +// -- Mavsdk Convenience Task Wrapper -- +// to make it easy for api_mavsdk to call functions + +void MavlinkTelem::apSetFlightMode(uint32_t ap_flight_mode) +{ + _tcsm_base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + _tcsm_custom_mode = ap_flight_mode; + SETTASK(TASK_AUTOPILOT, TASK_SENDCMD_DO_SET_MODE); +} + +void MavlinkTelem::apSetGroundSpeed(float speed) +{ + _tccs_speed_mps = speed; + _tccs_speed_type = 1; + SETTASK(TASK_AUTOPILOT, TASK_SENDCMD_DO_CHANGE_SPEED); +} + +void MavlinkTelem::apSimpleGotoPosAlt(int32_t lat, int32_t lon, float alt) +{ +// _tmii_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT; //Ardupilot doesn't seem to take this + _tmii_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + _tmii_cmd = MAV_CMD_NAV_WAYPOINT; + _tmii_current = 2; + _tmii_lat = lat; + _tmii_lon = lon; + _tmii_alt_m = alt; + SETTASK(TASK_AUTOPILOT, TASK_SENDMSG_MISSION_ITEM_INT); +} + +//alt and yaw can be NAN if they should be ignored +// this function is not very useful as it really moves very slowly +void MavlinkTelem::apGotoPosAltYawDeg(int32_t lat, int32_t lon, float alt, float yaw) +{ + _t_coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT; + _t_type_mask = + POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; + if (isnan(alt)) { + _t_type_mask |= POSITION_TARGET_TYPEMASK_Z_IGNORE; + alt = 1.0f; + } + if (isnan(yaw)) { + _t_type_mask |= POSITION_TARGET_TYPEMASK_YAW_IGNORE; + yaw = 0.0f; + } + _t_lat = lat; + _t_lon = lon; + _t_alt = alt; // m + _t_vx = _t_vy = _t_vz = 0.0f; + _t_yaw_rad = yaw * FDEGTORAD; // rad + _t_yaw_rad_rate = 0.0f; + SETTASK(TASK_AUTOPILOT, TASK_SENDMSG_SET_POSITION_TARGET_GLOBAL_INT); +} + +void MavlinkTelem::apGotoPosAltVel(int32_t lat, int32_t lon, float alt, float vx, float vy, float vz) +{ + _t_coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT; + _t_type_mask = + POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; + _t_lat = lat; + _t_lon = lon; + _t_alt = alt; // m + _t_vx = vx; // m/s + _t_vy = vy; + _t_vz = vz; + _t_yaw_rad = _t_yaw_rad_rate = 0.0f; // rad + SETTASK(TASK_AUTOPILOT, TASK_SENDMSG_SET_POSITION_TARGET_GLOBAL_INT); +} + +//note, we can enter negative yaw here, sign determines direction +void MavlinkTelem::apSetYawDeg(float yaw, bool relative) +{ + if (relative) { + _tccy_relative = 1.0f; + if (yaw < 0.0f) { + _tccy_dir = -1.0f; + yaw = -yaw; + } + else { + _tccy_dir = 1.0f; + } + } + else { + _tccy_relative = 0.0f; + _tccy_dir = 0.0f; + } + float res = fmodf(yaw, 360.0f); + if (res < 0.0f) res += 360.0f; + _tccy_yaw_deg = res; // is in deg, must be in range [0..360] + SETTASK(TASK_AUTOPILOT, TASK_SENDCMD_CONDITION_YAW); +} + +// -- Task handlers -- + +bool MavlinkTelem::doTaskAutopilot(void) +{ + if (!_task[TASK_AUTOPILOT]) return false; // no task pending + + if (!autopilot.compid) { + _task[TASK_AUTOPILOT] = _task[TASK_AP] = 0; + return false; + } + + // we give RC_CHANNELS_OVERRIDE highest priority + if (_task[TASK_AUTOPILOT] & TASK_SENDMSG_RC_CHANNELS_OVERRIDE) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDMSG_RC_CHANNELS_OVERRIDE); + if (autopilottype == MAV_AUTOPILOT_ARDUPILOTMEGA) { + // RC_CHHANELS_OVERRIDE requires the "correct" sysid, so try to work it out + // somewhat dirty, but that's how ArduPilot works, it only allows one GCS, and its sys id needs to be set by hand + // RC_CHHANELS_OVERRIDE is also considered as kind of a heartbeat, e.g. with respect to gcs failsafe + if (param.SYSID_MYGCS >= 0) + generateRcChannelsOverride(param.SYSID_MYGCS, _sysid, autopilot.compid, _tovr_chan_raw); + } + else { + //TODO: check what other flight stacks expect + generateRcChannelsOverride(_my_sysid, _sysid, autopilot.compid, _tovr_chan_raw); + } + return true; //do only one per loop + } + + if (_task[TASK_AUTOPILOT] & TASK_SENDCMD_DO_CHANGE_SPEED) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDCMD_DO_CHANGE_SPEED); + generateCmdDoChangeSpeed(_sysid, autopilot.compid, _tccs_speed_mps, _tccs_speed_type, true); + return true; //do only one per loop + } + if (_task[TASK_AUTOPILOT] & TASK_SENDMSG_MISSION_ITEM_INT) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDMSG_MISSION_ITEM_INT); + generateMissionItemInt(_sysid, autopilot.compid, _tmii_frame, _tmii_cmd, _tmii_current, + _tmii_lat, _tmii_lon, _tmii_alt_m); + return true; //do only one per loop + } + if (_task[TASK_AUTOPILOT] & TASK_SENDMSG_SET_POSITION_TARGET_GLOBAL_INT) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDMSG_SET_POSITION_TARGET_GLOBAL_INT); + generateSetPositionTargetGlobalInt(_sysid, autopilot.compid, _t_coordinate_frame, _t_type_mask, + _t_lat, _t_lon, _t_alt, _t_vx, _t_vy, _t_vz, _t_yaw_rad, _t_yaw_rad_rate); + return true; //do only one per loop + } + if (_task[TASK_AUTOPILOT] & TASK_SENDCMD_CONDITION_YAW) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDCMD_CONDITION_YAW); + generateCmdConditionYaw(_sysid, autopilot.compid, _tccy_yaw_deg, 0.0f, _tccy_dir, _tccy_relative); + return true; //do only one per loop + } + + return false; +} + +bool MavlinkTelem::doTaskAutopilotLowPriority(void) +{ + if (!_task[TASK_AUTOPILOT] && !_task[TASK_AP]) return false; // no task pending + + if (_task[TASK_AUTOPILOT] & TASK_SENDCMD_DO_SET_MODE) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDCMD_DO_SET_MODE); + generateCmdDoSetMode(_sysid, autopilot.compid, (MAV_MODE)_tcsm_base_mode, _tcsm_custom_mode); + return true; //do only one per loop + } + if (_task[TASK_AUTOPILOT] & TASK_SENDCMD_NAV_TAKEOFF) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDCMD_NAV_TAKEOFF); + generateCmdNavTakeoff(_sysid, autopilot.compid, _tcnt_alt_m, 1); + return true; //do only one per loop + } + if (_task[TASK_AUTOPILOT] & TASK_SENDMSG_PARAM_REQUEST_LIST) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDMSG_PARAM_REQUEST_LIST); + generateParamRequestList(_sysid, autopilot.compid); + return true; //do only one per loop + } + if (_task[TASK_AUTOPILOT] & TASK_SENDMSG_MISSION_REQUEST_LIST) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDMSG_MISSION_REQUEST_LIST); + generateMissionRequestList(_sysid, autopilot.compid, MAV_MISSION_TYPE_MISSION); + return true; //do only one per loop + } + if (_task[TASK_AUTOPILOT] & TASK_SENDMSG_MISSION_REQUEST_INT) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDMSG_MISSION_REQUEST_INT); + generateMissionRequestInt(_sysid, autopilot.compid, _tmri_seq, _tmri_missiontype); + return true; //do only one per loop + } + + if (_task[TASK_AUTOPILOT] & TASK_SENDREQUESTDATASTREAM_RAW_SENSORS) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDREQUESTDATASTREAM_RAW_SENSORS); + generateRequestDataStream(_sysid, autopilot.compid, MAV_DATA_STREAM_RAW_SENSORS, DATA_STREAM_RAW_SENSORS_RATE, 1); // 2 Hz + return true; //do only one per loop + } + if (_task[TASK_AUTOPILOT] & TASK_SENDREQUESTDATASTREAM_EXTENDED_STATUS) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDREQUESTDATASTREAM_EXTENDED_STATUS); + generateRequestDataStream(_sysid, autopilot.compid, MAV_DATA_STREAM_EXTENDED_STATUS, DATA_STREAM_EXTENDED_STATUS_RATE, 1); // 2 Hz + return true; //do only one per loop + } + if (_task[TASK_AUTOPILOT] & TASK_SENDREQUESTDATASTREAM_POSITION) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDREQUESTDATASTREAM_POSITION); + generateRequestDataStream(_sysid, autopilot.compid, MAV_DATA_STREAM_POSITION, DATA_STREAM_POSITION_RATE, 1); // do faster, 4 Hz + return true; //do only one per loop + } + if (_task[TASK_AUTOPILOT] & TASK_SENDREQUESTDATASTREAM_EXTRA1) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDREQUESTDATASTREAM_EXTRA1); + generateRequestDataStream(_sysid, autopilot.compid, MAV_DATA_STREAM_EXTRA1, DATA_STREAM_EXTRA1_RATE, 1); // do faster, 4 Hz + return true; //do only one per loop + } + if (_task[TASK_AUTOPILOT] & TASK_SENDREQUESTDATASTREAM_EXTRA2) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDREQUESTDATASTREAM_EXTRA2); + generateRequestDataStream(_sysid, autopilot.compid, MAV_DATA_STREAM_EXTRA2, DATA_STREAM_EXTRA2_RATE, 1); // 2 Hz + return true; //do only one per loop + } + if (_task[TASK_AUTOPILOT] & TASK_SENDREQUESTDATASTREAM_EXTRA3) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDREQUESTDATASTREAM_EXTRA3); + generateRequestDataStream(_sysid, autopilot.compid, MAV_DATA_STREAM_EXTRA3, DATA_STREAM_EXTRA3_RATE, 1); // 2 Hz + return true; //do only one per loop + } + if (_task[TASK_AUTOPILOT] & TASK_SENDREQUESTDATASTREAM_RC_CHANNELS) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDREQUESTDATASTREAM_RC_CHANNELS); + generateRequestDataStream(_sysid, autopilot.compid, MAV_DATA_STREAM_RC_CHANNELS, DATA_STREAM_RC_CHANNELS_RATE, 1); // 1 Hz + return true; //do only one per loop + } + + if (_task[TASK_AUTOPILOT] & TASK_SENDCMD_REQUEST_ATTITUDE) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDCMD_REQUEST_ATTITUDE); + generateCmdSetMessageInterval(_sysid, autopilot.compid, FASTMAVLINK_MSG_ID_ATTITUDE, MSG_ID_ATTITUDE_RATE, 1); // 100 ms = 10 Hz + return true; //do only one per loop + } + if (_task[TASK_AUTOPILOT] & TASK_SENDCMD_REQUEST_GLOBAL_POSITION_INT) { + RESETTASK(TASK_AUTOPILOT,TASK_SENDCMD_REQUEST_GLOBAL_POSITION_INT); + generateCmdSetMessageInterval(_sysid, autopilot.compid, FASTMAVLINK_MSG_ID_GLOBAL_POSITION_INT, MSG_ID_GLOBAL_POSITION_INT_RATE, 1); // 200 ms = 5 Hz + return true; //do only one per loop + } + + if (_task[TASK_AP] & TASK_AP_ARM) { //MAV_CMD_COMPONENT_ARM_DISARM + RESETTASK(TASK_AP, TASK_AP_ARM); + _generateCmdLong(_sysid, autopilot.compid, MAV_CMD_COMPONENT_ARM_DISARM, 1.0f); + return true; //do only one per loop + } + if (_task[TASK_AP] & TASK_AP_DISARM) { //MAV_CMD_COMPONENT_ARM_DISARM + RESETTASK(TASK_AP, TASK_AP_DISARM); + _generateCmdLong(_sysid, autopilot.compid, MAV_CMD_COMPONENT_ARM_DISARM, 0.0f); + return true; //do only one per loop + } + if (_task[TASK_AP] & TASK_AP_COPTER_TAKEOFF) { //MAV_CMD_NAV_TAKEOFF + RESETTASK(TASK_AP, TASK_AP_COPTER_TAKEOFF); + _generateCmdLong(_sysid, autopilot.compid, MAV_CMD_NAV_TAKEOFF, 0,0, 0.0f, 0,0,0, _tact_takeoff_alt_m); //must_navigate = true + return true; //do only one per loop + } + if (_task[TASK_AP] & TASK_AP_LAND) { //MAV_CMD_NAV_LAND + RESETTASK(TASK_AP, TASK_AP_LAND); + _generateCmdLong(_sysid, autopilot.compid, MAV_CMD_NAV_LAND); + return true; //do only one per loop + } + if (_task[TASK_AP] & TASK_AP_COPTER_FLYCLICK) { + RESETTASK(TASK_AP, TASK_AP_COPTER_FLYCLICK); + _generateCmdLong(_sysid, autopilot.compid, MAV_CMD_SOLO_BTN_FLY_CLICK); + return true; //do only one per loop + } + if (_task[TASK_AP] & TASK_AP_COPTER_FLYHOLD) { + RESETTASK(TASK_AP, TASK_AP_COPTER_FLYHOLD); + _generateCmdLong(_sysid, autopilot.compid, MAV_CMD_SOLO_BTN_FLY_HOLD, _tacf_takeoff_alt_m); + return true; //do only one per loop + } + if (_task[TASK_AP] & TASK_AP_COPTER_FLYPAUSE) { + RESETTASK(TASK_AP, TASK_AP_COPTER_FLYPAUSE); + _generateCmdLong(_sysid, autopilot.compid, MAV_CMD_SOLO_BTN_PAUSE_CLICK, 0.0f); //shoot = no + return true; //do only one per loop + } + + if (_task[TASK_AP] & TASK_AP_REQUESTBANNER) { //MAV_CMD_DO_SEND_BANNER + RESETTASK(TASK_AP, TASK_AP_REQUESTBANNER); + _generateCmdLong(_sysid, autopilot.compid, MAV_CMD_DO_SEND_BANNER); + return true; //do only one per loop + } + if (_task[TASK_AP] & TASK_AP_REQUESTPARAM_BATT_CAPACITY) { + RESETTASK(TASK_AP, TASK_AP_REQUESTPARAM_BATT_CAPACITY); + generateParamRequestRead(_sysid, autopilot.compid, "BATT_CAPACITY"); + return true; //do only one per loop + } + if (_task[TASK_AP] & TASK_AP_REQUESTPARAM_BATT2_CAPACITY) { + RESETTASK(TASK_AP, TASK_AP_REQUESTPARAM_BATT2_CAPACITY); + generateParamRequestRead(_sysid, autopilot.compid, "BATT2_CAPACITY"); + return true; //do only one per loop + } + if (_task[TASK_AP] & TASK_AP_REQUESTPARAM_WPNAV_SPEED) { + RESETTASK(TASK_AP, TASK_AP_REQUESTPARAM_WPNAV_SPEED); + generateParamRequestRead(_sysid, autopilot.compid, "WPNAV_SPEED"); + return true; //do only one per loop + } + if (_task[TASK_AP] & TASK_AP_REQUESTPARAM_WPNAV_ACCEL) { + RESETTASK(TASK_AP, TASK_AP_REQUESTPARAM_WPNAV_ACCEL); + generateParamRequestRead(_sysid, autopilot.compid, "WPNAV_ACCEL"); + return true; //do only one per loop + } + if (_task[TASK_AP] & TASK_AP_REQUESTPARAM_WPNAV_ACCEL_Z) { + RESETTASK(TASK_AP, TASK_AP_REQUESTPARAM_WPNAV_ACCEL_Z); + generateParamRequestRead(_sysid, autopilot.compid, "WPNAV_ACCEL_Z"); + return true; //do only one per loop + } + if (_task[TASK_AP] & TASK_AP_REQUESTPARAM_SYSID_MYGCS) { + RESETTASK(TASK_AP, TASK_AP_REQUESTPARAM_SYSID_MYGCS); + generateParamRequestRead(_sysid, autopilot.compid, "SYSID_MYGCS"); + return true; //do only one per loop + } + if (_task[TASK_AP] & TASK_AP_REQUESTPARAM_ARMING_CHECK) { + RESETTASK(TASK_AP, TASK_AP_REQUESTPARAM_ARMING_CHECK); + generateParamRequestRead(_sysid, autopilot.compid, "ARMING_CHECK"); + return true; //do only one per loop + } + + return false; +} + +// -- Handle incoming MAVLink messages, which are for the Autopilot -- + +void MavlinkTelem::handleMessageAutopilot(void) +{ + autopilot.is_receiving = MAVLINK_TELEM_RECEIVING_TIMEOUT; //we accept any msg from the autopilot to indicate it is alive + + switch (_msg.msgid) { + case FASTMAVLINK_MSG_ID_HEARTBEAT: { + fmav_heartbeat_t payload; + fmav_msg_heartbeat_decode(&payload, &_msg); + flightmode = payload.custom_mode; + autopilot.system_status = payload.system_status; + autopilot.is_armed = (payload.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) ? true : false; + autopilot.is_standby = (payload.system_status <= MAV_STATE_STANDBY) ? true : false; + autopilot.is_critical = (payload.system_status >= MAV_STATE_CRITICAL) ? true : false; + INCU8(autopilot.updated); + //autopilot.is_receiving = MAVLINK_TELEM_RECEIVING_TIMEOUT; + break; + } + + case FASTMAVLINK_MSG_ID_SYS_STATUS: { + fmav_sys_status_t payload; + fmav_msg_sys_status_decode(&payload, &_msg); + sysstatus.sensors_present = payload.onboard_control_sensors_present; + sysstatus.sensors_enabled = payload.onboard_control_sensors_enabled; + sysstatus.sensors_health = payload.onboard_control_sensors_health; + sysstatus.received = true; + INCU8(sysstatus.updated); + break; + } + + case FASTMAVLINK_MSG_ID_ATTITUDE: { + fmav_attitude_t payload; + fmav_msg_attitude_decode(&payload, &_msg); + att.roll_rad = payload.roll; + att.pitch_rad = payload.pitch; + att.yaw_rad = payload.yaw; + INCU8(att.updated); + clear_request(TASK_AUTOPILOT, TASK_SENDCMD_REQUEST_ATTITUDE); + autopilot.requests_waiting_mask &=~ AUTOPILOT_REQUESTWAITING_ATTITUDE; + break; + } + + case FASTMAVLINK_MSG_ID_GPS_RAW_INT: { + fmav_gps_raw_int_t payload; + fmav_msg_gps_raw_int_decode(&payload, &_msg); + gps1.fix = payload.fix_type; + gps1.sat = payload.satellites_visible; + gps1.hdop = payload.eph; + gps1.vdop = payload.epv; + gps1.lat = payload.lat; + gps1.lon = payload.lon; + gps1.alt_mm = payload.alt; + gps1.vel_cmps = payload.vel; + gps1.cog_cdeg = payload.cog; + INCU8(gps1.updated); + gps_instancemask |= 0x01; + clear_request(TASK_AUTOPILOT, TASK_SENDREQUESTDATASTREAM_EXTENDED_STATUS); + autopilot.requests_waiting_mask &=~ AUTOPILOT_REQUESTWAITING_GPS_RAW_INT; + telemetrySetValue(GPS_ALT_FIRST_ID, 0, 10, (int32_t)(payload.alt), UNIT_METERS, 3); + if (payload.vel != UINT16_MAX) { + telemetrySetValue(GPS_SPEED_FIRST_ID, 0, 11, (int32_t)(payload.vel), UNIT_METERS, 2); + } + break; + } + + case FASTMAVLINK_MSG_ID_GPS2_RAW: { + fmav_gps2_raw_t payload; + fmav_msg_gps2_raw_decode(&payload, &_msg); + gps2.fix = payload.fix_type; + gps2.sat = payload.satellites_visible; + gps2.hdop = payload.eph; + gps2.vdop = payload.epv; + gps2.lat = payload.lat; + gps2.lon = payload.lon; + gps2.alt_mm = payload.alt; + gps2.vel_cmps = payload.vel; + gps2.cog_cdeg = payload.cog; + INCU8(gps2.updated); + gps_instancemask |= 0x02; + break; + } + + case FASTMAVLINK_MSG_ID_GLOBAL_POSITION_INT: { + fmav_global_position_int_t payload; + fmav_msg_global_position_int_decode(&payload, &_msg); + gposition.lat = payload.lat; + gposition.lon = payload.lon; + gposition.alt_mm = payload.alt; + gposition.relative_alt_mm = payload.relative_alt; + gposition.vx_cmps = payload.vx; + gposition.vy_cmps = payload.vy; + gposition.vz_cmps = payload.vz; + gposition.hdg_cdeg = payload.hdg; + INCU8(gposition.updated); + clear_request(TASK_AUTOPILOT, TASK_SENDCMD_REQUEST_GLOBAL_POSITION_INT); + autopilot.requests_waiting_mask &=~ AUTOPILOT_REQUESTWAITING_GLOBAL_POSITION_INT; + break; + } + + case FASTMAVLINK_MSG_ID_VFR_HUD: { + fmav_vfr_hud_t payload; + fmav_msg_vfr_hud_decode(&payload, &_msg); + vfr.airspd_mps = payload.airspeed; + vfr.groundspd_mps = payload.groundspeed; + vfr.alt_m = payload.alt; + vfr.climbrate_mps = payload.climb; + vfr.heading_deg = payload.heading; + vfr.thro_pct = payload.throttle; + INCU8(vfr.updated); + clear_request(TASK_AUTOPILOT, TASK_SENDREQUESTDATASTREAM_EXTRA2); + autopilot.requests_waiting_mask &=~ AUTOPILOT_REQUESTWAITING_VFR_HUD; + telemetrySetValue(ALT_FIRST_ID, 0, 13, (int32_t)(payload.alt * 100.0f), UNIT_METERS, 2); + telemetrySetValue(VARIO_FIRST_ID, 0, 14, (int32_t)(payload.climb * 100.0f), UNIT_METERS_PER_SECOND, 2); + telemetrySetValue(AIR_SPEED_FIRST_ID, 0, 15, (int32_t)(payload.airspeed * 100.0f), UNIT_METERS_PER_SECOND, 2); + telemetrySetValue(GPS_COURS_FIRST_ID, 0, 16, (int32_t)payload.heading * 10, UNIT_DEGREE, 1); + break; + } + + /* let's use BATTERY_STATUS, is nearly the same thing + case FASTMAVLINK_MSG_ID_SYS_STATUS: { + fmav_sys_status_t payload; + fmav_msg_sys_status_decode(&payload, &_msg); + // voltage_battery uint16_t mV + // current_battery int16_t cA + // battery_remaining int8_t % + break; + }*/ + + case FASTMAVLINK_MSG_ID_BATTERY_STATUS: { + fmav_battery_status_t payload; + fmav_msg_battery_status_decode(&payload, &_msg); + int32_t voltage = 0; + int8_t cellcount = 0; + bool validcellcount = true; + for (uint8_t i=0; i<10; i++) { + if (payload.voltages[i] != UINT16_MAX) { + voltage += payload.voltages[i]; //uint16_t mV, UINT16_MAX if not known + if (payload.voltages[i] == UINT16_MAX-1) validcellcount = false; + cellcount++; + } + } + for (uint8_t i=0; i<4; i++) { //we assume this never is relevant if validcellcount = false + if (payload.voltages_ext[i] != 0) { + voltage += payload.voltages_ext[i]; //uint16_t mV, 0 if not known + cellcount++; + } + } + if (!validcellcount) cellcount = -1; + if (payload.id == 0) { + bat1.charge_consumed_mAh = payload.current_consumed; // mAh, -1 if not known + bat1.energy_consumed_hJ = payload.energy_consumed; // 0.1 kJ, -1 if not known + bat1.temperature_cC = payload.temperature; // centi-degrees C�, INT16_MAX if not known + bat1.voltage_mV = voltage; // mV + bat1.current_cA = payload.current_battery; // 10*mA, -1 if not known + bat1.remaining_pct = payload.battery_remaining; //(0%: 0, 100%: 100), -1 if not knwon + bat1.time_remaining = payload.time_remaining; // 0 if not knwon + bat1.charge_state = payload.charge_state; // 0 if not knwon + bat1.fault_bitmask = payload.fault_bitmask; + bat1.cellcount = cellcount; + INCU8(bat1.updated); + } + if (payload.id == 1) { + bat2.charge_consumed_mAh = payload.current_consumed; // mAh, -1 if not known + bat2.energy_consumed_hJ = payload.energy_consumed; // 0.1 kJ, -1 if not known + bat2.temperature_cC = payload.temperature; // centi-degrees C�, INT16_MAX if not known + bat2.voltage_mV = voltage; // mV + bat2.current_cA = payload.current_battery; // 10*mA, -1 if not known + bat2.remaining_pct = payload.battery_remaining; //(0%: 0, 100%: 100), -1 if not knwon + bat2.time_remaining = payload.time_remaining; // 0 if not knwon + bat2.charge_state = payload.charge_state; // 0 if not knwon + bat2.fault_bitmask = payload.fault_bitmask; + bat2.cellcount = cellcount; + INCU8(bat2.updated); + } + if (payload.id < 8) bat_instancemask |= (1 << payload.id); + telemetrySetValue(BATT_ID, 0, 17, voltage/100, UNIT_VOLTS, 1); + telemetrySetValue(VFAS_FIRST_ID, 0, 18, voltage/10, UNIT_VOLTS, 2); + telemetrySetValue(CURR_FIRST_ID, 0, 19, payload.current_battery/10, UNIT_AMPS, 1); + break; + } + + case FASTMAVLINK_MSG_ID_STATUSTEXT: { + fmav_statustext_t payload; + fmav_msg_statustext_decode(&payload, &_msg); + statustext.fifo.push(payload); + INCU8(statustext.updated); + break; + } + + case FASTMAVLINK_MSG_ID_EKF_STATUS_REPORT: { + fmav_ekf_status_report_t payload; + fmav_msg_ekf_status_report_decode(&payload, &_msg); + //we don't really need the other fields + ekf.flags = payload.flags; + INCU8(ekf.updated); + clear_request(TASK_AUTOPILOT, TASK_SENDREQUESTDATASTREAM_EXTRA3); + autopilot.requests_waiting_mask &=~ AUTOPILOT_REQUESTWAITING_EKF_STATUS_REPORT; + break; + } + + case FASTMAVLINK_MSG_ID_RC_CHANNELS_RAW: { //#35 + fmav_rc_channels_raw_t payload; + fmav_msg_rc_channels_raw_decode(&payload, &_msg); + radio.rssi35 = payload.rssi; + radio.is_receiving35 = MAVLINK_TELEM_RADIO_RECEIVING_TIMEOUT; + if (!radio.is_receiving && !radio.is_receiving65) { + telemetrySetRssiValue(radio.rssi35); + } + break; + } + + case FASTMAVLINK_MSG_ID_RC_CHANNELS: { //#65 + fmav_rc_channels_t payload; + fmav_msg_rc_channels_decode(&payload, &_msg); + radio.rssi65 = payload.rssi; + radio.is_receiving65 = MAVLINK_TELEM_RADIO_RECEIVING_TIMEOUT; + clear_request(TASK_AUTOPILOT, TASK_SENDREQUESTDATASTREAM_RC_CHANNELS); + if (!radio.is_receiving) { + telemetrySetRssiValue(radio.rssi65); + } + break; + } + + case FASTMAVLINK_MSG_ID_RANGEFINDER: { + fmav_rangefinder_t payload; + fmav_msg_rangefinder_decode(&payload, &_msg); + //we don't really need the other fields + rangefinder.distance = payload.distance; + INCU8(rangefinder.updated); + break; + } + + case FASTMAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: { + fmav_nav_controller_output_t payload; + fmav_msg_nav_controller_output_decode(&payload, &_msg); + //we don't really need the other fields + navControllerOutput.nav_bearing = payload.nav_bearing; + navControllerOutput.target_bearing = payload.target_bearing; + navControllerOutput.wp_dist = payload.wp_dist; + INCU8(navControllerOutput.updated); + break; + } + + case FASTMAVLINK_MSG_ID_MISSION_COUNT: { + fmav_mission_count_t payload; + fmav_msg_mission_count_decode(&payload, &_msg); + if (payload.mission_type != MAV_MISSION_TYPE_MISSION) break; //not a MISSION item + mission.count = payload.count; + INCU8(mission.updated); + clear_request(TASK_AUTOPILOT, TASK_SENDMSG_MISSION_REQUEST_LIST); + break; + } + + case FASTMAVLINK_MSG_ID_MISSION_CURRENT: { + fmav_mission_current_t payload; + fmav_msg_mission_current_decode(&payload, &_msg); + bool has_changed = (mission.seq_current != payload.seq); + mission.seq_current = payload.seq; + INCU8(mission.updated); + // if the current has changed, then trigger a new MISSION_REQUEST_INT + if (has_changed) requestMissionRequestInt(mission.seq_current); + break; + } + + case FASTMAVLINK_MSG_ID_MISSION_ITEM_INT: { + fmav_mission_item_int_t payload; + fmav_msg_mission_item_int_decode(&payload, &_msg); + if (payload.mission_type != MAV_MISSION_TYPE_MISSION) break; //not a MISSION item + if (payload.frame == MAV_FRAME_MISSION) break; //not a coordinate frame, indicates a mission command + missionItem.seq = payload.seq; + missionItem.frame = payload.frame; + missionItem.command = payload.command; + missionItem.x = payload.x; + missionItem.y = payload.y; + missionItem.z = payload.z; + INCU8(missionItem.updated); + clear_request(TASK_AUTOPILOT, TASK_SENDMSG_MISSION_REQUEST_INT); + break; + } + + case FASTMAVLINK_MSG_ID_PARAM_VALUE: { + fmav_param_value_t payload; + fmav_msg_param_value_decode(&payload, &_msg); + if (!strncmp(payload.param_id,"BATT_CAPACITY",16)) { + param.BATT_CAPACITY = payload.param_value; + clear_request(TASK_AP, TASK_AP_REQUESTPARAM_BATT_CAPACITY); + } + if (!strncmp(payload.param_id,"BATT2_CAPACITY",16)) { + param.BATT2_CAPACITY = payload.param_value; + clear_request(TASK_AP, TASK_AP_REQUESTPARAM_BATT2_CAPACITY); + } + if (!strncmp(payload.param_id,"WPNAV_SPEED",16)) { + param.WPNAV_SPEED = payload.param_value; + clear_request(TASK_AP, TASK_AP_REQUESTPARAM_WPNAV_SPEED); + } + if (!strncmp(payload.param_id,"WPNAV_ACCEL",16)) { + param.WPNAV_ACCEL = payload.param_value; + clear_request(TASK_AP, TASK_AP_REQUESTPARAM_WPNAV_ACCEL); + } + if (!strncmp(payload.param_id,"WPNAV_ACCEL_Z",16)) { + param.WPNAV_ACCEL_Z = payload.param_value; + clear_request(TASK_AP, TASK_AP_REQUESTPARAM_WPNAV_ACCEL_Z); + } + if (!strncmp(payload.param_id,"SYSID_MYGCS",16)) { + param.SYSID_MYGCS = payload.param_value; + clear_request(TASK_AP, TASK_AP_REQUESTPARAM_SYSID_MYGCS); + } + if (!strncmp(payload.param_id,"ARMING_CHECK",16)) { + param.ARMING_CHECK = payload.param_value; + clear_request(TASK_AP, TASK_AP_REQUESTPARAM_ARMING_CHECK); + } + break; + } + } +} + +// -- Resets -- + +void MavlinkTelem::_resetAutopilot(void) +{ + _task[TASK_AUTOPILOT] = 0; + _task[TASK_AP] = 0; + + autopilot.compid = 0; + autopilot.is_receiving = 0; + + autopilot.requests_triggered = 0; + autopilot.requests_waiting_mask = AUTOPILOT_REQUESTWAITING_ALL; + autopilot.is_initialized = false; + + autopilot.system_status = MAV_STATE_UNINIT; + autopilot.custom_mode = 0; + autopilot.is_armed = false; + autopilot.is_standby = true; + autopilot.is_critical = false; + autopilot.prearm_ok = false; + autopilot.updated = 0; + + sysstatus.sensors_present = 0; + sysstatus.sensors_enabled = 0; + sysstatus.sensors_health = 0; + sysstatus.received = false; + sysstatus.updated = 0; + + att.roll_rad = 0.0f; + att.pitch_rad = 0.0f; + att.yaw_rad = 0.0f; + att.updated = 0; + + gps1.fix = GPS_FIX_TYPE_NO_GPS; + gps1.sat = UINT8_MAX; + gps1.hdop = UINT16_MAX; + gps1.vdop = UINT16_MAX; + gps1.lat = 0; + gps1.lon = 0; + gps1.alt_mm = 0; + gps1.vel_cmps = UINT16_MAX; + gps1.cog_cdeg = UINT16_MAX; + gps1.updated = 0; + + gps2.fix = GPS_FIX_TYPE_NO_GPS; + gps2.sat = UINT8_MAX; + gps2.hdop = UINT16_MAX; + gps2.vdop = UINT16_MAX; + gps2.lat = 0; + gps2.lon = 0; + gps2.alt_mm = 0; + gps2.vel_cmps = UINT16_MAX; + gps2.cog_cdeg = UINT16_MAX; + gps2.updated = 0; + + gps_instancemask = 0; + + gposition.lat = 0; + gposition.lon = 0; + gposition.alt_mm = 0; + gposition.relative_alt_mm = 0; + gposition.vx_cmps = 0; + gposition.vy_cmps = 0; + gposition.vz_cmps = 0; + gposition.hdg_cdeg = UINT16_MAX; + gposition.updated = 0; + + vfr.airspd_mps = 0.0f; + vfr.groundspd_mps = 0.0f; + vfr.alt_m = 0.0f; + vfr.climbrate_mps = 0.0f; + vfr.heading_deg = 0; + vfr.thro_pct = 0; + vfr.updated = 0; + + bat1.charge_consumed_mAh = -1; + bat1.energy_consumed_hJ = -1; + bat1.temperature_cC = INT16_MAX; + bat1.voltage_mV = 0; + bat1.current_cA = -1; + bat1.remaining_pct = -1; + bat1.time_remaining = 0; + bat1.charge_state = 0; + bat1.fault_bitmask = 0; + bat1.cellcount = -1; + bat1.updated = 0; + + bat2.charge_consumed_mAh = -1; + bat2.energy_consumed_hJ = -1; + bat2.temperature_cC = INT16_MAX; + bat2.voltage_mV = 0; + bat2.current_cA = -1; + bat2.remaining_pct = -1; + bat2.time_remaining = 0; + bat2.charge_state = 0; + bat2.fault_bitmask = 0; + bat2.cellcount = -1; + bat2.updated = 0; + + bat_instancemask = 0; + + statustext.fifo.clear(); + statustext.updated = 0; + + ekf.flags = 0; + ekf.updated = 0; + + rangefinder.distance = 0.0f; + rangefinder.updated = 0; + + navControllerOutput.nav_bearing = 0; // Current desired heading in degrees + navControllerOutput.target_bearing = 0; // Bearing to current MISSION/target in degrees + navControllerOutput.wp_dist = 0; // Distance to active MISSION in meters + navControllerOutput.updated = 0; + + mission.count = 0; + mission.seq_current = UINT16_MAX; //to enforce a request at first incoming MISSION_CURRENT + mission.updated = 0; + + missionItem.seq = 0; // Waypoint ID (sequence number). Starts at zero. Increases monotonically for each waypoint, no gaps in the sequence (0,1,2,3,4). + missionItem.frame = 0; // The coordinate system of the waypoint. + missionItem.command = 0; // The scheduled action for the waypnt. + missionItem.x = 0; // PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + missionItem.y = 0; // PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 + missionItem.z = 0.0f; // PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + missionItem.updated = 0; + + param.number = -1; + param.BATT_CAPACITY = -1; + param.BATT2_CAPACITY = -1; + param.WPNAV_SPEED = NAN; + param.WPNAV_ACCEL = NAN; + param.WPNAV_ACCEL_Z = NAN; + param.SYSID_MYGCS = -1; + param.ARMING_CHECK = -1; +} + +// -- Miscellaneous -- + +// ArduPilot starts with sending heartbeat every 1 sec with FE, TimeSync every 5 sec with FE +// we then need to request the data stream +// TODO: we can also request them individually now + +void MavlinkTelem::setAutopilotStartupRequests(void) +{ + if (autopilottype == MAV_AUTOPILOT_ARDUPILOTMEGA) { + // 2 Hz sufficient + // MAVLINK_MSG_ID_GPS_RAW_INT + // MAVLINK_MSG_ID_GPS2_RAW + // MAVLINK_MSG_ID_SYS_STATUS + // MSG_NAV_CONTROLLER_OUTPUT + // MSG_CURRENT_WAYPOINT // MISSION_CURRENT + // cleared by MAVLINK_MSG_ID_GPS_RAW_INT (is send even when both GPS are 0, so we can use it to clear) + set_request(TASK_AUTOPILOT, TASK_SENDREQUESTDATASTREAM_EXTENDED_STATUS, 100, 200-4); + + // MAVLINK_MSG_ID_GLOBAL_POSITION_INT + // cleared by MAVLINK_MSG_ID_GLOBAL_POSITION_INT + //set_request(TASK_AUTOPILOT, TASK_SENDREQUESTDATASTREAM_POSITION, 100, 198); + + // MAVLINK_MSG_ID_ATTITUDE + // cleared by MAVLINK_MSG_ID_ATTITUDE + //set_request(TASK_AUTOPILOT, TASK_SENDREQUESTDATASTREAM_EXTRA1, 100, 211); + + // 2 Hz sufficient + // MAVLINK_MSG_ID_VFR_HUD + // cleared by MAVLINK_MSG_ID_VFR_HUD + set_request(TASK_AUTOPILOT, TASK_SENDREQUESTDATASTREAM_EXTRA2, 100, 200+20); + + // 2 Hz sufficient + // MAVLINK_MSG_ID_BATTERY_STATUS (only if batt monitor configured) + // MAVLINK_MSG_ID_BATTERY2 (only if batt2 monitor configured) + // MAVLINK_MSG_ID_EKF_STATUS_REPORT + // cleared by MAVLINK_MSG_ID_EKF_STATUS_REPORT + set_request(TASK_AUTOPILOT, TASK_SENDREQUESTDATASTREAM_EXTRA3, 100, 200+3); + + // 1 Hz sufficient + // MSG_SERVO_OUTPUT_RAW + // MSG_RC_CHANNELS + // MSG_RC_CHANNELS_RAW (only sent on a mavlink1 connection) + // cleared by MAVLINK_MSG_ID_RC_CHANNELS + set_request(TASK_AUTOPILOT, TASK_SENDREQUESTDATASTREAM_RC_CHANNELS, 100, 200-8); + + // call at high rates of 10 Hz, or 5 Hz + // MAVLINK_MSG_ID_ATTITUDE + // cleared by MAVLINK_MSG_ID_ATTITUDE + set_request(TASK_AUTOPILOT, TASK_SENDCMD_REQUEST_ATTITUDE, 100, 200+5); + + // call at high rates of 10 Hz, or 5 Hz + // MAVLINK_MSG_ID_GLOBAL_POSITION_INT + // cleared by MAVLINK_MSG_ID_GLOBAL_POSITION_INT + set_request(TASK_AUTOPILOT, TASK_SENDCMD_REQUEST_GLOBAL_POSITION_INT, 100, 200+7); + + set_request(TASK_AUTOPILOT, TASK_SENDMSG_MISSION_REQUEST_LIST, 10, 341); + + set_request(TASK_AP, TASK_AP_REQUESTPARAM_BATT_CAPACITY, 10, 200+25); + set_request(TASK_AP, TASK_AP_REQUESTPARAM_BATT2_CAPACITY, 10, 200+28); + set_request(TASK_AP, TASK_AP_REQUESTPARAM_SYSID_MYGCS, 10, 123); //we request it more frequently to get it sooner + set_request(TASK_AP, TASK_AP_REQUESTPARAM_ARMING_CHECK, 10, 200+33); + + push_task(TASK_AP, TASK_AP_REQUESTBANNER); + } + + // other autopilots + // TODO +} + + + + + +/* +ArduPilot Copter Streams: +updated 29.01.2021 + +static const ap_message STREAM_RAW_SENSORS_msgs[] = { + MSG_RAW_IMU, + MSG_SCALED_IMU2, + MSG_SCALED_IMU3, + MSG_SCALED_PRESSURE, + MSG_SCALED_PRESSURE2, + MSG_SCALED_PRESSURE3, + MSG_SENSOR_OFFSETS +}; +static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { + MSG_SYS_STATUS, + MSG_POWER_STATUS, + MSG_MEMINFO, + MSG_CURRENT_WAYPOINT, // MISSION_CURRENT + MSG_GPS_RAW, + MSG_GPS_RTK, + MSG_GPS2_RAW, + MSG_GPS2_RTK, + MSG_NAV_CONTROLLER_OUTPUT, + MSG_FENCE_STATUS, + MSG_POSITION_TARGET_GLOBAL_INT, +}; +static const ap_message STREAM_POSITION_msgs[] = { + MSG_LOCATION, + MSG_LOCAL_POSITION +}; +static const ap_message STREAM_RC_CHANNELS_msgs[] = { + MSG_SERVO_OUTPUT_RAW, + MSG_RC_CHANNELS, + MSG_RC_CHANNELS_RAW, // only sent on a mavlink1 connection +}; +static const ap_message STREAM_EXTRA1_msgs[] = { + MSG_ATTITUDE, + MSG_SIMSTATE, + MSG_AHRS2, + MSG_PID_TUNING // Up to four PID_TUNING messages are sent, depending on GCS_PID_MASK parameter +}; +static const ap_message STREAM_EXTRA2_msgs[] = { + MSG_VFR_HUD +}; +static const ap_message STREAM_EXTRA3_msgs[] = { + MSG_AHRS, + MSG_HWSTATUS, + MSG_SYSTEM_TIME, + MSG_RANGEFINDER, -> ROTATION_PITCH_270 + MSG_DISTANCE_SENSOR, -> send any and all distance_sensor messages. This starts by sending any distance sensors not used by a Proximity sensor, then sends the proximity sensor ones. + +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + MSG_TERRAIN, +#endif + MSG_BATTERY2, + MSG_BATTERY_STATUS, + MSG_MOUNT_STATUS, + MSG_OPTICAL_FLOW, + MSG_GIMBAL_REPORT, + MSG_MAG_CAL_REPORT, + MSG_MAG_CAL_PROGRESS, + MSG_EKF_STATUS_REPORT, + MSG_VIBRATION, + MSG_RPM, + MSG_ESC_TELEMETRY, + MSG_GENERATOR_STATUS, + MSG_WINCH_STATUS, +}; +static const ap_message STREAM_PARAMS_msgs[] = { + MSG_NEXT_PARAM +}; +static const ap_message STREAM_ADSB_msgs[] = { + MSG_ADSB_VEHICLE +}; + +const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { + MAV_STREAM_ENTRY(STREAM_RAW_SENSORS), + MAV_STREAM_ENTRY(STREAM_EXTENDED_STATUS), + MAV_STREAM_ENTRY(STREAM_POSITION), + MAV_STREAM_ENTRY(STREAM_RC_CHANNELS), + MAV_STREAM_ENTRY(STREAM_EXTRA1), + MAV_STREAM_ENTRY(STREAM_EXTRA2), + MAV_STREAM_ENTRY(STREAM_EXTRA3), + MAV_STREAM_ENTRY(STREAM_ADSB), + MAV_STREAM_ENTRY(STREAM_PARAMS), + MAV_STREAM_TERMINATOR // must have this at end of stream_entries +}; +*/ + +/* +AP battery vs sys_status + +=> BATTERY sends the very same data as SYS_STATUS, but SYS_STATUS has initialized() and health() tests in addition +=> not clear which is better +let's decide to use BATTERY + +BATTERY: + AP_BattMonitor::cells fake_cells; + sends total voltage splited up into cells of 65534 + + float current, consumed_mah, consumed_wh; + if (battery.current_amps(current, instance)) { + current *= 100; + } + else { + current = -1; + } + if (!battery.consumed_mah(consumed_mah, instance)) { + consumed_mah = -1; + } + if (battery.consumed_wh(consumed_wh, instance)) { + consumed_wh *= 36; + } + else { + consumed_wh = -1; + } + + mavlink_msg_battery_status_send(chan, + instance, // id + MAV_BATTERY_FUNCTION_UNKNOWN, // function + MAV_BATTERY_TYPE_UNKNOWN, // type + got_temperature ? ((int16_t) (temp * 100)) : INT16_MAX, // temperature. INT16_MAX if unknown + battery.has_cell_voltages(instance) ? battery.get_cell_voltages(instance).cells : fake_cells.cells, // cell voltages + current, // current in centiampere + consumed_mah, // total consumed current in milliampere.hour + consumed_wh, // consumed energy in hJ (hecto-Joules) + battery.capacity_remaining_pct(instance), + 0, // time remaining, seconds (not provided) + MAV_BATTERY_CHARGE_STATE_UNDEFINED); + +SYS_STATUS: + if (!gcs().vehicle_initialised()) { + return; + } + + const AP_BattMonitor &battery = AP::battery(); + float battery_current; + int8_t battery_remaining; + + if (battery.healthy() && battery.current_amps(battery_current)) { + battery_remaining = battery.capacity_remaining_pct(); + battery_current *= 100; + } + else { + battery_current = -1; + battery_remaining = -1; + } + + ... + + mavlink_msg_sys_status_send( + chan, + control_sensors_present, + control_sensors_enabled, + control_sensors_health, + static_cast(AP::scheduler().load_average() * 1000), + battery.voltage() * 1000, // mV + battery_current, // in 10mA units + battery_remaining, // in % + 0, // comm drops %, + 0, // comm drops in pkts, + errors1, + errors2, + 0, // errors3 + errors4); // errors4 +*/ + +/* +DATA_ID == 0x5006 then -- ROLLPITCH +DATA_ID == 0x5005 then -- VELANDYAW +DATA_ID == 0x5001 then -- AP STATUS +DATA_ID == 0x5002 then -- GPS STATUS +DATA_ID == 0x5003 then -- BATT +DATA_ID == 0x5008 then -- BATT2 +DATA_ID == 0x5004 then -- HOME +DATA_ID == 0x5000 then -- MESSAGES +DATA_ID == 0x5007 then -- PARAMS +DATA_ID == 0x5009 then -- WAYPOINTS @1Hz +DATA_ID == 0x50F1 then -- RC CHANNELS +DATA_ID == 0x50F2 then -- VFR + +AP +DATA_ID == 0x5000 then -- MESSAGES +DATA_ID == 0x5006 then -- ROLLPITCH +x0800 GPS LAT LON +DATA_ID == 0x5005 then -- VELANDYAW +DATA_ID == 0x5001 then -- AP STATUS +DATA_ID == 0x5002 then -- GPS STATUS +DATA_ID == 0x5004 then -- HOME +DATA_ID == 0x5008 then -- BATT2 +DATA_ID == 0x5003 then -- BATT +DATA_ID == 0x5007 then -- PARAMS + */ +/* + int32_t data = 12345 + _data; + _data++; +#if defined(LUA) + if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(sizeof(SportTelemetryPacket))) { + SportTelemetryPacket luaPacket; + luaPacket.physicalId = 0; //sensor_id, isn't used + luaPacket.primId = 0x10; //frame_id, only 0x10 is evaluated + luaPacket.dataId = 0x5003; //data_id: + luaPacket.value = data; //value + for (uint8_t i=0; ipush(luaPacket.raw[i]); + } + } +#endif + */ + + +/* +DroneKit +is_armable(self): + return self.mode != 'INITIALISING' and (self.gps_0.fix_type is not None and self.gps_0.fix_type > 1) and self._ekf_predposhorizabs +ekf_ok(self): + # use same check that ArduCopter::system.pde::position_ok() is using + if self.armed: return self._ekf_poshorizabs and not self._ekf_constposmode + else: return self._ekf_poshorizabs or self._ekf_predposhorizabs +groundspeed(self, speed): + command_long_encode(0, 0, mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, 0, 1, speed, -1, 0, 0, 0, 0) +airspeed(self, speed): + command_long_encode(0, 0, mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, 0, 0, speed, -1, 0, 0, 0, 0) +simple_takeoff(self, alt=None): + command_long_send(0, 0, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, altitude) +simple_goto(self, location, airspeed=None, groundspeed=None): + frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT + mission_item_send(0, 0, 0, frame, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2, 0, 0, 0, 0, 0, location.lat, location.lon, alt) + +gimbal +rotate(self, pitch, roll, yaw): + mount_configure_encode( 0, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, 1, 1, 1) + mount_control_encode(0, 1, pitch * 100, roll * 100, yaw * 100, 0) +target_location(self, roi): + mount_configure_encode(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT, 1, 1, 1) + command_long_encode(0, 1, mavutil.mavlink.MAV_CMD_DO_SET_ROI, 0, 0, 0, 0, 0, roi.lat, roi.lon, alt) +release(self): + mount_configure_encode(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, 1, 1, 1) + + +solo shots +class VectorPathHandler(PathHandler): +def move(self, channels): +def travel(self): + set_position_target_global_int_encode( + 0, 0, 1, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + 0b0000110111000000, + int(loc.lat * 10000000), int(loc.lon * 10000000), loc.alt, + velVector.x, velVector.y, velVector.z, + 0, 0, 0, + 0, 0) + + initStreamRates(self): + STREAM_RATES = { + mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS: 2, + mavutil.mavlink.MAV_DATA_STREAM_EXTRA1: 10, + mavutil.mavlink.MAV_DATA_STREAM_EXTRA2: 10, + mavutil.mavlink.MAV_DATA_STREAM_EXTRA3: 2, + mavutil.mavlink.MAV_DATA_STREAM_POSITION: 10, + mavutil.mavlink.MAV_DATA_STREAM_RAW_SENSORS: 2, + mavutil.mavlink.MAV_DATA_STREAM_RAW_CONTROLLER: 3, + mavutil.mavlink.MAV_DATA_STREAM_RC_CHANNELS: 5, + } + +class TwoPointPathHandler(PathHandler): +def MoveTowardsEndpt( self, channels ): + simple_goto(target) + command_long_encode(0, 1, mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, 0, 1, abs(self.currentSpeed), -1, 0.0, 0.0, 0.0, 0.0) + +class MultipointShot(): +def handleRCs(self, channels): + set_position_target_global_int_encode( + 0, 0, 1, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + 0b0000110111000000, + int(self.commandPos.lat * 10000000), int(self.commandPos.lon * 10000000), + self.commandPos.alt, + self.commandVel.x, self.commandVel.y, self.commandVel.z, + 0, 0, 0, + 0, 0) +def enterRecordMode(self): + mode = VehicleMode("LOITER") +def enterPlayMode(self): + mode = VehicleMode("GUIDED") + mount_configure_encode(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, 1, 1, 1) +def handleAttach(self, attach): + simple_goto(self.commandPos) + command_long_encode(0, 1, mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, 0, 1, ATTACH_SPEED, -1, 0.0, 0.0, 0.0, 0.0) + + + */ + + + + + + + +/* + MIXSRC_FIRST_CH, + MIXSRC_CH1 = MIXSRC_FIRST_CH, LUA_EXPORT_MULTIPLE("ch", "Channel CH%d", MAX_OUTPUT_CHANNELS) + MIXSRC_CH2, + MIXSRC_CH3, + MIXSRC_CH4, + MIXSRC_CH5, + MIXSRC_CH6, + MIXSRC_CH7, + MIXSRC_CH8, + MIXSRC_CH9, + MIXSRC_CH10, + MIXSRC_CH11, + MIXSRC_CH12, + MIXSRC_CH13, + MIXSRC_CH14, + MIXSRC_CH15, + MIXSRC_CH16, + MIXSRC_LAST_CH = MIXSRC_CH1+MAX_OUTPUT_CHANNELS-1, + +MAX_OUTPUT_CHANNELS = 32 + +extern int16_t channelOutputs[MAX_OUTPUT_CHANNELS]; + + +#define PPM_CENTER 1500 +#if defined(PPM_CENTER_ADJUSTABLE) + #define PPM_CH_CENTER(ch) (PPM_CENTER + limitAddress(ch)->ppmCenter) +#else + #define PPM_CH_CENTER(ch) (PPM_CENTER) +#endif + + + int16_t PPM_range = g_model.extendedLimits ? (512*LIMIT_EXT_PERCENT/100) * 2 : 512 * 2; // range of 0.7 .. 1.7msec + uint8_t firstCh = channelsStart; + uint8_t lastCh = min(MAX_OUTPUT_CHANNELS, firstCh + 8 + channelsCount); + for (uint32_t i=firstCh; i 31) + return 0; + return channelOutputs[ch] + 2 * PPM_CH_CENTER(ch) - 2*PPM_CENTER; +} + +extern uint8_t g_moduleIdx; + + + + int i = 0; + + for (int ch=1; ch<=MAX_OUTPUT_CHANNELS; ch++) { + MixData * md; + coord_t y = MENU_CONTENT_TOP + (cur-menuVerticalOffset)*FH; + if (isrcRaw && md->destCh+1 == ch) { + if (cur-menuVerticalOffset >= 0 && cur-menuVerticalOffset < NUM_BODY_LINES) { + putsChn(MENUS_MARGIN_LEFT, y, ch, 0); // show CHx + } + + +*/ + diff --git a/radio/src/telemetry/mavlink/mavlink_telem_camera.cpp b/radio/src/telemetry/mavlink/mavlink_telem_camera.cpp new file mode 100644 index 00000000000..d8444857290 --- /dev/null +++ b/radio/src/telemetry/mavlink/mavlink_telem_camera.cpp @@ -0,0 +1,268 @@ +/* + * (c) www.olliw.eu, OlliW, OlliW42 + */ + +#include "opentx.h" + +constexpr float FPI = 3.141592653589793f; +constexpr float FDEGTORAD = FPI/180.0f; +constexpr float FRADTODEG = 180.0f/FPI; + +#define INCU8(x) if ((x) < UINT8_MAX) { (x)++; } + +// -- Generate MAVLink messages -- +// these should never be called directly, should only by called by the task handler + +void MavlinkTelem::generateCmdRequestCameraInformation(uint8_t tsystem, uint8_t tcomponent) +{ + _generateCmdLong(tsystem, tcomponent, MAV_CMD_REQUEST_CAMERA_INFORMATION, 1, 0,0,0,0,0,0); +} + +void MavlinkTelem::generateCmdRequestCameraSettings(uint8_t tsystem, uint8_t tcomponent) +{ + _generateCmdLong(tsystem, tcomponent, MAV_CMD_REQUEST_CAMERA_SETTINGS, 1, 0,0,0,0,0,0); +} + +void MavlinkTelem::generateCmdRequestStorageInformation(uint8_t tsystem, uint8_t tcomponent) +{ + _generateCmdLong(tsystem, tcomponent, MAV_CMD_REQUEST_STORAGE_INFORMATION, 0, 1, 0,0,0,0,0); +} + +void MavlinkTelem::generateCmdRequestCameraCapturesStatus(uint8_t tsystem, uint8_t tcomponent) +{ + _generateCmdLong(tsystem, tcomponent, MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS, 1, 0,0,0,0,0,0); +} + +void MavlinkTelem::generateCmdSetCameraMode(uint8_t tsystem, uint8_t tcomponent, uint8_t mode) +{ + _generateCmdLong(tsystem, tcomponent, MAV_CMD_SET_CAMERA_MODE, 0, mode, 0,0,0,0,0); +} + +void MavlinkTelem::generateCmdImageStartCapture(uint8_t tsystem, uint8_t tcomponent) +{ + _generateCmdLong(tsystem, tcomponent, MAV_CMD_IMAGE_START_CAPTURE, 0, 0, 1, 0, 0,0,0); +} + +void MavlinkTelem::generateCmdVideoStartCapture(uint8_t tsystem, uint8_t tcomponent) +{ + _generateCmdLong(tsystem, tcomponent, MAV_CMD_VIDEO_START_CAPTURE, 0, 0.2f, 0,0,0,0,0); +} + +void MavlinkTelem::generateCmdVideoStopCapture(uint8_t tsystem, uint8_t tcomponent) +{ + _generateCmdLong(tsystem, tcomponent, MAV_CMD_VIDEO_STOP_CAPTURE, 0,0,0,0,0,0,0); +} + +// -- Mavsdk Convenience Task Wrapper -- +// to make it easy for api_mavsdk to call functions +// are short here and defined in mavlink_telem.h + +// -- Task handlers -- + +bool MavlinkTelem::doTaskCamera(void) +{ + if (!_task[TASK_CAMERA]) return false; // no task pending + + if (!camera.compid) { _task[TASK_CAMERA] = 0; return false; } + + if (_task[TASK_CAMERA] & TASK_SENDCMD_IMAGE_START_CAPTURE) { + RESETTASK(TASK_CAMERA, TASK_SENDCMD_IMAGE_START_CAPTURE); + generateCmdImageStartCapture(_sysid, camera.compid); + set_request(TASK_CAMERA, TASK_SENDREQUEST_CAMERA_CAPTURE_STATUS, 2); + return true; //do only one per loop + } + if (_task[TASK_CAMERA] & TASK_SENDCMD_VIDEO_START_CAPTURE) { + RESETTASK(TASK_CAMERA, TASK_SENDCMD_VIDEO_START_CAPTURE); + generateCmdVideoStartCapture(_sysid, camera.compid); + set_request(TASK_CAMERA, TASK_SENDREQUEST_CAMERA_CAPTURE_STATUS, 2); + return true; //do only one per loop + } + if (_task[TASK_CAMERA] & TASK_SENDCMD_VIDEO_STOP_CAPTURE) { + RESETTASK(TASK_CAMERA, TASK_SENDCMD_VIDEO_STOP_CAPTURE); + generateCmdVideoStopCapture(_sysid, camera.compid); + set_request(TASK_CAMERA, TASK_SENDREQUEST_CAMERA_CAPTURE_STATUS, 2); + return true; //do only one per loop + } + + return false; +} + +bool MavlinkTelem::doTaskCameraLowPriority(void) +{ + if (!_task[TASK_CAMERA]) return false; // no task pending + + if (_task[TASK_CAMERA] & TASK_SENDCMD_SET_CAMERA_VIDEO_MODE) { + RESETTASK(TASK_CAMERA, TASK_SENDCMD_SET_CAMERA_VIDEO_MODE); + generateCmdSetCameraMode(_sysid, camera.compid, CAMERA_MODE_VIDEO); + set_request(TASK_CAMERA, TASK_SENDREQUEST_CAMERA_SETTINGS, 2); + return true; //do only one per loop + } + if (_task[TASK_CAMERA] & TASK_SENDCMD_SET_CAMERA_PHOTO_MODE) { + RESETTASK(TASK_CAMERA, TASK_SENDCMD_SET_CAMERA_PHOTO_MODE); + generateCmdSetCameraMode(_sysid, camera.compid, CAMERA_MODE_IMAGE); + set_request(TASK_CAMERA, TASK_SENDREQUEST_CAMERA_SETTINGS, 2); + return true; //do only one per loop + } + + // the sequence here defines the startup sequence + if (_task[TASK_CAMERA] & TASK_SENDREQUEST_CAMERA_INFORMATION) { + RESETTASK(TASK_CAMERA, TASK_SENDREQUEST_CAMERA_INFORMATION); + generateCmdRequestCameraInformation(_sysid, camera.compid); + return true; //do only one per loop + } + if (_task[TASK_CAMERA] & TASK_SENDREQUEST_CAMERA_SETTINGS) { + RESETTASK(TASK_CAMERA, TASK_SENDREQUEST_CAMERA_SETTINGS); + generateCmdRequestCameraSettings(_sysid, camera.compid); + return true; //do only one per loop + } + if (_task[TASK_CAMERA] & TASK_SENDREQUEST_CAMERA_CAPTURE_STATUS) { + RESETTASK(TASK_CAMERA, TASK_SENDREQUEST_CAMERA_CAPTURE_STATUS); + generateCmdRequestCameraCapturesStatus(_sysid, camera.compid); + return true; //do only one per loop + } + if (_task[TASK_CAMERA] & TASK_SENDREQUEST_STORAGE_INFORMATION) { + RESETTASK(TASK_CAMERA, TASK_SENDREQUEST_STORAGE_INFORMATION); + generateCmdRequestStorageInformation(_sysid, camera.compid); + return true; //do only one per loop + } + + return false; +} + +// -- Handle incoming MAVLink messages, which are for the Camera -- + +void MavlinkTelem::handleMessageCamera(void) +{ + camera.is_receiving = MAVLINK_TELEM_RECEIVING_TIMEOUT; //we accept any msg from the camera to indicate it is alive + + switch (_msg.msgid) { + case FASTMAVLINK_MSG_ID_HEARTBEAT: { + fmav_heartbeat_t payload; + fmav_msg_heartbeat_decode(&payload, &_msg); + camera.system_status = payload.system_status; + camera.is_armed = (payload.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) ? true : false; + camera.is_standby = (payload.system_status <= MAV_STATE_STANDBY) ? true : false; + camera.is_critical = (payload.system_status >= MAV_STATE_CRITICAL) ? true : false; + INCU8(camera.updated); + //camera.is_receiving = MAVLINK_TELEM_RECEIVING_TIMEOUT; + break; + } + + case FASTMAVLINK_MSG_ID_CAMERA_INFORMATION: { + fmav_camera_information_t payload; + fmav_msg_camera_information_decode(&payload, &_msg); + memset(cameraInfo.vendor_name, 0, 32+1); + memcpy(cameraInfo.vendor_name, payload.vendor_name, 32); + memset(cameraInfo.model_name, 0, 32+1); + memcpy(cameraInfo.model_name, payload.model_name, 32); + cameraInfo.firmware_version = payload.firmware_version; + cameraInfo.flags = payload.flags; + cameraInfo.has_video = (cameraInfo.flags & CAMERA_CAP_FLAGS_CAPTURE_VIDEO); + cameraInfo.has_photo = (cameraInfo.flags & CAMERA_CAP_FLAGS_CAPTURE_IMAGE); + cameraInfo.has_modes = (cameraInfo.flags & CAMERA_CAP_FLAGS_HAS_MODES); + clear_request(TASK_CAMERA, TASK_SENDREQUEST_CAMERA_INFORMATION); + camera.requests_waiting_mask &=~ CAMERA_REQUESTWAITING_CAMERA_INFORMATION; + break; + } + + case FASTMAVLINK_MSG_ID_CAMERA_SETTINGS: { + fmav_camera_settings_t payload; + fmav_msg_camera_settings_decode(&payload, &_msg); + cameraStatus.mode = (payload.mode_id == CAMERA_MODE_IMAGE) ? CAMERA_MODE_IMAGE : CAMERA_MODE_VIDEO; + clear_request(TASK_CAMERA, TASK_SENDREQUEST_CAMERA_SETTINGS); + camera.requests_waiting_mask &=~ CAMERA_REQUESTWAITING_CAMERA_SETTINGS; + break; + } + + case FASTMAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS: { + fmav_camera_capture_status_t payload; + fmav_msg_camera_capture_status_decode(&payload, &_msg); + cameraStatus.recording_time_ms = payload.recording_time_ms; + cameraStatus.available_capacity_MiB = payload.available_capacity; + cameraStatus.video_on = (payload.video_status > 0); + cameraStatus.photo_on = (payload.image_status > 0); //0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress + clear_request(TASK_CAMERA, TASK_SENDREQUEST_CAMERA_CAPTURE_STATUS); + camera.requests_waiting_mask &=~ CAMERA_REQUESTWAITING_CAMERA_CAPTURE_STATUS; + break; + } + + case FASTMAVLINK_MSG_ID_STORAGE_INFORMATION: { + fmav_storage_information_t payload; + fmav_msg_storage_information_decode(&payload, &_msg); + if (payload.status == STORAGE_STATUS_READY) { + cameraInfo.total_capacity_MiB = payload.total_capacity; + cameraStatus.available_capacity_MiB = payload.available_capacity; + } + else { + cameraInfo.total_capacity_MiB = NAN; + cameraStatus.available_capacity_MiB = NAN; + } + clear_request(TASK_CAMERA, TASK_SENDREQUEST_STORAGE_INFORMATION); + break; + } + + case FASTMAVLINK_MSG_ID_BATTERY_STATUS: { + fmav_battery_status_t payload; + fmav_msg_battery_status_decode(&payload, &_msg); + int32_t voltage = 0; + bool has_voltage = false; + for (uint8_t i=0; i<10; i++) { + if (payload.voltages[i] != UINT16_MAX) { + voltage += payload.voltages[i]; //uint16_t mV, UINT16_MAX if not known + has_voltage = true; + } + } + cameraStatus.battery_voltage_V = (has_voltage) ? 0.001f * (float)voltage : NAN; + cameraStatus.battery_remaining_pct = payload.battery_remaining; // -1 if not known + break; + } + } +} + +// -- Startup Requests -- + +void MavlinkTelem::setCameraStartupRequests(void) +{ + set_request(TASK_CAMERA, TASK_SENDREQUEST_CAMERA_INFORMATION, 10, 200+0); //10x every ca 2sec + set_request(TASK_CAMERA, TASK_SENDREQUEST_CAMERA_SETTINGS, 10, 200+5); + set_request(TASK_CAMERA, TASK_SENDREQUEST_CAMERA_CAPTURE_STATUS, 10, 200+10); + set_request(TASK_CAMERA, TASK_SENDREQUEST_STORAGE_INFORMATION, 10, 200+15); +} + +// -- Resets -- + +void MavlinkTelem::_resetCamera(void) +{ + _task[TASK_CAMERA] = 0; + + camera.compid = 0; + camera.is_receiving = 0; + + camera.requests_triggered = 0; + camera.requests_waiting_mask = CAMERA_REQUESTWAITING_ALL; + camera.is_initialized = false; + + camera.system_status = MAV_STATE_UNINIT; + camera.custom_mode = 0; + camera.is_armed = false; + camera.is_standby = true; + camera.is_critical = false; + camera.prearm_ok = false; + camera.updated = 0; + + cameraInfo.vendor_name[0] = 0; + cameraInfo.model_name[0] = 0; + cameraInfo.flags = 0; + cameraInfo.has_video = false; + cameraInfo.has_photo = false; + cameraInfo.has_modes = false; + cameraInfo.total_capacity_MiB = NAN; + + cameraStatus.mode = 0; + cameraStatus.video_on = false; + cameraStatus.photo_on = false; + cameraStatus.available_capacity_MiB = NAN; + cameraStatus.recording_time_ms = UINT32_MAX; + cameraStatus.battery_voltage_V = NAN; + cameraStatus.battery_remaining_pct = -1; +} diff --git a/radio/src/telemetry/mavlink/mavlink_telem_gimbal.cpp b/radio/src/telemetry/mavlink/mavlink_telem_gimbal.cpp new file mode 100644 index 00000000000..b6492b7f800 --- /dev/null +++ b/radio/src/telemetry/mavlink/mavlink_telem_gimbal.cpp @@ -0,0 +1,612 @@ +/* + * (c) www.olliw.eu, OlliW, OlliW42 + */ + +#include "opentx.h" + +constexpr float FPI = 3.141592653589793f; +constexpr float FDEGTORAD = FPI/180.0f; +constexpr float FRADTODEG = 180.0f/FPI; + +#define INCU8(x) if ((x) < UINT8_MAX) { (x)++; } + +// quaternions and euler angles +// we do not use NED (roll-pitch-yaw) to convert quaternion to Euler angles and vice versa +// we use pitch-roll-yaw instead +// when the roll angle is zero, both are equivalent, this should be the majority of cases anyhow +// also, for most gimbals pitch-roll-yaw is appropriate +// the issue with NED is the gimbal lock at pitch +-90�, but pitch +-90� is a common operation point for gimbals +// the angles we store in this lib are thus pitch-roll-yaw Euler + +//NED +void calc_q_from_NED_angles_deg(float* q, float roll_deg, float pitch_deg, float yaw_deg) +{ + float cr2 = cosf(roll_deg * 0.5f * FDEGTORAD); + float sr2 = sinf(roll_deg * 0.5f * FDEGTORAD); + float cp2 = cosf(pitch_deg * 0.5f * FDEGTORAD); + float sp2 = sinf(pitch_deg * 0.5f * FDEGTORAD); + float cy2 = cosf(yaw_deg * 0.5f * FDEGTORAD); + float sy2 = sinf(yaw_deg * 0.5f * FDEGTORAD); + + q[0] = cr2*cp2*cy2 + sr2*sp2*sy2; + q[1] = sr2*cp2*cy2 - cr2*sp2*sy2; + q[2] = cr2*sp2*cy2 + sr2*cp2*sy2; + q[3] = cr2*cp2*sy2 - sr2*sp2*cy2; +} + +//equal for NED and G +void calc_q_from_pitchyaw_deg(float* q, float pitch_deg, float yaw_deg) +{ + float cp2 = cosf(pitch_deg * 0.5f * FDEGTORAD); + float sp2 = sinf(pitch_deg * 0.5f * FDEGTORAD); + float cy2 = cosf(yaw_deg * 0.5f * FDEGTORAD); + float sy2 = sinf(yaw_deg * 0.5f * FDEGTORAD); + + q[0] = cp2*cy2; + q[1] = -sp2*sy2; + q[2] = sp2*cy2; + q[3] = cp2*sy2; +} + +void calc_NED_angles_deg_from_q(float* roll_deg, float* pitch_deg, float* yaw_deg, float* q) +{ + *roll_deg = atan2f( 2.0f*(q[0]*q[1] + q[2]*q[3]), 1.0f - 2.0f*(q[1]*q[1] + q[2]*q[2]) ) * FRADTODEG; + + float arg = 2.0f*(q[0]*q[2] - q[1]*q[3]); + if (isnan(arg)) { + *pitch_deg = 0.0f; + } + else if (arg >= 1.0f) { + *pitch_deg = 90.0f; + } + else if (arg <= -1.0f) { + *pitch_deg = -90.0f; + } + else { + *pitch_deg = asinf(arg) * FRADTODEG; + } + + *yaw_deg = atan2f( 2.0f*(q[0]*q[3] + q[1]*q[2]), 1.0f - 2.0f*(q[2]*q[2] + q[3]*q[3]) ) * FRADTODEG; +} + +void calc_G_angles_deg_from_q(float* roll_deg, float* pitch_deg, float* yaw_deg, float* q) +{ + *pitch_deg = atan2f( q[0]*q[2] - q[1]*q[3], 0.5f - q[1]*q[1] - q[2]*q[2] ) * FRADTODEG; + + float arg = 2.0f*(q[0]*q[1] + q[2]*q[3]); + if (isnan(arg)) { + *roll_deg = 0.0f; + } + else if (arg >= 1.0f) { + *roll_deg = 90.0f; + } + else if (arg <= -1.0f) { + *roll_deg = -90.0f; + } + else { + *roll_deg = asinf(arg) * FRADTODEG; + } + + *yaw_deg = atan2f( q[0]*q[3] - q[1]*q[2], 0.5f - q[1]*q[1] - q[3]*q[3] ) * FRADTODEG; +} + +// -- Generate MAVLink messages -- +// these should never be called directly, should only be called by the task handler + +void MavlinkTelem::generateCmdDoMountConfigure(uint8_t tsystem, uint8_t tcomponent, uint8_t mode) +{ + _generateCmdLong(tsystem, tcomponent, + MAV_CMD_DO_MOUNT_CONFIGURE, + mode, 0,0,0,0,0,0 + ); +} + +//ArduPilot: if a mount has no pan control, then this will also yaw the copter in guided mode overwriting _fixed_yaw !! +void MavlinkTelem::generateCmdDoMountControl(uint8_t tsystem, uint8_t tcomponent, float pitch_deg, float yaw_deg, uint8_t mode) +{ + _generateCmdLong(tsystem, tcomponent, + MAV_CMD_DO_MOUNT_CONTROL, + pitch_deg, 0.0, yaw_deg, 0,0,0, mode + ); +} + +void MavlinkTelem::generateCmdRequestGimbalDeviceInformation(uint8_t tsystem, uint8_t tcomponent) +{ + _generateCmdLong(tsystem, tcomponent, + MAV_CMD_REQUEST_MESSAGE, + FASTMAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, 0,0,0,0,0,0 + ); +} + +//STorM32 specific +void MavlinkTelem::generateStorm32GimbalDeviceControl(uint8_t tsystem, uint8_t tcomponent, + float pitch_deg, float yaw_deg, uint16_t flags) +{ +float q[4]; + + if ((pitch_deg != NAN) && (yaw_deg != NAN)) { + calc_q_from_pitchyaw_deg(q, pitch_deg, yaw_deg); + } + else { + q[0] = q[1] = q[2] = q[3] = NAN; + } + + fmav_msg_storm32_gimbal_device_control_pack( + &_msg_out, _my_sysid, _my_compid, //_sys_id and not _my_sysid !!! we mimic being part of the autopilot system + tsystem, tcomponent, + flags, + q, + NAN, NAN, NAN, + &_status_out + ); + _msg_out_available = true; +} + +//STorM32 specific +void MavlinkTelem::generateCmdRequestStorm32GimbalManagerInformation(uint8_t tsystem, uint8_t tcomponent) +{ + _generateCmdLong(tsystem, tcomponent, + MAV_CMD_REQUEST_MESSAGE, + FASTMAVLINK_MSG_ID_STORM32_GIMBAL_MANAGER_INFORMATION, 0,0,0,0,0,0 + ); +} + +//STorM32 specific +void MavlinkTelem::generateStorm32GimbalManagerControl(uint8_t tsystem, uint8_t tcomponent, + uint8_t gimbal_id, float pitch_deg, float yaw_deg, uint16_t device_flags, uint16_t manager_flags) +{ +float q[4]; + + if ((pitch_deg != NAN) && (yaw_deg != NAN)) { + calc_q_from_pitchyaw_deg(q, pitch_deg, yaw_deg); + } + else { + q[0] = q[1] = q[2] = q[3] = NAN; + } + + fmav_msg_storm32_gimbal_manager_control_pack( + &_msg_out, _my_sysid, _my_compid, + tsystem, tcomponent, + gimbal_id, + MAV_STORM32_GIMBAL_MANAGER_CLIENT_GCS, //client + device_flags, manager_flags, + q, + NAN, NAN, NAN, + &_status_out + ); + _msg_out_available = true; +} + +//STorM32 specific +void MavlinkTelem::generateStorm32GimbalManagerControlPitchYaw(uint8_t tsystem, uint8_t tcomponent, + uint8_t gimbal_id, float pitch_deg, float yaw_deg, uint16_t device_flags, uint16_t manager_flags) +{ + fmav_msg_storm32_gimbal_manager_control_pitchyaw_pack( + &_msg_out, _my_sysid, _my_compid, + tsystem, tcomponent, + gimbal_id, + MAV_STORM32_GIMBAL_MANAGER_CLIENT_GCS, //client + device_flags, manager_flags, + pitch_deg*FDEGTORAD, yaw_deg*FDEGTORAD, + NAN, NAN, + &_status_out + ); + _msg_out_available = true; +} + +//STorM32 specific +void MavlinkTelem::generateCmdStorm32DoGimbalManagerControlPitchYaw(uint8_t tsystem, uint8_t tcomponent, + uint8_t gimbal_id, float pitch_deg, float yaw_deg, uint16_t device_flags, uint16_t manager_flags) +{ + _generateCmdLong(tsystem, tcomponent, + MAV_CMD_STORM32_DO_GIMBAL_MANAGER_CONTROL_PITCHYAW, + pitch_deg, yaw_deg, NAN, NAN, device_flags, manager_flags, + (uint16_t)gimbal_id + ((uint16_t)MAV_STORM32_GIMBAL_MANAGER_CLIENT_GCS < 8) + ); +} + +// -- Mavsdk Convenience Task Wrapper -- +// to make it easy for api_mavsdk to call functions + +void MavlinkTelem::sendGimbalTargetingMode(uint8_t mode) +{ + _t_gimbal_mode = mode; + gimbalmanagerOut.mount_mode = mode; + SETTASK(TASK_GIMBAL, TASK_SENDCMD_DO_MOUNT_CONFIGURE); +} + +void MavlinkTelem::sendGimbalPitchYawDeg(float pitch, float yaw) +{ + if (_storm32_gimbal_protocol_v2) { + sendStorm32GimbalManagerPitchYawDeg(pitch, yaw); + return; + } + _t_gimbal_pitch_deg = pitch; + _t_gimbal_yaw_deg = yaw; + _t_gimbal_mode = gimbalmanagerOut.mount_mode; + SETTASK(TASK_GIMBAL, TASK_SENDCMD_DO_MOUNT_CONTROL); +} + +//we shouldn't use this +void MavlinkTelem::sendStorm32GimbalDevicePitchYawDeg(float pitch, float yaw) +{ + _t_storm32GD_pitch_deg = pitch; + _t_storm32GD_yaw_deg = yaw; + //we assume a simple 3 axis gimbal + // our gimbal must EXACTLY have these capabilities for now, so we fake it + _t_storm32GD_flags |= (MAV_STORM32_GIMBAL_DEVICE_FLAGS_ROLL_LOCK | MAV_STORM32_GIMBAL_DEVICE_FLAGS_PITCH_LOCK); + _t_storm32GD_flags &=~ (MAV_STORM32_GIMBAL_DEVICE_FLAGS_YAW_LOCK | 0xFF00); + SETTASK(TASK_GIMBAL, TASK_SENDMSG_GIMBAL_DEVICE_CONTROL); +} + +void MavlinkTelem::setStorm32GimbalClientRetract(bool enable) +{ + if (!_storm32_gimbal_protocol_v2) return; + + gimbalmanagerOut.device_flags &=~ (MAV_STORM32_GIMBAL_DEVICE_FLAGS_RETRACT | MAV_STORM32_GIMBAL_DEVICE_FLAGS_NEUTRAL); + if( enable ){ + gimbalmanagerOut.device_flags |= MAV_STORM32_GIMBAL_DEVICE_FLAGS_RETRACT; + } +} + +void MavlinkTelem::setStorm32GimbalClientNeutral(bool enable) +{ + if (!_storm32_gimbal_protocol_v2) return; + + gimbalmanagerOut.device_flags &=~ (MAV_STORM32_GIMBAL_DEVICE_FLAGS_RETRACT | MAV_STORM32_GIMBAL_DEVICE_FLAGS_NEUTRAL); + if( enable ){ + gimbalmanagerOut.device_flags |= MAV_STORM32_GIMBAL_DEVICE_FLAGS_NEUTRAL; + } +} + +void MavlinkTelem::setStorm32GimbalClientLock(bool roll_lock, bool pitch_lock, bool yaw_lock) +{ + if (!_storm32_gimbal_protocol_v2) return; + + gimbalmanagerOut.device_flags &=~ (MAV_STORM32_GIMBAL_DEVICE_FLAGS_ROLL_LOCK | + MAV_STORM32_GIMBAL_DEVICE_FLAGS_PITCH_LOCK | + MAV_STORM32_GIMBAL_DEVICE_FLAGS_YAW_LOCK ); + if( roll_lock ){ + gimbalmanagerOut.device_flags |= MAV_STORM32_GIMBAL_DEVICE_FLAGS_ROLL_LOCK; + } + if( pitch_lock ){ + gimbalmanagerOut.device_flags |= MAV_STORM32_GIMBAL_DEVICE_FLAGS_PITCH_LOCK; + } + if( yaw_lock ){ + gimbalmanagerOut.device_flags |= MAV_STORM32_GIMBAL_DEVICE_FLAGS_YAW_LOCK; + } +} + +void MavlinkTelem::setStorm32GimbalClientFlags(uint16_t manager_flags) +{ + if (!_storm32_gimbal_protocol_v2) return; + + manager_flags |= MAV_STORM32_GIMBAL_MANAGER_FLAGS_SET_SUPERVISON; //for the moment the GCS always claims supervision + + gimbalmanagerOut.manager_flags = manager_flags; +} + +void MavlinkTelem::_refreshGimbalClientFlags(uint16_t* device_flags, uint16_t* manager_flags) +{ + gimbalmanagerOut.device_flags &=~ (MAV_STORM32_GIMBAL_DEVICE_FLAGS_RC_EXCLUSIVE | MAV_STORM32_GIMBAL_DEVICE_FLAGS_RC_MIXED); + + if (*manager_flags & MAV_STORM32_GIMBAL_MANAGER_FLAGS_RC_ACTIVE) { + gimbalmanagerOut.device_flags |= MAV_STORM32_GIMBAL_DEVICE_FLAGS_RC_MIXED; //we always do it mixed! + } + *device_flags = gimbalmanagerOut.device_flags; + + gimbalmanagerOut.manager_flags |= MAV_STORM32_GIMBAL_MANAGER_FLAGS_SET_SUPERVISON; + + *manager_flags = gimbalmanagerOut.manager_flags; +} + +void MavlinkTelem::sendStorm32GimbalManagerPitchYawDeg(float pitch, float yaw) +{ + if (!gimbalmanager.compid) return; //no gimbal manager + + _refreshGimbalClientFlags(&_t_storm32GM_gdflags, &_t_storm32GM_gmflags); + + if ((_t_storm32GM_gdflags & MAV_STORM32_GIMBAL_DEVICE_FLAGS_RETRACT) || + (_t_storm32GM_gdflags & MAV_STORM32_GIMBAL_DEVICE_FLAGS_NEUTRAL)) { + pitch = yaw = 0.0f; + } + _t_storm32GM_pitch_deg = pitch; + _t_storm32GM_yaw_deg = yaw; + SETTASK(TASK_GIMBAL, TASK_SENDMSG_GIMBAL_MANAGER_CONTROL_PITCHYAW); +} + +void MavlinkTelem::sendStorm32GimbalManagerControlPitchYawDeg(float pitch, float yaw) +{ + if (!gimbalmanager.compid) return; //no gimbal manager + + _refreshGimbalClientFlags(&_t_storm32GM_control_gdflags, &_t_storm32GM_control_gmflags); + + if ((_t_storm32GM_control_gdflags & MAV_STORM32_GIMBAL_DEVICE_FLAGS_RETRACT) || + (_t_storm32GM_control_gdflags & MAV_STORM32_GIMBAL_DEVICE_FLAGS_NEUTRAL)) { + pitch = yaw = 0.0f; + } + _t_storm32GM_control_pitch_deg = pitch; + _t_storm32GM_control_yaw_deg = yaw; + SETTASK(TASK_GIMBAL, TASK_SENDMSG_GIMBAL_MANAGER_CONTROL); +} + +void MavlinkTelem::sendStorm32GimbalManagerCmdPitchYawDeg(float pitch, float yaw) +{ + if (!gimbalmanager.compid) return; //no gimbal manager + + _refreshGimbalClientFlags(&_t_storm32GM_cmd_gdflags, &_t_storm32GM_cmd_gmflags); + + if ((_t_storm32GM_cmd_gdflags & MAV_STORM32_GIMBAL_DEVICE_FLAGS_RETRACT) || + (_t_storm32GM_cmd_gdflags & MAV_STORM32_GIMBAL_DEVICE_FLAGS_NEUTRAL)) { + pitch = yaw = 0.0f; + } + _t_storm32GM_cmd_pitch_deg = pitch; + _t_storm32GM_cmd_yaw_deg = yaw; + SETTASK(TASK_GIMBAL, TASK_SENDCMD_DO_GIMBAL_MANAGER_CONTROL_PITCHYAW); +} + +// -- Task handlers -- + +bool MavlinkTelem::doTaskGimbalAndGimbalClient(void) +{ + if (!_task[TASK_GIMBAL]) return false; // no task pending + + // if there is no gimbal, then there is also no gimbal manager which needs to be served + // so first if()-checking for gimbal, and then for gimbalmanager is ok + + if (!gimbal.compid) { // no gimbal + _task[TASK_GIMBAL] = 0; + return false; + } + + if (_task[TASK_GIMBAL] & TASK_SENDCMD_DO_MOUNT_CONFIGURE) { + RESETTASK(TASK_GIMBAL, TASK_SENDCMD_DO_MOUNT_CONFIGURE); + generateCmdDoMountConfigure(_sysid, autopilot.compid, _t_gimbal_mode); + return true; //do only one per loop + } + if (_task[TASK_GIMBAL] & TASK_SENDCMD_DO_MOUNT_CONTROL) { + RESETTASK(TASK_GIMBAL, TASK_SENDCMD_DO_MOUNT_CONTROL); + generateCmdDoMountControl(_sysid, autopilot.compid, _t_gimbal_pitch_deg, _t_gimbal_yaw_deg, _t_gimbal_mode); + return true; //do only one per loop + } + + if (_task[TASK_GIMBAL] & TASK_SENDREQUEST_GIMBAL_DEVICE_INFORMATION) { + RESETTASK(TASK_GIMBAL, TASK_SENDREQUEST_GIMBAL_DEVICE_INFORMATION); + generateCmdRequestGimbalDeviceInformation(_sysid, gimbal.compid); + return true; //do only one per loop + } + + if (_task[TASK_GIMBAL] & TASK_SENDMSG_GIMBAL_DEVICE_CONTROL) { + RESETTASK(TASK_GIMBAL, TASK_SENDMSG_GIMBAL_DEVICE_CONTROL); + generateStorm32GimbalDeviceControl(_sysid, gimbal.compid, _t_storm32GD_pitch_deg, _t_storm32GD_yaw_deg, _t_storm32GD_flags); + return true; //do only one per loop + } + + if (!gimbalmanager.compid) { // no gimbal manager + RESETTASK(TASK_GIMBAL, TASK_SENDMSG_GIMBAL_MANAGER_CONTROL_PITCHYAW | TASK_SENDMSG_GIMBAL_MANAGER_CONTROL | + TASK_SENDCMD_DO_GIMBAL_MANAGER_CONTROL_PITCHYAW | TASK_SENDREQUEST_GIMBAL_MANAGER_INFORMATION); + return false; + } + + if (_task[TASK_GIMBAL] & TASK_SENDMSG_GIMBAL_MANAGER_CONTROL_PITCHYAW) { + RESETTASK(TASK_GIMBAL, TASK_SENDMSG_GIMBAL_MANAGER_CONTROL_PITCHYAW); + generateStorm32GimbalManagerControlPitchYaw(_sysid, gimbalmanager.compid, gimbal.compid, + _t_storm32GM_pitch_deg, _t_storm32GM_yaw_deg, _t_storm32GM_gdflags, _t_storm32GM_gmflags); + return true; //do only one per loop + } + if (_task[TASK_GIMBAL] & TASK_SENDMSG_GIMBAL_MANAGER_CONTROL) { + RESETTASK(TASK_GIMBAL, TASK_SENDMSG_GIMBAL_MANAGER_CONTROL); + generateStorm32GimbalManagerControl(_sysid, gimbalmanager.compid, gimbal.compid, + _t_storm32GM_control_pitch_deg, _t_storm32GM_control_yaw_deg, _t_storm32GM_control_gdflags, _t_storm32GM_control_gmflags); + return true; //do only one per loop + } + if (_task[TASK_GIMBAL] & TASK_SENDCMD_DO_GIMBAL_MANAGER_CONTROL_PITCHYAW) { + RESETTASK(TASK_GIMBAL, TASK_SENDCMD_DO_GIMBAL_MANAGER_CONTROL_PITCHYAW); + generateCmdStorm32DoGimbalManagerControlPitchYaw(_sysid, gimbalmanager.compid, gimbal.compid, + _t_storm32GM_cmd_pitch_deg, _t_storm32GM_cmd_yaw_deg, _t_storm32GM_cmd_gdflags, _t_storm32GM_cmd_gmflags); + return true; //do only one per loop + } + if (_task[TASK_GIMBAL] & TASK_SENDREQUEST_GIMBAL_MANAGER_INFORMATION) { + RESETTASK(TASK_GIMBAL, TASK_SENDREQUEST_GIMBAL_MANAGER_INFORMATION); + generateCmdRequestStorm32GimbalManagerInformation(_sysid, gimbalmanager.compid); + return true; //do only one per loop + } + + return false; +} + +// -- Handle incoming MAVLink messages, which are for the Gimbal -- + +void MavlinkTelem::handleMessageGimbal(void) +{ + gimbal.is_receiving = MAVLINK_TELEM_RECEIVING_TIMEOUT; //we accept any msg from the gimbal to indicate it is alive + + switch (_msg.msgid) { + case FASTMAVLINK_MSG_ID_HEARTBEAT: { + fmav_heartbeat_t payload; + fmav_msg_heartbeat_decode(&payload, &_msg); + gimbal.system_status = payload.system_status; + gimbal.custom_mode = payload.custom_mode; + gimbal.is_armed = (payload.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) ? true : false; + gimbal.is_standby = (payload.system_status <= MAV_STATE_STANDBY) ? true : false; + gimbal.is_critical = (payload.system_status >= MAV_STATE_CRITICAL) ? true : false; + gimbal.prearm_ok = (payload.custom_mode & 0x80000000) ? false : true; + INCU8(gimbal.updated); + //gimbal.is_receiving = MAVLINK_TELEM_RECEIVING_TIMEOUT; + break; + } + + case FASTMAVLINK_MSG_ID_ATTITUDE: { + fmav_attitude_t payload; + fmav_msg_attitude_decode(&payload, &_msg); + gimbalAtt.roll_deg = payload.roll * FRADTODEG; + gimbalAtt.pitch_deg = payload.pitch * FRADTODEG; + gimbalAtt.yaw_deg_relative = payload.yaw * FRADTODEG; + if (gimbalAtt.yaw_deg_absolute > 180.0f) gimbalAtt.yaw_deg_absolute -= 360.0f; + gimbalAtt.yaw_deg_absolute = gimbalAtt.yaw_deg_relative + att.yaw_rad * FRADTODEG; + if (gimbalAtt.yaw_deg_absolute < -180.0f) gimbalAtt.yaw_deg_absolute += 360.0f; + INCU8(gimbalAtt.updated); + break; + } + + case FASTMAVLINK_MSG_ID_MOUNT_STATUS: { + fmav_mount_status_t payload; + fmav_msg_mount_status_decode(&payload, &_msg); + gimbalAtt.roll_deg = ((float)payload.pointing_b * 0.01f); + gimbalAtt.pitch_deg = ((float)payload.pointing_a * 0.01f); + gimbalAtt.yaw_deg_relative = ((float)payload.pointing_c * 0.01f); + if (gimbalAtt.yaw_deg_absolute > 180.0f) gimbalAtt.yaw_deg_absolute -= 360.0f; + gimbalAtt.yaw_deg_absolute = gimbalAtt.yaw_deg_relative + att.yaw_rad * FRADTODEG; + if (gimbalAtt.yaw_deg_absolute < -180.0f) gimbalAtt.yaw_deg_absolute += 360.0f; + INCU8(gimbalAtt.updated); + break; + } + + case FASTMAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION: { + fmav_gimbal_device_information_t payload; + fmav_msg_gimbal_device_information_decode(&payload, &_msg); + memset(gimbaldeviceInfo.vendor_name, 0, 32+1); + memcpy(gimbaldeviceInfo.vendor_name, payload.vendor_name, 32); + memset(gimbaldeviceInfo.model_name, 0, 32+1); + memcpy(gimbaldeviceInfo.model_name, payload.model_name, 32); + memset(gimbaldeviceInfo.custom_name, 0, 32+1); + memcpy(gimbaldeviceInfo.custom_name, payload.custom_name, 32); + gimbaldeviceInfo.firmware_version = payload.firmware_version; + gimbaldeviceInfo.hardware_version = payload.hardware_version; + //STorM32 has extra cap flags in custom area, which have to go into high word + gimbaldeviceInfo.cap_flags = payload.cap_flags + (((uint32_t)payload.custom_cap_flags) << 16); + INCU8(gimbaldeviceInfo.updated); + clear_request(TASK_GIMBAL, TASK_SENDREQUEST_GIMBAL_DEVICE_INFORMATION); + gimbal.requests_waiting_mask &=~ GIMBAL_REQUESTWAITING_GIMBAL_DEVICE_INFORMATION; + gimbalmanager.requests_waiting_mask &=~ GIMBAL_REQUESTWAITING_GIMBAL_DEVICE_INFORMATION; + // we do not copy to the gimbal manager capability flags, we rely on the gimbal manager to provide the correct ones + break; + } + + case FASTMAVLINK_MSG_ID_STORM32_GIMBAL_DEVICE_STATUS: { + fmav_storm32_gimbal_device_status_t payload; + fmav_msg_storm32_gimbal_device_status_decode(&payload, &_msg); + calc_G_angles_deg_from_q(&gimbalAtt.roll_deg, &gimbalAtt.pitch_deg, &gimbalAtt.yaw_deg_relative, payload.q); + gimbalAtt.yaw_deg_absolute = gimbalAtt.yaw_deg_relative + att.yaw_rad * FRADTODEG; + if (gimbalAtt.yaw_deg_absolute < -180.0f) gimbalAtt.yaw_deg_absolute += 360.0f; + INCU8(gimbalAtt.updated); + // update gimbal manager flags, do it alwayy, even if no gimbal manager has been discovered + gimbalmanagerStatus.device_flags = payload.flags; + break; + } + } +} + +// -- Handle incoming MAVLink messages, which are for the Gimbal Client -- + +void MavlinkTelem::handleMessageGimbalClient(void) +{ + //gimbalmanager.is_receiving = MAVLINK_TELEM_RECEIVING_TIMEOUT; //we accept any msg from the manager to indicate it is alive + //no: we should not do this for the manager, since this function is called for any message from the hosting component + + switch (_msg.msgid) { + case FASTMAVLINK_MSG_ID_STORM32_GIMBAL_MANAGER_INFORMATION: { + fmav_storm32_gimbal_manager_information_t payload; + fmav_msg_storm32_gimbal_manager_information_decode(&payload, &_msg); + if (payload.gimbal_id != gimbal.compid) break; //not for us + gimbalmanagerInfo.device_cap_flags = payload.device_cap_flags; + gimbalmanagerInfo.manager_cap_flags = payload.manager_cap_flags; + INCU8(gimbalmanagerInfo.updated); + clear_request(TASK_GIMBAL, TASK_SENDREQUEST_GIMBAL_MANAGER_INFORMATION); + gimbalmanager.requests_waiting_mask &=~ GIMBAL_REQUESTWAITING_GIMBAL_MANAGER_INFORMATION; + break; + } + + case FASTMAVLINK_MSG_ID_STORM32_GIMBAL_MANAGER_STATUS: { + fmav_storm32_gimbal_manager_status_t payload; + fmav_msg_storm32_gimbal_manager_status_decode(&payload, &_msg); + if (payload.gimbal_id != gimbal.compid) break; //not for us + gimbalmanagerStatus.supervisor = payload.supervisor; + gimbalmanagerStatus.device_flags = payload.device_flags; + gimbalmanagerStatus.manager_flags = payload.manager_flags; + gimbalmanagerStatus.profile = payload.profile; + INCU8(gimbalmanagerStatus.updated); + gimbalmanager.is_receiving = MAVLINK_TELEM_GIMBALMANAGER_RECEIVING_TIMEOUT; + break; + } + } +} + +// -- Startup Requests -- + +void MavlinkTelem::setGimbalStartupRequests(void) +{ + set_request(TASK_GIMBAL, TASK_SENDREQUEST_GIMBAL_DEVICE_INFORMATION, 10, 200-12); +} + +void MavlinkTelem::setGimbalClientStartupRequests(void) +{ + set_request(TASK_GIMBAL, TASK_SENDREQUEST_GIMBAL_DEVICE_INFORMATION, 10, 200-12); + set_request(TASK_GIMBAL, TASK_SENDREQUEST_GIMBAL_MANAGER_INFORMATION, 10, 200-18); +} + +// -- Resets -- + +void MavlinkTelem::_resetGimbalAndGimbalClient(void) +{ + _task[TASK_GIMBAL] = 0; + + gimbal.compid = 0; + gimbal.is_receiving = 0; + + gimbal.requests_triggered = 0; + gimbal.requests_waiting_mask = GIMBAL_REQUESTWAITING_ALL; + gimbal.is_initialized = false; + + gimbal.system_status = MAV_STATE_UNINIT; + gimbal.custom_mode = 0; + gimbal.is_armed = false; + gimbal.is_standby = true; + gimbal.is_critical = false; + gimbal.prearm_ok = false; + gimbal.updated = 0; + + gimbalAtt.roll_deg = 0.0f; + gimbalAtt.pitch_deg = 0.0f; + gimbalAtt.yaw_deg_relative = 0.0f; + gimbalAtt.yaw_deg_absolute = 0.0f; + gimbalAtt.updated = 0; + + _resetGimbalClient(); +} + +void MavlinkTelem::_resetGimbalClient(void) +{ + //_task[TASK_GIMBAL] = 0; // only reset gimbal client tasks, but not very important + + gimbalmanager.compid = 0; + gimbalmanager.is_receiving = 0; + + gimbalmanager.requests_triggered = 0; + gimbalmanager.requests_waiting_mask = GIMBALCLIENT_REQUESTWAITING_ALL; //this prevents it ever becomes initialized = true + gimbalmanager.is_initialized = false; + + gimbalmanager.updated = 0; + + gimbaldeviceInfo.vendor_name[0] = 0; + gimbaldeviceInfo.model_name[0] = 0; + gimbaldeviceInfo.custom_name[0] = 0; + gimbaldeviceInfo.firmware_version = 0; + gimbaldeviceInfo.hardware_version = 0; + gimbaldeviceInfo.cap_flags = 0; + gimbaldeviceInfo.updated = 0; + + gimbalmanagerInfo.device_cap_flags = 0; + gimbalmanagerInfo.manager_cap_flags = 0; + gimbalmanagerInfo.updated = 0; + + gimbalmanagerStatus.supervisor = 0; + gimbalmanagerStatus.device_flags = 0; + gimbalmanagerStatus.manager_flags = 0; + gimbalmanagerStatus.profile = 0; + gimbalmanagerStatus.updated = 0; + + gimbalmanagerOut.mount_mode = MAV_MOUNT_MODE_MAVLINK_TARGETING; + gimbalmanagerOut.device_flags = MAV_STORM32_GIMBAL_DEVICE_FLAGS_ROLL_LOCK | MAV_STORM32_GIMBAL_DEVICE_FLAGS_PITCH_LOCK; + gimbalmanagerOut.manager_flags = MAV_STORM32_GIMBAL_MANAGER_FLAGS_SET_SUPERVISON; +} diff --git a/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp b/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp new file mode 100644 index 00000000000..72d363a632f --- /dev/null +++ b/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp @@ -0,0 +1,495 @@ +/* + * (c) www.olliw.eu, OlliW, OlliW42 + */ + +#include "opentx.h" + +// -- CoOS RTOS mavlink task handlers -- + +RTOS_TASK_HANDLE mavlinkTaskId; +RTOS_DEFINE_STACK(mavlinkStack, MAVLINK_STACK_SIZE); + +struct MavlinkTaskStat { + uint16_t start = 0; + uint16_t run = 0; + uint16_t max = 0; + uint16_t loop = 0; +}; +struct MavlinkTaskStat mavlinkTaskStat; + +uint16_t mavlinkTaskRunTime(void) +{ + return mavlinkTaskStat.run/2; +} + +uint16_t mavlinkTaskRunTimeMax(void) +{ + return mavlinkTaskStat.max/2; +} + +uint16_t mavlinkTaskLoop(void) +{ + return mavlinkTaskStat.loop/2; +} + +TASK_FUNCTION(mavlinkTask) +{ + while (true) { + uint16_t start_last = mavlinkTaskStat.start; + mavlinkTaskStat.start = getTmr2MHz(); + + mavlinkTelem.wakeup(); + + mavlinkTaskStat.run = getTmr2MHz() - mavlinkTaskStat.start; + if (mavlinkTaskStat.run > mavlinkTaskStat.max) mavlinkTaskStat.max = mavlinkTaskStat.run; + mavlinkTaskStat.loop = (mavlinkTaskStat.start - start_last); + + RTOS_WAIT_TICKS(2); + } +} + +void mavlinkStart() +{ + RTOS_CREATE_TASK(mavlinkTaskId, mavlinkTask, "mavlink", mavlinkStack, MAVLINK_STACK_SIZE, MAVLINK_TASK_PRIO); +} + +// -- EXTERNAL BAY SERIAL handlers -- +// we essentially redo everything from scratch + +MAVLINK_RAM_SECTION Fifo mavlinkTelemExternalTxFifo_frame; +MAVLINK_RAM_SECTION Fifo mavlinkTelemExternalTxFifo; +MAVLINK_RAM_SECTION Fifo mavlinkTelemExternalRxFifo; + +void extmoduleOn(void) +{ + if(g_eeGeneral.mavlinkExternal != 1) GPIO_SetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN); +} + +void extmoduleOff(void) +{ + if(g_eeGeneral.mavlinkExternal != 1) GPIO_ResetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN); +} + +void extmoduleMavlinkTelemStop(void) +{ + USART_ITConfig(TELEMETRY_USART, USART_IT_RXNE, DISABLE); + USART_ITConfig(TELEMETRY_USART, USART_IT_TXE, DISABLE); + NVIC_DisableIRQ(TELEMETRY_USART_IRQn); + + NVIC_DisableIRQ(TELEMETRY_EXTI_IRQn); + NVIC_DisableIRQ(TELEMETRY_TIMER_IRQn); + DMA_ITConfig(TELEMETRY_DMA_Stream_TX, DMA_IT_TC, DISABLE); + NVIC_DisableIRQ(TELEMETRY_DMA_TX_Stream_IRQ); + + USART_DeInit(TELEMETRY_USART); + DMA_DeInit(TELEMETRY_DMA_Stream_TX); + + GPIO_ResetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN); //EXTERNAL_MODULE_OFF(); +} + +void extmoduleMavlinkTelemStart(void) +{ + GPIO_SetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN); //EXTERNAL_MODULE_ON(); + + // we don't want or need all this + NVIC_DisableIRQ(TELEMETRY_EXTI_IRQn); + NVIC_DisableIRQ(TELEMETRY_TIMER_IRQn); + NVIC_DisableIRQ(TELEMETRY_DMA_TX_Stream_IRQ); + + DMA_ITConfig(TELEMETRY_DMA_Stream_TX, DMA_IT_TC, DISABLE); + DMA_Cmd(TELEMETRY_DMA_Stream_TX, DISABLE); + USART_DMACmd(TELEMETRY_USART, USART_DMAReq_Tx, DISABLE); + DMA_DeInit(TELEMETRY_DMA_Stream_TX); + + EXTI_InitTypeDef EXTI_InitStructure; + EXTI_StructInit(&EXTI_InitStructure); + EXTI_InitStructure.EXTI_Line = TELEMETRY_EXTI_LINE; + EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; + EXTI_InitStructure.EXTI_Trigger = TELEMETRY_EXTI_TRIGGER; + EXTI_InitStructure.EXTI_LineCmd = DISABLE; + EXTI_Init(&EXTI_InitStructure); + + // is it called already? through telemetryInit() -> telemetryPortInit(FRSKY_SPORT_BAUDRATE) -> telemetryInitDirPin() + GPIO_PinAFConfig(TELEMETRY_GPIO, TELEMETRY_GPIO_PinSource_RX, TELEMETRY_GPIO_AF); + GPIO_PinAFConfig(TELEMETRY_GPIO, TELEMETRY_GPIO_PinSource_TX, TELEMETRY_GPIO_AF); + + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_InitStructure.GPIO_Pin = TELEMETRY_TX_GPIO_PIN | TELEMETRY_RX_GPIO_PIN; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_Init(TELEMETRY_GPIO, &GPIO_InitStructure); + + // is it called already? through telemetryInit() -> telemetryPortInit(FRSKY_SPORT_BAUDRATE) -> telemetryInitDirPin() + GPIO_InitStructure.GPIO_Pin = TELEMETRY_DIR_GPIO_PIN; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_Init(TELEMETRY_DIR_GPIO, &GPIO_InitStructure); + GPIO_ResetBits(TELEMETRY_DIR_GPIO, TELEMETRY_DIR_GPIO_PIN); + + // init uart itself + USART_InitTypeDef USART_InitStructure; + USART_InitStructure.USART_BaudRate = 400000; + USART_InitStructure.USART_WordLength = USART_WordLength_8b; + USART_InitStructure.USART_StopBits = USART_StopBits_1; + USART_InitStructure.USART_Parity = USART_Parity_No; + USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; + USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx; + USART_Init(TELEMETRY_USART, &USART_InitStructure); + + USART_Cmd(TELEMETRY_USART, ENABLE); + USART_ITConfig(TELEMETRY_USART, USART_IT_RXNE, ENABLE); + USART_ITConfig(TELEMETRY_USART, USART_IT_TXE, DISABLE); + NVIC_SetPriority(TELEMETRY_USART_IRQn, 6); + NVIC_EnableIRQ(TELEMETRY_USART_IRQn); +} + +void mavlinkTelemExternal_init(bool flag) +{ + if (flag) { + extmoduleStop(); //??? good or bad + // this overrides ext module settings, so nothing else to do + extmoduleMavlinkTelemStart(); + } + else { + extmoduleMavlinkTelemStop(); + //TODO: we should re-enable an external module if one is configured + } +} + +// this must be called regularly, at 2 ms +// 115200 bps = 86 us per byte => 12 bytes per ms = 24 bytes per 2 ms +// 3+24 bytes @ 400000 bps = 0.675 ms, 24 bytes @ 400000 bps = 0.6 ms => 1.275 ms +// => enough time for a tx and rx packet in a 2 ms slot +// however, the slots are not precisely fixed to 2 ms, can be shorter +// so design for lower data rate, we send at most 16 bytes per slot +// 16 bytes per slot = 8000 bytes/s = effectively 80000 bps, should be way enough +// 3+16 bytes @ 400000 bps = 0.475 ms, 16 bytes @ 400000 bps = 0.4 ms, => 0.875 ms +void mavlinkTelemExternal_wakeup(void) +{ + // we do it at the beginning, so it gives few cycles before TX is enabled + TELEMETRY_DIR_GPIO->BSRRL = TELEMETRY_DIR_GPIO_PIN; // enable output + TELEMETRY_USART->CR1 &= ~USART_CR1_RE; // turn off receiver + + uint32_t count = mavlinkTelemExternalTxFifo.size(); + if (count > 16) count = 16; + + // always send header, this synchronizes slave + mavlinkTelemExternalTxFifo_frame.push('O'); + mavlinkTelemExternalTxFifo_frame.push('W'); + mavlinkTelemExternalTxFifo_frame.push((uint8_t)count); + + // send payload + for (uint16_t i = 0; i < count; i++) { + uint8_t c; + mavlinkTelemExternalTxFifo.pop(c); + mavlinkTelemExternalTxFifo_frame.push(c); + } + + USART_ITConfig(TELEMETRY_USART, USART_IT_TXE, ENABLE); // enable TX interrupt, starts sending +} + +uint32_t mavlinkTelemExternalAvailable(void) +{ + return mavlinkTelemExternalRxFifo.size(); +} + +uint8_t mavlinkTelemExternalGetc(uint8_t* c) +{ + return mavlinkTelemExternalRxFifo.pop(*c); +} + +bool mavlinkTelemExternalHasSpace(uint16_t count) +{ + return mavlinkTelemExternalTxFifo.hasSpace(count); +} + +bool mavlinkTelemExternalPutBuf(const uint8_t *buf, const uint16_t count) +{ + if (!mavlinkTelemExternalTxFifo.hasSpace(count)) return false; + for (uint16_t i = 0; i < count; i++) mavlinkTelemExternalTxFifo.push(buf[i]); + return true; +} + +// -- AUX1, AUX2 handlers -- + +uint32_t _cvtBaudrate(uint16_t baud) +{ + switch (baud) { + case 0: return 57600; + case 1: return 115200; + case 2: return 38400; + case 3: return 19200; + } + return 57600; +} + +uint32_t mavlinkTelemAuxBaudrate(void) +{ + return _cvtBaudrate(g_eeGeneral.mavlinkBaudrate); +} + +uint32_t mavlinkTelemAux2Baudrate(void) +{ + return _cvtBaudrate(g_eeGeneral.mavlinkBaudrate2); +} + +#if defined(AUX_SERIAL) +MAVLINK_RAM_SECTION Fifo auxSerialTxFifo; +MAVLINK_RAM_SECTION Fifo mavlinkTelemAuxSerialRxFifo; +#endif + +#if defined(AUX2_SERIAL) +MAVLINK_RAM_SECTION Fifo aux2SerialTxFifo; +MAVLINK_RAM_SECTION Fifo mavlinkTelemAux2SerialRxFifo; +#endif + +#if defined(TELEMETRY_MAVLINK) && defined(USB_SERIAL) +MAVLINK_RAM_SECTION Fifo mavlinkTelemUsbRxFifo; +#endif + +#if defined(AUX_SERIAL) + +uint32_t mavlinkTelem1Available(void) +{ + if (!mavlinkTelem.serial1_enabled) return 0; + if (mavlinkTelem.serial1_isexternal) return mavlinkTelemExternalRxFifo.size(); + +// if (auxSerialMode != UART_MODE_MAVLINK) return 0; + return mavlinkTelemAuxSerialRxFifo.size(); +} + +// call only after check with mavlinkTelem2Available() +uint8_t mavlinkTelem1Getc(uint8_t* c) +{ + if (!mavlinkTelem.serial1_enabled) return 0; + if (mavlinkTelem.serial1_isexternal) return mavlinkTelemExternalRxFifo.pop(*c); + + return mavlinkTelemAuxSerialRxFifo.pop(*c); +} + +bool mavlinkTelem1HasSpace(uint16_t count) +{ + if (!mavlinkTelem.serial1_enabled) return 0; + if (mavlinkTelem.serial1_isexternal) return mavlinkTelemExternalTxFifo.hasSpace(count); + +// if (auxSerialMode != UART_MODE_MAVLINK) return false; + return auxSerialTxFifo.hasSpace(count); +} + +bool mavlinkTelem1PutBuf(const uint8_t* buf, const uint16_t count) +{ + if (!mavlinkTelem.serial1_enabled || !buf) return false; + if (mavlinkTelem.serial1_isexternal) return mavlinkTelemExternalPutBuf(buf, count); + + if (!auxSerialTxFifo.hasSpace(count)) return false; +// if (auxSerialMode != UART_MODE_MAVLINK || !buf || !auxSerialTxFifo.hasSpace(count)) { +// return false; +// } + for (uint16_t i = 0; i < count; i++) auxSerialTxFifo.push(buf[i]); + USART_ITConfig(AUX_SERIAL_USART, USART_IT_TXE, ENABLE); + return true; +} + +#else +uint32_t mavlinkTelem1Available(void){ return 0; } +uint8_t mavlinkTelem1Getc(uint8_t* c){ return 0; } +bool mavlinkTelem1HasSpace(uint16_t count){ return false; } +bool mavlinkTelem1PutBuf(const uint8_t* buf, const uint16_t count){ return false; } +#endif + +#if defined(AUX2_SERIAL) + +uint32_t mavlinkTelem2Available(void) +{ + if (!mavlinkTelem.serial2_enabled) return 0; + if (mavlinkTelem.serial2_isexternal) return mavlinkTelemExternalRxFifo.size(); + +// if (aux2SerialMode != UART_MODE_MAVLINK) return 0; + return mavlinkTelemAux2SerialRxFifo.size(); +} + +// call only after check with mavlinkTelem2Available() +uint8_t mavlinkTelem2Getc(uint8_t* c) +{ + if (!mavlinkTelem.serial2_enabled) return 0; + if (mavlinkTelem.serial2_isexternal) return mavlinkTelemExternalRxFifo.pop(*c); + + return mavlinkTelemAux2SerialRxFifo.pop(*c); +} + +bool mavlinkTelem2HasSpace(uint16_t count) +{ + if (!mavlinkTelem.serial2_enabled) return 0; + if (mavlinkTelem.serial2_isexternal) return mavlinkTelemExternalTxFifo.hasSpace(count); + +// if (aux2SerialMode != UART_MODE_MAVLINK) return false; + return aux2SerialTxFifo.hasSpace(count); +} + +bool mavlinkTelem2PutBuf(const uint8_t* buf, const uint16_t count) +{ + if (!mavlinkTelem.serial2_enabled || !buf) return false; + if (mavlinkTelem.serial2_isexternal) return mavlinkTelemExternalPutBuf(buf, count); + + if (!aux2SerialTxFifo.hasSpace(count)) return false; +// if (aux2SerialMode != UART_MODE_MAVLINK || !buf || !aux2SerialTxFifo.hasSpace(count)) { +// return false; +// } + for (uint16_t i = 0; i < count; i++) aux2SerialTxFifo.push(buf[i]); + USART_ITConfig(AUX2_SERIAL_USART, USART_IT_TXE, ENABLE); + return true; +} + +#else +uint32_t mavlinkTelem2Available(void){ return 0; } +uint8_t mavlinkTelem2Getc(uint8_t* c){ return 0; } +bool mavlinkTelem2HasSpace(uint16_t count){ return false; } +bool mavlinkTelem2PutBuf(const uint8_t* buf, const uint16_t count){ return false; } +#endif + +// -- USB handlers -- + +#if defined(TELEMETRY_MAVLINK_USB_SERIAL) + +uint32_t mavlinkTelem3Available(void) +{ + if (getSelectedUsbMode() != USB_MAVLINK_MODE) return 0; + return mavlinkTelemUsbRxFifo.size(); +} + +// call only after check with mavlinkTelem2Available() +uint8_t mavlinkTelem3Getc(uint8_t* c) +{ + return mavlinkTelemUsbRxFifo.pop(*c); +} + +bool mavlinkTelem3HasSpace(uint16_t count) +{ + if (getSelectedUsbMode() != USB_MAVLINK_MODE) return false; + return true; //?? +} + +bool mavlinkTelem3PutBuf(const uint8_t* buf, const uint16_t count) +{ + if (getSelectedUsbMode() != USB_MAVLINK_MODE || !buf) { + return false; + } + for (uint16_t i = 0; i < count; i++) { + usbSerialPutc(buf[i]); + } + return true; +} + +#else +uint32_t mavlinkTelem3Available(void){ return 0; } +uint8_t mavlinkTelem3Getc(uint8_t* c){ return 0; } +bool mavlinkTelem3HasSpace(uint16_t count){ return false; } +bool mavlinkTelem3PutBuf(const uint8_t* buf, const uint16_t count){ return false; } +#endif + +// -- MavlinkTelem stuff -- + +// map aux1,aux2,external onto serial1 & serial2 +void MavlinkTelem::map_serials(void) +{ + if (_external_enabled) { + if (_aux1_enabled && _aux2_enabled) { + // shit, what should we do??? we give aux,aux2 priority + serial1_enabled = serial2_enabled = true; + serial1_isexternal = serial2_isexternal = false; + } + else if (_aux1_enabled && !_aux2_enabled) { + serial1_enabled = true; + serial1_isexternal = false; + serial2_enabled = serial2_isexternal = true; + } + else if (!_aux1_enabled && _aux2_enabled) { + serial1_enabled = serial1_isexternal = true; + serial2_enabled = true; + serial2_isexternal = false; + } + else { + serial1_enabled = serial1_isexternal = true; + serial2_enabled = serial2_isexternal = false; + } + } + else{ + serial1_enabled = _aux1_enabled; + serial2_enabled = _aux1_enabled; + serial1_isexternal = serial2_isexternal = false; + } +} + +void MavlinkTelem::telemetrySetValue(uint16_t id, uint8_t subId, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec) +{ + if (g_model.mavlinkRssi) { + if (!radio.is_receiving && !radio.is_receiving65 && !radio.is_receiving35) return; + } + + if (g_model.mavlinkMimicSensors) { + setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_SPORT, id, subId, instance, value, unit, prec); + telemetryStreaming = MAVLINK_TELEM_RADIO_RECEIVING_TIMEOUT; //2 * TELEMETRY_TIMEOUT10ms; // 2 seconds + } +} + +// only for MAVLINK_MSG_ID_RADIO_STATUS, MAVLINK_MSG_ID_RC_CHANNELS, MAVLINK_MSG_ID_RC_CHANNELS_RAW +void MavlinkTelem::telemetrySetRssiValue(uint8_t rssi) +{ + if (g_model.mavlinkRssiScale > 0) { + if (g_model.mavlinkRssiScale < 255) { //if not full range, respect UINT8_MAX + if (rssi == UINT8_MAX) rssi = 0; + } + if (rssi > g_model.mavlinkRssiScale) rssi = g_model.mavlinkRssiScale; //constrain + rssi = (uint8_t)( ((uint16_t)rssi * 100) / g_model.mavlinkRssiScale); //scale to 0..99 + } + else { //mavlink default + if (rssi == UINT8_MAX) rssi = 0; + } + + radio.rssi_scaled = rssi; + + if (g_model.mavlinkRssi) { + if (!radio.is_receiving && !radio.is_receiving65 && !radio.is_receiving35) return; + } + + if (g_model.mavlinkRssi) { + telemetryData.rssi.set(rssi); + telemetryStreaming = MAVLINK_TELEM_RADIO_RECEIVING_TIMEOUT; //2 * TELEMETRY_TIMEOUT10ms; // 2 seconds + } + + if (g_model.mavlinkRssi || g_model.mavlinkMimicSensors) { + setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_SPORT, RSSI_ID, 0, 1, (int32_t)rssi, UNIT_DB, 0); + telemetryStreaming = MAVLINK_TELEM_RADIO_RECEIVING_TIMEOUT; //2 * TELEMETRY_TIMEOUT10ms; // 2 seconds + } + //#if defined(MULTIMODULE) + //{ TX_RSSI_ID, TX_RSSI_ID, 0, ZSTR_TX_RSSI , UNIT_DB , 0 }, + //{ TX_LQI_ID , TX_LQI_ID, 0, ZSTR_TX_QUALITY, UNIT_RAW, 0 }, +} + +// is probably not needed, aren't they reset by telementryStreaming timeout? +void MavlinkTelem::telemetryResetRssiValue(void) +{ + if (radio.is_receiving || radio.is_receiving65 || radio.is_receiving35) return; + + radio.rssi_scaled = 0; + + if (g_model.mavlinkRssi) + telemetryData.rssi.reset(); + + if (g_model.mavlinkRssi || g_model.mavlinkMimicSensors) + setTelemetryValue(PROTOCOL_TELEMETRY_FRSKY_SPORT, RSSI_ID, 0, 1, 0, UNIT_DB, 0); +} + +bool MavlinkTelem::telemetryVoiceEnabled(void) +{ + if (!g_model.mavlinkRssi && !g_model.mavlinkMimicSensors) return true; + + if (g_model.mavlinkRssi && !radio.rssi_voice_disabled) return true; + + return false; +} + diff --git a/radio/src/telemetry/mavlink/mavlink_telem_mavapi.cpp b/radio/src/telemetry/mavlink/mavlink_telem_mavapi.cpp new file mode 100644 index 00000000000..f1e51bb2762 --- /dev/null +++ b/radio/src/telemetry/mavlink/mavlink_telem_mavapi.cpp @@ -0,0 +1,139 @@ +/* +* (c) www.olliw.eu, OlliW, OlliW42 +*/ + +#include "opentx.h" + +// -- Receive stuff -- + +// we probably need to differentiate not only by msgid, but also by sysid-compid +// if two components send the same message at (too) high rate considering only msgid leads to message loss + +uint8_t MavlinkTelem::_mavapiMsgInFindOrAdd(uint32_t msgid) +{ + for (uint8_t i = 0; i < MAVMSGLIST_MAX; i++) { + if (!_mavapi_rx_list[i]) continue; + if (_mavapi_rx_list[i]->msgid == msgid) { return i; } + } + for (uint8_t i = 0; i < MAVMSGLIST_MAX; i++) { + if (_mavapi_rx_list[i]) continue; + // free spot, so add it + _mavapi_rx_list[i] = (MavMsg*)malloc(sizeof(MavMsg)); + if (!_mavapi_rx_list[i]) return UINT8_MAX; // grrrr + _mavapi_rx_list[i]->msgid = msgid; + _mavapi_rx_list[i]->payload_ptr = NULL; + _mavapi_rx_list[i]->updated = false; + return i; + } + return UINT8_MAX; +} + +void MavlinkTelem::mavapiHandleMessage(fmav_message_t* msg) +{ + if (!_mavapi_rx_enabled) return; + + uint8_t i = _mavapiMsgInFindOrAdd(msg->msgid); + if (i == UINT8_MAX) return; + + if (!_mavapi_rx_list[i]->payload_ptr) _mavapi_rx_list[i]->payload_ptr = malloc(msg->payload_max_len); + if (!_mavapi_rx_list[i]->payload_ptr) return; // grrrr + + _mavapi_rx_list[i]->sysid = msg->sysid; + _mavapi_rx_list[i]->compid = msg->compid; + _mavapi_rx_list[i]->target_sysid = msg->target_sysid; + _mavapi_rx_list[i]->target_compid = msg->target_compid; + memcpy(_mavapi_rx_list[i]->payload_ptr, msg->payload, msg->payload_max_len); + _mavapi_rx_list[i]->updated = true; + _mavapi_rx_list[i]->timestamp = time10us(); +} + +void MavlinkTelem::mavapiMsgInEnable(bool flag) +{ + _mavapi_rx_enabled = flag; +} + +uint8_t MavlinkTelem::mavapiMsgInCount(void) +{ + if (!_mavapi_rx_enabled) return 0; + + uint8_t cnt = 0; + for (uint8_t i = 0; i < MAVMSGLIST_MAX; i++) if(_mavapi_rx_list[i]) cnt++; + return cnt; +} + +MavlinkTelem::MavMsg* MavlinkTelem::mavapiMsgInGet(uint32_t msgid) +{ + if (!_mavapi_rx_enabled) return NULL; + + uint8_t i_found = UINT8_MAX; + for (uint8_t i = 0; i < MAVMSGLIST_MAX; i++) { + if (!_mavapi_rx_list[i]) continue; + if (!_mavapi_rx_list[i]->payload_ptr) continue; // it must have been received completely + if (_mavapi_rx_list[i]->msgid == msgid) { i_found = i; } + } + if (i_found == UINT8_MAX) return NULL; + return _mavapi_rx_list[i_found]; +} + +MavlinkTelem::MavMsg* MavlinkTelem::mavapiMsgInGetLast(void) +{ + if (!_mavapi_rx_enabled) return NULL; + + uint32_t t_max = 0; + uint8_t i_found = UINT8_MAX; + for (uint8_t i = 0; i < MAVMSGLIST_MAX; i++) { + if (!_mavapi_rx_list[i]) continue; + if (!_mavapi_rx_list[i]->payload_ptr) continue; // it must have been received completely + if (_mavapi_rx_list[i]->timestamp > t_max) { t_max = _mavapi_rx_list[i]->timestamp; i_found = i; } + } + if (i_found == UINT8_MAX) return NULL; + return _mavapi_rx_list[i_found]; +} + +// -- Send stuff -- + +void MavlinkTelem::mavapiMsgOutEnable(bool flag) +{ + _mavapi_tx_enabled = flag; + + if (_mavapi_tx_enabled && _mavapiMsgOutFifo == NULL) { + _mavapiMsgOutFifo = (fmav_message_t*)malloc(sizeof(fmav_message_t) * MAVOUTFIFO_MAX); + if (!_mavapiMsgOutFifo) _mavapi_tx_enabled = false; // grrrr + } +} + + +// returns the pointer into which we should write, without advancing write index, probe()-like +fmav_message_t* MavlinkTelem::mavapiMsgOutPtr(void) +{ + if (!_mavapi_tx_enabled) return NULL; + + uint32_t wi_next = (_wi + 1) & (MAVOUTFIFO_MAX - 1); + if (wi_next == _ri) return NULL; // blocking push, push not allowed if full + return &(_mavapiMsgOutFifo[_wi]); +} + +// advances write index, and sets task, push()-like +void MavlinkTelem::mavapiMsgOutSet(void) +{ + if (!_mavapi_tx_enabled) return; + + _wi = (_wi + 1) & (MAVOUTFIFO_MAX - 1); + SETTASK(TASK_ME, TASK_SENDMSG_MAVLINK_API); +} + +// generate from msg at read index, and advance read index, pop()-like +void MavlinkTelem::mavapiGenerateMessage(void) +{ + if (!_mavapi_tx_enabled) return; + + if (_wi == _ri) return; // empty + fmav_message_t* msgptr = &(_mavapiMsgOutFifo[_ri]); + _ri = (_ri + 1) & (MAVOUTFIFO_MAX - 1); + memcpy(&_msg_out, msgptr, sizeof(fmav_message_t)); + fmav_finalize_msg(&_msg_out, &_status_out); + _msg_out_available = true; +} + + + diff --git a/radio/src/telemetry/mavlink/mavlink_telem_qshot.cpp b/radio/src/telemetry/mavlink/mavlink_telem_qshot.cpp new file mode 100644 index 00000000000..72850ee8d39 --- /dev/null +++ b/radio/src/telemetry/mavlink/mavlink_telem_qshot.cpp @@ -0,0 +1,117 @@ +/* + * (c) www.olliw.eu, OlliW, OlliW42 + */ + +#include "opentx.h" + +// -- Generate MAVLink messages -- +// these should never be called directly, should only be called by the task handler + +//STorM32 specific +void MavlinkTelem::generateCmdDoQShotConfigure(uint8_t tsystem, uint8_t tcomponent, uint8_t mode, uint8_t shot_state) +{ + _generateCmdLong(tsystem, tcomponent, + MAV_CMD_QSHOT_DO_CONFIGURE, + mode, shot_state, 0,0,0,0,0 + ); +} + +//STorM32 specific +void MavlinkTelem::generateQShotStatus(uint8_t mode, uint8_t shot_state) +{ + fmav_msg_qshot_status_pack( + &_msg_out, _my_sysid, _my_compid, + mode, shot_state, + &_status_out + ); + _msg_out_available = true; +} + +void MavlinkTelem::generateButtonChange(uint8_t button_state) +{ + fmav_msg_button_change_pack( + &_msg_out, _my_sysid, _my_compid, + time_boot_ms(), + 0, + button_state, + &_status_out + ); + _msg_out_available = true; +} + +// -- Mavsdk Convenience Task Wrapper -- +// to make it easy for api_mavsdk to call functions + +void MavlinkTelem::sendQShotCmdConfigure(uint8_t mode, uint8_t shot_state) +{ + _t_qshot_cmd_mode = mode; + qshot.mode = mode; + qshot.shot_state = shot_state; + SETTASK(TASK_ME, TASK_SENDCMD_DO_QSHOT_CONFIGFURE); +} + +void MavlinkTelem::sendQShotStatus(uint8_t mode, uint8_t shot_state) +{ + _t_qshot_mode = mode; + qshot.mode = mode; + qshot.shot_state = shot_state; + SETTASK(TASK_ME, TASK_SENDMSG_QSHOT_STATUS); +} + +void MavlinkTelem::sendQShotButtonState(uint8_t button_state) +{ + _t_qshot_button_state = button_state; + SETTASK(TASK_ME, TASK_SENDMSG_QSHOT_BUTTON_STATE); +} + +// -- Task handlers -- + +bool MavlinkTelem::doTaskQShot(void) +{ + if (!_task[TASK_ME]) return false; // no task pending + + if (_task[TASK_ME] & TASK_SENDCMD_DO_QSHOT_CONFIGFURE) { + RESETTASK(TASK_ME, TASK_SENDCMD_DO_QSHOT_CONFIGFURE); + generateCmdDoQShotConfigure(0, 0, _t_qshot_cmd_mode, _t_qshot_cmd_shot_state); //broadcast + return true; //do only one per loop + } + if (_task[TASK_ME] & TASK_SENDMSG_QSHOT_STATUS) { + RESETTASK(TASK_ME, TASK_SENDMSG_QSHOT_STATUS); + generateQShotStatus(_t_qshot_mode, _t_qshot_shot_state); + return true; //do only one per loop + } + if (_task[TASK_ME] & TASK_SENDMSG_QSHOT_BUTTON_STATE) { + RESETTASK(TASK_ME, TASK_SENDMSG_QSHOT_BUTTON_STATE); + generateButtonChange(_t_qshot_button_state); + return true; //do only one per loop + } + + return false; +} + +// -- Handle incoming MAVLink messages, which are for qshot -- + +void MavlinkTelem::handleMessageQShot(void) +{ + switch (_msg.msgid) { + case FASTMAVLINK_MSG_ID_QSHOT_STATUS: { + fmav_qshot_status_t payload; + fmav_msg_qshot_status_decode(&payload, &_msg); + qshot.mode = payload.mode; + qshot.shot_state = payload.shot_state; + break; + } + } +} + +// -- Startup Requests -- + +// -- Resets -- + +void MavlinkTelem::_resetQShot(void) +{ + _task[TASK_ME] = 0; + + qshot.mode = MAV_QSHOT_MODE_UNDEFINED; + qshot.shot_state = 0; +} diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index 34c06aeeb1c..eefb062f1ba 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -27,7 +27,10 @@ #include "libopenui.h" #endif -uint8_t telemetryStreaming = 0; +//OW +//uint8_t telemetryStreaming = 0; +uint16_t telemetryStreaming = 0; +//OWEND uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1) uint8_t telemetryRxBufferCount = 0; @@ -204,7 +207,15 @@ void telemetryWakeup() } #endif +//OW +#if defined(TELEMETRY_MAVLINK) + if (!g_model.rssiAlarms.disabled && mavlinkTelem.telemetryVoiceEnabled()) { +#else +//OWEND if (!g_model.rssiAlarms.disabled) { +//OW +#endif +//OWEND if (TELEMETRY_STREAMING()) { if (TELEMETRY_RSSI() < g_model.rssiAlarms.getCriticalRssi() ) { AUDIO_RSSI_RED(); diff --git a/radio/src/telemetry/telemetry.h b/radio/src/telemetry/telemetry.h index e7063a0e7fe..e4121008189 100644 --- a/radio/src/telemetry/telemetry.h +++ b/radio/src/telemetry/telemetry.h @@ -41,7 +41,10 @@ #include "flysky_ibus.h" #endif -extern uint8_t telemetryStreaming; // >0 (true) == data is streaming in. 0 = no data detected for some time +//OW +//extern uint8_t telemetryStreaming; // >0 (true) == data is streaming in. 0 = no data detected for some time +extern uint16_t telemetryStreaming; // >0 (true) == data is streaming in. 0 = no data detected for some time +//OWEND inline bool TELEMETRY_STREAMING() { diff --git a/radio/src/thirdparty/Lua/src/lauxlib.h b/radio/src/thirdparty/Lua/src/lauxlib.h index 9cd4c02d387..1b6216a54e7 100644 --- a/radio/src/thirdparty/Lua/src/lauxlib.h +++ b/radio/src/thirdparty/Lua/src/lauxlib.h @@ -34,6 +34,12 @@ extern const luaL_Reg tab_funcs[]; extern const luaL_Reg opentxLib[]; extern const luaL_Reg lcdLib[]; extern const luaL_Reg modelLib[]; +//OW +#if defined(TELEMETRY_MAVLINK) +extern const luaL_Reg mavlinkLib[]; +extern const luaL_Reg mavsdkLib[]; +#endif +//OWEND LUALIB_API void (luaL_checkversion_) (lua_State *L, lua_Number ver); #define luaL_checkversion(L) luaL_checkversion_(L, LUA_VERSION_NUM) diff --git a/radio/src/thirdparty/Lua/src/linit.c b/radio/src/thirdparty/Lua/src/linit.c index 4706a3ab785..8ed7b98eae3 100644 --- a/radio/src/thirdparty/Lua/src/linit.c +++ b/radio/src/thirdparty/Lua/src/linit.c @@ -49,6 +49,12 @@ const luaR_table lua_rotable[] = {"lcd", lcdLib, NULL}, {"model", modelLib, NULL}, {"__baselib", baselib, NULL}, +//OW +#if defined(TELEMETRY_MAVLINK) + {"mavlink", mavlinkLib, mavlinkConstants}, + {"mavsdk", mavsdkLib, mavsdkConstants}, +#endif +//OWEND {LUA_IOLIBNAME, iolib, NULL}, {LUA_STRLIBNAME, strlib, NULL}, {LUA_MATHLIBNAME, mathlib, mathlib_vals}, diff --git a/radio/src/thirdparty/Lua/src/lrotable.h b/radio/src/thirdparty/Lua/src/lrotable.h index 1b935f4d1d2..9c92e412305 100644 --- a/radio/src/thirdparty/Lua/src/lrotable.h +++ b/radio/src/thirdparty/Lua/src/lrotable.h @@ -20,6 +20,12 @@ typedef struct extern const luaR_value_entry baselib_vals[]; extern const luaR_value_entry mathlib_vals[]; extern const luaR_value_entry opentxConstants[]; +//OW +#if defined(TELEMETRY_MAVLINK) +extern const luaR_value_entry mavlinkConstants[]; +extern const luaR_value_entry mavsdkConstants[]; +#endif +//OWEND // A mapping between table name and its entries typedef struct diff --git a/radio/src/thirdparty/Mavlink/.gitignore b/radio/src/thirdparty/Mavlink/.gitignore new file mode 100644 index 00000000000..a6ab96813cf --- /dev/null +++ b/radio/src/thirdparty/Mavlink/.gitignore @@ -0,0 +1,2 @@ +/out +*.h \ No newline at end of file diff --git a/radio/src/thirdparty/Mavlink/README.md b/radio/src/thirdparty/Mavlink/README.md new file mode 100644 index 00000000000..c8a43650cce --- /dev/null +++ b/radio/src/thirdparty/Mavlink/README.md @@ -0,0 +1,7 @@ +you need to run two py scripts before building: + +- run fmav_generate_c_library.py in order to generate the fastMavlink C code in the out folder + +- run fmav_generate_lua_lib.py in order to generate the C code for the mavlink lua support + + diff --git a/radio/src/thirdparty/Mavlink/edgetx.xml b/radio/src/thirdparty/Mavlink/edgetx.xml new file mode 100644 index 00000000000..745afb61eda --- /dev/null +++ b/radio/src/thirdparty/Mavlink/edgetx.xml @@ -0,0 +1,8 @@ + + + mavlink/external/dialects/storm32.xml + mavlink/message_definitions/v1.0/ardupilotmega.xml + 0 + + + diff --git a/radio/src/thirdparty/Mavlink/fmav_generate_c_library.py b/radio/src/thirdparty/Mavlink/fmav_generate_c_library.py new file mode 100644 index 00000000000..45fc32f367e --- /dev/null +++ b/radio/src/thirdparty/Mavlink/fmav_generate_c_library.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python +''' +fmav_generate_c_library.py +calls fastMavlink generator for C header files +(c) OlliW, OlliW42, www.olliw.eu +''' +import os +import shutil +import re +import sys + +#options to set + +mavlinkpathtorepository = 'fastmavlink' + +mavlinkdialect = "edgetx.xml" + +mavlinkoutputdirectory = 'out' + +''' +Imports +''' +sys.path.insert(0,mavlinkpathtorepository) + +from generator import fmav_gen as mavgen +from generator.modules import fmav_flags as mavflags + +''' +Generates the header files and place them in the output directory. +''' + +outdir = mavlinkoutputdirectory +xmlfile = mavlinkdialect + +print('----------') +print('kill out dir') +try: + shutil.rmtree(outdir) +except: + pass +os.mkdir(outdir) +print('----------') + +opts = mavgen.Opts(outdir, parse_flags=mavflags.PARSE_FLAGS_WARNING_ENUM_VALUE_MISSING) +args = [xmlfile] +try: + mavgen.fmavgen(opts,args) + print('Successfully Generated Headers', 'Headers generated successfully.') + +except Exception as ex: + exStr = str(ex) + print('Error Generating Headers','{0!s}'.format(exStr)) + exit() + diff --git a/radio/src/thirdparty/Mavlink/fmav_generate_lua_lib.py b/radio/src/thirdparty/Mavlink/fmav_generate_lua_lib.py new file mode 100644 index 00000000000..e353aeb09fa --- /dev/null +++ b/radio/src/thirdparty/Mavlink/fmav_generate_lua_lib.py @@ -0,0 +1,284 @@ +#!/usr/bin/env python +''' +fmav_generate_lua_lib.py +calls fastMavlink generator modules +(c) OlliW, OlliW42, www.olliw.eu +''' +import os, sys + +#options to set + +mavlinkpathtorepository = 'fastmavlink' + +mavlinkdialect = "edgetx.xml" + +''' +Imports +''' +sys.path.insert(0, mavlinkpathtorepository) + +from generator.modules import fmav_parse as mavparse +from generator.modules import mavtemplate +from generator.modules import fmav_flags as mavflags + +''' +Attention: names must not be longer than 32 chars +''' + +def excludeMessage(msg): + #return True + return False + if msg.name in ['AHRS','ATTITUDE','VIBRATION']: return False + #if msg.name in ['AHRS']: return False + return True + + +def cvtInvalidAttr(invalid_str): + if invalid_str == 'NaN': + return 'NAN' + else: + return invalid_str + +def shortenName(name, width): + nameshort = name[:] + if len(nameshort) > width: + print('WARNING: msg id '+name+' too long') + nn = str.split(name, '_') + nameshort = nn[0]+'_' + for i in range(1,len(nn)-1): + nameshort += nn[i][:3] + '_' + nameshort += nn[-1] + if len(nameshort) > width: + print(' ! ! ! msg id too long even after shortening') + nameshort = nameshort[:width] + return nameshort + +def shortenNameEnum(name, width): + nameshort = name[:] + if nameshort[:4] == 'MAV_': nameshort = nameshort[4:] + if len(nameshort) > width: + print('WARNING: msg id '+name+' too long') + nn = str.split(name, '_') + nameshort = nn[0]+'_' + for i in range(1,len(nn)-1): + nameshort += nn[i][:3] + nameshort += '_' + nn[-1] + if len(nameshort) > width: + print(' ! ! ! enum field too long even after shortening') + nameshort = nameshort[:width] + return nameshort + +def msgFieldCount(msg, excludetargets): + count = 0 + for field in msg.fields: + if field.array_length == 0: + if excludetargets and msg.is_target_system_field(field): continue + if excludetargets and msg.is_target_component_field(field): continue + count += 1 + else: + count += 1 + return count + +def generateLibMessageForPush(msg, m): + m.append(' case FASTMAVLINK_MSG_ID_'+msg.name+': { // #'+str(msg.id)) + if msgFieldCount(msg,True) > 0: + m.append(' fmav_'+msg.name.lower()+'_t* payload = (fmav_'+msg.name.lower()+'_t*)(mavmsg->payload_ptr);') + #for field in msg.ordered_fields: + for field in msg.fields: + nameshort = shortenName(field.name, 32) + if field.array_length == 0: + #we strip of target fields as their values are already in the msg structure + if msg.is_target_system_field(field): continue + if msg.is_target_component_field(field): continue + m.append(' lua_pushtablenumber(L, "'+nameshort+'", payload->'+field.name+');') + else: + m.append(' lua_pushstring(L, "'+nameshort+'"); // array '+field.name+'['+str(field.array_length)+']' ) + m.append(' lua_newtable(L);') + m.append(' for (int i = 0; i < '+str(field.array_length)+'; i++) { ') + m.append(' lua_pushtableinumber(L, i, payload->'+field.name+'[i]);') + m.append(' }') + m.append(' lua_rawset(L, -3);') + m.append(' return;') + m.append(' }') + + +def generateLibMessageForCheck(msg, m): + m.append(' case FASTMAVLINK_MSG_ID_'+msg.name+': { // #'+str(msg.id)) + if msgFieldCount(msg,False) > 0: + m.append(' fmav_'+msg.name.lower()+'_t* payload = (fmav_'+msg.name.lower()+'_t*)(msg_out->payload);') + for field in msg.fields: + nameshort = shortenName(field.name, 32) + if field.array_length == 0: + #we substitute target fields + if msg.is_target_system_field(field): nameshort = 'target_sysid' + if msg.is_target_component_field(field): nameshort = 'target_compid' + if msg.name == 'HEARTBEAT' and nameshort == 'mavlink_version': + m.append(' payload->mavlink_version = FASTMAVLINK_MAVLINK_VERSION;') + continue + invalid = '0' + if field.invalid: + invalid = cvtInvalidAttr(field.invalid) + m.append(' lua_checktablenumber(L, payload->'+field.name+', "'+nameshort+'", '+invalid+');') + else: + m.append(' lua_pushstring(L, "'+nameshort+'"); // array '+field.name+'['+str(field.array_length)+']' ) + m.append(' lua_gettable(L, -2);') + invalid = '[0]' + if field.invalid: + invalid = cvtInvalidAttr(field.invalid) + invalid = invalid[1:-1] + if invalid.find(',') < 0: + invalid = cvtInvalidAttr(invalid) + m.append(' for (int i = 0; i < '+str(field.array_length)+'; i++) { ') + m.append(' lua_checktableinumber(L, payload->'+field.name+'[i], i, '+invalid+');') + m.append(' }') + else: + invalid = cvtInvalidAttr(invalid[:-1]) + m.append(' lua_checktableinumber(L, payload->'+field.name+'[0], 0, '+invalid+');') + if field.array_length > 0: + m.append(' for (int i = 1; i < '+str(field.array_length)+'; i++) { ') + m.append(' lua_checktableinumber(L, payload->'+field.name+'[i], i, '+'0'+');') + m.append(' }') + m.append(' lua_pop(L, 1);') + + for field in msg.fields: + if msg.is_target_system_field(field): + m.append(' msg_out->target_sysid = payload->'+field.name+';') + for field in msg.fields: + if msg.is_target_component_field(field): + m.append(' msg_out->target_compid = payload->'+field.name+';') + m.append(' msg_out->crc_extra = FASTMAVLINK_MSG_'+msg.name+'_CRCEXTRA;') + m.append(' msg_out->payload_max_len = FASTMAVLINK_MSG_'+msg.name+'_PAYLOAD_LEN_MAX;') + m.append(' return 1;') + m.append(' }') + + +def generateLuaLibHeaders(dialectname): + print("Run XML %s" % os.path.basename(dialectname)) + dialectnamewoext = os.path.splitext(os.path.basename(dialectname))[0] + xml_list = mavparse.generateXmlList(dialectname) + messagesoutname = os.path.splitext(os.path.basename(dialectname))[0] + '_lua_lib_messages.h' + constantsoutname = os.path.splitext(os.path.basename(dialectname))[0] + '_lua_lib_constants.h' + + for xml in xml_list: + print(xml.basename) + print("Found %u messages and %u enums in %u XML files" % + (mavparse.totalNumberOfMessages(xml_list), mavparse.totalNumberOfEnums(xml_list), len(xml_list))) + + # find dialectxml, and generate complete enum dictionary + dialectxml = None + enums_all_by_name = {} + for xml in xml_list: + if xml.basename == dialectnamewoext: + dialectxml = xml + for enum in xml.enums_merged: + if not enum.name in enums_all_by_name.keys(): + enums_all_by_name[enum.name] = enum + + print("Messages") + print('->',dialectxml.basename) + m = [] + m.append('''//------------------------------------------------------------ +// mavlink messages +//------------------------------------------------------------ +// all message from opentx.xml +// auto generated + + +//------------------------------------------------------------ +// push +//------------------------------------------------------------ + +#define lua_pushtableinteger_raw(L, k, v) (lua_pushstring(L,(k)), lua_pushinteger(L,(v)), lua_rawset(L,-3)) +#define lua_pushtablenumber_raw(L, k, v) (lua_pushstring(L,(k)), lua_pushnumber(L,(v)), lua_rawset(L,-3)) +#define lua_pushtablestring_raw(L, k, v) (lua_pushstring(L,(k)), lua_pushstring(L,(v)), lua_rawset(L,-3)) +#define lua_pushtableinumber_raw(L, i, v) (lua_pushnumber(L,(i)), lua_pushnumber(L,(v)), lua_rawset(L,-3)) +#define lua_pushtableinumber(L, i, v) (lua_pushnumber(L,(i)), lua_pushnumber(L,(v)), lua_settable(L,-3)) + +static void luaMavlinkPushMavMsg(lua_State *L, MavlinkTelem::MavMsg* mavmsg) +{ + lua_newtable(L); + lua_pushtableinteger(L, "sysid", mavmsg->sysid); + lua_pushtableinteger(L, "compid", mavmsg->compid); + lua_pushtableinteger(L, "msgid", mavmsg->msgid); + lua_pushtableinteger(L, "target_sysid", mavmsg->target_sysid); + lua_pushtableinteger(L, "target_compid", mavmsg->target_compid); + lua_pushtableboolean(L, "updated", mavmsg->updated); + + switch (mavmsg->msgid) {''') + for msgid in sorted(dialectxml.messages_all_by_id.keys()): + msg = dialectxml.messages_all_by_id[msgid] + if excludeMessage(msg): continue + generateLibMessageForPush(msg, m) + m.append(''' } +} + + +//------------------------------------------------------------ +// check +//------------------------------------------------------------ + +#define lua_checktableinteger(L, r, k, d) (lua_pushstring(L,(k)), lua_gettable(L,-2 ), r = (lua_isnumber(L,-1))?lua_tointeger(L,-1):(d), lua_pop(L,1)) +#define lua_checktablenumber(L, r, k, d) (lua_pushstring(L,(k)), lua_gettable(L,-2 ), r = (lua_isnumber(L,-1))?lua_tonumber(L,-1):(d), lua_pop(L,1)) +#define lua_checktableinumber(L, r, i, d) (lua_pushnumber(L,(i)), lua_gettable(L,-2 ), r = (lua_isnumber(L,-1))?lua_tonumber(L,-1):(d), lua_pop(L,1)) + +static uint8_t luaMavlinkCheckMsgOut(lua_State *L, fmav_message_t* msg_out) +{ + uint32_t msgid; + lua_checktableinteger(L, msgid, "msgid", UINT32_MAX); + if (msgid == UINT32_MAX) return 0; + + msg_out->sysid = mavlinkTelem.mySysId(); + msg_out->compid = mavlinkTelem.myCompId(); + msg_out->msgid = msgid; + msg_out->target_sysid = 0; + msg_out->target_compid = 0; + + switch (msgid) {''') + for msgid in sorted(dialectxml.messages_all_by_id.keys()): + msg = dialectxml.messages_all_by_id[msgid] + if excludeMessage(msg): continue + generateLibMessageForCheck(msg, m) + m.append(''' } + return 0; +} +''') + + # constants + print("Constants") + print('->',dialectxml.basename) + s = [] + s.append('''//------------------------------------------------------------ +// mavlink constants +//------------------------------------------------------------ +// all message and enum constants from edgetx.xml +// auto generated + +#define MAVLINK_LIB_CONSTANTS \\''') + for msgid in sorted(dialectxml.messages_all_by_id.keys()): + name = dialectxml.messages_all_by_id[msgid].name + nameshort = shortenName(name, 30) + s.append(' { "M_'+nameshort+'", FASTMAVLINK_MSG_ID_'+name+' }, \\') + for enumname in enums_all_by_name: + s.append( ' \\') + for entry in enums_all_by_name[enumname].entry: + nameshort = shortenNameEnum(entry.name, 32) + s.append(' { "'+nameshort+'", '+str(entry.value)+' }, \\') + + F = open(constantsoutname, mode='w') + for ss in s: + F.write(ss) + F.write('\n') + F.write('\n') + F.close() + + F = open(messagesoutname, mode='w') + for mm in m: + F.write(mm) + F.write('\n') + F.write('\n') + F.close() + + +if __name__ == "__main__": + dialectname = os.path.join(mavlinkdialect) + generateLuaLibHeaders(dialectname) diff --git a/radio/src/translations.cpp b/radio/src/translations.cpp index b97d27c480c..b99f38f146c 100644 --- a/radio/src/translations.cpp +++ b/radio/src/translations.cpp @@ -972,3 +972,17 @@ const char STR_ABOUT_PARENTS_4[] = TR_ABOUT_PARENTS_4; const char STR_AUTH_FAILURE[] = TR_AUTH_FAILURE; const char STR_PROTOCOL[] = TR_PROTOCOL; const char STR_RACING_MODE[] = TR_RACING_MODE; + +//OW +#if defined(TELEMETRY_MAVLINK) +const char STR_MAVLINK[] = TR_MAVLINK; +const char STR_MAVLINK_RSSI[] = TR_MAVLINK_RSSI; +const char STR_MAVLINK_RSSI_SCALE[] = TR_MAVLINK_RSSI_SCALE; +const char STR_MAVLINK_SENSOR_MIMICRY[] = TR_MAVLINK_SENSOR_MIMICRY; +const char STR_MAVLINK_RC_OVERRIDE[] = TR_MAVLINK_RC_OVERRIDE; +ISTR(MAVLINK_AUX_SERIAL_MODES); +ISTR(MAVLINK_AUX_BAUDRATES); +const char STR_MAVLINK_EXTERNAL[] = TR_MAVLINK_EXTERNAL; +const char STR_USB_MAVLINK[] = TR_USB_MAVLINK; +#endif +//OWEND diff --git a/radio/src/translations.h b/radio/src/translations.h index 84fa3f02db8..4173422e56d 100644 --- a/radio/src/translations.h +++ b/radio/src/translations.h @@ -1113,4 +1113,18 @@ extern const char STR_RACING_MODE[]; #define CHR_HOUR TR_CHR_HOUR #define CHR_INPUT TR_CHR_INPUT +//OW +#if defined(TELEMETRY_MAVLINK) +extern const char STR_MAVLINK[]; +extern const char STR_MAVLINK_RSSI[]; +extern const char STR_MAVLINK_RSSI_SCALE[]; +extern const char STR_MAVLINK_SENSOR_MIMICRY[]; +extern const char STR_MAVLINK_RC_OVERRIDE[]; +extern const char STR_MAVLINK_AUX_SERIAL_MODES[]; +extern const char STR_MAVLINK_AUX_BAUDRATES[]; +extern const char STR_MAVLINK_EXTERNAL[]; +extern const char STR_USB_MAVLINK[]; +#endif +//OWEND + #endif // _TRANSLATIONS_H_ diff --git a/radio/src/translations/untranslated.h b/radio/src/translations/untranslated.h index a734e63c64b..a62b6f04852 100644 --- a/radio/src/translations/untranslated.h +++ b/radio/src/translations/untranslated.h @@ -317,3 +317,17 @@ #define CHAR_CHANNEL STR_CHAR_CHANNEL[0] #define CHAR_TELEMETRY STR_CHAR_TELEMETRY[0] #define CHAR_LUA STR_CHAR_LUA[0] + +//OW +#define TR_MAVLINK "Mavlink" +#define TR_MAVLINK_RSSI "Rssi" +#define TR_MAVLINK_RSSI_SCALE "Rssi Scale" +#define TR_MAVLINK_SENSOR_MIMICRY "Sensor Mimicry" +#define TR_MAVLINK_RC_OVERRIDE "RC Override" +#define LEN_MAVLINK_AUX_SERIAL_MODES "\015" +#define TR_MAVLINK_AUX_SERIAL_MODES "OFF\0 ""Telem Mirror\0""Telemetry In\0""SBUS Trainer\0""LUA\0 ""Mavlink\0 " +#define LEN_MAVLINK_AUX_BAUDRATES "\006" +#define TR_MAVLINK_AUX_BAUDRATES "57600\0""115200""38400\0""19200\0" +#define TR_MAVLINK_EXTERNAL "Mavlink external" +#define TR_USB_MAVLINK "USB Mavlink (VCP)" +//OWEND From 719d2a22d7fe41bbc13b10f0bae5699ad0d3b9ea Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Sat, 5 Jun 2021 09:53:33 +0200 Subject: [PATCH 03/16] Removed Olli's markers Command used on OSX: `sed -i.tmp "/^\/\/OW/d" $(grep -l -rI "^//OW" *)` --- radio/src/dataconstants.h | 2 -- radio/src/datastructs.h | 8 -------- radio/src/gui/colorlcd/model_setup.cpp | 2 -- radio/src/gui/colorlcd/radio_hardware.cpp | 10 ---------- radio/src/gui/gui_common.cpp | 4 ---- radio/src/main.cpp | 6 ------ radio/src/opentx.cpp | 2 -- radio/src/opentx.h | 3 --- .../common/arm/stm32/aux_serial_driver.cpp | 16 ---------------- .../targets/common/arm/stm32/usb_driver.cpp | 2 -- .../src/targets/common/arm/stm32/usb_driver.h | 2 -- .../src/targets/common/arm/stm32/usbd_cdc.cpp | 2 -- radio/src/targets/common/arm/stm32/usbd_desc.c | 10 +--------- radio/src/targets/horus/board.h | 2 -- radio/src/targets/horus/hal.h | 2 -- radio/src/targets/horus/telemetry_driver.cpp | 18 ------------------ radio/src/tasks.cpp | 4 ---- radio/src/tasks.h | 2 -- radio/src/telemetry/telemetry.cpp | 6 ------ radio/src/telemetry/telemetry.h | 2 -- radio/src/thirdparty/Lua/src/lauxlib.h | 2 -- radio/src/thirdparty/Lua/src/linit.c | 2 -- radio/src/thirdparty/Lua/src/lrotable.h | 2 -- radio/src/translations.cpp | 2 -- radio/src/translations.h | 2 -- radio/src/translations/untranslated.h | 2 -- 26 files changed, 1 insertion(+), 116 deletions(-) diff --git a/radio/src/dataconstants.h b/radio/src/dataconstants.h index 7ead996792d..b62fc4de002 100644 --- a/radio/src/dataconstants.h +++ b/radio/src/dataconstants.h @@ -242,11 +242,9 @@ enum UartModes { UART_MODE_TELEMETRY, UART_MODE_SBUS_TRAINER, UART_MODE_LUA, -//OW #if defined(TELEMETRY_MAVLINK) UART_MODE_MAVLINK, #endif -//OWEND UART_MODE_COUNT, UART_MODE_MAX = UART_MODE_COUNT-1 }; diff --git a/radio/src/datastructs.h b/radio/src/datastructs.h index 6125bd644eb..db2f8ed7fd9 100644 --- a/radio/src/datastructs.h +++ b/radio/src/datastructs.h @@ -651,7 +651,6 @@ PACK(struct ModelData { char modelRegistrationID[PXX2_LEN_REGISTRATION_ID]; -//OW #if defined(TELEMETRY_MAVLINK) uint16_t _mavlinkEnabled:1; // currently not used uint16_t mavlinkRssi:1; @@ -663,7 +662,6 @@ PACK(struct ModelData { uint8_t _mavlinkDummy2; // currently not used // needs to adapt CHKSIZE below //if not all are use compiled optiomizes to lowest size, which may raise error #endif -//OWEND bool isTrainerTraineeEnable() const { @@ -859,7 +857,6 @@ PACK(struct RadioData { NOBACKUP(int8_t uartSampleMode:2); // See UartSampleModes -//OW #if defined(TELEMETRY_MAVLINK) uint16_t mavlinkBaudrate:3; uint16_t mavlinkBaudrate2:3; @@ -867,7 +864,6 @@ PACK(struct RadioData { uint16_t _mavlinkDummy:8; // currently not used // needs to adapt CHKSIZE below #endif -//OWEND }); #undef SWITCHES_WARNING_DATA @@ -989,9 +985,6 @@ static inline void check_struct() CHKSIZE(RadioData, 899); CHKSIZE(ModelData, 6604); #elif defined(PCBHORUS) -//OW -// CHKSIZE(RadioData, 901); -// CHKSIZE(ModelData, 11020); #if defined(TELEMETRY_MAVLINK) CHKSIZE(RadioData, 902+2); CHKSIZE(ModelData, 11020+4); @@ -999,7 +992,6 @@ static inline void check_struct() CHKSIZE(RadioData, 902); CHKSIZE(ModelData, 11020); #endif -//OWEND #endif #undef CHKSIZE diff --git a/radio/src/gui/colorlcd/model_setup.cpp b/radio/src/gui/colorlcd/model_setup.cpp index 8fd316263cd..af86059c194 100644 --- a/radio/src/gui/colorlcd/model_setup.cpp +++ b/radio/src/gui/colorlcd/model_setup.cpp @@ -1218,7 +1218,6 @@ void ModelSetupPage::build(FormWindow * window) grid.addWindow(new TrainerModuleWindow(window, {0, grid.getWindowHeight(), LCD_W, 0})); } -//OW #if defined(TELEMETRY_MAVLINK) // Mavlink { @@ -1242,7 +1241,6 @@ void ModelSetupPage::build(FormWindow * window) grid.nextLine(); } #endif -//OWEND window->setInnerHeight(grid.getWindowHeight()); } diff --git a/radio/src/gui/colorlcd/radio_hardware.cpp b/radio/src/gui/colorlcd/radio_hardware.cpp index fd86e77dc6a..228a1a3c595 100644 --- a/radio/src/gui/colorlcd/radio_hardware.cpp +++ b/radio/src/gui/colorlcd/radio_hardware.cpp @@ -278,16 +278,12 @@ void RadioHardwarePage::build(FormWindow * window) #if defined(AUX_SERIAL) new StaticText(window, grid.getLabelSlot(), STR_AUX_SERIAL_MODE); -//OW #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 -//OWEND auto aux = new Choice(window, grid.getFieldSlot(1,0), STR_AUX_SERIAL_MODES, 0, UART_MODE_MAX, GET_SET_DEFAULT(g_eeGeneral.auxSerialMode)); -//OW #endif -//OWEND aux->setAvailableHandler([=](int value) { return isAuxModeAvailable; }); @@ -296,16 +292,12 @@ void RadioHardwarePage::build(FormWindow * window) #if defined(AUX2_SERIAL) new StaticText(window, grid.getLabelSlot(), STR_AUX2_SERIAL_MODE); -//OW #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 -//OWEND auto aux2 = new Choice(window, grid.getFieldSlot(1,0), STR_AUX_SERIAL_MODES, 0, UART_MODE_MAX, GET_SET_DEFAULT(g_eeGeneral.aux2SerialMode)); -//OW #endif -//OWEND aux2->setAvailableHandler([=](int value) { return isAux2ModeAvailable; }); @@ -317,13 +309,11 @@ void RadioHardwarePage::build(FormWindow * window) grid.nextLine(); #endif -//OW #if defined(TELEMETRY_MAVLINK) new StaticText(window, grid.getLabelSlot(), STR_MAVLINK_EXTERNAL); new CheckBox(window, grid.getFieldSlot(), GET_SET_DEFAULT(g_eeGeneral.mavlinkExternal)); grid.nextLine(); #endif -//OWEND // ADC filter new StaticText(window, grid.getLabelSlot(), STR_JITTER_FILTER); diff --git a/radio/src/gui/gui_common.cpp b/radio/src/gui/gui_common.cpp index 67351bb6f9d..37baac8afc8 100644 --- a/radio/src/gui/gui_common.cpp +++ b/radio/src/gui/gui_common.cpp @@ -386,13 +386,11 @@ bool isAuxModeAvailable(int mode) #if defined(AUX2_SERIAL) if (mode == UART_MODE_SBUS_TRAINER) return g_eeGeneral.aux2SerialMode != UART_MODE_SBUS_TRAINER; -//OW #if defined(TELEMETRY_MAVLINK) else if (mode == UART_MODE_MAVLINK) return true; #endif -//OWEND #if defined(RADIO_TX16S) else return (g_model.trainerData.mode != TRAINER_MODE_MASTER_BATTERY_COMPARTMENT || g_eeGeneral.aux2SerialMode == UART_MODE_SBUS_TRAINER); @@ -406,13 +404,11 @@ bool isAux2ModeAvailable(int mode) #if defined(AUX_SERIAL) if (mode == UART_MODE_SBUS_TRAINER) return g_eeGeneral.auxSerialMode != UART_MODE_SBUS_TRAINER; -//OW #if defined(TELEMETRY_MAVLINK) else if (mode == UART_MODE_MAVLINK) return true; #endif -//OWEND #if defined(RADIO_TX16S) else return (g_model.trainerData.mode != TRAINER_MODE_MASTER_BATTERY_COMPARTMENT || g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER); diff --git a/radio/src/main.cpp b/radio/src/main.cpp index 10a2cb81de7..cb71bbd348e 100644 --- a/radio/src/main.cpp +++ b/radio/src/main.cpp @@ -70,13 +70,11 @@ void openUsbMenu() TRACE("USB mass storage"); setSelectedUsbMode(USB_MASS_STORAGE_MODE); }); -//OW #if defined(TELEMETRY_MAVLINK_USB_SERIAL) _usbMenu->addLine(STR_USB_MAVLINK, [] { setSelectedUsbMode(USB_MAVLINK_MODE); }); #endif -//OWEND #if defined(DEBUG) _usbMenu->addLine(STR_USB_SERIAL, [] { TRACE("USB serial"); @@ -95,13 +93,11 @@ void onUSBConnectMenu(const char *result) else if (result == STR_USB_JOYSTICK) { setSelectedUsbMode(USB_JOYSTICK_MODE); } -//OW #if defined(TELEMETRY_MAVLINK_USB_SERIAL) else if (result == STR_USB_MAVLINK) { setSelectedUsbMode(USB_MAVLINK_MODE); } #endif -//OWEND else if (result == STR_USB_SERIAL) { setSelectedUsbMode(USB_SERIAL_MODE); } @@ -115,11 +111,9 @@ void openUsbMenu() if (popupMenuHandler != onUSBConnectMenu) { POPUP_MENU_ADD_ITEM(STR_USB_JOYSTICK); POPUP_MENU_ADD_ITEM(STR_USB_MASS_STORAGE); -//OW #if defined(TELEMETRY_MAVLINK_USB_SERIAL) POPUP_MENU_ADD_ITEM(USB_MAVLINK_MODE); #endif -//OWEND #if defined(DEBUG) POPUP_MENU_ADD_ITEM(STR_USB_SERIAL); #endif diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp index 7090cb24b7b..7116d89f9a9 100644 --- a/radio/src/opentx.cpp +++ b/radio/src/opentx.cpp @@ -227,12 +227,10 @@ void per10ms() outputTelemetryBuffer.per10ms(); -//OW #if defined(TELEMETRY_MAVLINK) mavlinkTelem.tick10ms(); //XX checkEventLockTmo(); #endif -//OWEND heartbeat |= HEART_TIMER_10MS; } diff --git a/radio/src/opentx.h b/radio/src/opentx.h index 7216b121a04..5540ef77260 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -18,7 +18,6 @@ * GNU General Public License for more details. */ -//OW TELEMETRY_MAVLINK #pragma once @@ -1017,11 +1016,9 @@ constexpr uint8_t SD_SCREEN_FILE_LENGTH = 64; #include "bluetooth.h" #endif -//OW #if defined(TELEMETRY_MAVLINK) #include "telemetry/mavlink/mavlink_telem.h" #endif -//OWEND constexpr uint8_t TEXT_FILENAME_MAXLEN = 40; diff --git a/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp b/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp index ba461cec022..082d41f27c3 100644 --- a/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp +++ b/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp @@ -23,12 +23,10 @@ #if defined(AUX_SERIAL) uint8_t auxSerialMode = UART_MODE_COUNT; // Prevent debug output before port is setup -//OW //Fifo auxSerialTxFifo; #if !defined(TELEMETRY_MAVLINK) Fifo auxSerialTxFifo; #endif -//OWEND #if defined(AUX_SERIAL_DMA_Stream_RX) AuxSerialRxFifo auxSerialRxFifo __DMA (AUX_SERIAL_DMA_Stream_RX); #else @@ -157,7 +155,6 @@ void auxSerialInit(unsigned int mode, unsigned int protocol) case UART_MODE_LUA: auxSerialSetup(LUA_DEFAULT_BAUDRATE, false); AUX_SERIAL_POWER_ON(); -//OW break; #if defined(TELEMETRY_MAVLINK) @@ -168,7 +165,6 @@ void auxSerialInit(unsigned int mode, unsigned int protocol) mavlinkTelemAuxSerialRxFifo.clear(); break; #endif -//OWEND } } @@ -198,14 +194,12 @@ void auxSerialStop() USART_DeInit(AUX_SERIAL_USART); -//OW #if defined(TELEMETRY_MAVLINK) if (auxSerialMode == UART_MODE_MAVLINK) { auxSerialTxFifo.clear(); mavlinkTelemAuxSerialRxFifo.clear(); } #endif -//OWEND } uint8_t auxSerialTracesEnabled() @@ -231,7 +225,6 @@ extern "C" void AUX_SERIAL_USART_IRQHandler(void) USART_ITConfig(AUX_SERIAL_USART, USART_IT_TXE, DISABLE); } } -//OW #if defined(TELEMETRY_MAVLINK) if (auxSerialMode == UART_MODE_MAVLINK) { if (USART_GetITStatus(AUX_SERIAL_USART, USART_IT_RXNE) != RESET) { @@ -242,7 +235,6 @@ extern "C" void AUX_SERIAL_USART_IRQHandler(void) return; } #endif -//OWEND #if defined(CLI) if (getSelectedUsbMode() != USB_SERIAL_MODE) { @@ -280,12 +272,10 @@ extern "C" void AUX_SERIAL_USART_IRQHandler(void) #if defined(AUX2_SERIAL) uint8_t aux2SerialMode = UART_MODE_COUNT; // Prevent debug output before port is setup -//OW //Fifo aux2SerialTxFifo; #if !defined(TELEMETRY_MAVLINK) Fifo aux2SerialTxFifo; #endif -//OWEND AuxSerialRxFifo aux2SerialRxFifo __DMA (AUX2_SERIAL_DMA_Stream_RX); void aux2SerialSetup(unsigned int baudrate, bool dma, uint16_t length, uint16_t parity, uint16_t stop) @@ -393,7 +383,6 @@ void aux2SerialInit(unsigned int mode, unsigned int protocol) case UART_MODE_LUA: aux2SerialSetup(LUA_DEFAULT_BAUDRATE, false); AUX2_SERIAL_POWER_ON(); -//OW break; #if defined(TELEMETRY_MAVLINK) @@ -404,7 +393,6 @@ void aux2SerialInit(unsigned int mode, unsigned int protocol) mavlinkTelemAux2SerialRxFifo.clear(); break; #endif -//OWEND } } @@ -430,14 +418,12 @@ void aux2SerialStop() { DMA_DeInit(AUX2_SERIAL_DMA_Stream_RX); USART_DeInit(AUX2_SERIAL_USART); -//OW #if defined(TELEMETRY_MAVLINK) if (aux2SerialMode == UART_MODE_MAVLINK) { aux2SerialTxFifo.clear(); mavlinkTelemAux2SerialRxFifo.clear(); } #endif -//OWEND } uint8_t aux2SerialTracesEnabled() @@ -463,7 +449,6 @@ extern "C" void AUX2_SERIAL_USART_IRQHandler(void) USART_ITConfig(AUX2_SERIAL_USART, USART_IT_TXE, DISABLE); } } -//OW #if defined(TELEMETRY_MAVLINK) if (aux2SerialMode == UART_MODE_MAVLINK) { if (USART_GetITStatus(AUX2_SERIAL_USART, USART_IT_RXNE) != RESET) { @@ -474,7 +459,6 @@ extern "C" void AUX2_SERIAL_USART_IRQHandler(void) return; } #endif -//OWEND #if defined(CLI) if (getSelectedUsbMode() != USB_SERIAL_MODE) { diff --git a/radio/src/targets/common/arm/stm32/usb_driver.cpp b/radio/src/targets/common/arm/stm32/usb_driver.cpp index 7ba7ceb5911..e61caf5186e 100644 --- a/radio/src/targets/common/arm/stm32/usb_driver.cpp +++ b/radio/src/targets/common/arm/stm32/usb_driver.cpp @@ -79,9 +79,7 @@ void usbStart() #endif #if defined(USB_SERIAL) case USB_SERIAL_MODE: -//OW case USB_MAVLINK_MODE: -//OWEND // initialize USB as CDC device (virtual serial port) USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb); break; diff --git a/radio/src/targets/common/arm/stm32/usb_driver.h b/radio/src/targets/common/arm/stm32/usb_driver.h index 5855ae41c86..fa2d92113e8 100644 --- a/radio/src/targets/common/arm/stm32/usb_driver.h +++ b/radio/src/targets/common/arm/stm32/usb_driver.h @@ -29,9 +29,7 @@ enum usbMode { USB_JOYSTICK_MODE, USB_MASS_STORAGE_MODE, USB_SERIAL_MODE, -//OW USB_MAVLINK_MODE, -//OWEND #if defined(USB_SERIAL) USB_MAX_MODE=USB_SERIAL_MODE #else diff --git a/radio/src/targets/common/arm/stm32/usbd_cdc.cpp b/radio/src/targets/common/arm/stm32/usbd_cdc.cpp index f7783ca73ef..ce595ad9cbe 100644 --- a/radio/src/targets/common/arm/stm32/usbd_cdc.cpp +++ b/radio/src/targets/common/arm/stm32/usbd_cdc.cpp @@ -230,13 +230,11 @@ static uint16_t VCP_DataRx (uint8_t* Buf, uint32_t Len) } } #endif -//OW #if defined(TELEMETRY_MAVLINK) && defined(USB_SERIAL) for (uint32_t i = 0; i < Len; i++) { mavlinkTelemUsbRxFifo.push(Buf[i]); } #endif -//OWEND return USBD_OK; } diff --git a/radio/src/targets/common/arm/stm32/usbd_desc.c b/radio/src/targets/common/arm/stm32/usbd_desc.c index 84499a50d95..066aae8490b 100644 --- a/radio/src/targets/common/arm/stm32/usbd_desc.c +++ b/radio/src/targets/common/arm/stm32/usbd_desc.c @@ -140,9 +140,7 @@ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length) break; case USB_SERIAL_MODE: -//OW case USB_MAVLINK_MODE: -//OWEND vid = USBD_CDC_VID; pid = USBD_CDC_PID; break; @@ -214,9 +212,7 @@ uint8_t * USBD_USR_ProductStrDescriptor( uint8_t speed , uint16_t *length) USBD_GetString ((uint8_t*)USBD_HID_PRODUCT_FS_STRING, USBD_StrDesc, length); break; case USB_SERIAL_MODE: -//OW case USB_MAVLINK_MODE: -//OWEND USBD_GetString ((uint8_t*)USBD_CDC_PRODUCT_FS_STRING, USBD_StrDesc, length); break; case USB_MASS_STORAGE_MODE: @@ -267,9 +263,7 @@ uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length) USBD_GetString ((uint8_t*)USBD_HID_CONFIGURATION_FS_STRING, USBD_StrDesc, length); break; case USB_SERIAL_MODE: -//OW case USB_MAVLINK_MODE: -//OWEND USBD_GetString ((uint8_t*)USBD_CDC_CONFIGURATION_FS_STRING, USBD_StrDesc, length); break; case USB_MASS_STORAGE_MODE: @@ -294,9 +288,7 @@ uint8_t * USBD_USR_InterfaceStrDescriptor( uint8_t speed , uint16_t *length) USBD_GetString ((uint8_t*)USBD_HID_INTERFACE_FS_STRING, USBD_StrDesc, length); break; case USB_SERIAL_MODE: -//OW case USB_MAVLINK_MODE: -//OWEND USBD_GetString ((uint8_t*)USBD_CDC_INTERFACE_FS_STRING, USBD_StrDesc, length); break; case USB_MASS_STORAGE_MODE: @@ -306,4 +298,4 @@ uint8_t * USBD_USR_InterfaceStrDescriptor( uint8_t speed , uint16_t *length) return USBD_StrDesc; } -/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ \ No newline at end of file +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/radio/src/targets/horus/board.h b/radio/src/targets/horus/board.h index daf9e5d8a18..4165e54a204 100644 --- a/radio/src/targets/horus/board.h +++ b/radio/src/targets/horus/board.h @@ -153,7 +153,6 @@ void SDRAM_Init(); #define INTERNAL_MODULE_OFF() GPIO_ResetBits(INTMODULE_PWR_GPIO, INTMODULE_PWR_GPIO_PIN) #endif -//OW //#define EXTERNAL_MODULE_ON() GPIO_SetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) //#define EXTERNAL_MODULE_OFF() GPIO_ResetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) //#define IS_INTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(INTMODULE_PWR_GPIO, INTMODULE_PWR_GPIO_PIN) == Bit_SET) @@ -171,7 +170,6 @@ void SDRAM_Init(); #define IS_INTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(INTMODULE_PWR_GPIO, INTMODULE_PWR_GPIO_PIN) == Bit_SET) #define IS_EXTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) == Bit_SET) #endif -//OWEND #if !defined(PXX2) #define IS_PXX2_INTERNAL_ENABLED() (false) diff --git a/radio/src/targets/horus/hal.h b/radio/src/targets/horus/hal.h index 6c0f969a9b9..47f180a66ce 100644 --- a/radio/src/targets/horus/hal.h +++ b/radio/src/targets/horus/hal.h @@ -479,7 +479,6 @@ #define AUX2_SERIAL_RCC_APB2Periph 0 #endif -//OW #if (defined(RADIO_T16) || defined(RADIO_T18)) && defined(AUX_SERIAL) #undef AUX_SERIAL_RCC_AHB1Periph #undef AUX_SERIAL_RCC_APB1Periph @@ -520,7 +519,6 @@ #define GPS_PWR_GPIO GPIOB #define GPS_PWR_GPIO_PIN GPIO_Pin_0 // PB.00 #endif -//OWEND // Telemetry #define TELEMETRY_RCC_AHB1Periph (RCC_AHB1Periph_GPIOD | RCC_AHB1Periph_DMA1) diff --git a/radio/src/targets/horus/telemetry_driver.cpp b/radio/src/targets/horus/telemetry_driver.cpp index 93a04492325..31473acb7ad 100644 --- a/radio/src/targets/horus/telemetry_driver.cpp +++ b/radio/src/targets/horus/telemetry_driver.cpp @@ -28,10 +28,8 @@ DMAFifo telemetryDMAFifo __DMA (TELEMETRY_DMA_Stream_RX); uint8_t telemetryFifoMode; #endif -//OW extern Fifo mavlinkTelemExternalTxFifo_frame; extern Fifo mavlinkTelemExternalRxFifo; -//OWEND static void telemetryInitDirPin() { @@ -47,11 +45,9 @@ static void telemetryInitDirPin() void telemetryPortInit(uint32_t baudrate, uint8_t mode) { -//OW #if defined(TELEMETRY_MAVLINK) if (g_eeGeneral.mavlinkExternal == 1) return; #endif -//OWEND if (baudrate == 0) { USART_DeInit(TELEMETRY_USART); return; @@ -154,11 +150,9 @@ static uint16_t probeTimeFromStartBit; void telemetryPortInvertedInit(uint32_t baudrate) { -//OW #if defined(TELEMETRY_MAVLINK) if (g_eeGeneral.mavlinkExternal == 1) return; #endif -//OWEND if (baudrate == 0) { NVIC_DisableIRQ(TELEMETRY_EXTI_IRQn); NVIC_DisableIRQ(TELEMETRY_TIMER_IRQn); @@ -256,11 +250,9 @@ void telemetryPortInvertedRxBit() void telemetryPortSetDirectionOutput() { -//OW #if defined(TELEMETRY_MAVLINK) if (g_eeGeneral.mavlinkExternal == 1) return; #endif -//OWEND #if defined(GHOST) && SPORT_MAX_BAUDRATE < 400000 if (isModuleGhost(EXTERNAL_MODULE)) { TELEMETRY_USART->BRR = BRR_400K; @@ -277,11 +269,9 @@ void sportWaitTransmissionComplete() void telemetryPortSetDirectionInput() { -//OW #if defined(TELEMETRY_MAVLINK) if (g_eeGeneral.mavlinkExternal == 1) return; #endif -//OWEND sportWaitTransmissionComplete(); #if defined(GHOST) && SPORT_MAX_BAUDRATE < 400000 if (isModuleGhost(EXTERNAL_MODULE) && g_eeGeneral.telemetryBaudrate == GHST_TELEMETRY_RATE_115K) { @@ -294,11 +284,9 @@ void telemetryPortSetDirectionInput() void sportSendByte(uint8_t byte) { -//OW #if defined(TELEMETRY_MAVLINK) if (g_eeGeneral.mavlinkExternal == 1) return; #endif -//OWEND telemetryPortSetDirectionOutput(); while (!(TELEMETRY_USART->SR & USART_SR_TXE)); @@ -307,11 +295,9 @@ void sportSendByte(uint8_t byte) void sportSendByteLoop(uint8_t byte) { -//OW #if defined(TELEMETRY_MAVLINK) if (g_eeGeneral.mavlinkExternal == 1) return; #endif -//OWEND telemetryPortSetDirectionOutput(); outputTelemetryBuffer.data[0] = byte; @@ -340,11 +326,9 @@ void sportSendByteLoop(uint8_t byte) void sportSendBuffer(const uint8_t * buffer, uint32_t count) { -//OW #if defined(TELEMETRY_MAVLINK) if (g_eeGeneral.mavlinkExternal == 1) return; #endif -//OWEND telemetryPortSetDirectionOutput(); DMA_InitTypeDef DMA_InitStructure; @@ -391,7 +375,6 @@ extern "C" void TELEMETRY_USART_IRQHandler(void) { DEBUG_INTERRUPT(INT_TELEM_USART); -//OW #if defined(TELEMETRY_MAVLINK) if (g_eeGeneral.mavlinkExternal == 1) { @@ -426,7 +409,6 @@ extern "C" void TELEMETRY_USART_IRQHandler(void) return; } #endif -//OWEND uint32_t status = TELEMETRY_USART->SR; diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp index 57777f6b064..998c057e726 100644 --- a/radio/src/tasks.cpp +++ b/radio/src/tasks.cpp @@ -41,11 +41,9 @@ void stackPaint() #if defined(CLI) cliStack.paint(); #endif -//OW #if defined(TELEMETRY_MAVLINK) mavlinkStack.paint(); #endif -//OWEND } volatile uint16_t timeForcePowerOffPressed = 0; @@ -333,11 +331,9 @@ void tasksStart() RTOS_CREATE_TASK(audioTaskId, audioTask, "audio", audioStack, AUDIO_STACK_SIZE, AUDIO_TASK_PRIO); #endif -//OW #if defined(TELEMETRY_MAVLINK) mavlinkStart(); #endif -//OWEND RTOS_START(); } diff --git a/radio/src/tasks.h b/radio/src/tasks.h index 87f2990151e..ee62c54e4bc 100644 --- a/radio/src/tasks.h +++ b/radio/src/tasks.h @@ -49,7 +49,6 @@ extern RTOS_DEFINE_STACK(audioStack, AUDIO_STACK_SIZE); extern RTOS_MUTEX_HANDLE mixerMutex; -//OW #if defined(TELEMETRY_MAVLINK) #define MAVLINK_STACK_SIZE 400 //consumes 4x #define MAVLINK_TASK_PRIO 8 @@ -57,7 +56,6 @@ extern RTOS_MUTEX_HANDLE mixerMutex; extern RTOS_TASK_HANDLE mavlinkTaskId; extern RTOS_DEFINE_STACK(mavlinkStack, MAVLINK_STACK_SIZE); #endif -//OWEND void stackPaint(); void tasksStart(); diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index eefb062f1ba..7cae4d7f59f 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -27,10 +27,8 @@ #include "libopenui.h" #endif -//OW //uint8_t telemetryStreaming = 0; uint16_t telemetryStreaming = 0; -//OWEND uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1) uint8_t telemetryRxBufferCount = 0; @@ -207,15 +205,11 @@ void telemetryWakeup() } #endif -//OW #if defined(TELEMETRY_MAVLINK) if (!g_model.rssiAlarms.disabled && mavlinkTelem.telemetryVoiceEnabled()) { #else -//OWEND if (!g_model.rssiAlarms.disabled) { -//OW #endif -//OWEND if (TELEMETRY_STREAMING()) { if (TELEMETRY_RSSI() < g_model.rssiAlarms.getCriticalRssi() ) { AUDIO_RSSI_RED(); diff --git a/radio/src/telemetry/telemetry.h b/radio/src/telemetry/telemetry.h index e4121008189..4e232f3fe10 100644 --- a/radio/src/telemetry/telemetry.h +++ b/radio/src/telemetry/telemetry.h @@ -41,10 +41,8 @@ #include "flysky_ibus.h" #endif -//OW //extern uint8_t telemetryStreaming; // >0 (true) == data is streaming in. 0 = no data detected for some time extern uint16_t telemetryStreaming; // >0 (true) == data is streaming in. 0 = no data detected for some time -//OWEND inline bool TELEMETRY_STREAMING() { diff --git a/radio/src/thirdparty/Lua/src/lauxlib.h b/radio/src/thirdparty/Lua/src/lauxlib.h index 1b6216a54e7..ea57b97a281 100644 --- a/radio/src/thirdparty/Lua/src/lauxlib.h +++ b/radio/src/thirdparty/Lua/src/lauxlib.h @@ -34,12 +34,10 @@ extern const luaL_Reg tab_funcs[]; extern const luaL_Reg opentxLib[]; extern const luaL_Reg lcdLib[]; extern const luaL_Reg modelLib[]; -//OW #if defined(TELEMETRY_MAVLINK) extern const luaL_Reg mavlinkLib[]; extern const luaL_Reg mavsdkLib[]; #endif -//OWEND LUALIB_API void (luaL_checkversion_) (lua_State *L, lua_Number ver); #define luaL_checkversion(L) luaL_checkversion_(L, LUA_VERSION_NUM) diff --git a/radio/src/thirdparty/Lua/src/linit.c b/radio/src/thirdparty/Lua/src/linit.c index 8ed7b98eae3..b93426e7bd6 100644 --- a/radio/src/thirdparty/Lua/src/linit.c +++ b/radio/src/thirdparty/Lua/src/linit.c @@ -49,12 +49,10 @@ const luaR_table lua_rotable[] = {"lcd", lcdLib, NULL}, {"model", modelLib, NULL}, {"__baselib", baselib, NULL}, -//OW #if defined(TELEMETRY_MAVLINK) {"mavlink", mavlinkLib, mavlinkConstants}, {"mavsdk", mavsdkLib, mavsdkConstants}, #endif -//OWEND {LUA_IOLIBNAME, iolib, NULL}, {LUA_STRLIBNAME, strlib, NULL}, {LUA_MATHLIBNAME, mathlib, mathlib_vals}, diff --git a/radio/src/thirdparty/Lua/src/lrotable.h b/radio/src/thirdparty/Lua/src/lrotable.h index 9c92e412305..90772d87fd3 100644 --- a/radio/src/thirdparty/Lua/src/lrotable.h +++ b/radio/src/thirdparty/Lua/src/lrotable.h @@ -20,12 +20,10 @@ typedef struct extern const luaR_value_entry baselib_vals[]; extern const luaR_value_entry mathlib_vals[]; extern const luaR_value_entry opentxConstants[]; -//OW #if defined(TELEMETRY_MAVLINK) extern const luaR_value_entry mavlinkConstants[]; extern const luaR_value_entry mavsdkConstants[]; #endif -//OWEND // A mapping between table name and its entries typedef struct diff --git a/radio/src/translations.cpp b/radio/src/translations.cpp index b99f38f146c..006adac1cae 100644 --- a/radio/src/translations.cpp +++ b/radio/src/translations.cpp @@ -973,7 +973,6 @@ const char STR_AUTH_FAILURE[] = TR_AUTH_FAILURE; const char STR_PROTOCOL[] = TR_PROTOCOL; const char STR_RACING_MODE[] = TR_RACING_MODE; -//OW #if defined(TELEMETRY_MAVLINK) const char STR_MAVLINK[] = TR_MAVLINK; const char STR_MAVLINK_RSSI[] = TR_MAVLINK_RSSI; @@ -985,4 +984,3 @@ ISTR(MAVLINK_AUX_BAUDRATES); const char STR_MAVLINK_EXTERNAL[] = TR_MAVLINK_EXTERNAL; const char STR_USB_MAVLINK[] = TR_USB_MAVLINK; #endif -//OWEND diff --git a/radio/src/translations.h b/radio/src/translations.h index 4173422e56d..218f2a8c44d 100644 --- a/radio/src/translations.h +++ b/radio/src/translations.h @@ -1113,7 +1113,6 @@ extern const char STR_RACING_MODE[]; #define CHR_HOUR TR_CHR_HOUR #define CHR_INPUT TR_CHR_INPUT -//OW #if defined(TELEMETRY_MAVLINK) extern const char STR_MAVLINK[]; extern const char STR_MAVLINK_RSSI[]; @@ -1125,6 +1124,5 @@ extern const char STR_MAVLINK_AUX_BAUDRATES[]; extern const char STR_MAVLINK_EXTERNAL[]; extern const char STR_USB_MAVLINK[]; #endif -//OWEND #endif // _TRANSLATIONS_H_ diff --git a/radio/src/translations/untranslated.h b/radio/src/translations/untranslated.h index a62b6f04852..307a785d771 100644 --- a/radio/src/translations/untranslated.h +++ b/radio/src/translations/untranslated.h @@ -318,7 +318,6 @@ #define CHAR_TELEMETRY STR_CHAR_TELEMETRY[0] #define CHAR_LUA STR_CHAR_LUA[0] -//OW #define TR_MAVLINK "Mavlink" #define TR_MAVLINK_RSSI "Rssi" #define TR_MAVLINK_RSSI_SCALE "Rssi Scale" @@ -330,4 +329,3 @@ #define TR_MAVLINK_AUX_BAUDRATES "57600\0""115200""38400\0""19200\0" #define TR_MAVLINK_EXTERNAL "Mavlink external" #define TR_USB_MAVLINK "USB Mavlink (VCP)" -//OWEND From 0bdb439ee73740f119a775b2a94cdbb7bf1f4a01 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Sat, 5 Jun 2021 09:57:33 +0200 Subject: [PATCH 04/16] Removed more of Olli's markers Command used on OSX: `sed -i.tmp "/^# \/\/OW/d" $(grep -l -rI "//OW" *)` --- radio/src/targets/horus/CMakeLists.txt | 8 -------- radio/src/telemetry/mavlink/mavlink_telem.h | 2 +- 2 files changed, 1 insertion(+), 9 deletions(-) diff --git a/radio/src/targets/horus/CMakeLists.txt b/radio/src/targets/horus/CMakeLists.txt index 17144a392f2..9446c1e25f2 100644 --- a/radio/src/targets/horus/CMakeLists.txt +++ b/radio/src/targets/horus/CMakeLists.txt @@ -47,7 +47,6 @@ endif() add_definitions(-DPCBFRSKY) -# //OW if (TELEMETRY_MAVLINK) if (DEBUG OR CLI) #set(TELEMETRY_MAVLINK NO) @@ -77,7 +76,6 @@ if (TELEMETRY_MAVLINK) message(FATAL_ERROR "fmav_generate_lua_lib.py could not be run: ${FMAV_GENERATE_LUALIB_RESULT}") endif() endif() -# //OWEND if (PCB STREQUAL X10) set(PCBREV "STD" CACHE STRING "PCB Revision") @@ -113,7 +111,6 @@ if (PCB STREQUAL X10) option(INTERNAL_MODULE_MULTI "Support for MULTI internal module" OFF) option(BLUETOOTH "Support for bluetooth module" OFF) add_definitions(-DMANUFACTURER_JUMPER) -# //OW if (TELEMETRY_MAVLINK) option(INTERNAL_GPS "Support for internal ublox GPS" OFF) set(AUX_SERIAL ON) @@ -122,7 +119,6 @@ if (PCB STREQUAL X10) endif() set(INTERNAL_GPS_BAUDRATE "9600" CACHE STRING "Baud rate for internal GPS") endif() -# //OWEND elseif (PCBREV STREQUAL TX16S) set(FLAVOUR tx16s) set(LUA_EXPORT lua_export_t16) @@ -150,7 +146,6 @@ if (PCB STREQUAL X10) option(BLUETOOTH "Support for bluetooth module" OFF) set(HARDWARE_TOUCH YES) add_definitions(-DMANUFACTURER_JUMPER) -# //OW if (TELEMETRY_MAVLINK) option(INTERNAL_GPS "Support for internal ublox GPS" OFF) set(AUX_SERIAL ON) @@ -159,7 +154,6 @@ if (PCB STREQUAL X10) endif() set(INTERNAL_GPS_BAUDRATE "9600" CACHE STRING "Baud rate for internal GPS") endif() -# //OWEND else() set(FLAVOUR x10) option(INTERNAL_MODULE_PXX1 "Support for PXX1 internal module" ON) @@ -323,7 +317,6 @@ if (SBUS_TRAINER) ) endif() -# //OW if (TELEMETRY_MAVLINK) set(SRC ${SRC} telemetry/mavlink/mavlink_telem.cpp @@ -338,7 +331,6 @@ if (TELEMETRY_MAVLINK) set(SRC ${SRC} lua/api_mavlink.cpp lua/api_mavsdk.cpp) endif() endif() -# //OWEND set(FIRMWARE_TARGET_SRC ${FIRMWARE_TARGET_SRC} diff --git a/radio/src/telemetry/mavlink/mavlink_telem.h b/radio/src/telemetry/mavlink/mavlink_telem.h index 7c7c656fe25..8b57253700c 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem.h +++ b/radio/src/telemetry/mavlink/mavlink_telem.h @@ -2,7 +2,7 @@ * (c) www.olliw.eu, OlliW, OlliW42 */ -#define MAVLINKTELEMVERSIONSTR "v27" //OW +#define MAVLINKTELEMVERSIONSTR "v27" #define MAVLINK_RAM_SECTION __attribute__((section (".ram"))) From 21d157001bb4db480ef5aaf4f5c6bc1cb04838df Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 8 Jun 2021 07:15:51 +0200 Subject: [PATCH 05/16] Use GPLv2 boilerplate --- radio/src/lua/api_mavlink.cpp | 21 ++++++++++++++++- radio/src/lua/api_mavsdk.cpp | 21 ++++++++++++++++- radio/src/telemetry/mavlink/mavlink_telem.cpp | 19 +++++++++++++++ radio/src/telemetry/mavlink/mavlink_telem.h | 19 +++++++++++++++ .../mavlink/mavlink_telem_autopilot.cpp | 19 +++++++++++++++ .../mavlink/mavlink_telem_camera.cpp | 19 +++++++++++++++ .../mavlink/mavlink_telem_gimbal.cpp | 19 +++++++++++++++ .../mavlink/mavlink_telem_interface.cpp | 19 +++++++++++++++ .../mavlink/mavlink_telem_mavapi.cpp | 23 +++++++++++++++++-- .../telemetry/mavlink/mavlink_telem_qshot.cpp | 19 +++++++++++++++ 10 files changed, 194 insertions(+), 4 deletions(-) diff --git a/radio/src/lua/api_mavlink.cpp b/radio/src/lua/api_mavlink.cpp index fea8774605c..61f35325f4e 100644 --- a/radio/src/lua/api_mavlink.cpp +++ b/radio/src/lua/api_mavlink.cpp @@ -1,5 +1,24 @@ /* - * (c) www.olliw.eu, OlliW, OlliW42 + * 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 diff --git a/radio/src/lua/api_mavsdk.cpp b/radio/src/lua/api_mavsdk.cpp index aa27cabab02..71196776460 100644 --- a/radio/src/lua/api_mavsdk.cpp +++ b/radio/src/lua/api_mavsdk.cpp @@ -1,5 +1,24 @@ /* - * (c) www.olliw.eu, OlliW, OlliW42 + * 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 diff --git a/radio/src/telemetry/mavlink/mavlink_telem.cpp b/radio/src/telemetry/mavlink/mavlink_telem.cpp index 0dc0e9bdefd..a358b3824c6 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem.cpp @@ -1,5 +1,24 @@ /* + * 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 "opentx.h" diff --git a/radio/src/telemetry/mavlink/mavlink_telem.h b/radio/src/telemetry/mavlink/mavlink_telem.h index 8b57253700c..7da563e3491 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem.h +++ b/radio/src/telemetry/mavlink/mavlink_telem.h @@ -1,5 +1,24 @@ /* + * 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. */ #define MAVLINKTELEMVERSIONSTR "v27" diff --git a/radio/src/telemetry/mavlink/mavlink_telem_autopilot.cpp b/radio/src/telemetry/mavlink/mavlink_telem_autopilot.cpp index 57b8d5265f3..2db25636bcc 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem_autopilot.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem_autopilot.cpp @@ -1,5 +1,24 @@ /* + * 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 "opentx.h" diff --git a/radio/src/telemetry/mavlink/mavlink_telem_camera.cpp b/radio/src/telemetry/mavlink/mavlink_telem_camera.cpp index d8444857290..91cb963c258 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem_camera.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem_camera.cpp @@ -1,5 +1,24 @@ /* + * 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 "opentx.h" diff --git a/radio/src/telemetry/mavlink/mavlink_telem_gimbal.cpp b/radio/src/telemetry/mavlink/mavlink_telem_gimbal.cpp index b6492b7f800..8c5012b3e89 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem_gimbal.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem_gimbal.cpp @@ -1,5 +1,24 @@ /* + * 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 "opentx.h" diff --git a/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp b/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp index 72d363a632f..7b47409c5b3 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp @@ -1,5 +1,24 @@ /* + * 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 "opentx.h" diff --git a/radio/src/telemetry/mavlink/mavlink_telem_mavapi.cpp b/radio/src/telemetry/mavlink/mavlink_telem_mavapi.cpp index f1e51bb2762..7e11c9afd18 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem_mavapi.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem_mavapi.cpp @@ -1,6 +1,25 @@ /* -* (c) www.olliw.eu, OlliW, OlliW42 -*/ + * 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 "opentx.h" diff --git a/radio/src/telemetry/mavlink/mavlink_telem_qshot.cpp b/radio/src/telemetry/mavlink/mavlink_telem_qshot.cpp index 72850ee8d39..fc1f6824b42 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem_qshot.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem_qshot.cpp @@ -1,5 +1,24 @@ /* + * 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 "opentx.h" From 263f1d65df49453c93854659443c773303030e30 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 8 Jun 2021 07:29:21 +0200 Subject: [PATCH 06/16] Removed USB_MAVLINK_MODE --- radio/src/main.cpp | 15 +-------------- radio/src/targets/common/arm/stm32/usb_driver.cpp | 1 - radio/src/targets/common/arm/stm32/usb_driver.h | 1 - radio/src/targets/common/arm/stm32/usbd_desc.c | 4 ---- radio/src/telemetry/mavlink/mavlink_telem.cpp | 2 +- .../telemetry/mavlink/mavlink_telem_interface.cpp | 10 +++++++--- 6 files changed, 9 insertions(+), 24 deletions(-) diff --git a/radio/src/main.cpp b/radio/src/main.cpp index cb71bbd348e..765283503a2 100644 --- a/radio/src/main.cpp +++ b/radio/src/main.cpp @@ -70,12 +70,7 @@ void openUsbMenu() TRACE("USB mass storage"); setSelectedUsbMode(USB_MASS_STORAGE_MODE); }); -#if defined(TELEMETRY_MAVLINK_USB_SERIAL) - _usbMenu->addLine(STR_USB_MAVLINK, [] { - setSelectedUsbMode(USB_MAVLINK_MODE); - }); -#endif -#if defined(DEBUG) +#if defined(USB_SERIAL) _usbMenu->addLine(STR_USB_SERIAL, [] { TRACE("USB serial"); setSelectedUsbMode(USB_SERIAL_MODE); @@ -93,11 +88,6 @@ void onUSBConnectMenu(const char *result) else if (result == STR_USB_JOYSTICK) { setSelectedUsbMode(USB_JOYSTICK_MODE); } -#if defined(TELEMETRY_MAVLINK_USB_SERIAL) - else if (result == STR_USB_MAVLINK) { - setSelectedUsbMode(USB_MAVLINK_MODE); - } -#endif else if (result == STR_USB_SERIAL) { setSelectedUsbMode(USB_SERIAL_MODE); } @@ -111,9 +101,6 @@ void openUsbMenu() if (popupMenuHandler != onUSBConnectMenu) { POPUP_MENU_ADD_ITEM(STR_USB_JOYSTICK); POPUP_MENU_ADD_ITEM(STR_USB_MASS_STORAGE); -#if defined(TELEMETRY_MAVLINK_USB_SERIAL) - POPUP_MENU_ADD_ITEM(USB_MAVLINK_MODE); -#endif #if defined(DEBUG) POPUP_MENU_ADD_ITEM(STR_USB_SERIAL); #endif diff --git a/radio/src/targets/common/arm/stm32/usb_driver.cpp b/radio/src/targets/common/arm/stm32/usb_driver.cpp index e61caf5186e..7e2c5abf1dd 100644 --- a/radio/src/targets/common/arm/stm32/usb_driver.cpp +++ b/radio/src/targets/common/arm/stm32/usb_driver.cpp @@ -79,7 +79,6 @@ void usbStart() #endif #if defined(USB_SERIAL) case USB_SERIAL_MODE: - case USB_MAVLINK_MODE: // initialize USB as CDC device (virtual serial port) USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb); break; diff --git a/radio/src/targets/common/arm/stm32/usb_driver.h b/radio/src/targets/common/arm/stm32/usb_driver.h index fa2d92113e8..350ceace0d2 100644 --- a/radio/src/targets/common/arm/stm32/usb_driver.h +++ b/radio/src/targets/common/arm/stm32/usb_driver.h @@ -29,7 +29,6 @@ enum usbMode { USB_JOYSTICK_MODE, USB_MASS_STORAGE_MODE, USB_SERIAL_MODE, - USB_MAVLINK_MODE, #if defined(USB_SERIAL) USB_MAX_MODE=USB_SERIAL_MODE #else diff --git a/radio/src/targets/common/arm/stm32/usbd_desc.c b/radio/src/targets/common/arm/stm32/usbd_desc.c index 066aae8490b..65f02da6b97 100644 --- a/radio/src/targets/common/arm/stm32/usbd_desc.c +++ b/radio/src/targets/common/arm/stm32/usbd_desc.c @@ -140,7 +140,6 @@ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length) break; case USB_SERIAL_MODE: - case USB_MAVLINK_MODE: vid = USBD_CDC_VID; pid = USBD_CDC_PID; break; @@ -212,7 +211,6 @@ uint8_t * USBD_USR_ProductStrDescriptor( uint8_t speed , uint16_t *length) USBD_GetString ((uint8_t*)USBD_HID_PRODUCT_FS_STRING, USBD_StrDesc, length); break; case USB_SERIAL_MODE: - case USB_MAVLINK_MODE: USBD_GetString ((uint8_t*)USBD_CDC_PRODUCT_FS_STRING, USBD_StrDesc, length); break; case USB_MASS_STORAGE_MODE: @@ -263,7 +261,6 @@ uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length) USBD_GetString ((uint8_t*)USBD_HID_CONFIGURATION_FS_STRING, USBD_StrDesc, length); break; case USB_SERIAL_MODE: - case USB_MAVLINK_MODE: USBD_GetString ((uint8_t*)USBD_CDC_CONFIGURATION_FS_STRING, USBD_StrDesc, length); break; case USB_MASS_STORAGE_MODE: @@ -288,7 +285,6 @@ uint8_t * USBD_USR_InterfaceStrDescriptor( uint8_t speed , uint16_t *length) USBD_GetString ((uint8_t*)USBD_HID_INTERFACE_FS_STRING, USBD_StrDesc, length); break; case USB_SERIAL_MODE: - case USB_MAVLINK_MODE: USBD_GetString ((uint8_t*)USBD_CDC_INTERFACE_FS_STRING, USBD_StrDesc, length); break; case USB_MASS_STORAGE_MODE: diff --git a/radio/src/telemetry/mavlink/mavlink_telem.cpp b/radio/src/telemetry/mavlink/mavlink_telem.cpp index a358b3824c6..63cf7b8f075 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem.cpp @@ -437,7 +437,7 @@ void MavlinkTelem::wakeup() bool aux1_enabled = (g_eeGeneral.auxSerialMode == UART_MODE_MAVLINK); bool aux2_enabled = (g_eeGeneral.aux2SerialMode == UART_MODE_MAVLINK); #if defined(TELEMETRY_MAVLINK_USB_SERIAL) - bool usb_enabled = (getSelectedUsbMode() == USB_MAVLINK_MODE); + bool usb_enabled = (getSelectedUsbMode() == USB_SERIAL_MODE); #else bool usb_enabled = false; #endif diff --git a/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp b/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp index 7b47409c5b3..ce5587f42a1 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp @@ -372,11 +372,13 @@ bool mavlinkTelem2PutBuf(const uint8_t* buf, const uint16_t count){ return false // -- USB handlers -- +//TODO: is it really necessary to have this extra define? #if defined(TELEMETRY_MAVLINK_USB_SERIAL) uint32_t mavlinkTelem3Available(void) { - if (getSelectedUsbMode() != USB_MAVLINK_MODE) return 0; + // TODO: check if MAVLink has been assigned to USB serial + if (getSelectedUsbMode() != USB_SERIAL_MODE) return 0; return mavlinkTelemUsbRxFifo.size(); } @@ -388,13 +390,15 @@ uint8_t mavlinkTelem3Getc(uint8_t* c) bool mavlinkTelem3HasSpace(uint16_t count) { - if (getSelectedUsbMode() != USB_MAVLINK_MODE) return false; + // TODO: check if MAVLink has been assigned to USB serial + if (getSelectedUsbMode() != USB_SERIAL_MODE) return false; return true; //?? } bool mavlinkTelem3PutBuf(const uint8_t* buf, const uint16_t count) { - if (getSelectedUsbMode() != USB_MAVLINK_MODE || !buf) { + // TODO: check if MAVLink has been assigned to USB serial + if (getSelectedUsbMode() != USB_SERIAL_MODE || !buf) { return false; } for (uint16_t i = 0; i < count; i++) { From 4b4ca212c24741ddd68cb24afe47d29eec9e8f72 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 8 Jun 2021 07:52:18 +0200 Subject: [PATCH 07/16] Moved Mavlink include away from opentx.h --- radio/src/lua/api_mavlink.cpp | 1 + radio/src/lua/api_mavsdk.cpp | 2 +- radio/src/opentx.h | 4 ---- radio/src/tasks.cpp | 4 ++++ radio/src/telemetry/mavlink/mavlink_telem.cpp | 1 + radio/src/telemetry/mavlink/mavlink_telem_autopilot.cpp | 1 + radio/src/telemetry/mavlink/mavlink_telem_camera.cpp | 1 + radio/src/telemetry/mavlink/mavlink_telem_gimbal.cpp | 1 + radio/src/telemetry/mavlink/mavlink_telem_interface.cpp | 1 + radio/src/telemetry/mavlink/mavlink_telem_mavapi.cpp | 1 + radio/src/telemetry/mavlink/mavlink_telem_qshot.cpp | 1 + radio/src/telemetry/telemetry.cpp | 4 ++++ 12 files changed, 17 insertions(+), 5 deletions(-) diff --git a/radio/src/lua/api_mavlink.cpp b/radio/src/lua/api_mavlink.cpp index 61f35325f4e..a4a5a1c3c19 100644 --- a/radio/src/lua/api_mavlink.cpp +++ b/radio/src/lua/api_mavlink.cpp @@ -25,6 +25,7 @@ #include #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" diff --git a/radio/src/lua/api_mavsdk.cpp b/radio/src/lua/api_mavsdk.cpp index 71196776460..1d54281f252 100644 --- a/radio/src/lua/api_mavsdk.cpp +++ b/radio/src/lua/api_mavsdk.cpp @@ -25,7 +25,7 @@ #include #include "opentx.h" #include "lua_api.h" - +#include "telemetry/mavlink/mavlink_telem.h" constexpr float FPI = 3.141592653589793f; constexpr float FDEGTORAD = FPI/180.0f; diff --git a/radio/src/opentx.h b/radio/src/opentx.h index 5540ef77260..8cecea28ced 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -1016,10 +1016,6 @@ constexpr uint8_t SD_SCREEN_FILE_LENGTH = 64; #include "bluetooth.h" #endif -#if defined(TELEMETRY_MAVLINK) -#include "telemetry/mavlink/mavlink_telem.h" -#endif - constexpr uint8_t TEXT_FILENAME_MAXLEN = 40; union ReusableBuffer diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp index 998c057e726..0700424401e 100644 --- a/radio/src/tasks.cpp +++ b/radio/src/tasks.cpp @@ -21,6 +21,10 @@ #include "opentx.h" #include "mixer_scheduler.h" +#if defined(TELEMETRY_MAVLINK) +#include "telemetry/mavlink/mavlink_telem.h" +#endif + RTOS_TASK_HANDLE menusTaskId; RTOS_DEFINE_STACK(menusStack, MENUS_STACK_SIZE); diff --git a/radio/src/telemetry/mavlink/mavlink_telem.cpp b/radio/src/telemetry/mavlink/mavlink_telem.cpp index 63cf7b8f075..37f911028d7 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem.cpp @@ -22,6 +22,7 @@ */ #include "opentx.h" +#include "telemetry/mavlink/mavlink_telem.h" MAVLINK_RAM_SECTION MavlinkTelem mavlinkTelem; diff --git a/radio/src/telemetry/mavlink/mavlink_telem_autopilot.cpp b/radio/src/telemetry/mavlink/mavlink_telem_autopilot.cpp index 2db25636bcc..bd8a15ba3ba 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem_autopilot.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem_autopilot.cpp @@ -22,6 +22,7 @@ */ #include "opentx.h" +#include "telemetry/mavlink/mavlink_telem.h" constexpr float FPI = 3.141592653589793f; constexpr float FDEGTORAD = FPI/180.0f; diff --git a/radio/src/telemetry/mavlink/mavlink_telem_camera.cpp b/radio/src/telemetry/mavlink/mavlink_telem_camera.cpp index 91cb963c258..b28c4d15a26 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem_camera.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem_camera.cpp @@ -22,6 +22,7 @@ */ #include "opentx.h" +#include "telemetry/mavlink/mavlink_telem.h" constexpr float FPI = 3.141592653589793f; constexpr float FDEGTORAD = FPI/180.0f; diff --git a/radio/src/telemetry/mavlink/mavlink_telem_gimbal.cpp b/radio/src/telemetry/mavlink/mavlink_telem_gimbal.cpp index 8c5012b3e89..3b6ca1d20e2 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem_gimbal.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem_gimbal.cpp @@ -22,6 +22,7 @@ */ #include "opentx.h" +#include "telemetry/mavlink/mavlink_telem.h" constexpr float FPI = 3.141592653589793f; constexpr float FDEGTORAD = FPI/180.0f; diff --git a/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp b/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp index ce5587f42a1..6c14ff71a31 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp @@ -22,6 +22,7 @@ */ #include "opentx.h" +#include "telemetry/mavlink/mavlink_telem.h" // -- CoOS RTOS mavlink task handlers -- diff --git a/radio/src/telemetry/mavlink/mavlink_telem_mavapi.cpp b/radio/src/telemetry/mavlink/mavlink_telem_mavapi.cpp index 7e11c9afd18..2c7aa9834b5 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem_mavapi.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem_mavapi.cpp @@ -22,6 +22,7 @@ */ #include "opentx.h" +#include "telemetry/mavlink/mavlink_telem.h" // -- Receive stuff -- diff --git a/radio/src/telemetry/mavlink/mavlink_telem_qshot.cpp b/radio/src/telemetry/mavlink/mavlink_telem_qshot.cpp index fc1f6824b42..5c98bde22ff 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem_qshot.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem_qshot.cpp @@ -22,6 +22,7 @@ */ #include "opentx.h" +#include "telemetry/mavlink/mavlink_telem.h" // -- Generate MAVLink messages -- // these should never be called directly, should only be called by the task handler diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index 7cae4d7f59f..edb47a9de78 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -27,6 +27,10 @@ #include "libopenui.h" #endif +#if defined(TELEMETRY_MAVLINK) +#include "telemetry/mavlink/mavlink_telem.h" +#endif + //uint8_t telemetryStreaming = 0; uint16_t telemetryStreaming = 0; uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1) From b1dbedd79e5b247b6ae01b6570133bde3d9101fd Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 8 Jun 2021 07:57:31 +0200 Subject: [PATCH 08/16] Removed unrelated statistics methods --- radio/src/lua/api_mavlink.cpp | 54 ----------------------------------- 1 file changed, 54 deletions(-) diff --git a/radio/src/lua/api_mavlink.cpp b/radio/src/lua/api_mavlink.cpp index a4a5a1c3c19..ed999f65a84 100644 --- a/radio/src/lua/api_mavlink.cpp +++ b/radio/src/lua/api_mavlink.cpp @@ -48,57 +48,6 @@ static int luaMavlinkGetChannelStatus(lua_State * L) return 1; } -//-- some statistics -- - -extern int _end; -extern int _heap_end; -extern unsigned char *heap; - -static int luaMavlinkGetMemUsed(lua_State * L) -{ - uint32_t s = luaGetMemUsed(lsScripts); -#if defined(COLORLCD) - uint32_t w = luaGetMemUsed(lsWidgets); - uint32_t e = luaExtraMemoryUsage; -#else - uint32_t w = 0; - uint32_t e = 0; -#endif - lua_createtable(L, 0, 6); - lua_pushtableinteger(L, "scripts", s); - lua_pushtableinteger(L, "widgets", w); - lua_pushtableinteger(L, "extra", e); - lua_pushtableinteger(L, "total", s+w+e); - lua_pushtableinteger(L, "heap_used", (int)(heap - (unsigned char *)&_end)); - lua_pushtableinteger(L, "heap_free", (int)((unsigned char *)&_heap_end - heap)); - return 1; -} - -static int luaMavlinkGetStackUsed(lua_State * L) -{ - lua_createtable(L, 0, 10); - lua_pushtableinteger(L, "main_available", stackAvailable()*4); - lua_pushtableinteger(L, "main_size", stackSize()*4); - lua_pushtableinteger(L, "menus_available", menusStack.available()*4); - lua_pushtableinteger(L, "menus_size", menusStack.size()); - lua_pushtableinteger(L, "mixer_available", mixerStack.available()*4); - lua_pushtableinteger(L, "mixer_size", mixerStack.size()); - lua_pushtableinteger(L, "audio_available", audioStack.available()*4); - lua_pushtableinteger(L, "audio_size", audioStack.size()); - lua_pushtableinteger(L, "mavlink_available", mavlinkStack.available()*4); - lua_pushtableinteger(L, "mavlink_size", mavlinkStack.size()); - return 1; -} - -static int luaMavlinkGetTaskStats(lua_State *L) -{ - lua_newtable(L); - lua_pushtableinteger(L, "time", mavlinkTaskRunTime()); - lua_pushtableinteger(L, "max", mavlinkTaskRunTimeMax()); - lua_pushtableinteger(L, "loop", mavlinkTaskLoop()); - return 1; -} - //-- mavlink api -- static int luaMavlinkGetSystemId(lua_State *L) @@ -215,9 +164,6 @@ static int luaMavlinkSendMessage(lua_State *L) const luaL_Reg mavlinkLib[] = { { "getVersion", luaMavlinkGetVersion }, { "getChannelStatus", luaMavlinkGetChannelStatus }, - { "getMemUsed", luaMavlinkGetMemUsed }, - { "getStackUsed", luaMavlinkGetStackUsed }, - { "getTaskStats", luaMavlinkGetTaskStats }, { "getSystemId", luaMavlinkGetSystemId }, { "getAutopilotIds", luaMavlinkGetAutopilotIds }, From b35df22a1b74584141e4fcf56cdd68a421c8fb22 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 8 Jun 2021 08:08:11 +0200 Subject: [PATCH 09/16] Added another missing include --- radio/src/opentx.cpp | 4 ++++ radio/src/targets/common/arm/stm32/aux_serial_driver.cpp | 1 + radio/src/targets/common/arm/stm32/usbd_cdc.cpp | 4 ++++ 3 files changed, 9 insertions(+) diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp index 7116d89f9a9..add6fd08b94 100644 --- a/radio/src/opentx.cpp +++ b/radio/src/opentx.cpp @@ -28,6 +28,10 @@ #include "view_main.h" #endif +#if defined(TELEMETRY_MAVLINK) +#include "telemetry/mavlink/mavlink_telem.h" +#endif + #if defined(PCBSKY9X) #include "audio_driver.h" #endif diff --git a/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp b/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp index 082d41f27c3..79d28206c2e 100644 --- a/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp +++ b/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp @@ -20,6 +20,7 @@ #include "opentx.h" #include "targets/horus/board.h" +#include "telemetry/mavlink/mavlink_telem.h" #if defined(AUX_SERIAL) uint8_t auxSerialMode = UART_MODE_COUNT; // Prevent debug output before port is setup diff --git a/radio/src/targets/common/arm/stm32/usbd_cdc.cpp b/radio/src/targets/common/arm/stm32/usbd_cdc.cpp index ce595ad9cbe..e59cbea21c5 100644 --- a/radio/src/targets/common/arm/stm32/usbd_cdc.cpp +++ b/radio/src/targets/common/arm/stm32/usbd_cdc.cpp @@ -24,6 +24,10 @@ #include "opentx.h" +#if defined(TELEMETRY_MAVLINK) +#include "telemetry/mavlink/mavlink_telem.h" +#endif + extern "C" { /* Includes ------------------------------------------------------------------*/ From 25c244e69f19c6ecdb68396cd0da888785cf73fb Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 8 Jun 2021 08:08:28 +0200 Subject: [PATCH 10/16] Cleanup Mavlink model settings --- radio/src/datastructs.h | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/radio/src/datastructs.h b/radio/src/datastructs.h index db2f8ed7fd9..a1203bf4378 100644 --- a/radio/src/datastructs.h +++ b/radio/src/datastructs.h @@ -652,14 +652,12 @@ PACK(struct ModelData { char modelRegistrationID[PXX2_LEN_REGISTRATION_ID]; #if defined(TELEMETRY_MAVLINK) - uint16_t _mavlinkEnabled:1; // currently not used - uint16_t mavlinkRssi:1; - uint16_t _mavlinkDummy:2; // currently not used - uint16_t mavlinkMimicSensors:3; // currently just off/on, but allow e.g. FrSky, CF, FrSky passthrough. - uint16_t mavlinkRcOverride:1; - uint16_t _mavlinkGpsIcon:1; // currently not used - uint8_t mavlinkRssiScale; - uint8_t _mavlinkDummy2; // currently not used + // 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 @@ -987,7 +985,7 @@ static inline void check_struct() #elif defined(PCBHORUS) #if defined(TELEMETRY_MAVLINK) CHKSIZE(RadioData, 902+2); - CHKSIZE(ModelData, 11020+4); + CHKSIZE(ModelData, 11020+2); #else CHKSIZE(RadioData, 902); CHKSIZE(ModelData, 11020); From 0ee8f07a2069b6b657cc972da39c465d0f316566 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 8 Jun 2021 09:47:52 +0200 Subject: [PATCH 11/16] Fixed conditional mavlink_telem.h --- radio/src/targets/common/arm/stm32/aux_serial_driver.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp b/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp index 79d28206c2e..724d0aa066b 100644 --- a/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp +++ b/radio/src/targets/common/arm/stm32/aux_serial_driver.cpp @@ -20,7 +20,10 @@ #include "opentx.h" #include "targets/horus/board.h" + +#if defined(TELEMETRY_MAVLINK) #include "telemetry/mavlink/mavlink_telem.h" +#endif #if defined(AUX_SERIAL) uint8_t auxSerialMode = UART_MODE_COUNT; // Prevent debug output before port is setup From 11843b8bdc6d6230397114968093c4613c247543 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 8 Jun 2021 10:04:27 +0200 Subject: [PATCH 12/16] Removed (HID) / (SD) / (Debug) off the USB modes strings --- radio/src/translations/cn.h.txt | 6 +++--- radio/src/translations/cz.h.txt | 6 +++--- radio/src/translations/de.h.txt | 6 +++--- radio/src/translations/en.h.txt | 6 +++--- radio/src/translations/es.h.txt | 6 +++--- radio/src/translations/fi.h.txt | 6 +++--- radio/src/translations/fr.h.txt | 6 +++--- radio/src/translations/it.h.txt | 6 +++--- radio/src/translations/nl.h.txt | 6 +++--- radio/src/translations/pl.h.txt | 6 +++--- radio/src/translations/pt.h.txt | 6 +++--- radio/src/translations/se.h.txt | 6 +++--- radio/src/translations/tw.h.txt | 6 +++--- 13 files changed, 39 insertions(+), 39 deletions(-) diff --git a/radio/src/translations/cn.h.txt b/radio/src/translations/cn.h.txt index 1f0e07dd3aa..1028d64023d 100644 --- a/radio/src/translations/cn.h.txt +++ b/radio/src/translations/cn.h.txt @@ -853,9 +853,9 @@ #define TR_RESET_TELEMETRY "复位回传参数" #define TR_STATISTICS "统计" #define TR_ABOUT_US "关于" -#define TR_USB_JOYSTICK "USB 游戏柄 (HID)" -#define TR_USB_MASS_STORAGE "USB 存储器 (SD)" -#define TR_USB_SERIAL "USB 串口 (调试)" +#define TR_USB_JOYSTICK "USB 游戏柄" +#define TR_USB_MASS_STORAGE "USB 存储器" +#define TR_USB_SERIAL "USB 串口" #define TR_SETUP_SCREENS "设置显示页面" #define TR_MONITOR_SCREENS "查看器" #define TR_AND_SWITCH "与开关" diff --git a/radio/src/translations/cz.h.txt b/radio/src/translations/cz.h.txt index 67ba25ac13c..dc82973ed62 100644 --- a/radio/src/translations/cz.h.txt +++ b/radio/src/translations/cz.h.txt @@ -942,9 +942,9 @@ #define TR_RESET_TELEMETRY "Telemetrii" #define TR_STATISTICS "Statistika" #define TR_ABOUT_US "O nás" -#define TR_USB_JOYSTICK "USB Joystick (HID)" -#define TR_USB_MASS_STORAGE "USB Disk (SD)" -#define TR_USB_SERIAL "USB Serial (Debug)" +#define TR_USB_JOYSTICK "USB Joystick" +#define TR_USB_MASS_STORAGE "USB Disk" +#define TR_USB_SERIAL "USB Serial" #define TR_SETUP_SCREENS "Obrazovky nastavení" #define TR_MONITOR_SCREENS "Monitory" #define TR_AND_SWITCH "AND Spínač" diff --git a/radio/src/translations/de.h.txt b/radio/src/translations/de.h.txt index 7486afd005c..c312f1b8842 100644 --- a/radio/src/translations/de.h.txt +++ b/radio/src/translations/de.h.txt @@ -947,9 +947,9 @@ #define TR_RESET_TELEMETRY "Reset Telemetrie" #define TR_STATISTICS "Statistik Timer Gas" #define TR_ABOUT_US "Die Programmierer" -#define TR_USB_JOYSTICK "USB Joystick (HID)" -#define TR_USB_MASS_STORAGE "USB Speicher (SD)" -#define TR_USB_SERIAL "USB Seriell (Debug)" +#define TR_USB_JOYSTICK "USB Joystick" +#define TR_USB_MASS_STORAGE "USB Speicher" +#define TR_USB_SERIAL "USB Seriell" #define TR_SETUP_SCREENS "Setup Hautbildschirme" #define TR_MONITOR_SCREENS "Monitore Mischer Kanal Logik" #define TR_AND_SWITCH "UND Schalt" //UND mit weiterem Schaltern diff --git a/radio/src/translations/en.h.txt b/radio/src/translations/en.h.txt index b990e448442..753ea60cddc 100644 --- a/radio/src/translations/en.h.txt +++ b/radio/src/translations/en.h.txt @@ -952,9 +952,9 @@ #define TR_RESET_TELEMETRY "Reset telemetry" #define TR_STATISTICS "Statistics" #define TR_ABOUT_US "About" -#define TR_USB_JOYSTICK "USB Joystick (HID)" -#define TR_USB_MASS_STORAGE "USB Storage (SD)" -#define TR_USB_SERIAL "USB Serial (Debug)" +#define TR_USB_JOYSTICK "USB Joystick" +#define TR_USB_MASS_STORAGE "USB Storage" +#define TR_USB_SERIAL "USB Serial" #define TR_SETUP_SCREENS "Setup screens" #define TR_MONITOR_SCREENS "Monitors" #define TR_AND_SWITCH "AND switch" diff --git a/radio/src/translations/es.h.txt b/radio/src/translations/es.h.txt index cdfdc068f22..2dc9fabfad0 100644 --- a/radio/src/translations/es.h.txt +++ b/radio/src/translations/es.h.txt @@ -945,9 +945,9 @@ #define TR_RESET_TELEMETRY "Reset Telemetría" #define TR_STATISTICS "Estadísticas" #define TR_ABOUT_US "Nosotros" -#define TR_USB_JOYSTICK "Joystick USB (HID)" -#define TR_USB_MASS_STORAGE "Almaz. USB (SD)" -#define TR_USB_SERIAL "Serie USB (Debug)" +#define TR_USB_JOYSTICK "Joystick USB" +#define TR_USB_MASS_STORAGE "Almaz. USB" +#define TR_USB_SERIAL "Serie USB" #define TR_SETUP_SCREENS "Pantallas config" #define TR_MONITOR_SCREENS "Monitores" #define TR_AND_SWITCH TR("Inter. AND", "Interruptor AND") diff --git a/radio/src/translations/fi.h.txt b/radio/src/translations/fi.h.txt index 17bd0b0c10f..4efab5b5729 100644 --- a/radio/src/translations/fi.h.txt +++ b/radio/src/translations/fi.h.txt @@ -963,9 +963,9 @@ #define TR_RESET_TELEMETRY "Reset Telemetry" #define TR_STATISTICS "Statistics" #define TR_ABOUT_US "About" -#define TR_USB_JOYSTICK "USB Joystick (HID)" -#define TR_USB_MASS_STORAGE "USB Storage (SD)" -#define TR_USB_SERIAL "USB Serial (Debug)" +#define TR_USB_JOYSTICK "USB Joystick" +#define TR_USB_MASS_STORAGE "USB Storage" +#define TR_USB_SERIAL "USB Serial" #define TR_SETUP_SCREENS "Setup screens" #define TR_MONITOR_SCREENS "Monitors" #define TR_AND_SWITCH "AND Switch" diff --git a/radio/src/translations/fr.h.txt b/radio/src/translations/fr.h.txt index d2ac7ff0408..93f3d5ba792 100644 --- a/radio/src/translations/fr.h.txt +++ b/radio/src/translations/fr.h.txt @@ -968,9 +968,9 @@ #define TR_RESET_TELEMETRY TR("Réinit. Télém.", "Réinit. Télémétrie") #define TR_STATISTICS "Statistiques" #define TR_ABOUT_US "A propos" -#define TR_USB_JOYSTICK "USB Joystick (HID)" -#define TR_USB_MASS_STORAGE "Stockage USB (SD)" -#define TR_USB_SERIAL "Port série (Debug)" +#define TR_USB_JOYSTICK "USB Joystick" +#define TR_USB_MASS_STORAGE "Stockage USB" +#define TR_USB_SERIAL "Port série" #define TR_SETUP_SCREENS "Configuration écrans" #define TR_MONITOR_SCREENS "Moniteurs" #define TR_AND_SWITCH "ET suppl." diff --git a/radio/src/translations/it.h.txt b/radio/src/translations/it.h.txt index db28fde805f..6cba6f754a7 100644 --- a/radio/src/translations/it.h.txt +++ b/radio/src/translations/it.h.txt @@ -969,9 +969,9 @@ #define TR_RESET_TELEMETRY "Azzera Telemetria" #define TR_STATISTICS "Statistiche" #define TR_ABOUT_US "Info su" -#define TR_USB_JOYSTICK "Joystick USB (HID)" -#define TR_USB_MASS_STORAGE "Storage USB (SD)" -#define TR_USB_SERIAL "Seriale USB (Debug)" +#define TR_USB_JOYSTICK "Joystick USB" +#define TR_USB_MASS_STORAGE "Storage USB" +#define TR_USB_SERIAL "Seriale USB" #define TR_SETUP_SCREENS "Schermate conf." #define TR_MONITOR_SCREENS "Monitors" #define TR_AND_SWITCH "Inter. AND" diff --git a/radio/src/translations/nl.h.txt b/radio/src/translations/nl.h.txt index 4b4783a3869..dd31090cd02 100644 --- a/radio/src/translations/nl.h.txt +++ b/radio/src/translations/nl.h.txt @@ -952,9 +952,9 @@ #define TR_RESET_TELEMETRY "Reset Telemetrie" #define TR_STATISTICS "Statistieken" #define TR_ABOUT_US "De Programmeurs" -#define TR_USB_JOYSTICK "USB Joystick (HID)" -#define TR_USB_MASS_STORAGE "USB Storage (SD)" -#define TR_USB_SERIAL "USB Serial (Debug)" +#define TR_USB_JOYSTICK "USB Joystick" +#define TR_USB_MASS_STORAGE "USB Storage" +#define TR_USB_SERIAL "USB Serial" #define TR_SETUP_SCREENS "Setup screens" #define TR_MONITOR_SCREENS "Monitors" #define TR_AND_SWITCH "AND Switch" diff --git a/radio/src/translations/pl.h.txt b/radio/src/translations/pl.h.txt index 4b14f6f25d8..3291e246af7 100644 --- a/radio/src/translations/pl.h.txt +++ b/radio/src/translations/pl.h.txt @@ -966,9 +966,9 @@ #define TR_RESET_TELEMETRY "Wyczyść telemetrię" #define TR_STATISTICS "Statystyki" #define TR_ABOUT_US "O nas" -#define TR_USB_JOYSTICK "USB Joystick (HID)" -#define TR_USB_MASS_STORAGE "USB Storage (SD)" -#define TR_USB_SERIAL "USB Serial (Debug)" +#define TR_USB_JOYSTICK "USB Joystick" +#define TR_USB_MASS_STORAGE "USB Storage" +#define TR_USB_SERIAL "USB Serial" #define TR_SETUP_SCREENS "Setup screens" #define TR_MONITOR_SCREENS "Monitors" #define TR_AND_SWITCH "Przełącznik AND" diff --git a/radio/src/translations/pt.h.txt b/radio/src/translations/pt.h.txt index 0669c02c4ef..891b20cf025 100644 --- a/radio/src/translations/pt.h.txt +++ b/radio/src/translations/pt.h.txt @@ -967,9 +967,9 @@ #define TR_RESET_TELEMETRY "Reset Telemetry" #define TR_STATISTICS "Statistics" #define TR_ABOUT_US "About Us" -#define TR_USB_JOYSTICK "USB Joystick (HID)" -#define TR_USB_MASS_STORAGE "USB Storage (SD)" -#define TR_USB_SERIAL "USB Serial (Debug)" +#define TR_USB_JOYSTICK "USB Joystick" +#define TR_USB_MASS_STORAGE "USB Storage" +#define TR_USB_SERIAL "USB Serial" #define TR_SETUP_SCREENS "Setup screens" #define TR_MONITOR_SCREENS "Monitors" #define TR_AND_SWITCH "AND Switch" diff --git a/radio/src/translations/se.h.txt b/radio/src/translations/se.h.txt index c39926ca72c..76f66bb65bf 100644 --- a/radio/src/translations/se.h.txt +++ b/radio/src/translations/se.h.txt @@ -969,9 +969,9 @@ #define TR_RESET_TELEMETRY "Nollställ Telemetri" #define TR_STATISTICS "Statistik" #define TR_ABOUT_US "Om Oss" -#define TR_USB_JOYSTICK "USB Joystick (HID)" -#define TR_USB_MASS_STORAGE "USB Storage (SD)" -#define TR_USB_SERIAL "USB Serial (Debug)" +#define TR_USB_JOYSTICK "USB Joystick" +#define TR_USB_MASS_STORAGE "USB Storage" +#define TR_USB_SERIAL "USB Serial" #define TR_SETUP_SCREENS "Setup screens" #define TR_MONITOR_SCREENS "Monitors" #define TR_AND_SWITCH "OCH Brytare" diff --git a/radio/src/translations/tw.h.txt b/radio/src/translations/tw.h.txt index 0913cd8f4b2..038b012deb0 100644 --- a/radio/src/translations/tw.h.txt +++ b/radio/src/translations/tw.h.txt @@ -853,9 +853,9 @@ #define TR_RESET_TELEMETRY "重啟回傳參數" #define TR_STATISTICS "統計" #define TR_ABOUT_US "關於" -#define TR_USB_JOYSTICK "USB 遊戲柄 (HID)" -#define TR_USB_MASS_STORAGE "USB 存儲器 (SD)" -#define TR_USB_SERIAL "USB 端口 (調試)" +#define TR_USB_JOYSTICK "USB 遊戲柄" +#define TR_USB_MASS_STORAGE "USB 存儲器" +#define TR_USB_SERIAL "USB 端口" #define TR_SETUP_SCREENS "設置顯示頁面" #define TR_MONITOR_SCREENS "查看器" #define TR_AND_SWITCH "與開關" From f9ab39923d16d702d5e4ab6737297c70bfb4e07c Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 8 Jun 2021 11:17:46 +0200 Subject: [PATCH 13/16] Unify AUX / AUX2 support definitions and allow for all radios (where possible) --- radio/src/targets/horus/board.cpp | 2 + radio/src/targets/horus/hal.h | 80 ++++++++++--------------------- 2 files changed, 27 insertions(+), 55 deletions(-) diff --git a/radio/src/targets/horus/board.cpp b/radio/src/targets/horus/board.cpp index f5a9d0978c5..f870b08f8cb 100644 --- a/radio/src/targets/horus/board.cpp +++ b/radio/src/targets/horus/board.cpp @@ -114,7 +114,9 @@ void boardInit() FLYSKY_HALL_RCC_AHB1Periph | #endif AUX_SERIAL_RCC_AHB1Periph | + AUX_SERIAL_PWR_RCC_AHB1Periph | AUX2_SERIAL_RCC_AHB1Periph | + AUX2_SERIAL_PWR_RCC_AHB1Periph | TELEMETRY_RCC_AHB1Periph | TRAINER_RCC_AHB1Periph | BT_RCC_AHB1Periph | diff --git a/radio/src/targets/horus/hal.h b/radio/src/targets/horus/hal.h index 47f180a66ce..4c56f20c8e0 100644 --- a/radio/src/targets/horus/hal.h +++ b/radio/src/targets/horus/hal.h @@ -409,8 +409,8 @@ #endif // Serial Port (DEBUG) -#if (defined(PCBX12S) || (defined(RADIO_TX16S)) && !defined(HARDWARE_EXTERNAL_ACCESS_MOD)) - #define AUX_SERIAL_RCC_AHB1Periph (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_DMA1) +#if defined(AUX_SERIAL) && !defined(HARDWARE_EXTERNAL_ACCESS_MOD) + #define AUX_SERIAL_RCC_AHB1Periph (RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_DMA1) #define AUX_SERIAL_RCC_APB1Periph RCC_APB1Periph_USART3 #define AUX_SERIAL_RCC_APB2Periph 0 #define AUX_SERIAL_GPIO GPIOB @@ -424,19 +424,23 @@ #define AUX_SERIAL_USART_IRQn USART3_IRQn #define AUX_SERIAL_DMA_Stream_RX DMA1_Stream1 #define AUX_SERIAL_DMA_Channel_RX DMA_Channel_4 -#if defined(RADIO_TX16S) - #define AUX_SERIAL_PWR_GPIO GPIOA - #define AUX_SERIAL_PWR_GPIO_PIN GPIO_Pin_15 // PA.15 - #define TRAINER_BATTERY_COMPARTMENT // allows serial port TTL trainer -#endif + #if defined(RADIO_TX16S) + #define AUX_SERIAL_PWR_RCC_AHB1Periph RCC_AHB1Periph_GPIOA + #define AUX_SERIAL_PWR_GPIO GPIOA + #define AUX_SERIAL_PWR_GPIO_PIN GPIO_Pin_15 // PA.15 + #define TRAINER_BATTERY_COMPARTMENT // allows serial port TTL trainer + #else + #define AUX_SERIAL_PWR_RCC_AHB1Periph 0 + #endif #else #define AUX_SERIAL_RCC_AHB1Periph 0 #define AUX_SERIAL_RCC_APB1Periph 0 #define AUX_SERIAL_RCC_APB2Periph 0 + #define AUX_SERIAL_PWR_RCC_AHB1Periph 0 #endif #if defined(AUX2_SERIAL) - #define AUX2_SERIAL_RCC_AHB1Periph (RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOG | RCC_AHB1Periph_DMA2) + #define AUX2_SERIAL_RCC_AHB1Periph (RCC_AHB1Periph_GPIOG | RCC_AHB1Periph_DMA2) #define AUX2_SERIAL_RCC_APB1Periph 0 #define AUX2_SERIAL_RCC_APB2Periph RCC_APB2Periph_USART6 #define AUX2_SERIAL_USART USART6 @@ -450,12 +454,17 @@ #define AUX2_SERIAL_USART_IRQHandler USART6_IRQHandler #define AUX2_SERIAL_DMA_Stream_RX DMA2_Stream6 #define AUX2_SERIAL_DMA_Channel_RX DMA_Channel_5 - #define AUX2_SERIAL_PWR_GPIO GPIOB - #define AUX2_SERIAL_PWR_GPIO_PIN GPIO_Pin_0 // PB.00 -#if defined(RADIO_TX16S) - #define TRAINER_BATTERY_COMPARTMENT // allows serial port TTL trainer -#endif -#elif defined(RADIO_TX16S) && defined(INTERNAL_GPS) + #if defined(RADIO_TX16S) + #define AUX2_SERIAL_PWR_RCC_AHB1Periph RCC_AHB1Periph_GPIOB + #define AUX2_SERIAL_PWR_GPIO GPIOB + #define AUX2_SERIAL_PWR_GPIO_PIN GPIO_Pin_0 // PB.00 + #else + #define AUX2_SERIAL_PWR_RCC_AHB1Periph 0 + #endif + #if defined(RADIO_TX16S) + #define TRAINER_BATTERY_COMPARTMENT // allows serial port TTL trainer + #endif +#elif !defined(PCBX12S) && defined(INTERNAL_GPS) #define GPS_RCC_AHB1Periph (RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOG) #define GPS_RCC_APB1Periph 0 #define GPS_RCC_APB2Periph RCC_APB2Periph_USART6 @@ -473,51 +482,12 @@ #define AUX2_SERIAL_RCC_AHB1Periph 0 #define AUX2_SERIAL_RCC_APB1Periph 0 #define AUX2_SERIAL_RCC_APB2Periph 0 + #define AUX2_SERIAL_PWR_RCC_AHB1Periph 0 #else #define AUX2_SERIAL_RCC_AHB1Periph 0 #define AUX2_SERIAL_RCC_APB1Periph 0 #define AUX2_SERIAL_RCC_APB2Periph 0 -#endif - -#if (defined(RADIO_T16) || defined(RADIO_T18)) && defined(AUX_SERIAL) - #undef AUX_SERIAL_RCC_AHB1Periph - #undef AUX_SERIAL_RCC_APB1Periph - #undef AUX_SERIAL_RCC_APB2Periph - - #define AUX_SERIAL_RCC_AHB1Periph (RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_DMA1) - #define AUX_SERIAL_RCC_APB1Periph RCC_APB1Periph_USART3 - #define AUX_SERIAL_RCC_APB2Periph 0 - #define AUX_SERIAL_GPIO GPIOB - #define AUX_SERIAL_GPIO_PIN_TX GPIO_Pin_10 // PB.10 - #define AUX_SERIAL_GPIO_PIN_RX GPIO_Pin_11 // PB.11 - #define AUX_SERIAL_GPIO_PinSource_TX GPIO_PinSource10 - #define AUX_SERIAL_GPIO_PinSource_RX GPIO_PinSource11 - #define AUX_SERIAL_GPIO_AF GPIO_AF_USART3 - #define AUX_SERIAL_USART USART3 - #define AUX_SERIAL_USART_IRQHandler USART3_IRQHandler - #define AUX_SERIAL_USART_IRQn USART3_IRQn - #define AUX_SERIAL_DMA_Stream_RX DMA1_Stream1 - #define AUX_SERIAL_DMA_Channel_RX DMA_Channel_4 -#endif -#if (defined(RADIO_T16) || defined(RADIO_T18)) && defined(AUX2_SERIAL) - #undef AUX2_SERIAL_PWR_GPIO - #undef AUX2_SERIAL_PWR_GPIO_PIN -#endif -#if (defined(RADIO_T16) || defined(RADIO_T18)) && defined(INTERNAL_GPS) - #define GPS_RCC_AHB1Periph (RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOG) - #define GPS_RCC_APB1Periph 0 - #define GPS_RCC_APB2Periph RCC_APB2Periph_USART6 - #define GPS_USART USART6 - #define GPS_GPIO_AF GPIO_AF_USART6 - #define GPS_USART_IRQn USART6_IRQn - #define GPS_USART_IRQHandler USART6_IRQHandler - #define GPS_UART_GPIO GPIOG - #define GPS_TX_GPIO_PIN GPIO_Pin_14 // PG.14 - #define GPS_RX_GPIO_PIN GPIO_Pin_9 // PG.09 - #define GPS_TX_GPIO_PinSource GPIO_PinSource14 - #define GPS_RX_GPIO_PinSource GPIO_PinSource9 - #define GPS_PWR_GPIO GPIOB - #define GPS_PWR_GPIO_PIN GPIO_Pin_0 // PB.00 + #define AUX2_SERIAL_PWR_RCC_AHB1Periph 0 #endif // Telemetry From adaafdc48988dc76cd83ae0d8d044f88b012bdbc Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 8 Jun 2021 12:09:14 +0200 Subject: [PATCH 14/16] Remove mavlinkExternal The necessary hardware is neither available nor disclosed. --- radio/src/datastructs.h | 10 +- radio/src/gui/colorlcd/radio_hardware.cpp | 6 - radio/src/lua/api_mavsdk.cpp | 3 +- radio/src/targets/horus/board.h | 21 +- radio/src/targets/horus/telemetry_driver.cpp | 60 ------ radio/src/telemetry/mavlink/mavlink_telem.cpp | 15 +- radio/src/telemetry/mavlink/mavlink_telem.h | 13 +- .../mavlink/mavlink_telem_interface.cpp | 199 +----------------- 8 files changed, 21 insertions(+), 306 deletions(-) diff --git a/radio/src/datastructs.h b/radio/src/datastructs.h index a1203bf4378..574c55d0ab4 100644 --- a/radio/src/datastructs.h +++ b/radio/src/datastructs.h @@ -856,10 +856,10 @@ PACK(struct RadioData { NOBACKUP(int8_t uartSampleMode:2); // See UartSampleModes #if defined(TELEMETRY_MAVLINK) - uint16_t mavlinkBaudrate:3; - uint16_t mavlinkBaudrate2:3; - uint16_t mavlinkExternal:2; - uint16_t _mavlinkDummy:8; // currently not used + // TODO: use specific structure + uint8_t mavlinkBaudrate:3; + uint8_t mavlinkBaudrate2:3; + uint8_t mavlinkSpare:2; // needs to adapt CHKSIZE below #endif }); @@ -984,7 +984,7 @@ static inline void check_struct() CHKSIZE(ModelData, 6604); #elif defined(PCBHORUS) #if defined(TELEMETRY_MAVLINK) - CHKSIZE(RadioData, 902+2); + CHKSIZE(RadioData, 902+1); CHKSIZE(ModelData, 11020+2); #else CHKSIZE(RadioData, 902); diff --git a/radio/src/gui/colorlcd/radio_hardware.cpp b/radio/src/gui/colorlcd/radio_hardware.cpp index 228a1a3c595..bc200c91087 100644 --- a/radio/src/gui/colorlcd/radio_hardware.cpp +++ b/radio/src/gui/colorlcd/radio_hardware.cpp @@ -309,12 +309,6 @@ void RadioHardwarePage::build(FormWindow * window) grid.nextLine(); #endif -#if defined(TELEMETRY_MAVLINK) - new StaticText(window, grid.getLabelSlot(), STR_MAVLINK_EXTERNAL); - new CheckBox(window, grid.getFieldSlot(), GET_SET_DEFAULT(g_eeGeneral.mavlinkExternal)); - grid.nextLine(); -#endif - // ADC filter new StaticText(window, grid.getLabelSlot(), STR_JITTER_FILTER); new CheckBox(window, grid.getFieldSlot(1,0), GET_SET_INVERTED(g_eeGeneral.jitterFilter)); diff --git a/radio/src/lua/api_mavsdk.cpp b/radio/src/lua/api_mavsdk.cpp index 1d54281f252..54f1d7732be 100644 --- a/radio/src/lua/api_mavsdk.cpp +++ b/radio/src/lua/api_mavsdk.cpp @@ -410,7 +410,8 @@ static int luaMavsdkCameraTakePhoto(lua_State *L) static int luaMavsdkMavTelemIsEnabled(lua_State *L) { - bool flag = (g_eeGeneral.auxSerialMode == UART_MODE_MAVLINK) || (g_eeGeneral.aux2SerialMode == UART_MODE_MAVLINK) || (g_eeGeneral.mavlinkExternal == 1); + bool flag = (g_eeGeneral.auxSerialMode == UART_MODE_MAVLINK) || + (g_eeGeneral.aux2SerialMode == UART_MODE_MAVLINK); lua_pushboolean(L, flag); return 1; } diff --git a/radio/src/targets/horus/board.h b/radio/src/targets/horus/board.h index 4165e54a204..b248791a1b2 100644 --- a/radio/src/targets/horus/board.h +++ b/radio/src/targets/horus/board.h @@ -153,23 +153,10 @@ void SDRAM_Init(); #define INTERNAL_MODULE_OFF() GPIO_ResetBits(INTMODULE_PWR_GPIO, INTMODULE_PWR_GPIO_PIN) #endif -//#define EXTERNAL_MODULE_ON() GPIO_SetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) -//#define EXTERNAL_MODULE_OFF() GPIO_ResetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) -//#define IS_INTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(INTMODULE_PWR_GPIO, INTMODULE_PWR_GPIO_PIN) == Bit_SET) -//#define IS_EXTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) == Bit_SET) -#if defined(TELEMETRY_MAVLINK) - void extmoduleOn(void); - void extmoduleOff(void); - #define EXTERNAL_MODULE_ON() extmoduleOn() - #define EXTERNAL_MODULE_OFF() extmoduleOff() - #define IS_INTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(INTMODULE_PWR_GPIO, INTMODULE_PWR_GPIO_PIN) == Bit_SET) - #define IS_EXTERNAL_MODULE_ON() (g_eeGeneral.mavlinkExternal != 1 && GPIO_ReadInputDataBit(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) == Bit_SET) -#else - #define EXTERNAL_MODULE_ON() GPIO_SetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) - #define EXTERNAL_MODULE_OFF() GPIO_ResetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) - #define IS_INTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(INTMODULE_PWR_GPIO, INTMODULE_PWR_GPIO_PIN) == Bit_SET) - #define IS_EXTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) == Bit_SET) -#endif +#define EXTERNAL_MODULE_ON() GPIO_SetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) +#define EXTERNAL_MODULE_OFF() GPIO_ResetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) +#define IS_INTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(INTMODULE_PWR_GPIO, INTMODULE_PWR_GPIO_PIN) == Bit_SET) +#define IS_EXTERNAL_MODULE_ON() (GPIO_ReadInputDataBit(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN) == Bit_SET) #if !defined(PXX2) #define IS_PXX2_INTERNAL_ENABLED() (false) diff --git a/radio/src/targets/horus/telemetry_driver.cpp b/radio/src/targets/horus/telemetry_driver.cpp index 31473acb7ad..bc6d0292340 100644 --- a/radio/src/targets/horus/telemetry_driver.cpp +++ b/radio/src/targets/horus/telemetry_driver.cpp @@ -28,9 +28,6 @@ DMAFifo telemetryDMAFifo __DMA (TELEMETRY_DMA_Stream_RX); uint8_t telemetryFifoMode; #endif -extern Fifo mavlinkTelemExternalTxFifo_frame; -extern Fifo mavlinkTelemExternalRxFifo; - static void telemetryInitDirPin() { GPIO_InitTypeDef GPIO_InitStructure; @@ -45,9 +42,6 @@ static void telemetryInitDirPin() void telemetryPortInit(uint32_t baudrate, uint8_t mode) { -#if defined(TELEMETRY_MAVLINK) - if (g_eeGeneral.mavlinkExternal == 1) return; -#endif if (baudrate == 0) { USART_DeInit(TELEMETRY_USART); return; @@ -150,9 +144,6 @@ static uint16_t probeTimeFromStartBit; void telemetryPortInvertedInit(uint32_t baudrate) { -#if defined(TELEMETRY_MAVLINK) - if (g_eeGeneral.mavlinkExternal == 1) return; -#endif if (baudrate == 0) { NVIC_DisableIRQ(TELEMETRY_EXTI_IRQn); NVIC_DisableIRQ(TELEMETRY_TIMER_IRQn); @@ -250,9 +241,6 @@ void telemetryPortInvertedRxBit() void telemetryPortSetDirectionOutput() { -#if defined(TELEMETRY_MAVLINK) - if (g_eeGeneral.mavlinkExternal == 1) return; -#endif #if defined(GHOST) && SPORT_MAX_BAUDRATE < 400000 if (isModuleGhost(EXTERNAL_MODULE)) { TELEMETRY_USART->BRR = BRR_400K; @@ -269,9 +257,6 @@ void sportWaitTransmissionComplete() void telemetryPortSetDirectionInput() { -#if defined(TELEMETRY_MAVLINK) - if (g_eeGeneral.mavlinkExternal == 1) return; -#endif sportWaitTransmissionComplete(); #if defined(GHOST) && SPORT_MAX_BAUDRATE < 400000 if (isModuleGhost(EXTERNAL_MODULE) && g_eeGeneral.telemetryBaudrate == GHST_TELEMETRY_RATE_115K) { @@ -284,9 +269,6 @@ void telemetryPortSetDirectionInput() void sportSendByte(uint8_t byte) { -#if defined(TELEMETRY_MAVLINK) - if (g_eeGeneral.mavlinkExternal == 1) return; -#endif telemetryPortSetDirectionOutput(); while (!(TELEMETRY_USART->SR & USART_SR_TXE)); @@ -295,9 +277,6 @@ void sportSendByte(uint8_t byte) void sportSendByteLoop(uint8_t byte) { -#if defined(TELEMETRY_MAVLINK) - if (g_eeGeneral.mavlinkExternal == 1) return; -#endif telemetryPortSetDirectionOutput(); outputTelemetryBuffer.data[0] = byte; @@ -326,9 +305,6 @@ void sportSendByteLoop(uint8_t byte) void sportSendBuffer(const uint8_t * buffer, uint32_t count) { -#if defined(TELEMETRY_MAVLINK) - if (g_eeGeneral.mavlinkExternal == 1) return; -#endif telemetryPortSetDirectionOutput(); DMA_InitTypeDef DMA_InitStructure; @@ -375,43 +351,7 @@ extern "C" void TELEMETRY_USART_IRQHandler(void) { DEBUG_INTERRUPT(INT_TELEM_USART); -#if defined(TELEMETRY_MAVLINK) - if (g_eeGeneral.mavlinkExternal == 1) { - - if (USART_GetITStatus(TELEMETRY_USART, USART_IT_TXE) != RESET) { - uint8_t txchar; - if (mavlinkTelemExternalTxFifo_frame.pop(txchar)) { - USART_SendData(TELEMETRY_USART, txchar); - } - else { - USART_ITConfig(TELEMETRY_USART, USART_IT_TXE, DISABLE); - // the uart send register is double buffered - // hence, the last byte is still send when we disable TXE - // hence, we can't switch to output here since this would kill the last byte - // so we enable TC interrupt and do it when - USART_ClearITPendingBit(TELEMETRY_USART, USART_IT_TC); - USART_ITConfig(TELEMETRY_USART, USART_IT_TC, ENABLE); - } - } - else if (USART_GetITStatus(TELEMETRY_USART, USART_IT_TC) != RESET) { - USART_ITConfig(TELEMETRY_USART, USART_IT_TC, DISABLE); - USART_ClearITPendingBit(TELEMETRY_USART, USART_IT_TC); - TELEMETRY_DIR_GPIO->BSRRH = TELEMETRY_DIR_GPIO_PIN; // output disable - TELEMETRY_USART->CR1 |= USART_CR1_RE; // turn on receiver - } - - if (USART_GetITStatus(TELEMETRY_USART, USART_IT_RXNE) != RESET) { - USART_ClearITPendingBit(TELEMETRY_USART, USART_IT_RXNE); - uint8_t c = USART_ReceiveData(TELEMETRY_USART); - mavlinkTelemExternalRxFifo.push(c); - } - - return; - } -#endif - uint32_t status = TELEMETRY_USART->SR; - if ((status & USART_SR_TC) && (TELEMETRY_USART->CR1 & USART_CR1_TCIE)) { TELEMETRY_USART->CR1 &= ~USART_CR1_TCIE; telemetryPortSetDirectionInput(); diff --git a/radio/src/telemetry/mavlink/mavlink_telem.cpp b/radio/src/telemetry/mavlink/mavlink_telem.cpp index 37f911028d7..dd0d3550792 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem.cpp @@ -442,17 +442,17 @@ void MavlinkTelem::wakeup() #else bool usb_enabled = false; #endif - bool external_enabled = (g_eeGeneral.mavlinkExternal == 1) && !s_pulses_paused; - if ((_aux1_enabled != aux1_enabled) || (_aux2_enabled != aux2_enabled) || - (_aux1_baudrate != g_eeGeneral.mavlinkBaudrate) || (_aux2_baudrate != g_eeGeneral.mavlinkBaudrate2) || - (_external_enabled != external_enabled)) { + // check for config updates + if ((_aux1_enabled != aux1_enabled) || + (_aux2_enabled != aux2_enabled) || + (_aux1_baudrate != g_eeGeneral.mavlinkBaudrate) || + (_aux2_baudrate != g_eeGeneral.mavlinkBaudrate2)) { + _aux1_enabled = aux1_enabled; _aux2_enabled = aux2_enabled; _aux1_baudrate = g_eeGeneral.mavlinkBaudrate; _aux2_baudrate = g_eeGeneral.mavlinkBaudrate2; - _external_enabled = external_enabled; - mavlinkTelemExternal_init(external_enabled); map_serials(); _reset(); } @@ -462,8 +462,6 @@ void MavlinkTelem::wakeup() fmav_router_clearout_link(3); } - if (external_enabled) mavlinkTelemExternal_wakeup(); - // skip out if not one of the serial1, serial2 is enabled if (!serial1_enabled && !serial1_enabled) return; @@ -626,7 +624,6 @@ void MavlinkTelem::_reset(void) #if !defined(AUX2_SERIAL) if (g_eeGeneral.aux2SerialMode == UART_MODE_MAVLINK) g_eeGeneral.aux2SerialMode = UART_MODE_NONE_OR_DEBUG; #endif - if (g_eeGeneral.mavlinkExternal > 1) g_eeGeneral.mavlinkExternal = 0; _my_sysid = MAVLINK_TELEM_MY_SYSID; _my_compid = MAVLINK_TELEM_MY_COMPID; diff --git a/radio/src/telemetry/mavlink/mavlink_telem.h b/radio/src/telemetry/mavlink/mavlink_telem.h index 7da563e3491..a1470572abb 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem.h +++ b/radio/src/telemetry/mavlink/mavlink_telem.h @@ -51,14 +51,6 @@ extern Fifo mavlinkTelemAux2SerialRxFifo; extern Fifo mavlinkTelemUsbRxFifo; #endif -//TODO: since we want to allow only 2 channels max, this is memory waste, -//i.e., we should just have 2 instead of three Fifos. -extern Fifo mavlinkTelemExternalTxFifo_frame; -extern Fifo mavlinkTelemExternalRxFifo; - -void mavlinkTelemExternal_init(bool flag); -void mavlinkTelemExternal_wakeup(void); - uint32_t mavlinkTelem1Available(void); uint8_t mavlinkTelem1Getc(uint8_t *c); bool mavlinkTelem1HasSpace(uint16_t count); @@ -896,16 +888,13 @@ class MavlinkTelem bool _aux2_enabled = false; uint32_t _aux2_baudrate = UINT32_MAX; // to enforce change bool _usb_enabled = false; - bool _external_enabled = false; void map_serials(void); public: - // map of aux1, aux2, external onto serial1, serial2 + // map of aux1, aux2 onto serial1, serial2 bool serial1_enabled = false; bool serial2_enabled = false; - bool serial1_isexternal = false; - bool serial2_isexternal = false; }; extern MavlinkTelem mavlinkTelem; diff --git a/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp b/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp index 6c14ff71a31..0a3b6d38b11 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem_interface.cpp @@ -73,167 +73,6 @@ void mavlinkStart() RTOS_CREATE_TASK(mavlinkTaskId, mavlinkTask, "mavlink", mavlinkStack, MAVLINK_STACK_SIZE, MAVLINK_TASK_PRIO); } -// -- EXTERNAL BAY SERIAL handlers -- -// we essentially redo everything from scratch - -MAVLINK_RAM_SECTION Fifo mavlinkTelemExternalTxFifo_frame; -MAVLINK_RAM_SECTION Fifo mavlinkTelemExternalTxFifo; -MAVLINK_RAM_SECTION Fifo mavlinkTelemExternalRxFifo; - -void extmoduleOn(void) -{ - if(g_eeGeneral.mavlinkExternal != 1) GPIO_SetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN); -} - -void extmoduleOff(void) -{ - if(g_eeGeneral.mavlinkExternal != 1) GPIO_ResetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN); -} - -void extmoduleMavlinkTelemStop(void) -{ - USART_ITConfig(TELEMETRY_USART, USART_IT_RXNE, DISABLE); - USART_ITConfig(TELEMETRY_USART, USART_IT_TXE, DISABLE); - NVIC_DisableIRQ(TELEMETRY_USART_IRQn); - - NVIC_DisableIRQ(TELEMETRY_EXTI_IRQn); - NVIC_DisableIRQ(TELEMETRY_TIMER_IRQn); - DMA_ITConfig(TELEMETRY_DMA_Stream_TX, DMA_IT_TC, DISABLE); - NVIC_DisableIRQ(TELEMETRY_DMA_TX_Stream_IRQ); - - USART_DeInit(TELEMETRY_USART); - DMA_DeInit(TELEMETRY_DMA_Stream_TX); - - GPIO_ResetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN); //EXTERNAL_MODULE_OFF(); -} - -void extmoduleMavlinkTelemStart(void) -{ - GPIO_SetBits(EXTMODULE_PWR_GPIO, EXTMODULE_PWR_GPIO_PIN); //EXTERNAL_MODULE_ON(); - - // we don't want or need all this - NVIC_DisableIRQ(TELEMETRY_EXTI_IRQn); - NVIC_DisableIRQ(TELEMETRY_TIMER_IRQn); - NVIC_DisableIRQ(TELEMETRY_DMA_TX_Stream_IRQ); - - DMA_ITConfig(TELEMETRY_DMA_Stream_TX, DMA_IT_TC, DISABLE); - DMA_Cmd(TELEMETRY_DMA_Stream_TX, DISABLE); - USART_DMACmd(TELEMETRY_USART, USART_DMAReq_Tx, DISABLE); - DMA_DeInit(TELEMETRY_DMA_Stream_TX); - - EXTI_InitTypeDef EXTI_InitStructure; - EXTI_StructInit(&EXTI_InitStructure); - EXTI_InitStructure.EXTI_Line = TELEMETRY_EXTI_LINE; - EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; - EXTI_InitStructure.EXTI_Trigger = TELEMETRY_EXTI_TRIGGER; - EXTI_InitStructure.EXTI_LineCmd = DISABLE; - EXTI_Init(&EXTI_InitStructure); - - // is it called already? through telemetryInit() -> telemetryPortInit(FRSKY_SPORT_BAUDRATE) -> telemetryInitDirPin() - GPIO_PinAFConfig(TELEMETRY_GPIO, TELEMETRY_GPIO_PinSource_RX, TELEMETRY_GPIO_AF); - GPIO_PinAFConfig(TELEMETRY_GPIO, TELEMETRY_GPIO_PinSource_TX, TELEMETRY_GPIO_AF); - - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_InitStructure.GPIO_Pin = TELEMETRY_TX_GPIO_PIN | TELEMETRY_RX_GPIO_PIN; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_Init(TELEMETRY_GPIO, &GPIO_InitStructure); - - // is it called already? through telemetryInit() -> telemetryPortInit(FRSKY_SPORT_BAUDRATE) -> telemetryInitDirPin() - GPIO_InitStructure.GPIO_Pin = TELEMETRY_DIR_GPIO_PIN; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_Init(TELEMETRY_DIR_GPIO, &GPIO_InitStructure); - GPIO_ResetBits(TELEMETRY_DIR_GPIO, TELEMETRY_DIR_GPIO_PIN); - - // init uart itself - USART_InitTypeDef USART_InitStructure; - USART_InitStructure.USART_BaudRate = 400000; - USART_InitStructure.USART_WordLength = USART_WordLength_8b; - USART_InitStructure.USART_StopBits = USART_StopBits_1; - USART_InitStructure.USART_Parity = USART_Parity_No; - USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; - USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx; - USART_Init(TELEMETRY_USART, &USART_InitStructure); - - USART_Cmd(TELEMETRY_USART, ENABLE); - USART_ITConfig(TELEMETRY_USART, USART_IT_RXNE, ENABLE); - USART_ITConfig(TELEMETRY_USART, USART_IT_TXE, DISABLE); - NVIC_SetPriority(TELEMETRY_USART_IRQn, 6); - NVIC_EnableIRQ(TELEMETRY_USART_IRQn); -} - -void mavlinkTelemExternal_init(bool flag) -{ - if (flag) { - extmoduleStop(); //??? good or bad - // this overrides ext module settings, so nothing else to do - extmoduleMavlinkTelemStart(); - } - else { - extmoduleMavlinkTelemStop(); - //TODO: we should re-enable an external module if one is configured - } -} - -// this must be called regularly, at 2 ms -// 115200 bps = 86 us per byte => 12 bytes per ms = 24 bytes per 2 ms -// 3+24 bytes @ 400000 bps = 0.675 ms, 24 bytes @ 400000 bps = 0.6 ms => 1.275 ms -// => enough time for a tx and rx packet in a 2 ms slot -// however, the slots are not precisely fixed to 2 ms, can be shorter -// so design for lower data rate, we send at most 16 bytes per slot -// 16 bytes per slot = 8000 bytes/s = effectively 80000 bps, should be way enough -// 3+16 bytes @ 400000 bps = 0.475 ms, 16 bytes @ 400000 bps = 0.4 ms, => 0.875 ms -void mavlinkTelemExternal_wakeup(void) -{ - // we do it at the beginning, so it gives few cycles before TX is enabled - TELEMETRY_DIR_GPIO->BSRRL = TELEMETRY_DIR_GPIO_PIN; // enable output - TELEMETRY_USART->CR1 &= ~USART_CR1_RE; // turn off receiver - - uint32_t count = mavlinkTelemExternalTxFifo.size(); - if (count > 16) count = 16; - - // always send header, this synchronizes slave - mavlinkTelemExternalTxFifo_frame.push('O'); - mavlinkTelemExternalTxFifo_frame.push('W'); - mavlinkTelemExternalTxFifo_frame.push((uint8_t)count); - - // send payload - for (uint16_t i = 0; i < count; i++) { - uint8_t c; - mavlinkTelemExternalTxFifo.pop(c); - mavlinkTelemExternalTxFifo_frame.push(c); - } - - USART_ITConfig(TELEMETRY_USART, USART_IT_TXE, ENABLE); // enable TX interrupt, starts sending -} - -uint32_t mavlinkTelemExternalAvailable(void) -{ - return mavlinkTelemExternalRxFifo.size(); -} - -uint8_t mavlinkTelemExternalGetc(uint8_t* c) -{ - return mavlinkTelemExternalRxFifo.pop(*c); -} - -bool mavlinkTelemExternalHasSpace(uint16_t count) -{ - return mavlinkTelemExternalTxFifo.hasSpace(count); -} - -bool mavlinkTelemExternalPutBuf(const uint8_t *buf, const uint16_t count) -{ - if (!mavlinkTelemExternalTxFifo.hasSpace(count)) return false; - for (uint16_t i = 0; i < count; i++) mavlinkTelemExternalTxFifo.push(buf[i]); - return true; -} - // -- AUX1, AUX2 handlers -- uint32_t _cvtBaudrate(uint16_t baud) @@ -276,7 +115,6 @@ MAVLINK_RAM_SECTION Fifo mavlinkTelemUsbRxFifo; uint32_t mavlinkTelem1Available(void) { if (!mavlinkTelem.serial1_enabled) return 0; - if (mavlinkTelem.serial1_isexternal) return mavlinkTelemExternalRxFifo.size(); // if (auxSerialMode != UART_MODE_MAVLINK) return 0; return mavlinkTelemAuxSerialRxFifo.size(); @@ -286,7 +124,6 @@ uint32_t mavlinkTelem1Available(void) uint8_t mavlinkTelem1Getc(uint8_t* c) { if (!mavlinkTelem.serial1_enabled) return 0; - if (mavlinkTelem.serial1_isexternal) return mavlinkTelemExternalRxFifo.pop(*c); return mavlinkTelemAuxSerialRxFifo.pop(*c); } @@ -294,7 +131,6 @@ uint8_t mavlinkTelem1Getc(uint8_t* c) bool mavlinkTelem1HasSpace(uint16_t count) { if (!mavlinkTelem.serial1_enabled) return 0; - if (mavlinkTelem.serial1_isexternal) return mavlinkTelemExternalTxFifo.hasSpace(count); // if (auxSerialMode != UART_MODE_MAVLINK) return false; return auxSerialTxFifo.hasSpace(count); @@ -303,7 +139,6 @@ bool mavlinkTelem1HasSpace(uint16_t count) bool mavlinkTelem1PutBuf(const uint8_t* buf, const uint16_t count) { if (!mavlinkTelem.serial1_enabled || !buf) return false; - if (mavlinkTelem.serial1_isexternal) return mavlinkTelemExternalPutBuf(buf, count); if (!auxSerialTxFifo.hasSpace(count)) return false; // if (auxSerialMode != UART_MODE_MAVLINK || !buf || !auxSerialTxFifo.hasSpace(count)) { @@ -326,7 +161,6 @@ bool mavlinkTelem1PutBuf(const uint8_t* buf, const uint16_t count){ return false uint32_t mavlinkTelem2Available(void) { if (!mavlinkTelem.serial2_enabled) return 0; - if (mavlinkTelem.serial2_isexternal) return mavlinkTelemExternalRxFifo.size(); // if (aux2SerialMode != UART_MODE_MAVLINK) return 0; return mavlinkTelemAux2SerialRxFifo.size(); @@ -336,7 +170,6 @@ uint32_t mavlinkTelem2Available(void) uint8_t mavlinkTelem2Getc(uint8_t* c) { if (!mavlinkTelem.serial2_enabled) return 0; - if (mavlinkTelem.serial2_isexternal) return mavlinkTelemExternalRxFifo.pop(*c); return mavlinkTelemAux2SerialRxFifo.pop(*c); } @@ -344,7 +177,6 @@ uint8_t mavlinkTelem2Getc(uint8_t* c) bool mavlinkTelem2HasSpace(uint16_t count) { if (!mavlinkTelem.serial2_enabled) return 0; - if (mavlinkTelem.serial2_isexternal) return mavlinkTelemExternalTxFifo.hasSpace(count); // if (aux2SerialMode != UART_MODE_MAVLINK) return false; return aux2SerialTxFifo.hasSpace(count); @@ -353,7 +185,6 @@ bool mavlinkTelem2HasSpace(uint16_t count) bool mavlinkTelem2PutBuf(const uint8_t* buf, const uint16_t count) { if (!mavlinkTelem.serial2_enabled || !buf) return false; - if (mavlinkTelem.serial2_isexternal) return mavlinkTelemExternalPutBuf(buf, count); if (!aux2SerialTxFifo.hasSpace(count)) return false; // if (aux2SerialMode != UART_MODE_MAVLINK || !buf || !aux2SerialTxFifo.hasSpace(count)) { @@ -417,35 +248,11 @@ bool mavlinkTelem3PutBuf(const uint8_t* buf, const uint16_t count){ return false // -- MavlinkTelem stuff -- -// map aux1,aux2,external onto serial1 & serial2 +// map aux1,aux2 onto serial1 & serial2 void MavlinkTelem::map_serials(void) { - if (_external_enabled) { - if (_aux1_enabled && _aux2_enabled) { - // shit, what should we do??? we give aux,aux2 priority - serial1_enabled = serial2_enabled = true; - serial1_isexternal = serial2_isexternal = false; - } - else if (_aux1_enabled && !_aux2_enabled) { - serial1_enabled = true; - serial1_isexternal = false; - serial2_enabled = serial2_isexternal = true; - } - else if (!_aux1_enabled && _aux2_enabled) { - serial1_enabled = serial1_isexternal = true; - serial2_enabled = true; - serial2_isexternal = false; - } - else { - serial1_enabled = serial1_isexternal = true; - serial2_enabled = serial2_isexternal = false; - } - } - else{ - serial1_enabled = _aux1_enabled; - serial2_enabled = _aux1_enabled; - serial1_isexternal = serial2_isexternal = false; - } + serial1_enabled = _aux1_enabled; + serial2_enabled = _aux2_enabled; } void MavlinkTelem::telemetrySetValue(uint16_t id, uint8_t subId, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec) From 024cdcd1ed8546ea863d168aeede721c3a09f3f8 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 8 Jun 2021 12:35:32 +0200 Subject: [PATCH 15/16] telemetryStreaming is now 16 bits so the delay can be pushed over 2.5s --- radio/src/telemetry/telemetry.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index edb47a9de78..a1b04cbb6e7 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -31,7 +31,6 @@ #include "telemetry/mavlink/mavlink_telem.h" #endif -//uint8_t telemetryStreaming = 0; uint16_t telemetryStreaming = 0; uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1) uint8_t telemetryRxBufferCount = 0; From 2fb494bcd58ef958741c012b115ac5db67d19ff5 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 8 Jun 2021 12:41:48 +0200 Subject: [PATCH 16/16] Cleanup --- radio/src/telemetry/mavlink/mavlink_telem.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/radio/src/telemetry/mavlink/mavlink_telem.cpp b/radio/src/telemetry/mavlink/mavlink_telem.cpp index dd0d3550792..c6b6528605c 100644 --- a/radio/src/telemetry/mavlink/mavlink_telem.cpp +++ b/radio/src/telemetry/mavlink/mavlink_telem.cpp @@ -438,6 +438,7 @@ void MavlinkTelem::wakeup() bool aux1_enabled = (g_eeGeneral.auxSerialMode == UART_MODE_MAVLINK); bool aux2_enabled = (g_eeGeneral.aux2SerialMode == UART_MODE_MAVLINK); #if defined(TELEMETRY_MAVLINK_USB_SERIAL) + //TODO: check which function the USB serial has been assigned to bool usb_enabled = (getSelectedUsbMode() == USB_SERIAL_MODE); #else bool usb_enabled = false; @@ -619,10 +620,12 @@ void MavlinkTelem::_reset(void) #define UART_MODE_NONE_OR_DEBUG UART_MODE_NONE #endif #if !defined(AUX_SERIAL) - if (g_eeGeneral.auxSerialMode == UART_MODE_MAVLINK) g_eeGeneral.auxSerialMode = UART_MODE_NONE_OR_DEBUG; + if (g_eeGeneral.auxSerialMode == UART_MODE_MAVLINK) + g_eeGeneral.auxSerialMode = UART_MODE_NONE_OR_DEBUG; #endif #if !defined(AUX2_SERIAL) - if (g_eeGeneral.aux2SerialMode == UART_MODE_MAVLINK) g_eeGeneral.aux2SerialMode = UART_MODE_NONE_OR_DEBUG; + if (g_eeGeneral.aux2SerialMode == UART_MODE_MAVLINK) + g_eeGeneral.aux2SerialMode = UART_MODE_NONE_OR_DEBUG; #endif _my_sysid = MAVLINK_TELEM_MY_SYSID;