diff --git a/lib/BUTTON/devButton.cpp b/lib/BUTTON/devButton.cpp index 2d82f449..23ba847c 100644 --- a/lib/BUTTON/devButton.cpp +++ b/lib/BUTTON/devButton.cpp @@ -1,6 +1,7 @@ #include #include "common.h" #include "device.h" +#include "config.h" #if defined(PIN_BUTTON) #include "logging.h" @@ -9,7 +10,7 @@ static Button button; extern unsigned long rebootTime; -void RebootIntoWifi(); +void RebootIntoWifi(wifi_service_t service); static void shortPress() { @@ -19,7 +20,7 @@ static void shortPress() } else { - RebootIntoWifi(); + RebootIntoWifi(WIFI_SERVICE_UPDATE); } } diff --git a/lib/MAVLink/MAVLink.cpp b/lib/MAVLink/MAVLink.cpp new file mode 100644 index 00000000..9e478480 --- /dev/null +++ b/lib/MAVLink/MAVLink.cpp @@ -0,0 +1,51 @@ +#if defined(MAVLINK_ENABLED) +#include +#include "MAVLink.h" +#include + +// Returns whether the message is a control message, i.e. a message where we don't want to +// pass the message to the flight controller, but rather handle it ourselves. If the message +// is a control message, the function handles it internally. +bool MAVLink::handleControlMessage(mavlink_message_t *msg) +{ + bool shouldForward = true; + // Check for messages addressed to the Backpack + switch (msg->msgid) + { + case MAVLINK_MSG_ID_COMMAND_INT: + mavlink_command_int_t commandMsg; + mavlink_msg_command_int_decode(msg, &commandMsg); + if (commandMsg.target_component == MAV_COMP_ID_UDP_BRIDGE) + { + shouldForward = false; + constexpr uint8_t ELRS_MODE_CHANGE = 0x8; + switch (commandMsg.command) + { + case MAV_CMD_USER_1: + switch ((int)commandMsg.param1) + { + case ELRS_MODE_CHANGE: + switch ((int)commandMsg.param2) + { + case 0: // TX_NORMAL_MODE + config.SetStartWiFiOnBoot(false); + ESP.restart(); + break; + case 1: // TX_MAVLINK_MODE + if (config.GetWiFiService() != WIFI_SERVICE_MAVLINK_TX) + { + config.SetWiFiService(WIFI_SERVICE_MAVLINK_TX); + config.SetStartWiFiOnBoot(true); + config.Commit(); + ESP.restart(); + } + break; + } + } + } + break; + } + } + return shouldForward; +} +#endif \ No newline at end of file diff --git a/lib/MAVLink/MAVLink.h b/lib/MAVLink/MAVLink.h new file mode 100644 index 00000000..a9b5edf1 --- /dev/null +++ b/lib/MAVLink/MAVLink.h @@ -0,0 +1,9 @@ +#if defined(MAVLINK_ENABLED) +#include "common/mavlink.h" + +class MAVLink +{ +public: + static bool handleControlMessage(mavlink_message_t* msg); +}; +#endif \ No newline at end of file diff --git a/lib/WIFI/devWIFI.cpp b/lib/WIFI/devWIFI.cpp index c2777ecd..98946433 100644 --- a/lib/WIFI/devWIFI.cpp +++ b/lib/WIFI/devWIFI.cpp @@ -34,11 +34,16 @@ #include "WebContent.h" #include "config.h" + +#if defined(MAVLINK_ENABLED) +#include +#endif #if defined(TARGET_VRX_BACKPACK) extern VrxBackpackConfig config; extern bool sendRTCChangesToVrx; #elif defined(TARGET_TX_BACKPACK) extern TxBackpackConfig config; +extern wifi_service_t wifiService; #elif defined(TARGET_TIMER_BACKPACK) extern TimerBackpackConfig config; #else @@ -51,7 +56,8 @@ static const char *myHostname = "elrs_vrx"; static const char *wifi_ap_ssid = "ExpressLRS VRx Backpack"; #elif defined(TARGET_TX_BACKPACK) static const char *myHostname = "elrs_txbp"; -static const char *wifi_ap_ssid = "ExpressLRS TX Backpack"; +static char wifi_ap_ssid[33]; // will be set depending on wifiService + #elif defined(TARGET_TIMER_BACKPACK) static const char *myHostname = "elrs_timer"; static const char *wifi_ap_ssid = "ExpressLRS Timer Backpack"; @@ -75,6 +81,7 @@ static volatile unsigned long changeTime = 0; static const byte DNS_PORT = 53; static IPAddress apIP(10, 0, 0, 1); static IPAddress netMsk(255, 255, 255, 0); +static IPAddress apBroadcast(10, 0, 0, 255); static DNSServer dnsServer; static AsyncWebServer server(80); @@ -93,6 +100,34 @@ static AsyncEventSource logging("/logging"); static char logBuffer[256]; static int logPos = 0; +#if defined(MAVLINK_ENABLED) +// This is the port that we will listen for mavlink packets on +constexpr uint16_t MAVLINK_PORT_LISTEN = 14555; +// This is the port that we will send mavlink packets to +constexpr uint16_t MAVLINK_PORT_SEND = 14550; +// Size of the buffer that we will use to store mavlink packets in units of mavlink_message_t +constexpr size_t MAVLINK_BUF_SIZE = 16; +// Threshold at which we will flush the buffer +constexpr size_t MAVLINK_BUF_THRESHOLD = MAVLINK_BUF_SIZE / 2; +// Timeout for flushing the buffer in ms +constexpr size_t MAVLINK_BUF_TIMEOUT = 500; + +WiFiUDP mavlinkUDP; + +typedef struct { + uint32_t packets_downlink; // packets from the aircraft + uint32_t packets_uplink; // packets to the aircraft + uint32_t drops_downlink; // dropped packets from the aircraft + //uint32_t drops_uplink; // framing in the uplink direction cost too much time + uint32_t overflows_downlink; // buffer overflows from the aircraft +} mavlink_stats_t; +mavlink_stats_t mavlink_stats; +// Buffer for storing mavlink packets from the aircraft so that we can send them to the GCS efficiently +mavlink_message_t mavlink_to_gcs_buf[MAVLINK_BUF_SIZE]; +// Count of the number of messages in the buffer +uint8_t mavlink_to_gcs_buf_count = 0; +#endif + /** Is this an IP? */ static boolean isIp(String str) { @@ -443,6 +478,24 @@ static void WebUploadRTCUpdateHandler(AsyncWebServerRequest *request) { #endif } +#if defined(MAVLINK_ENABLED) +static void WebMAVLinkHandler(AsyncWebServerRequest *request) +{ + DynamicJsonDocument json(1024); + json["enabled"] = wifiService == WIFI_SERVICE_MAVLINK_TX; + json["counters"]["packets_down"] = mavlink_stats.packets_downlink; + json["counters"]["packets_up"] = mavlink_stats.packets_uplink; + json["counters"]["drops_down"] = mavlink_stats.drops_downlink; + json["counters"]["overflows_down"] = mavlink_stats.overflows_downlink; + json["ports"]["listen"] = MAVLINK_PORT_LISTEN; + json["ports"]["send"] = MAVLINK_PORT_SEND; + + AsyncResponseStream *response = request->beginResponseStream("application/json"); + serializeJson(json, *response); + request->send(response); +} +#endif + static void wifiOff() { wifiStarted = false; @@ -582,6 +635,9 @@ static void startServices() #if defined(AAT_BACKPACK) WebAatInit(server); #endif +#if defined(MAVLINK_ENABLED) + server.on("/mavlink", HTTP_GET, WebMAVLinkHandler); +#endif server.on("/log.js", WebUpdateSendContent); server.on("/log.html", WebUpdateSendContent); @@ -595,6 +651,9 @@ static void startServices() dnsServer.setErrorReplyCode(DNSReplyCode::NoError); startMDNS(); +#if defined(MAVLINK_ENABLED) + mavlinkUDP.begin(MAVLINK_PORT_LISTEN); +#endif servicesStarted = true; DBGLN("HTTPUpdateServer ready! Open http://%s.local in your browser", myHostname); @@ -639,6 +698,18 @@ static void HandleWebUpdate() #endif changeTime = now; WiFi.softAPConfig(apIP, apIP, netMsk); +#if defined(TARGET_TX_BACKPACK) + if (wifiService == WIFI_SERVICE_UPDATE) { + strcpy(wifi_ap_ssid, "ExpressLRS TX Backpack"); + } else if (wifiService == WIFI_SERVICE_MAVLINK_TX) { + // Generate a unique SSID using config.address as hex + sprintf(wifi_ap_ssid, "ExpressLRS TX Backpack %02X%02X%02X", + firmwareOptions.uid[3], + firmwareOptions.uid[4], + firmwareOptions.uid[5] + ); + } +#endif WiFi.softAP(wifi_ap_ssid, wifi_ap_password); WiFi.scanNetworks(true); startServices(); @@ -665,23 +736,121 @@ static void HandleWebUpdate() if (servicesStarted) { - while (Serial.available()) { + while (Serial.available()) + { +#if defined(MAVLINK_ENABLED) + if (wifiService == WIFI_SERVICE_UPDATE) + { + int val = Serial.read(); + logBuffer[logPos++] = val; + logBuffer[logPos] = 0; + if (val == '\n' || logPos == sizeof(logBuffer) - 1) + { + logging.send(logBuffer); + logPos = 0; + } + } + else if (wifiService == WIFI_SERVICE_MAVLINK_TX) + { + mavlink_status_t status; + char val = Serial.read(); + static uint8_t expectedSeq = 0; + static bool expectedSeqSet = false; + if (mavlink_to_gcs_buf_count >= MAVLINK_BUF_SIZE) + { + mavlink_stats.overflows_downlink++; + mavlink_to_gcs_buf_count = 0; + } + mavlink_message_t msg; + if (mavlink_frame_char(MAVLINK_COMM_0, val, &msg, &status) != MAVLINK_FRAMING_INCOMPLETE) + { + bool shouldForward = MAVLink::handleControlMessage(&msg); + if (shouldForward) + { + // Track gaps in the sequence number, add to a dropped counter + uint8_t seq = msg.seq; + if (expectedSeqSet && seq != expectedSeq) + { + // account for rollovers + if (seq < expectedSeq) + { + mavlink_stats.drops_downlink += (UINT8_MAX - expectedSeq) + seq; + } + else + { + mavlink_stats.drops_downlink += seq - expectedSeq; + } + } + expectedSeq = seq + 1; + expectedSeqSet = true; + // Forward the message to the GCS + mavlink_to_gcs_buf[mavlink_to_gcs_buf_count] = msg; + mavlink_to_gcs_buf_count++; + mavlink_stats.packets_downlink++; + } + } + } +#else int val = Serial.read(); logBuffer[logPos++] = val; logBuffer[logPos] = 0; - if (val == '\n' || logPos == sizeof(logBuffer)-1) { + if (val == '\n' || logPos == sizeof(logBuffer) - 1) + { logging.send(logBuffer); logPos = 0; } +#endif } +#if defined(MAVLINK_ENABLED) + // Dump the mavlink_to_gcs_buf to the GCS + static unsigned long last_mavlink_to_gcs_dump = 0; + if ( + mavlink_to_gcs_buf_count > MAVLINK_BUF_THRESHOLD || // buffer is full enough + (mavlink_to_gcs_buf_count > 0 && (millis() - last_mavlink_to_gcs_dump) > MAVLINK_BUF_TIMEOUT) // buffer hasn't been flushed in a while + ) + { + IPAddress broadcast = WiFi.getMode() == WIFI_STA ? WiFi.broadcastIP() : apBroadcast; + mavlinkUDP.beginPacket(broadcast, MAVLINK_PORT_SEND); + for (uint8_t i = 0; i < mavlink_to_gcs_buf_count; i++) + { + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + uint16_t len = mavlink_msg_to_send_buffer(buf, &mavlink_to_gcs_buf[i]); + size_t sent = mavlinkUDP.write(buf, len); + if (sent < len) + { + break; + } + } + mavlinkUDP.endPacket(); + mavlink_to_gcs_buf_count = 0; + last_mavlink_to_gcs_dump = millis(); + } +#endif + +#if defined(MAVLINK_ENABLED) + // Check if we have MAVLink UDP data to send over UART + if (wifiService == WIFI_SERVICE_MAVLINK_TX) { + int packetSize = mavlinkUDP.parsePacket(); + if (packetSize) { + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + mavlinkUDP.read(buf, packetSize); + Serial.write(buf, packetSize); + mavlink_stats.packets_uplink++; // this isn't necessarily a single mavlink packet, but framing was too time expensive. + } + } +#endif dnsServer.processNextRequest(); #if defined(PLATFORM_ESP8266) MDNS.update(); #endif // When in STA mode, a small delay reduces power use from 90mA to 30mA when idle // In AP mode, it doesn't seem to make a measurable difference, but does not hurt +#if defined(MAVLINK_ENABLED) + if (!updater.isRunning() && wifiService != WIFI_SERVICE_MAVLINK_TX) +#else if (!updater.isRunning()) +#endif delay(1); if (do_flash) { do_flash = false; diff --git a/lib/config/config.cpp b/lib/config/config.cpp index c42caf66..ccaaaf3e 100644 --- a/lib/config/config.cpp +++ b/lib/config/config.cpp @@ -51,6 +51,7 @@ TxBackpackConfig::SetDefaults() m_config.ssid[0] = 0; m_config.password[0] = 0; memset(m_config.address, 0, 6); + m_config.wifiService = WIFI_SERVICE_UPDATE; m_modified = true; Commit(); } @@ -83,6 +84,12 @@ TxBackpackConfig::SetGroupAddress(const uint8_t address[6]) m_modified = true; } +void +TxBackpackConfig::SetWiFiService(wifi_service_t service) +{ + m_config.wifiService = service; + m_modified = true; +} #endif ///////////////////////////////////////////////////// diff --git a/lib/config/config.h b/lib/config/config.h index 3020ea16..237d379b 100644 --- a/lib/config/config.h +++ b/lib/config/config.h @@ -11,13 +11,21 @@ #define VRX_BACKPACK_CONFIG_VERSION 3 #define TIMER_BACKPACK_CONFIG_VERSION 3 + +typedef enum { + WIFI_SERVICE_UPDATE, + WIFI_SERVICE_MAVLINK_TX, +} wifi_service_t; + #if defined(TARGET_TX_BACKPACK) + typedef struct { - uint32_t version; - bool startWiFi; - char ssid[33]; - char password[65]; - uint8_t address[6]; + uint32_t version; + bool startWiFi; + char ssid[33]; + char password[65]; + uint8_t address[6]; + wifi_service_t wifiService; } tx_backpack_config_t; class TxBackpackConfig @@ -32,6 +40,7 @@ class TxBackpackConfig char *GetSSID() { return m_config.ssid; } char *GetPassword() { return m_config.password; } uint8_t *GetGroupAddress() { return m_config.address; } + wifi_service_t GetWiFiService() { return m_config.wifiService; } // Setters void SetStorageProvider(ELRS_EEPROM *eeprom); @@ -40,6 +49,7 @@ class TxBackpackConfig void SetSSID(const char *ssid); void SetPassword(const char *ssid); void SetGroupAddress(const uint8_t address[6]); + void SetWiFiService(wifi_service_t service); private: tx_backpack_config_t m_config; diff --git a/src/Timer_main.cpp b/src/Timer_main.cpp index b73f1665..d84692d7 100644 --- a/src/Timer_main.cpp +++ b/src/Timer_main.cpp @@ -87,7 +87,7 @@ void registerPeer(uint8_t* address); esp_now_peer_info_t peerInfo; #endif -void RebootIntoWifi() +void RebootIntoWifi(wifi_service_t service = WIFI_SERVICE_UPDATE) { DBGLN("Rebooting into wifi update mode..."); config.SetStartWiFiOnBoot(true); diff --git a/src/Tx_main.cpp b/src/Tx_main.cpp index 3ce4facf..855e9ab6 100644 --- a/src/Tx_main.cpp +++ b/src/Tx_main.cpp @@ -22,6 +22,10 @@ #include "devButton.h" #include "devLED.h" +#if defined(MAVLINK_ENABLED) +#include +#endif + /////////// GLOBALS /////////// uint8_t bindingAddress[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; @@ -29,6 +33,8 @@ uint8_t bindingAddress[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; const uint8_t version[] = {LATEST_VERSION}; connectionState_e connectionState = starting; +// Assume we are in wifi update mode until we know otherwise +wifi_service_t wifiService = WIFI_SERVICE_UPDATE; unsigned long rebootTime = 0; bool cacheFull = false; @@ -65,10 +71,14 @@ void sendMSPViaEspnow(mspPacket_t *packet); esp_now_peer_info_t peerInfo; #endif -void RebootIntoWifi() +void RebootIntoWifi(wifi_service_t service = WIFI_SERVICE_UPDATE) { DBGLN("Rebooting into wifi update mode..."); config.SetStartWiFiOnBoot(true); +#if defined(TARGET_TX_BACKPACK) + // TODO it might be better to add wifi service to each type of backpack + config.SetWiFiService(service); +#endif config.Commit(); rebootTime = millis(); } @@ -236,6 +246,46 @@ void SendCachedMSP() } } +#if defined(MAVLINK_ENABLED) +// This function is only used when we're not yet in WiFi MAVLink mode - its main purpose is to detect +// heartbeat packets and switch to WiFi MAVLink mode if we receive 3 heartbeats in 5 seconds, or if we +// receive a command to switch to WiFi MAVLink mode. +void ProcessMAVLinkFromTX(mavlink_message_t *mavlink_rx_message, mavlink_status_t *mavlink_status) +{ + // See if we have an explicit control message + MAVLink::handleControlMessage(mavlink_rx_message); + static unsigned long heartbeatTimestamps[3] = {0, 0, 0}; + switch (mavlink_rx_message->msgid) + { + case MAVLINK_MSG_ID_HEARTBEAT: + { + mavlink_heartbeat_t heartbeat; + mavlink_msg_heartbeat_decode(mavlink_rx_message, &heartbeat); + if (mavlink_rx_message->compid == MAV_COMP_ID_AUTOPILOT1) + { + // If we hit this line we shouldn't be in WiFi update mode but let's be certain. + if (connectionState != wifiUpdate) + { + unsigned long now = millis(); + // Push the timestamp we received the heartbeat back into the array + for (int i = 0; i < 2; i++) + { + heartbeatTimestamps[i] = heartbeatTimestamps[i + 1]; + } + heartbeatTimestamps[2] = now; + // If we have received 3 heartbeats in the last 5 seconds, switch to WiFi update mode + if (now - heartbeatTimestamps[0] < 5000) + { + RebootIntoWifi(WIFI_SERVICE_MAVLINK_TX); + } + } + } + break; + } + } +} +#endif + void SetSoftMACAddress() { if (!firmwareOptions.hasUID) @@ -295,6 +345,7 @@ void setup() Serial1.setDebugOutput(true); #endif Serial.begin(460800); + Serial.setRxBufferSize(4096); options_init(); @@ -311,6 +362,12 @@ void setup() if (config.GetStartWiFiOnBoot()) { config.SetStartWiFiOnBoot(false); + if (config.GetWiFiService() != WIFI_SERVICE_UPDATE) + { + // Store the intended service mode before writing default back to persistent flash + wifiService = config.GetWiFiService(); + config.SetWiFiService(WIFI_SERVICE_UPDATE); + } config.Commit(); connectionState = wifiUpdate; devicesTriggerEvent(); @@ -378,6 +435,15 @@ void loop() ProcessMSPPacketFromTX(msp.getReceivedPacket()); msp.markPacketReceived(); } +#if defined(MAVLINK_ENABLED) + // Process MAVLink messages to switch to wifi update mode + mavlink_message_t mavlink_rx_message; + mavlink_status_t mavlink_status; + if (mavlink_parse_char(MAVLINK_COMM_0, c, &mavlink_rx_message, &mavlink_status)) + { + ProcessMAVLinkFromTX(&mavlink_rx_message, &mavlink_status); + } +#endif } if (cacheFull && sendCached) diff --git a/src/Vrx_main.cpp b/src/Vrx_main.cpp index 17f7a97c..dd20d8c0 100644 --- a/src/Vrx_main.cpp +++ b/src/Vrx_main.cpp @@ -127,7 +127,7 @@ void SetupEspNow(); ///////////////////////////////////// -void RebootIntoWifi() +void RebootIntoWifi(wifi_service_t service = WIFI_SERVICE_UPDATE) { DBGLN("Rebooting into wifi update mode..."); config.SetStartWiFiOnBoot(true); diff --git a/src/module_base.cpp b/src/module_base.cpp index 89bf42e7..24e27068 100644 --- a/src/module_base.cpp +++ b/src/module_base.cpp @@ -5,8 +5,9 @@ #include "device.h" #include "msptypes.h" #include "logging.h" +#include -void RebootIntoWifi(); +void RebootIntoWifi(wifi_service_t service = WIFI_SERVICE_UPDATE); bool BindingExpired(uint32_t now); extern uint8_t backpackVersion[]; extern bool headTrackingEnabled; diff --git a/targets/common.ini b/targets/common.ini index fd01d700..5fc84ebe 100644 --- a/targets/common.ini +++ b/targets/common.ini @@ -14,6 +14,8 @@ lib_deps = [common_env_data] build_src_filter = +<*> -<.git/> - - - - - -<*.py> -<*test*.*> build_flags = -Wall -Iinclude +# keep in sync with the version in the main firmware +mavlink_lib_dep = https://github.com/mavlink/c_library_v2.git#e54a8d2e8cf7985e689ad1c8c8f37dc0800ea87b # ------------------------- COMMON ESP8285 DEFINITIONS ----------------- [env_common_esp8285] @@ -79,6 +81,8 @@ lib_ignore = STM32UPDATE build_flags = ${common_env_data.build_flags} -D TARGET_TX_BACKPACK +lib_deps = + ${env.lib_deps} build_src_filter = ${common_env_data.build_src_filter} - - - - - - - - - # ------------------------- COMMON RAPIDFIRE-BACKPACK DEFINITIONS ----------------- diff --git a/targets/txbp_esp.ini b/targets/txbp_esp.ini index c9afc8f8..4e5b2c33 100644 --- a/targets/txbp_esp.ini +++ b/targets/txbp_esp.ini @@ -4,9 +4,13 @@ [env:ESP_TX_Backpack_via_UART] extends = env_common_esp8285, tx_backpack_common +lib_deps = + ${tx_backpack_common.lib_deps} + ${common_env_data.mavlink_lib_dep} build_flags = ${env_common_esp8285.build_flags} ${tx_backpack_common.build_flags} + -D MAVLINK_ENABLED=1 -D PIN_BUTTON=0 -D PIN_LED=16 upload_resetmethod = nodemcu @@ -27,9 +31,13 @@ upload_command = python "$PROJECT_DIR/python/external/esptool/esptool.py" --pass [env:ESP32C3_TX_Backpack_via_UART] extends = env_common_esp32c3, tx_backpack_common +lib_deps = + ${tx_backpack_common.lib_deps} + ${common_env_data.mavlink_lib_dep} build_flags = ${env_common_esp32c3.build_flags} ${tx_backpack_common.build_flags} + -D MAVLINK_ENABLED=1 -D PIN_BUTTON=9 -D PIN_LED=8 upload_resetmethod = nodemcu