Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 :
3 : // Only support ELRS simulation in SITL (not Sim on Hardware)
4 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
5 :
6 : #include "SIM_ELRS.h"
7 : #include <SITL/SITL.h>
8 :
9 : #include <AP_HAL_SITL/AP_HAL_SITL.h>
10 : #include <AP_HAL_SITL/UARTDriver.h>
11 :
12 : #include "include/mavlink/v2.0/all/mavlink.h"
13 :
14 : // Example command: -A --serial2=sim:ELRS
15 : // TCP connection will be started on normal AP port eg 5763 for serial 2
16 :
17 : // Baud rate must be set correctly
18 : // param set SERIAL2_BAUD 460
19 :
20 : using namespace SITL;
21 :
22 0 : ELRS::ELRS(const uint8_t portNumber, HALSITL::SITL_State_Common *sitl_state) :
23 : // Mirror typical ELRS UART buffer sizes
24 : SerialDevice::SerialDevice(64, 128),
25 : // Mirror MAVLink buffer sizes
26 0 : mavlinkInputBuffer(2048),
27 0 : mavlinkOutputBuffer(2048),
28 : // 255 is typically used by the GCS, for RC override to work in ArduPilot `MAV_GCS_SYSID` must be set to this value (255 is the default)
29 0 : this_system_id(255),
30 : // Strictly this is not a valid source component ID
31 0 : this_component_id(MAV_COMPONENT::MAV_COMP_ID_ALL),
32 : // Typical setup is about 500 B /s
33 0 : input_data_rate(500),
34 0 : output_data_rate(500),
35 0 : target_address("127.0.0.1"),
36 0 : target_port(5761 + portNumber)
37 : {
38 :
39 : // Setup TCP server
40 0 : listener.reuseaddress();
41 0 : listener.bind(target_address, target_port);
42 0 : listener.listen(1);
43 0 : listener.set_blocking(false);
44 :
45 0 : }
46 :
47 0 : void ELRS::update()
48 : {
49 : // Connect to incoming TCP
50 0 : if (sock == nullptr) {
51 0 : sock = listener.accept(0);
52 0 : if (sock != nullptr) {
53 0 : sock->set_blocking(false);
54 0 : sock->reuseaddress();
55 0 : ::printf("ELRS connected to %s:%u\n", target_address, (unsigned)target_port);
56 : }
57 : }
58 0 : if (sock == nullptr) {
59 0 : return;
60 : }
61 :
62 : // Read from AP into radio
63 0 : const uint32_t input_space = mavlinkInputBuffer.space();
64 0 : if (input_space > 0) {
65 0 : uint8_t buf[input_space];
66 0 : ssize_t len = read_from_autopilot((char*)buf, input_space);
67 0 : mavlinkInputBuffer.write(buf, len);
68 0 : }
69 :
70 : // Send from radio to GCS
71 0 : const uint32_t send_bytes = input_limit.max_bytes(input_data_rate);
72 0 : if (send_bytes > 0) {
73 0 : uint8_t buf[send_bytes];
74 0 : const uint32_t len = mavlinkInputBuffer.read(buf, send_bytes);
75 0 : if (len > 0) {
76 0 : sock->send(buf, len);
77 : }
78 0 : }
79 :
80 : // Incoming data from GCS to radio
81 0 : const uint32_t receive_bytes = output_limit.max_bytes(output_data_rate);
82 0 : if (receive_bytes > 0) {
83 0 : uint8_t buf[receive_bytes];
84 0 : const ssize_t len = sock->recv(buf, receive_bytes, 1);
85 0 : if (len > 0) {
86 0 : mavlinkOutputBuffer.write(buf, len);
87 0 : } else if (len == 0) {
88 : // EOF, go back to waiting for a new connection
89 0 : delete sock;
90 0 : sock = nullptr;
91 : }
92 0 : }
93 :
94 : // Write from radio to AP
95 0 : sendQueuedData();
96 : }
97 :
98 : // Function to behave like MAVLink libs `mavlink_parse_char` but use local buffer
99 0 : uint8_t ELRS::mavlink_parse_char_helper(uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
100 : {
101 0 : uint8_t msg_received = mavlink_frame_char_buffer(&mavlink.rxmsg, &mavlink.status, c, r_message, r_mavlink_status);
102 0 : if ((msg_received == MAVLINK_FRAMING_BAD_CRC) || (msg_received == MAVLINK_FRAMING_BAD_SIGNATURE)) {
103 0 : return 0;
104 : }
105 0 : return msg_received;
106 : }
107 :
108 : // Send incoming data to AP, this is a re-implementation of the ELRS function found here:
109 : // https://github.com/ExpressLRS/ExpressLRS/blob/0d31863f34ca16a036e94a9c2a56038ae56c7f9e/src/src/rx-serial/SerialMavlink.cpp#L78
110 0 : void ELRS::sendQueuedData()
111 : {
112 :
113 : // Send radio messages at 100Hz
114 0 : const uint32_t now = AP_HAL::millis();
115 0 : if ((now - lastSentFlowCtrl) > 10) {
116 0 : lastSentFlowCtrl = now;
117 :
118 : // Space remaining as a percentage.
119 0 : const uint8_t percentage_remaining = (mavlinkInputBuffer.space() * 100) / mavlinkInputBuffer.get_size();
120 :
121 : // Populate radio status packet
122 0 : const mavlink_radio_status_t radio_status {
123 : rxerrors: 0,
124 : fixed: 0,
125 : rssi: UINT8_MAX, // Unknown
126 : remrssi: UINT8_MAX, // Unknown
127 : txbuf: percentage_remaining,
128 : noise: UINT8_MAX, // Unknown
129 : remnoise: UINT8_MAX, // Unknown
130 0 : };
131 :
132 : uint8_t buf[MAVLINK_MSG_ID_RADIO_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES];
133 : mavlink_message_t msg;
134 0 : mavlink_msg_radio_status_encode(this_system_id, this_component_id, &msg, &radio_status);
135 0 : uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
136 0 : write_to_autopilot((char*)buf, len);
137 : }
138 :
139 : // Read one byte at a time until were done
140 : while (true) {
141 : uint8_t c;
142 0 : if (!mavlinkOutputBuffer.read_byte(&c)) {
143 0 : break;
144 : }
145 :
146 : mavlink_message_t msg;
147 : mavlink_status_t status;
148 :
149 : // Try parse a mavlink message
150 0 : if (mavlink_parse_char_helper(c, &msg, &status)) {
151 : // Message decoded successfully
152 :
153 : // Forward message to the UART
154 : uint8_t buf[MAVLINK_MAX_PACKET_LEN];
155 0 : uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
156 0 : uint16_t written = write_to_autopilot((char*)buf, len);
157 0 : if ((written != uint16_t(-1)) && (len != written)) {
158 0 : ::fprintf(stderr, "Failed to write full msg, wanted %u achieved %u (msg id: %u)\n", len, written, msg.msgid);
159 : }
160 : }
161 0 : }
162 :
163 0 : }
164 :
165 : #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
