SITL: add ELRS MAVLink radio simulator

libraries/SITL/SIM_ELRS.cpp

@@ -0,0 +1,173 @@
+#include <AP_HAL/AP_HAL.h>
+
+// Only support ELRS simulation in SITL (not Sim on Hardware)
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+
+#include "SIM_ELRS.h"
+#include <SITL/SITL.h>
+
+#include <AP_HAL_SITL/AP_HAL_SITL.h>
+#include <AP_HAL_SITL/UARTDriver.h>
+
+#include "include/mavlink/v2.0/all/mavlink.h"
+
+#define MAV_FTP_OPCODE_OPENFILERO 4
+
+// Example command: -A --serial2=sim:ELRS:tcp:3
+
+using namespace SITL;
+
+ELRS::ELRS(const uint8_t portNumber, HALSITL::SITL_State_Common *sitl_state) :
+    SerialDevice::SerialDevice(),
+    // 255 is typically used by the GCS, for RC override to work in ArduPilot `SYSID_MYGCS` must be set to this value (255 is the default)
+    this_system_id(255),
+    // Strictly this is not a valid source component ID
+    this_component_id(MAV_COMPONENT::MAV_COMP_ID_ALL)
+{
+    // Create a UART to talk to the GCS as normal
+    uart = new HALSITL::UARTDriver(portNumber, (HALSITL::SITL_State*)sitl_state);
+    if (uart == nullptr) {
+        AP_HAL::panic("ELRS uart nullptr");
+        return;
+    }
+
+    // Set buffers and data rates to match typical ELRs config
+    uart->set_buffer_size(2048, 2048);
+
+    // 200Hz 1:2 -> 2000bps none burst and 3922bps burst -> 200 B/s to 392B/s
+    uart->set_data_rate_limit(392, 392);
+}
+
+// Called when begin is called on main serial port
+void ELRS::begin(uint32_t baud, uint16_t rxS, uint16_t txS, const char* path)
+{
+    if (uart == nullptr) {
+        AP_HAL::panic("ELRS uart nullptr");
+        return;
+    }
+
+    // Start of path must be "sim:ELRS:" since we exist
+    // Offset by those 9 characters for child uart
+    uart->begin(baud, rxS, txS, path + 9);
+
+    // Call through to base class method
+    SerialDevice::begin(baud, rxS, txS, nullptr);
+}
+
+void ELRS::uart_timer_tick()
+{
+    if (uart == nullptr) {
+        AP_HAL::panic("ELRS uart nullptr");
+        return;
+    }
+    uart->_timer_tick();
+}
+
+void ELRS::update()
+{
+    if (uart == nullptr) {
+        AP_HAL::panic("Radio_ELRs uart nullptr");
+        return;
+    }
+
+    // Read from AP and directly send out
+    while (true) {
+        uint8_t buf[100];
+        ssize_t len = read_from_autopilot((char*)buf, sizeof(buf));
+        if (len == 0) {
+            break;
+        }
+        uart->write(buf, len);
+    }
+
+    // Write to AP from link
+    sendQueuedData();
+}
+
+// Function to behave like MAVLink libs `mavlink_parse_char` but use local buffer
+uint8_t ELRS::mavlink_parse_char_helper(uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
+{
+    uint8_t msg_received = mavlink_frame_char_buffer(&mavlink.rxmsg, &mavlink.status, c, r_message, r_mavlink_status);
+    if ((msg_received == MAVLINK_FRAMING_BAD_CRC) || (msg_received == MAVLINK_FRAMING_BAD_SIGNATURE)) {
+        return 0;
+    }
+    return msg_received;
+}
+
+// Send incoming data to AP, this is a re-implementation of the ELRS function found here:
+// https://github.com/ExpressLRS/ExpressLRS/blob/0d31863f34ca16a036e94a9c2a56038ae56c7f9e/src/src/rx-serial/SerialMavlink.cpp#L78
+void ELRS::sendQueuedData()
+{
+    // Buffer used to store MAVLink messages, either generated locally or passing-through
+    uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+
+    // Send radio messages at 100Hz
+    const uint32_t now = AP_HAL::millis();
+    if ((now - lastSentFlowCtrl) > 10) {
+        lastSentFlowCtrl = now; 
+
+        // We set size to 2048 so we can work out the space remaining as a percentage.
+        const uint8_t percentage_remaining = (uart->txspace() * 100) / 2048;
+
+        // Populate radio status packet
+        const mavlink_radio_status_t radio_status {
+            rxerrors: 0,
+            fixed: 0,
+            rssi: UINT8_MAX, // Unknown
+            remrssi: UINT8_MAX, // Unknown
+            txbuf: percentage_remaining,
+            noise: UINT8_MAX, // Unknown
+            remnoise: UINT8_MAX, // Unknown
+        };
+
+        mavlink_message_t msg;
+        mavlink_msg_radio_status_encode(this_system_id, this_component_id, &msg, &radio_status);
+        uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
+        write_to_autopilot((char*)buf, len);
+    }
+
+    // Read one byte at a time until were done
+    while (true) {
+        uint8_t c;
+        if (!uart->read(c)) {
+            break;
+        }
+
+        mavlink_message_t msg;
+        mavlink_status_t status;
+
+        // Try parse a mavlink message
+        if (mavlink_parse_char_helper(c, &msg, &status)) {
+            // Message decoded successfully
+
+            // Check if its the ftp file request
+            if (msg.msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
+                // If it is, we want to deliberately hack the ftp url, so that the request fails
+                // Borrowed and modified from mLRS
+                mavlink_msg_file_transfer_protocol_get_payload(&msg, buf);
+
+                uint8_t target_component = buf[0];
+                uint8_t opcode = buf[3];
+                char* url = (char*)(buf + 12);
+
+                if (((target_component == MAV_COMP_ID_AUTOPILOT1) || (target_component == MAV_COMP_ID_ALL)) &&
+                    (opcode == MAV_FTP_OPCODE_OPENFILERO)) {
+                    if (!strncmp(url, "@PARAM/param.pck", 16)) {
+                        url[1] = url[7] = url[13] = 'x'; // now fake it to "@xARAM/xaram.xck"
+                        mavlink_message_t msg2;
+                        mavlink_msg_file_transfer_protocol_pack(msg.sysid, msg.compid, &msg2, 0, 0, target_component, (uint8_t*)url);
+                        uint16_t len = mavlink_msg_to_send_buffer(buf, &msg2);
+                        write_to_autopilot((char*)buf, len);
+                    }
+                }
+
+            } else {
+                uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
+                write_to_autopilot((char*)buf, len);
+            }
+        }
+    }
+
+}
+
+#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL

libraries/SITL/SIM_ELRS.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#include <SITL/SITL.h>
+#include "SIM_SerialDevice.h"
+
+namespace SITL {
+
+//class HALSITL;
+//class HALSITL::UARTDriver;
+class ELRS : public SerialDevice {
+public:
+    ELRS(const uint8_t portNumber, HALSITL::SITL_State_Common *sitl_state);
+
+    void begin(uint32_t baud, uint16_t rxS, uint16_t txS, const char* path) override;
+
+    void uart_timer_tick() override;
+
+    uint32_t device_baud() const override { return 460800; }
+
+    void update();
+
+private:
+    void sendQueuedData();
+
+    struct {
+        mavlink_message_t rxmsg;
+        mavlink_status_t status;
+    } mavlink;
+
+    uint8_t mavlink_parse_char_helper(uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status);
+
+    uint32_t lastSentFlowCtrl;
+    HALSITL::UARTDriver *uart;
+
+    const uint8_t this_system_id;
+    const uint8_t this_component_id;
+};
+
+}