diff --git a/libraries/AP_HAL/utility/DataRateLimit.cpp b/libraries/AP_HAL/utility/DataRateLimit.cpp new file mode 100644 index 0000000000000..a61259428b919 --- /dev/null +++ b/libraries/AP_HAL/utility/DataRateLimit.cpp @@ -0,0 +1,25 @@ +#include "DataRateLimit.h" + +#include + +// 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); +} diff --git a/libraries/AP_HAL/utility/DataRateLimit.h b/libraries/AP_HAL/utility/DataRateLimit.h new file mode 100644 index 0000000000000..ec8f55e37a305 --- /dev/null +++ b/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 . + */ +#pragma once + +#include + +// 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; +}; diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp index de94ee65900be..52f67237ac2be 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.cpp +++ b/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(); + } } /* diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h index 19e7356c3b605..de3d75d657ac4 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.h +++ b/libraries/AP_HAL_SITL/SITL_State_common.h @@ -53,6 +53,8 @@ #include #include +#include + #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; diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index cb645d11f625e..8a9600ca4d6c1 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/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 diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h index 88803c9ddbe40..11a272d7524ef 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.h +++ b/libraries/AP_HAL_SITL/UARTDriver.h @@ -9,6 +9,7 @@ #include #include #include +#include #include @@ -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; diff --git a/libraries/SITL/SIM_ELRS.cpp b/libraries/SITL/SIM_ELRS.cpp new file mode 100644 index 0000000000000..3136c762124b4 --- /dev/null +++ b/libraries/SITL/SIM_ELRS.cpp @@ -0,0 +1,165 @@ +#include + +// Only support ELRS simulation in SITL (not Sim on Hardware) +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + +#include "SIM_ELRS.h" +#include + +#include +#include + +#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 diff --git a/libraries/SITL/SIM_ELRS.h b/libraries/SITL/SIM_ELRS.h new file mode 100644 index 0000000000000..cd0460a1bae55 --- /dev/null +++ b/libraries/SITL/SIM_ELRS.h @@ -0,0 +1,51 @@ +#pragma once + +#include +#include "SIM_SerialDevice.h" +#include +#include + +namespace SITL { + +class ELRS : public SerialDevice { +public: + ELRS(const uint8_t portNumber, HALSITL::SITL_State_Common *sitl_state); + + 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); + + ByteBuffer mavlinkInputBuffer; + ByteBuffer mavlinkOutputBuffer; + + DataRateLimit input_limit; + DataRateLimit output_limit; + + uint32_t lastSentFlowCtrl; + + const uint8_t this_system_id; + const uint8_t this_component_id; + + // Air data rate limits in bytes per second + const float input_data_rate; + const float output_data_rate; + + // Sockets for communicating with GCS + SocketAPM_native listener {false}; + SocketAPM_native *sock = nullptr; + const char *target_address; + const uint16_t target_port; + +}; + +} diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index abd099baf1809..cdbeb8405d7ec 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -1195,6 +1195,11 @@ const Location post_origin { /* report SITL state via MAVLink SIMSTATE*/ void SIM::simstate_send(mavlink_channel_t chan) const { + if (stop_MAVLink_sim_state) { + // Sim only MAVLink messages disabled to give more relaistic data rates + return; + } + float yaw; // convert to same conventions as DCM @@ -1220,6 +1225,11 @@ void SIM::simstate_send(mavlink_channel_t chan) const /* report SITL state via MAVLink SIM_STATE */ void SIM::sim_state_send(mavlink_channel_t chan) const { + if (stop_MAVLink_sim_state) { + // Sim only MAVLink messages disabled to give more relaistic data rates + return; + } + // convert to same conventions as DCM float yaw = state.yawDeg; if (yaw > 180) { diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 5198ba1f32b32..779c763ada9ee 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -543,6 +543,11 @@ class SIM { AP_Int16 osd_rows; AP_Int16 osd_columns; #endif + + // Allow inhibiting of SITL only sim state messages over MAVLink + // This gives more realistic data rates for testing links + void set_stop_MAVLink_sim_state() { stop_MAVLink_sim_state = true; } + bool stop_MAVLink_sim_state; }; } // namespace SITL