Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : CRSF protocol decoder based on betaflight implementation
17 : Code by Andy Piper
18 : */
19 :
20 : #include "AP_RCProtocol_config.h"
21 :
22 : #if AP_RCPROTOCOL_CRSF_ENABLED
23 :
24 : #include "AP_RCProtocol.h"
25 : #include "AP_RCProtocol_CRSF.h"
26 : #include <AP_HAL/AP_HAL.h>
27 : #include <AP_Math/AP_Math.h>
28 : #include <AP_Math/crc.h>
29 : #include <AP_Vehicle/AP_Vehicle_Type.h>
30 : #include <AP_RCTelemetry/AP_CRSF_Telem.h>
31 : #include <AP_SerialManager/AP_SerialManager.h>
32 :
33 :
34 : #define CRSF_SUBSET_RC_STARTING_CHANNEL_BITS 5
35 : #define CRSF_SUBSET_RC_STARTING_CHANNEL_MASK 0x1F
36 : #define CRSF_SUBSET_RC_RES_CONFIGURATION_BITS 2
37 : #define CRSF_SUBSET_RC_RES_CONFIGURATION_MASK 0x03
38 : #define CRSF_SUBSET_RC_RESERVED_CONFIGURATION_BITS 1
39 :
40 : #define CRSF_RC_CHANNEL_SCALE_LEGACY 0.62477120195241f
41 : #define CRSF_SUBSET_RC_RES_CONF_10B 0
42 : #define CRSF_SUBSET_RC_RES_BITS_10B 10
43 : #define CRSF_SUBSET_RC_RES_MASK_10B 0x03FF
44 : #define CRSF_SUBSET_RC_CHANNEL_SCALE_10B 1.0f
45 : #define CRSF_SUBSET_RC_RES_CONF_11B 1
46 : #define CRSF_SUBSET_RC_RES_BITS_11B 11
47 : #define CRSF_SUBSET_RC_RES_MASK_11B 0x07FF
48 : #define CRSF_SUBSET_RC_CHANNEL_SCALE_11B 0.5f
49 : #define CRSF_SUBSET_RC_RES_CONF_12B 2
50 : #define CRSF_SUBSET_RC_RES_BITS_12B 12
51 : #define CRSF_SUBSET_RC_RES_MASK_12B 0x0FFF
52 : #define CRSF_SUBSET_RC_CHANNEL_SCALE_12B 0.25f
53 : #define CRSF_SUBSET_RC_RES_CONF_13B 3
54 : #define CRSF_SUBSET_RC_RES_BITS_13B 13
55 : #define CRSF_SUBSET_RC_RES_MASK_13B 0x1FFF
56 : #define CRSF_SUBSET_RC_CHANNEL_SCALE_13B 0.125f
57 :
58 : /*
59 : * CRSF protocol
60 : *
61 : * CRSF protocol uses a single wire half duplex uart connection.
62 : * The master sends one frame every 4ms and the slave replies between two frames from the master.
63 : *
64 : * 420000 baud
65 : * not inverted
66 : * 8 Bit
67 : * 1 Stop bit
68 : * Big endian
69 : * 416666 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte
70 : * Max frame size is 64 bytes
71 : * A 64 byte frame plus 1 sync byte can be transmitted in 1393 microseconds.
72 : *
73 : * CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1500 microseconds
74 : *
75 : * Every frame has the structure:
76 : * <Device address><Frame length><Type><Payload><CRC>
77 : *
78 : * Device address: (uint8_t)
79 : * Frame length: length in bytes including Type (uint8_t)
80 : * Type: (uint8_t)
81 : * CRC: (uint8_t)
82 : *
83 : */
84 :
85 : extern const AP_HAL::HAL& hal;
86 :
87 : //#define CRSF_DEBUG
88 : //#define CRSF_DEBUG_CHARS
89 : //#define CRSF_DEBUG_TELEM
90 : //#define CRSF_DEBUG_PARAMS
91 : #if defined(CRSF_DEBUG) || defined(CRSF_DEBUG_TELEM) || defined(CRSF_DEBUG_PARAMS)
92 : # define debug(fmt, args...) hal.console->printf("CRSF: " fmt "\n", ##args)
93 : # define get_frame_type(byte, subtype) AP_CRSF_Protocol::get_frame_type(byte, subtype)
94 : #else
95 : # define debug(fmt, args...) do {} while(0)
96 : #endif
97 :
98 : #define CRSF_FRAME_TIMEOUT_US 50000U // 50ms to account for failure of the frame sync and long scheduling delays
99 : #define CRSF_INTER_FRAME_TIME_US_250HZ 4000U // At fastest, frames are sent by the transmitter every 4 ms, 250 Hz
100 : #define CRSF_INTER_FRAME_TIME_US_150HZ 6667U // At medium, frames are sent by the transmitter every 6.667 ms, 150 Hz
101 : #define CRSF_INTER_FRAME_TIME_US_50HZ 20000U // At slowest, frames are sent by the transmitter every 20ms, 50 Hz
102 : #define CRSF_HEADER_TYPE_LEN (CRSF_HEADER_LEN + 1) // header length including type
103 :
104 : #define CRSF_DIGITAL_CHANNEL_MIN 172
105 : #define CRSF_DIGITAL_CHANNEL_MAX 1811
106 :
107 : #define CRSF_BAUDRATE_1MBIT 1000000U
108 : #define CRSF_BAUDRATE_2MBIT 2000000U
109 :
110 : const uint16_t AP_RCProtocol_CRSF::RF_MODE_RATES[RFMode::RF_MODE_MAX_MODES] = {
111 : 4, 50, 150, 250, // CRSF
112 : 4, 25, 50, 100, 100, 150, 200, 250, 333, 500, 250, 500, 500, 1000, 50 // ELRS
113 : };
114 :
115 : AP_RCProtocol_CRSF* AP_RCProtocol_CRSF::_singleton;
116 :
117 2094 : AP_RCProtocol_CRSF::AP_RCProtocol_CRSF(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend)
118 : {
119 : #if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
120 2094 : if (_singleton != nullptr) {
121 0 : AP_HAL::panic("Duplicate CRSF handler");
122 : }
123 :
124 2094 : _singleton = this;
125 : #else
126 0 : if (_singleton == nullptr) {
127 0 : _singleton = this;
128 : }
129 : #endif
130 : #if HAL_CRSF_TELEM_ENABLED && !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
131 2094 : _uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_CRSF, 0);
132 2094 : if (_uart) {
133 0 : start_uart();
134 : }
135 : #endif
136 2094 : }
137 :
138 908 : AP_RCProtocol_CRSF::~AP_RCProtocol_CRSF() {
139 454 : _singleton = nullptr;
140 908 : }
141 :
142 : // get the protocol string
143 0 : const char* AP_RCProtocol_CRSF::get_protocol_string(ProtocolType protocol) const {
144 0 : if (protocol == ProtocolType::PROTOCOL_ELRS) {
145 0 : return "ELRS";
146 0 : } else if (_crsf_v3_active) {
147 0 : return "CRSFv3";
148 : } else {
149 0 : return "CRSFv2";
150 : }
151 : }
152 :
153 : // return the link rate as defined by the LinkStatistics
154 0 : uint16_t AP_RCProtocol_CRSF::get_link_rate(ProtocolType protocol) const {
155 0 : if (protocol == ProtocolType::PROTOCOL_ELRS) {
156 0 : return RF_MODE_RATES[_link_status.rf_mode + RFMode::CRSF_RF_MAX_MODES];
157 0 : } else if (protocol == ProtocolType::PROTOCOL_TRACER) {
158 0 : return 250;
159 : } else {
160 0 : return RF_MODE_RATES[_link_status.rf_mode];
161 : }
162 : }
163 :
164 : // process a byte provided by a uart from rc stack
165 0 : void AP_RCProtocol_CRSF::process_byte(uint8_t byte, uint32_t baudrate)
166 : {
167 : // reject RC data if we have been configured for standalone mode
168 0 : if ((baudrate != CRSF_BAUDRATE && baudrate != CRSF_BAUDRATE_1MBIT && baudrate != CRSF_BAUDRATE_2MBIT) || _uart) {
169 0 : return;
170 : }
171 0 : _process_byte(byte);
172 : }
173 :
174 : // process a byte provided by a uart
175 0 : void AP_RCProtocol_CRSF::_process_byte(uint8_t byte)
176 : {
177 : //debug("process_byte(0x%x)", byte);
178 0 : const uint32_t now = AP_HAL::micros();
179 :
180 : // extra check for overflow, should never happen since it will have been handled in check_frame()
181 0 : if (_frame_ofs >= sizeof(_frame)) {
182 0 : _frame_ofs = 0;
183 : }
184 :
185 : // check for long frame gaps
186 : // we took too long decoding, start again - the RX will only send complete frames so this is unlikely to fail,
187 : // however thread scheduling can introduce longer delays even when the data has been received
188 0 : if (_frame_ofs > 0 && (now - _start_frame_time_us) > CRSF_FRAME_TIMEOUT_US) {
189 0 : _frame_ofs = 0;
190 : }
191 :
192 : // start of a new frame
193 0 : if (_frame_ofs == 0) {
194 0 : _start_frame_time_us = now;
195 : }
196 :
197 0 : _frame_bytes[_frame_ofs++] = byte;
198 :
199 0 : if (!check_frame(now)) {
200 0 : skip_to_next_frame(now);
201 : }
202 0 : }
203 :
204 : // check if a frame is valid. Return false if the frame is definitely
205 : // invalid. Return true if we need more bytes
206 0 : bool AP_RCProtocol_CRSF::check_frame(uint32_t timestamp_us)
207 : {
208 : // overflow check
209 0 : if (_frame_ofs >= sizeof(_frame)) {
210 0 : return false;
211 : }
212 :
213 : // need a header to get the length
214 0 : if (_frame_ofs < CRSF_HEADER_TYPE_LEN) {
215 0 : return true;
216 : }
217 :
218 0 : if (_frame.device_address != AP_CRSF_Protocol::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
219 0 : return false;
220 : }
221 :
222 : // check validity of the length byte if we have received it
223 0 : if (_frame_ofs >= CRSF_HEADER_TYPE_LEN &&
224 0 : _frame.length > CRSF_FRAME_PAYLOAD_MAX) {
225 0 : return false;
226 : }
227 :
228 0 : if (_frame.length < CRSF_FRAME_LENGTH_MIN) {
229 : // invalid short frame
230 0 : return false;
231 : }
232 :
233 : // decode whatever we got and expect
234 0 : if (_frame_ofs >= _frame.length + CRSF_HEADER_LEN) {
235 0 : const uint8_t crc = crc8_dvb_s2_update(0, &_frame_bytes[CRSF_HEADER_LEN], _frame.length - 1);
236 :
237 : //debug("check_frame(0x%x, 0x%x)", _frame.device_address, _frame.length);
238 :
239 0 : if (crc != _frame.payload[_frame.length - 2]) {
240 0 : return false;
241 : }
242 :
243 0 : log_data(AP_RCProtocol::CRSF, timestamp_us, (const uint8_t*)&_frame, _frame.length + CRSF_HEADER_LEN);
244 :
245 : // decode here
246 0 : if (decode_crsf_packet()) {
247 0 : _last_tx_frame_time_us = timestamp_us; // we have received a frame from the transmitter
248 0 : add_input(MAX_CHANNELS, _channels, false, _link_status.rssi, _link_status.link_quality);
249 : }
250 :
251 : // we consumed the frame
252 0 : const auto len = _frame.length + CRSF_HEADER_LEN;
253 0 : _frame_ofs -= len;
254 0 : if (_frame_ofs > 0) {
255 0 : memmove(_frame_bytes, _frame_bytes+len, _frame_ofs);
256 : }
257 :
258 0 : _last_frame_time_us = _last_rx_frame_time_us = timestamp_us;
259 :
260 0 : return true;
261 : }
262 :
263 : // more bytes to come
264 0 : return true;
265 : }
266 :
267 : // called when parsing or CRC fails on a frame
268 0 : void AP_RCProtocol_CRSF::skip_to_next_frame(uint32_t timestamp_us)
269 : {
270 0 : if (_frame_ofs <= 1) {
271 0 : return;
272 : }
273 :
274 : /*
275 : look for a frame header in the remaining bytes
276 : */
277 0 : const uint8_t *new_header = (const uint8_t *)memchr(&_frame_bytes[1], AP_CRSF_Protocol::CRSF_ADDRESS_FLIGHT_CONTROLLER, _frame_ofs - 1);
278 0 : if (new_header == nullptr) {
279 0 : _frame_ofs = 0;
280 0 : return;
281 : }
282 :
283 : /*
284 : setup the current state as the remaining bytes, if any
285 : */
286 0 : _frame_ofs -= (new_header - _frame_bytes);
287 0 : if (_frame_ofs > 0) {
288 0 : memmove(_frame_bytes, new_header, _frame_ofs);
289 : }
290 :
291 0 : _start_frame_time_us = timestamp_us;
292 :
293 : // we could now have a good frame
294 0 : check_frame(timestamp_us);
295 : }
296 :
297 36177494 : void AP_RCProtocol_CRSF::update(void)
298 : {
299 : // if we are in standalone mode, process data from the uart
300 36177494 : if (_uart) {
301 0 : uint32_t now = AP_HAL::millis();
302 : // for some reason it's necessary to keep trying to start the uart until we get data
303 0 : if (now - _last_uart_start_time_ms > 1000U && _last_frame_time_us == 0) {
304 0 : start_uart();
305 0 : _last_uart_start_time_ms = now;
306 : }
307 0 : uint32_t n = _uart->available();
308 0 : n = MIN(n, 255U);
309 0 : for (uint8_t i = 0; i < n; i++) {
310 0 : int16_t b = _uart->read();
311 0 : if (b >= 0) {
312 0 : _process_byte(uint8_t(b));
313 : }
314 : }
315 : }
316 :
317 : // never received RC frames, but have received CRSF frames so make sure we give the telemetry opportunity to run
318 36177494 : uint32_t now = AP_HAL::micros();
319 72354988 : if ((_last_frame_time_us > 0
320 : #if HAL_CRSF_TELEM_ENABLED
321 36177494 : || bind_in_progress()
322 : #endif
323 0 : ) && (!get_rc_input_count() || !is_tx_active())
324 72354988 : && now - _last_frame_time_us > CRSF_INTER_FRAME_TIME_US_250HZ) {
325 : // don't send telemetry unless the UART we are dealing with is configured to send it
326 0 : AP_HAL::UARTDriver *uart = get_available_UART();
327 0 : if (_uart || (uart && (uart->get_baud_rate() == CRSF_BAUDRATE || uart->get_baud_rate() == ELRS_BAUDRATE))) {
328 0 : process_telemetry(false);
329 0 : _last_frame_time_us = now;
330 : }
331 : }
332 :
333 : #if AP_RC_CHANNEL_ENABLED
334 : //Check if LQ is to be reported in place of RSSI
335 36177494 : _use_lq_for_rssi = rc().option_is_enabled(RC_Channels::Option::USE_CRSF_LQ_AS_RSSI);
336 : #endif
337 36177494 : }
338 :
339 : // write out a frame of any type
340 0 : void AP_RCProtocol_CRSF::write_frame(Frame* frame)
341 : {
342 0 : AP_HAL::UARTDriver *uart = get_current_UART();
343 :
344 0 : if (!uart) {
345 0 : return;
346 : }
347 : // calculate crc
348 0 : uint8_t crc = crc8_dvb_s2(0, frame->type);
349 0 : for (uint8_t i = 0; i < frame->length - 2; i++) {
350 0 : crc = crc8_dvb_s2(crc, frame->payload[i]);
351 : }
352 0 : frame->payload[frame->length - 2] = crc;
353 :
354 0 : uart->write((uint8_t*)frame, frame->length + 2);
355 0 : uart->flush();
356 :
357 : #if defined(CRSF_DEBUG) || defined(CRSF_DEBUG_PARAMS)
358 : #ifdef CRSF_DEBUG_PARAMS
359 : switch (frame->type) {
360 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY:
361 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_READ:
362 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_WRITE:
363 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_COMMAND:
364 : #endif
365 :
366 : hal.console->printf("CRSF: writing %s:", get_frame_type(frame->type, frame->payload[0]));
367 : for (uint8_t i = 0; i < frame->length + 2; i++) {
368 : uint8_t val = ((uint8_t*)frame)[i];
369 : #ifdef CRSF_DEBUG_CHARS
370 : if (val >= 32 && val <= 126) {
371 : hal.console->printf(" 0x%x '%c'", val, (char)val);
372 : } else {
373 : #endif
374 : hal.console->printf(" 0x%x", val);
375 : #ifdef CRSF_DEBUG_CHARS
376 : }
377 : #endif
378 : }
379 : hal.console->printf("\n");
380 : #ifdef CRSF_DEBUG_PARAMS
381 : }
382 : #endif
383 : #endif // defined(CRSF_DEBUG) || defined(CRSF_DEBUG_PARAMS)
384 : }
385 :
386 0 : bool AP_RCProtocol_CRSF::decode_crsf_packet()
387 : {
388 : #ifdef CRSF_DEBUG
389 : hal.console->printf("CRSF: received %s:", get_frame_type(_frame.type));
390 : uint8_t* fptr = (uint8_t*)&_frame;
391 : for (uint8_t i = 0; i < _frame.length + 2; i++) {
392 : #ifdef CRSF_DEBUG_CHARS
393 : if (fptr[i] >= 32 && fptr[i] <= 126) {
394 : hal.console->printf(" 0x%x '%c'", fptr[i], (char)fptr[i]);
395 : } else {
396 : #endif
397 : hal.console->printf(" 0x%x", fptr[i]);
398 : #ifdef CRSF_DEBUG_CHARS
399 : }
400 : #endif
401 : }
402 : hal.console->printf("\n");
403 : #endif
404 :
405 0 : bool rc_active = false;
406 :
407 0 : switch (_frame.type) {
408 0 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
409 : // scale factors defined by TBS - TICKS_TO_US(x) ((x - 992) * 5 / 8 + 1500)
410 0 : decode_11bit_channels((const uint8_t*)(&_frame.payload), CRSF_MAX_CHANNELS, _channels, 5U, 8U, 880U);
411 0 : _crsf_v3_active = false;
412 0 : rc_active = !_uart; // only accept RC data if we are not in standalone mode
413 0 : break;
414 0 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_LINK_STATISTICS:
415 0 : process_link_stats_frame((uint8_t*)&_frame.payload);
416 0 : break;
417 0 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED:
418 0 : decode_variable_bit_channels((const uint8_t*)(&_frame.payload), _frame.length, CRSF_MAX_CHANNELS, _channels);
419 0 : _crsf_v3_active = true;
420 0 : rc_active = !_uart; // only accept RC data if we are not in standalone mode
421 0 : break;
422 0 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_LINK_STATISTICS_RX:
423 0 : process_link_stats_rx_frame((uint8_t*)&_frame.payload);
424 0 : break;
425 0 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_LINK_STATISTICS_TX:
426 0 : process_link_stats_tx_frame((uint8_t*)&_frame.payload);
427 0 : break;
428 0 : default:
429 0 : break;
430 : }
431 : #if HAL_CRSF_TELEM_ENABLED
432 0 : if (AP_CRSF_Telem::process_frame(FrameType(_frame.type), (uint8_t*)&_frame.payload, _frame.length - 2U)) {
433 : #if defined(CRSF_DEBUG_TELEM) || defined(CRSF_DEBUG_PARAMS)
434 : switch (_frame.type) {
435 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
436 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_LINK_STATISTICS:
437 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED:
438 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_LINK_STATISTICS_RX:
439 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_LINK_STATISTICS_TX:
440 : break;
441 : #ifdef CRSF_DEBUG_PARAMS
442 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY:
443 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_READ:
444 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_WRITE:
445 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_COMMAND:
446 : #else
447 : default:
448 : #endif
449 : hal.console->printf("CRSF: received %s:", get_frame_type(_frame.type));
450 : uint8_t* fptr = (uint8_t*)&_frame;
451 : for (uint8_t i = 0; i < _frame.length + 2; i++) {
452 : hal.console->printf(" 0x%x", fptr[i]);
453 : }
454 : hal.console->printf("\n");
455 : }
456 : #endif // defined(CRSF_DEBUG_TELEM) || defined(CRSF_DEBUG_PARAMS)
457 0 : process_telemetry();
458 : }
459 : // process any pending baudrate changes before reading another frame
460 0 : if (_new_baud_rate > 0) {
461 0 : AP_HAL::UARTDriver *uart = get_current_UART();
462 :
463 0 : if (uart) {
464 : // wait for all the pending data to be sent
465 0 : while (uart->tx_pending()) {
466 0 : hal.scheduler->delay_microseconds(10);
467 : }
468 : // now wait for 4ms to account for RX transmission and processing
469 0 : hal.scheduler->delay(4);
470 : // change the baud rate
471 0 : uart->begin(_new_baud_rate);
472 : }
473 0 : _new_baud_rate = 0;
474 : }
475 : #endif
476 :
477 0 : return rc_active;
478 : }
479 :
480 : /*
481 : decode channels from the standard 11bit format (used by CRSF, SBUS, FPort and FPort2)
482 : must be used on multiples of 8 channels
483 : */
484 0 : void AP_RCProtocol_CRSF::decode_variable_bit_channels(const uint8_t* payload, uint8_t frame_length, uint8_t nchannels, uint16_t *values)
485 : {
486 0 : const SubsetChannelsFrame* channel_data = (const SubsetChannelsFrame*)payload;
487 :
488 : // get the channel resolution settings
489 : uint8_t channelBits;
490 : uint16_t channelMask;
491 : float channelScale;
492 :
493 0 : switch (channel_data->res_configuration) {
494 0 : case CRSF_SUBSET_RC_RES_CONF_10B:
495 0 : channelBits = CRSF_SUBSET_RC_RES_BITS_10B;
496 0 : channelMask = CRSF_SUBSET_RC_RES_MASK_10B;
497 0 : channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_10B;
498 0 : break;
499 0 : default:
500 : case CRSF_SUBSET_RC_RES_CONF_11B:
501 0 : channelBits = CRSF_SUBSET_RC_RES_BITS_11B;
502 0 : channelMask = CRSF_SUBSET_RC_RES_MASK_11B;
503 0 : channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_11B;
504 0 : break;
505 0 : case CRSF_SUBSET_RC_RES_CONF_12B:
506 0 : channelBits = CRSF_SUBSET_RC_RES_BITS_12B;
507 0 : channelMask = CRSF_SUBSET_RC_RES_MASK_12B;
508 0 : channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_12B;
509 0 : break;
510 0 : case CRSF_SUBSET_RC_RES_CONF_13B:
511 0 : channelBits = CRSF_SUBSET_RC_RES_BITS_13B;
512 0 : channelMask = CRSF_SUBSET_RC_RES_MASK_13B;
513 0 : channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_13B;
514 0 : break;
515 : }
516 :
517 : // calculate the number of channels packed
518 0 : uint8_t numOfChannels = MIN(uint8_t(((frame_length - 2) * 8 - CRSF_SUBSET_RC_STARTING_CHANNEL_BITS) / channelBits), CRSF_MAX_CHANNELS);
519 :
520 : // unpack the channel data
521 0 : uint8_t bitsMerged = 0;
522 0 : uint32_t readValue = 0;
523 0 : uint8_t readByteIndex = 1;
524 :
525 0 : for (uint8_t n = 0; n < numOfChannels; n++) {
526 0 : while (bitsMerged < channelBits) {
527 : // check for corrupt frame
528 0 : if (readByteIndex >= CRSF_FRAME_PAYLOAD_MAX) {
529 0 : return;
530 : }
531 0 : uint8_t readByte = payload[readByteIndex++];
532 0 : readValue |= ((uint32_t) readByte) << bitsMerged;
533 0 : bitsMerged += 8;
534 : }
535 : // check for corrupt frame
536 0 : if (uint8_t(channel_data->starting_channel + n) >= CRSF_MAX_CHANNELS) {
537 0 : return;
538 : }
539 0 : _channels[channel_data->starting_channel + n] =
540 0 : uint16_t(channelScale * float(uint16_t(readValue & channelMask)) + 988);
541 0 : readValue >>= channelBits;
542 0 : bitsMerged -= channelBits;
543 : }
544 : }
545 :
546 : // send out telemetry
547 0 : bool AP_RCProtocol_CRSF::process_telemetry(bool check_constraint)
548 : {
549 :
550 0 : AP_HAL::UARTDriver *uart = get_current_UART();
551 0 : if (!uart) {
552 0 : return false;
553 : }
554 :
555 0 : if (!telem_available) {
556 : #if HAL_CRSF_TELEM_ENABLED
557 0 : if (AP_CRSF_Telem::get_telem_data(&_telemetry_frame, is_tx_active())) {
558 0 : telem_available = true;
559 : } else {
560 0 : return false;
561 : }
562 : #else
563 : return false;
564 : #endif
565 : }
566 0 : write_frame(&_telemetry_frame);
567 : // get fresh telem_data in the next call
568 0 : telem_available = false;
569 :
570 0 : return true;
571 : }
572 :
573 : #if AP_OSD_LINK_STATS_EXTENSIONS_ENABLED
574 : // Define the static tx powers array
575 : constexpr uint16_t AP_RCProtocol_CRSF::tx_powers[];
576 : #endif
577 :
578 : // process link statistics to get RSSI
579 0 : void AP_RCProtocol_CRSF::process_link_stats_frame(const void* data)
580 : {
581 0 : const LinkStatisticsFrame* link = (const LinkStatisticsFrame*)data;
582 :
583 : uint8_t rssi_dbm;
584 0 : if (link->active_antenna == 0) {
585 0 : rssi_dbm = link->uplink_rssi_ant1;
586 : } else {
587 0 : rssi_dbm = link->uplink_rssi_ant2;
588 : }
589 0 : _link_status.link_quality = link->uplink_status;
590 :
591 0 : if (_use_lq_for_rssi) {
592 0 : _link_status.rssi = derive_scaled_lq_value(link->uplink_status);
593 : } else{
594 : // AP rssi: -1 for unknown, 0 for no link, 255 for maximum link
595 0 : if (rssi_dbm < 50) {
596 0 : _link_status.rssi = 255;
597 0 : } else if (rssi_dbm > 120) {
598 0 : _link_status.rssi = 0;
599 : } else {
600 : // this is an approximation recommended by Remo from TBS
601 0 : _link_status.rssi = int16_t(roundf((1.0f - (rssi_dbm - 50.0f) / 70.0f) * 255.0f));
602 : }
603 : }
604 :
605 : // Define the max number of RFModes based on ELRS modes, which is larger than Crossfire
606 0 : const uint8_t max_modes = (RFMode::RF_MODE_MAX_MODES - RFMode::CRSF_RF_MAX_MODES) - 1U; // Subtract 1 due to zero-indexing
607 0 : _link_status.rf_mode = MIN(link->rf_mode, max_modes); // Cap to avoid memory spills in the conversion tables
608 :
609 : #if AP_OSD_LINK_STATS_EXTENSIONS_ENABLED
610 : // Populate the extra data items
611 : if (link->uplink_status > 0) {
612 : _link_status.rssi_dbm = rssi_dbm;
613 : _link_status.tx_power = -1;
614 : if (link->uplink_tx_power < ARRAY_SIZE(AP_RCProtocol_CRSF::tx_powers)) {
615 : _link_status.tx_power = AP_RCProtocol_CRSF::tx_powers[link->uplink_tx_power];
616 : }
617 : _link_status.snr = link->uplink_snr;
618 : _link_status.active_antenna = link->active_antenna;
619 : } else {
620 : // This means LQ is zero, so set all values to "no signal" state
621 : _link_status.rssi_dbm = -1;
622 : _link_status.tx_power = -1;
623 : _link_status.snr = INT8_MIN;
624 : _link_status.active_antenna = -1;
625 : }
626 : #endif
627 :
628 : // Report to frontend
629 0 : frontend._rc_link_status.link_quality = _link_status.link_quality;
630 : #if AP_OSD_LINK_STATS_EXTENSIONS_ENABLED
631 : frontend._rc_link_status.rssi_dbm = _link_status.rssi_dbm;
632 : frontend._rc_link_status.snr = _link_status.snr;
633 : frontend._rc_link_status.tx_power = _link_status.tx_power;
634 : frontend._rc_link_status.active_antenna = _link_status.active_antenna;
635 : #endif
636 0 : }
637 :
638 : // process link statistics to get RX RSSI
639 0 : void AP_RCProtocol_CRSF::process_link_stats_rx_frame(const void* data)
640 : {
641 0 : const LinkStatisticsRXFrame* link = (const LinkStatisticsRXFrame*)data;
642 :
643 0 : if (_use_lq_for_rssi) {
644 0 : _link_status.rssi = derive_scaled_lq_value(link->link_quality);
645 : } else {
646 0 : _link_status.rssi = link->rssi_percent * 255.0f * 0.01f;
647 : }
648 0 : }
649 :
650 : // process link statistics to get TX RSSI
651 0 : void AP_RCProtocol_CRSF::process_link_stats_tx_frame(const void* data)
652 : {
653 0 : const LinkStatisticsTXFrame* link = (const LinkStatisticsTXFrame*)data;
654 :
655 0 : if (_use_lq_for_rssi) {
656 0 : _link_status.rssi = derive_scaled_lq_value(link->link_quality);
657 : } else {
658 0 : _link_status.rssi = link->rssi_percent * 255.0f * 0.01f;
659 : }
660 0 : }
661 :
662 : // start the uart if we have one
663 0 : void AP_RCProtocol_CRSF::start_uart()
664 : {
665 0 : _uart->configure_parity(0);
666 0 : _uart->set_stop_bits(1);
667 0 : _uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
668 0 : _uart->set_options(_uart->get_options() & ~AP_HAL::UARTDriver::OPTION_RXINV);
669 0 : _uart->begin(get_bootstrap_baud_rate());
670 0 : }
671 :
672 : // change the baudrate of the protocol if we are able
673 0 : bool AP_RCProtocol_CRSF::change_baud_rate(uint32_t baudrate)
674 : {
675 0 : AP_HAL::UARTDriver* uart = get_available_UART();
676 0 : if (uart == nullptr) {
677 0 : return false;
678 : }
679 : #if !defined(STM32H7)
680 0 : if (baudrate > get_bootstrap_baud_rate() && !uart->is_dma_enabled()) {
681 0 : return false;
682 : }
683 : #endif
684 0 : if (baudrate > CRSF_BAUDRATE_2MBIT) {
685 0 : return false;
686 : }
687 :
688 0 : _new_baud_rate = baudrate;
689 :
690 0 : return true;
691 : }
692 :
693 : // change the bootstrap baud rate to ELRS standard if configured
694 4 : void AP_RCProtocol_CRSF::process_handshake(uint32_t baudrate)
695 : {
696 4 : AP_HAL::UARTDriver *uart = get_current_UART();
697 :
698 : // only change the baudrate if we are bootstrapping CRSF
699 4 : if (uart == nullptr
700 4 : || baudrate != CRSF_BAUDRATE
701 0 : || baudrate == get_bootstrap_baud_rate()
702 0 : || uart->get_baud_rate() == get_bootstrap_baud_rate()
703 8 : || !protocol_enabled(AP_RCProtocol::CRSF)) {
704 4 : return;
705 : }
706 :
707 0 : uart->begin(get_bootstrap_baud_rate());
708 : }
709 :
710 : //returns uplink link quality on 0-255 scale
711 0 : int16_t AP_RCProtocol_CRSF::derive_scaled_lq_value(uint8_t uplink_lq)
712 : {
713 0 : return int16_t(roundf(constrain_float(uplink_lq*2.5f,0,255)));
714 : }
715 :
716 : // start bind
717 : #if HAL_CRSF_TELEM_ENABLED
718 0 : void AP_RCProtocol_CRSF::start_bind(void)
719 : {
720 0 : AP_CRSF_Telem* telem = AP::crsf_telem();
721 0 : if (telem != nullptr) {
722 0 : telem->start_bind();
723 : }
724 0 : }
725 :
726 36177494 : bool AP_RCProtocol_CRSF::bind_in_progress(void)
727 : {
728 : #if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) && !APM_BUILD_TYPE(APM_BUILD_Replay)
729 36177494 : AP_CRSF_Telem* telem = AP::crsf_telem();
730 36177494 : if (telem != nullptr) {
731 36177494 : return telem->bind_in_progress();
732 : }
733 : #endif
734 0 : return false;
735 : }
736 : #endif
737 :
738 : namespace AP {
739 0 : AP_RCProtocol_CRSF* crsf() {
740 0 : return AP_RCProtocol_CRSF::get_singleton();
741 : }
742 : };
743 :
744 : #endif // AP_RCPROTOCOL_CRSF_ENABLED
|
