SITL: add ELRS simulator

libraries/AP_HAL/utility/DataRateLimit.cpp

@@ -0,0 +1,25 @@
+#include "DataRateLimit.h"
+
+#include <AP_HAL/AP_HAL.h>
+
+// Return the max number of bytes that can be sent since the last call given byte/s rate limit
+uint32_t DataRateLimit::max_bytes(const float bytes_per_sec)
+{
+    // Time since last call
+    const uint32_t now_us = AP_HAL::micros();
+    const float dt = (now_us - last_us) * 1.0e-6;
+    last_us = now_us;
+
+    // Maximum number of bytes that could be transferred in that time
+    float max_bytes = bytes_per_sec * dt;
+
+    // Add on the remainder from the last call, this prevents cumulative rounding errors
+    max_bytes += remainder;
+
+    // Get integer number of bytes and store the remainder
+    float max_bytes_int;
+    remainder = modf(max_bytes, &max_bytes_int);
+
+    // Add 0.5 to make sure the float rounds to the correct int
+    return uint32_t(max_bytes_int + 0.5);
+}

libraries/AP_HAL/utility/DataRateLimit.h

@@ -0,0 +1,26 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   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.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+
+// Returns the max number of bytes that can be sent since the last call given byte/s rate limit
+class DataRateLimit {
+public:
+    uint32_t max_bytes(const float bytes_per_sec);
+private:
+    uint32_t last_us;
+    float remainder;
+};

libraries/AP_HAL_SITL/SITL_State_common.cpp

@@ -25,7 +25,7 @@ extern const AP_HAL::HAL& hal;
 using namespace HALSITL;
 
 #define streq(a, b) (!strcmp(a, b))
-SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const char *arg)
+SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const char *arg, const uint8_t portNumber)
 {
     if (streq(name, "benewake_tf02")) {
         if (benewake_tf02 != nullptr) {
@@ -314,6 +314,14 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const
         }
         gps[x-1] = new SITL::GPS(x-1);
         return gps[x-1];
+    } else if (streq(name, "ELRS")) {
+        // Only allocate if not done already
+        // MAVLink serial ports have begin called several times
+        if (elrs == nullptr) {
+            elrs = new SITL::ELRS(portNumber, this);
+            _sitl->set_stop_MAVLink_sim_state();
+        }
+        return elrs;
     }
 
     AP_HAL::panic("unknown simulated device: %s", name);
@@ -481,6 +489,10 @@ void SITL_State_Common::sim_update(void)
             gps[i]->update();
         }
     }
+
+    if (elrs != nullptr) {
+        elrs->update();
+    }
 }
 
 /*

libraries/AP_HAL_SITL/SITL_State_common.h

@@ -53,6 +53,8 @@
 #include <SITL/SIM_Loweheiser.h>
 #include <SITL/SIM_FETtecOneWireESC.h>
 
+#include <SITL/SIM_ELRS.h>
+
 #include "AP_HAL_SITL.h"
 #include "AP_HAL_SITL_Namespace.h"
 #include "HAL_SITL_Class.h"
@@ -88,7 +90,7 @@ class HALSITL::SITL_State_Common {
 
     // create a simulated serial device; type of device is given by
     // name parameter
-    SITL::SerialDevice *create_serial_sim(const char *name, const char *arg);
+    SITL::SerialDevice *create_serial_sim(const char *name, const char *arg, const uint8_t portNumber);
 
     // simulated airspeed, sonar and battery monitor
     float sonar_pin_voltage;    // pin 0
@@ -231,6 +233,9 @@ class HALSITL::SITL_State_Common {
     // simulated GPS devices
     SITL::GPS *gps[2];  // constrained by # of parameter sets
 
+    // Simulated ELRS radio
+    SITL::ELRS *elrs;
+
     // returns a voltage between 0V to 5V which should appear as the
     // voltage from the sensor
     float _sonar_pin_voltage() const;

libraries/AP_HAL_SITL/UARTDriver.cpp

@@ -77,11 +77,11 @@ void UARTDriver::_begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
     if (strcmp(path, "GPS1") == 0) {
         /* gps */
         _connected = true;
-        _sim_serial_device = _sitlState->create_serial_sim("gps:1", "");
+        _sim_serial_device = _sitlState->create_serial_sim("gps:1", "", _portNumber);
     } else if (strcmp(path, "GPS2") == 0) {
         /* 2nd gps */
         _connected = true;
-        _sim_serial_device = _sitlState->create_serial_sim("gps:2", "");
+        _sim_serial_device = _sitlState->create_serial_sim("gps:2", "", _portNumber);
     } else {
         /* parse type:args:flags string for path. 
            For example:
@@ -109,7 +109,7 @@ void UARTDriver::_begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
             // add sanity check here that we're doing mavlink on this port?
             ::printf("SIM-ADSB connection on SERIAL%u\n", _portNumber);
             _connected = true;
-            _sim_serial_device = _sitlState->create_serial_sim("adsb", nullptr);
+            _sim_serial_device = _sitlState->create_serial_sim("adsb", nullptr, _portNumber);
         } else
 #endif
         if (strcmp(devtype, "tcp") == 0) {
@@ -132,7 +132,7 @@ void UARTDriver::_begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
             if (!_connected) {
                 ::printf("SIM connection %s:%s on SERIAL%u\n", args1, args2, _portNumber);
                 _connected = true;
-                _sim_serial_device = _sitlState->create_serial_sim(args1, args2);
+                _sim_serial_device = _sitlState->create_serial_sim(args1, args2, _portNumber);
             }
         } else if (strcmp(devtype, "udpclient") == 0) {
             // udp client connection
@@ -815,13 +815,11 @@ void UARTDriver::handle_writing_from_writebuffer_to_device()
     SITL::SIM *_sitl = AP::sitl();
     if (_sitl && _sitl->telem_baudlimit_enable) {
         // limit byte rate to configured baudrate
-        uint32_t now = AP_HAL::micros();
-        float dt = 1.0e-6 * (now - last_write_tick_us);
-        max_bytes = _uart_baudrate * dt / 10;
+        // Byte rate is bit rate divided by 10. 8 bits of data + start/stop bits
+        max_bytes = baud_limits.write.max_bytes(float(_uart_baudrate) * 0.1);
         if (max_bytes == 0) {
             return;
         }
-        last_write_tick_us = now;
     }
 #endif
     if (_packetise) {
@@ -884,13 +882,11 @@ void UARTDriver::handle_reading_from_device_to_readbuffer()
     SITL::SIM *_sitl = AP::sitl();
     if (_sitl && _sitl->telem_baudlimit_enable) {
         // limit byte rate to configured baudrate
-        uint32_t now = AP_HAL::micros();
-        float dt = 1.0e-6 * (now - last_read_tick_us);
-        max_bytes = _uart_baudrate * dt / 10;
+        // Byte rate is bit rate divided by 10. 8 bits of data + start/stop bits
+        max_bytes = baud_limits.read.max_bytes(float(_uart_baudrate) * 0.1);
         if (max_bytes == 0) {
             return;
         }
-        last_read_tick_us = now;
     }
 #endif
 

libraries/AP_HAL_SITL/UARTDriver.h

@@ -9,6 +9,7 @@
 #include <AP_HAL/utility/Socket_native.h>
 #include <AP_HAL/utility/RingBuffer.h>
 #include <AP_CSVReader/AP_CSVReader.h>
+#include <AP_HAL/utility/DataRateLimit.h>
 
 #include <SITL/SIM_SerialDevice.h>
 
@@ -113,8 +114,10 @@ class HALSITL::UARTDriver : public AP_HAL::UARTDriver {
     uint16_t _mc_myport;
 
     // for baud-rate limiting:
-    uint32_t last_read_tick_us;
-    uint32_t last_write_tick_us;
+    struct {
+        DataRateLimit write;
+        DataRateLimit read;
+    } baud_limits;
 
     HAL_Semaphore write_mtx;
 

libraries/SITL/SIM_ELRS.cpp

@@ -0,0 +1,165 @@
+#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"
+
+// Example command: -A --serial2=sim:ELRS
+// TCP connection will be started on normal AP port eg 5763 for serial 2
+
+// Baud rate must be set correctly
+// param set SERIAL2_BAUD 460
+
+using namespace SITL;
+
+ELRS::ELRS(const uint8_t portNumber, HALSITL::SITL_State_Common *sitl_state) :
+    // Mirror typical ELRS UART buffer sizes
+    SerialDevice::SerialDevice(64, 128),
+    target_address("127.0.0.1"),
+    target_port(5761 + portNumber),
+    // Mirror MAVLink buffer sizes
+    mavlinkInputBuffer(2048),
+    mavlinkOutputBuffer(2048),
+    // Typical setup is about 500 B /s
+    input_data_rate(500),
+    output_data_rate(500),
+    // 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)
+{
+
+    // Setup TCP server
+    listener.reuseaddress();
+    listener.bind(target_address, target_port);
+    listener.listen(1);
+    listener.set_blocking(false);
+
+}
+
+void ELRS::update()
+{
+    // Connect to incoming TCP
+    if (sock == nullptr) {
+        sock = listener.accept(0);
+        if (sock != nullptr) {
+            sock->set_blocking(false);
+            sock->reuseaddress();
+            ::printf("ELRS connected to %s:%u\n", target_address, (unsigned)target_port);
+        }
+    }
+    if (sock == nullptr) {
+        return;
+    }
+
+    // Read from AP into radio
+    const uint32_t input_space = mavlinkInputBuffer.space();
+    if (input_space > 0) {
+        uint8_t buf[input_space];
+        ssize_t len = read_from_autopilot((char*)buf, input_space);
+        mavlinkInputBuffer.write(buf, len);
+    }
+
+    // Send from radio to GCS
+    const uint32_t send_bytes = input_limit.max_bytes(input_data_rate);
+    if (send_bytes > 0) {
+        uint8_t buf[send_bytes];
+        const uint32_t len = mavlinkInputBuffer.read(buf, send_bytes);
+        if (len > 0) {
+            sock->send(buf, len);
+        }
+    }
+
+    // Incoming data from GCS to radio
+    const uint32_t receive_bytes = output_limit.max_bytes(output_data_rate);
+    if (receive_bytes > 0) {
+        uint8_t buf[receive_bytes];
+        const ssize_t len = sock->recv(buf, receive_bytes, 1);
+        if (len > 0) {
+            mavlinkOutputBuffer.write(buf, len);
+        } else if (len == 0) {
+            // EOF, go back to waiting for a new connection
+            delete sock;
+            sock = nullptr;
+        }
+    }
+
+    // Write from radio to AP
+    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()
+{
+
+    // Send radio messages at 100Hz
+    const uint32_t now = AP_HAL::millis();
+    if ((now - lastSentFlowCtrl) > 10) {
+        lastSentFlowCtrl = now; 
+
+        // Space remaining as a percentage.
+        const uint8_t percentage_remaining = (mavlinkInputBuffer.space() * 100) / mavlinkInputBuffer.get_size();
+
+        // 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
+        };
+
+        uint8_t buf[MAVLINK_MSG_ID_RADIO_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES];
+        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 (!mavlinkOutputBuffer.read_byte(&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
+
+            // Forward message to the UART
+            uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+            uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
+            uint16_t written = write_to_autopilot((char*)buf, len);
+            if ((written != uint16_t(-1)) && (len != written)) {
+                ::fprintf(stderr, "Failed to write full msg, wanted %u achieved %u (msg id: %u)\n", len, written, msg.msgid);
+            }
+        }
+    }
+
+}
+
+#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL