Line data Source code
1 : /*
2 : * This file is free software: you can redistribute it and/or modify it
3 : * under the terms of the GNU General Public License as published by the
4 : * Free Software Foundation, either version 3 of the License, or
5 : * (at your option) any later version.
6 : *
7 : * This file is distributed in the hope that it will be useful, but
8 : * WITHOUT ANY WARRANTY; without even the implied warranty of
9 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
10 : * See the GNU General Public License for more details.
11 : *
12 : * You should have received a copy of the GNU General Public License along
13 : * with this program. If not, see <http://www.gnu.org/licenses/>.
14 : *
15 : * Code by Andrew Tridgell and Siddharth Bharat Purohit
16 : */
17 :
18 : #include "AP_RCProtocol_config.h"
19 :
20 : #include "AP_RCProtocol.h"
21 :
22 : #if AP_RCPROTOCOL_ENABLED
23 :
24 : #include "AP_RCProtocol_PPMSum.h"
25 : #include "AP_RCProtocol_DSM.h"
26 : #include "AP_RCProtocol_Emlid_RCIO.h"
27 : #include "AP_RCProtocol_IBUS.h"
28 : #include "AP_RCProtocol_IOMCU.h"
29 : #include "AP_RCProtocol_SBUS.h"
30 : #include "AP_RCProtocol_SUMD.h"
31 : #include "AP_RCProtocol_SRXL.h"
32 : #include "AP_RCProtocol_SRXL2.h"
33 : #include "AP_RCProtocol_CRSF.h"
34 : #include "AP_RCProtocol_ST24.h"
35 : #include "AP_RCProtocol_FPort.h"
36 : #include "AP_RCProtocol_FPort2.h"
37 : #include "AP_RCProtocol_DroneCAN.h"
38 : #include "AP_RCProtocol_GHST.h"
39 : #include "AP_RCProtocol_MAVLinkRadio.h"
40 : #include "AP_RCProtocol_Joystick_SFML.h"
41 : #include "AP_RCProtocol_UDP.h"
42 : #include "AP_RCProtocol_FDM.h"
43 : #include "AP_RCProtocol_Radio.h"
44 : #include <AP_Math/AP_Math.h>
45 : #include <RC_Channel/RC_Channel.h>
46 :
47 : #include <AP_Vehicle/AP_Vehicle_Type.h>
48 :
49 : #include <GCS_MAVLink/GCS.h>
50 :
51 : extern const AP_HAL::HAL& hal;
52 :
53 2094 : void AP_RCProtocol::init()
54 : {
55 : #if AP_RCPROTOCOL_PPMSUM_ENABLED
56 2094 : backend[AP_RCProtocol::PPMSUM] = NEW_NOTHROW AP_RCProtocol_PPMSum(*this);
57 : #endif
58 : #if AP_RCPROTOCOL_IBUS_ENABLED
59 2094 : backend[AP_RCProtocol::IBUS] = NEW_NOTHROW AP_RCProtocol_IBUS(*this);
60 : #endif
61 : #if AP_RCPROTOCOL_SBUS_ENABLED
62 2094 : backend[AP_RCProtocol::SBUS] = NEW_NOTHROW AP_RCProtocol_SBUS(*this, true, 100000);
63 : #endif
64 : #if AP_RCPROTOCOL_FASTSBUS_ENABLED
65 2094 : backend[AP_RCProtocol::FASTSBUS] = NEW_NOTHROW AP_RCProtocol_SBUS(*this, true, 200000);
66 : #endif
67 : #if AP_RCPROTOCOL_DSM_ENABLED
68 2094 : backend[AP_RCProtocol::DSM] = NEW_NOTHROW AP_RCProtocol_DSM(*this);
69 : #endif
70 : #if AP_RCPROTOCOL_SUMD_ENABLED
71 2094 : backend[AP_RCProtocol::SUMD] = NEW_NOTHROW AP_RCProtocol_SUMD(*this);
72 : #endif
73 : #if AP_RCPROTOCOL_SRXL_ENABLED
74 2094 : backend[AP_RCProtocol::SRXL] = NEW_NOTHROW AP_RCProtocol_SRXL(*this);
75 : #endif
76 : #if AP_RCPROTOCOL_SBUS_NI_ENABLED
77 2094 : backend[AP_RCProtocol::SBUS_NI] = NEW_NOTHROW AP_RCProtocol_SBUS(*this, false, 100000);
78 : #endif
79 : #if AP_RCPROTOCOL_SRXL2_ENABLED
80 2094 : backend[AP_RCProtocol::SRXL2] = NEW_NOTHROW AP_RCProtocol_SRXL2(*this);
81 : #endif
82 : #if AP_RCPROTOCOL_CRSF_ENABLED
83 2094 : backend[AP_RCProtocol::CRSF] = NEW_NOTHROW AP_RCProtocol_CRSF(*this);
84 : #endif
85 : #if AP_RCPROTOCOL_FPORT2_ENABLED
86 2094 : backend[AP_RCProtocol::FPORT2] = NEW_NOTHROW AP_RCProtocol_FPort2(*this, true);
87 : #endif
88 : #if AP_RCPROTOCOL_ST24_ENABLED
89 2094 : backend[AP_RCProtocol::ST24] = NEW_NOTHROW AP_RCProtocol_ST24(*this);
90 : #endif
91 : #if AP_RCPROTOCOL_FPORT_ENABLED
92 2094 : backend[AP_RCProtocol::FPORT] = NEW_NOTHROW AP_RCProtocol_FPort(*this, true);
93 : #endif
94 : #if AP_RCPROTOCOL_DRONECAN_ENABLED
95 2094 : backend[AP_RCProtocol::DRONECAN] = NEW_NOTHROW AP_RCProtocol_DroneCAN(*this);
96 : #endif
97 : #if AP_RCPROTOCOL_GHST_ENABLED
98 2094 : backend[AP_RCProtocol::GHST] = NEW_NOTHROW AP_RCProtocol_GHST(*this);
99 : #endif
100 : #if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED
101 2094 : backend[AP_RCProtocol::MAVLINK_RADIO] = NEW_NOTHROW AP_RCProtocol_MAVLinkRadio(*this);
102 : #endif
103 : #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
104 : backend[AP_RCProtocol::JOYSTICK_SFML] = NEW_NOTHROW AP_RCProtocol_Joystick_SFML(*this);
105 : #endif
106 : #if AP_RCPROTOCOL_UDP_ENABLED
107 2094 : const auto UDP_backend = NEW_NOTHROW AP_RCProtocol_UDP(*this);
108 2094 : backend[AP_RCProtocol::UDP] = UDP_backend;
109 : #endif
110 : #if AP_RCPROTOCOL_FDM_ENABLED
111 2094 : const auto FDM_backend = NEW_NOTHROW AP_RCProtocol_FDM(*this);;
112 2094 : backend[AP_RCProtocol::FDM] = FDM_backend;
113 : #if AP_RCPROTOCOL_UDP_ENABLED
114 : // the UDP-Packed16Bit backend gives way to the FDM backend:
115 2094 : UDP_backend->set_fdm_backend(FDM_backend);
116 : #endif // AP_RCPROTOCOL_UDP_ENABLED
117 : #endif // AP_RCPROTOCOL_FDM_ENABLED
118 : #if AP_RCPROTOCOL_RADIO_ENABLED
119 : backend[AP_RCProtocol::RADIO] = NEW_NOTHROW AP_RCProtocol_Radio(*this);
120 : #endif
121 : #if AP_RCPROTOCOL_IOMCU_ENABLED
122 : backend[AP_RCProtocol::IOMCU] = NEW_NOTHROW AP_RCProtocol_IOMCU(*this);
123 : #endif // AP_RCPROTOCOL_IOMCU_ENABLED
124 : #if AP_RCPROTOCOL_EMLID_RCIO_ENABLED
125 : backend[AP_RCProtocol::EMLID_RCIO] = NEW_NOTHROW AP_RCProtocol_Emlid_RCIO(*this);
126 : #endif
127 2094 : }
128 :
129 454 : AP_RCProtocol::~AP_RCProtocol()
130 : {
131 9080 : for (uint8_t i = 0; i < ARRAY_SIZE(backend); i++) {
132 8626 : if (backend[i] != nullptr) {
133 8172 : delete backend[i];
134 8172 : backend[i] = nullptr;
135 : }
136 : }
137 454 : }
138 :
139 101171948 : bool AP_RCProtocol::should_search(uint32_t now_ms) const
140 : {
141 : #if AP_RCPROTOCOL_FDM_ENABLED && AP_RCPROTOCOL_UDP_ENABLED
142 : // force re-detection when FDM is active and active backend is UDP values
143 201953951 : if (_detected_protocol == AP_RCProtocol::UDP &&
144 100782003 : ((AP_RCProtocol_FDM*)backend[AP_RCProtocol::FDM])->active()) {
145 0 : return true;
146 : }
147 : #endif // AP_RCPROTOCOL_FDM_ENABLED && AP_RCPROTOCOL_UDP_ENABLED
148 : #if AP_RC_CHANNEL_ENABLED && !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
149 101171948 : if (_detected_protocol != AP_RCProtocol::NONE && !rc().option_is_enabled(RC_Channels::Option::MULTI_RECEIVER_SUPPORT)) {
150 100782003 : return false;
151 : }
152 : #else
153 : // on IOMCU don't allow protocol to change once detected
154 0 : if (_detected_protocol != AP_RCProtocol::NONE) {
155 0 : return false;
156 : }
157 : #endif
158 389945 : return (now_ms - _last_input_ms >= 200);
159 : }
160 :
161 0 : void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1)
162 : {
163 0 : uint32_t now = AP_HAL::millis();
164 0 : bool searching = should_search(now);
165 :
166 : #if AP_RC_CHANNEL_ENABLED
167 0 : rc_protocols_mask = rc().enabled_protocols();
168 : #endif
169 :
170 0 : if (_detected_protocol != AP_RCProtocol::NONE &&
171 0 : !protocol_enabled(_detected_protocol)) {
172 0 : _detected_protocol = AP_RCProtocol::NONE;
173 : }
174 :
175 0 : if (_detected_protocol != AP_RCProtocol::NONE && _detected_with_bytes && !searching) {
176 : // we're using byte inputs, discard pulses
177 0 : return;
178 : }
179 : // first try current protocol
180 0 : if (_detected_protocol != AP_RCProtocol::NONE && !searching) {
181 0 : backend[_detected_protocol]->process_pulse(width_s0, width_s1);
182 0 : if (backend[_detected_protocol]->new_input()) {
183 0 : _new_input = true;
184 0 : _last_input_ms = now;
185 : }
186 0 : return;
187 : }
188 :
189 : // otherwise scan all protocols
190 0 : for (uint8_t i = 0; i < ARRAY_SIZE(backend); i++) {
191 0 : if (_disabled_for_pulses & (1U << i)) {
192 : // this protocol is disabled for pulse input
193 0 : continue;
194 : }
195 0 : if (backend[i] != nullptr) {
196 0 : if (!protocol_enabled(rcprotocol_t(i))) {
197 0 : continue;
198 : }
199 0 : const uint32_t frame_count = backend[i]->get_rc_frame_count();
200 0 : const uint32_t input_count = backend[i]->get_rc_input_count();
201 0 : backend[i]->process_pulse(width_s0, width_s1);
202 0 : const uint32_t frame_count2 = backend[i]->get_rc_frame_count();
203 0 : if (frame_count2 > frame_count) {
204 0 : if (requires_3_frames((rcprotocol_t)i) && frame_count2 < 3) {
205 0 : continue;
206 : }
207 0 : _new_input = (input_count != backend[i]->get_rc_input_count());
208 0 : _detected_protocol = (enum AP_RCProtocol::rcprotocol_t)i;
209 0 : for (uint8_t j = 0; j < ARRAY_SIZE(backend); j++) {
210 0 : if (backend[j]) {
211 0 : backend[j]->reset_rc_frame_count();
212 : }
213 : }
214 0 : _last_input_ms = now;
215 0 : _detected_with_bytes = false;
216 0 : break;
217 : }
218 : }
219 : }
220 : }
221 :
222 : /*
223 : process an array of pulses. n must be even
224 : */
225 0 : void AP_RCProtocol::process_pulse_list(const uint32_t *widths, uint16_t n, bool need_swap)
226 : {
227 0 : if (n & 1) {
228 0 : return;
229 : }
230 0 : while (n) {
231 0 : uint32_t widths0 = widths[0];
232 0 : uint32_t widths1 = widths[1];
233 0 : if (need_swap) {
234 0 : uint32_t tmp = widths1;
235 0 : widths1 = widths0;
236 0 : widths0 = tmp;
237 : }
238 0 : widths1 -= widths0;
239 0 : process_pulse(widths0, widths1);
240 0 : widths += 2;
241 0 : n -= 2;
242 : }
243 : }
244 :
245 0 : bool AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate)
246 : {
247 0 : uint32_t now = AP_HAL::millis();
248 0 : bool searching = should_search(now);
249 :
250 : #if AP_RC_CHANNEL_ENABLED
251 0 : rc_protocols_mask = rc().enabled_protocols();
252 : #endif
253 :
254 0 : if (_detected_protocol != AP_RCProtocol::NONE &&
255 0 : !protocol_enabled(_detected_protocol)) {
256 0 : _detected_protocol = AP_RCProtocol::NONE;
257 : }
258 :
259 0 : if (_detected_protocol != AP_RCProtocol::NONE && !_detected_with_bytes && !searching) {
260 : // we're using pulse inputs, discard bytes
261 0 : return false;
262 : }
263 :
264 : // first try current protocol
265 0 : if (_detected_protocol != AP_RCProtocol::NONE && !searching) {
266 0 : backend[_detected_protocol]->process_byte(byte, baudrate);
267 0 : if (backend[_detected_protocol]->new_input()) {
268 0 : _new_input = true;
269 0 : _last_input_ms = now;
270 : }
271 0 : return true;
272 : }
273 :
274 : // otherwise scan all protocols
275 0 : for (uint8_t i = 0; i < ARRAY_SIZE(backend); i++) {
276 0 : if (backend[i] != nullptr) {
277 0 : if (!protocol_enabled(rcprotocol_t(i))) {
278 0 : continue;
279 : }
280 0 : const uint32_t frame_count = backend[i]->get_rc_frame_count();
281 0 : const uint32_t input_count = backend[i]->get_rc_input_count();
282 0 : backend[i]->process_byte(byte, baudrate);
283 0 : const uint32_t frame_count2 = backend[i]->get_rc_frame_count();
284 0 : if (frame_count2 > frame_count) {
285 0 : if (requires_3_frames((rcprotocol_t)i) && frame_count2 < 3) {
286 0 : continue;
287 : }
288 0 : _new_input = (input_count != backend[i]->get_rc_input_count());
289 0 : _detected_protocol = (enum AP_RCProtocol::rcprotocol_t)i;
290 0 : _last_input_ms = now;
291 0 : _detected_with_bytes = true;
292 0 : for (uint8_t j = 0; j < ARRAY_SIZE(backend); j++) {
293 0 : if (backend[j]) {
294 0 : backend[j]->reset_rc_frame_count();
295 : }
296 : }
297 : // stop decoding pulses to save CPU
298 0 : hal.rcin->pulse_input_enable(false);
299 0 : return true;
300 : }
301 : }
302 : }
303 0 : return false;
304 : }
305 :
306 : // handshake if nothing else has succeeded so far
307 4 : void AP_RCProtocol::process_handshake( uint32_t baudrate)
308 : {
309 : // if we ever succeeded before then do not handshake
310 4 : if (_detected_protocol != AP_RCProtocol::NONE || _last_input_ms > 0) {
311 0 : return;
312 : }
313 :
314 : // otherwise handshake all protocols
315 80 : for (uint8_t i = 0; i < ARRAY_SIZE(backend); i++) {
316 76 : if (backend[i] != nullptr) {
317 72 : backend[i]->process_handshake(baudrate);
318 : }
319 : }
320 : }
321 :
322 : /*
323 : check for bytes from an additional uart. This is used to support RC
324 : protocols from SERIALn_PROTOCOL
325 : */
326 1 : void AP_RCProtocol::SerialConfig::apply_to_uart(AP_HAL::UARTDriver *uart) const
327 : {
328 1 : uart->configure_parity(parity);
329 1 : uart->set_stop_bits(stop_bits);
330 1 : if (invert_rx) {
331 0 : uart->set_options(uart->get_options() | AP_HAL::UARTDriver::OPTION_RXINV);
332 : } else {
333 1 : uart->set_options(uart->get_options() & ~AP_HAL::UARTDriver::OPTION_RXINV);
334 : }
335 1 : uart->begin(baud, 128, 128);
336 1 : }
337 :
338 : static const AP_RCProtocol::SerialConfig serial_configs[] {
339 : // BAUD PRTY STOP INVERT-RX
340 : // inverted and uninverted 115200 8N1:
341 : { 115200, 0, 1, false },
342 : { 115200, 0, 1, true },
343 : // SBUS settings, even parity, 2 stop bits:
344 : { 100000, 2, 2, true },
345 : #if AP_RCPROTOCOL_FASTSBUS_ENABLED
346 : // FastSBUS:
347 : { 200000, 2, 2, true },
348 : #endif
349 : #if AP_RCPROTOCOL_CRSF_ENABLED || AP_RCPROTOCOL_GHST_ENABLED
350 : // CrossFire:
351 : { 416666, 0, 1, false },
352 : // CRSFv3 can negotiate higher rates which are sticky on soft reboot
353 : { 2000000, 0, 1, false },
354 : #endif
355 : };
356 :
357 : static_assert(ARRAY_SIZE(serial_configs) > 0, "must have at least one serial config");
358 :
359 279535355 : void AP_RCProtocol::check_added_uart(void)
360 : {
361 279535355 : if (!added.uart) {
362 279535351 : return;
363 : }
364 42964 : uint32_t now = AP_HAL::millis();
365 42964 : bool searching = should_search(now);
366 42964 : if (!searching && !_detected_with_bytes) {
367 : // not using this uart
368 42960 : return;
369 : }
370 4 : if (!added.opened) {
371 1 : added.opened = true;
372 1 : added.last_config_change_ms = AP_HAL::millis();
373 1 : serial_configs[added.config_num].apply_to_uart(added.uart);
374 : }
375 : #if AP_RC_CHANNEL_ENABLED
376 4 : rc_protocols_mask = rc().enabled_protocols();
377 : #endif
378 4 : const uint32_t current_baud = serial_configs[added.config_num].baud;
379 4 : process_handshake(current_baud);
380 :
381 4 : uint32_t n = added.uart->available();
382 4 : n = MIN(n, 255U);
383 4 : for (uint8_t i=0; i<n; i++) {
384 0 : int16_t b = added.uart->read();
385 0 : if (b >= 0) {
386 0 : process_byte(uint8_t(b), current_baud);
387 : }
388 : }
389 4 : if (searching) {
390 4 : if (now - added.last_config_change_ms > 1000) {
391 : // change configs if not detected once a second
392 0 : added.config_num++;
393 0 : if (added.config_num >= ARRAY_SIZE(serial_configs)) {
394 0 : added.config_num = 0;
395 : }
396 0 : added.opened = false;
397 : }
398 : // power loss on CRSF requires re-bootstrap because the baudrate is reset to the default. The CRSF side will
399 : // drop back down to 416k if it has received 200 incorrect characters (or none at all)
400 0 : } else if (_detected_protocol != AP_RCProtocol::NONE
401 : // protocols that want to be able to renegotiate should return false in is_rx_active()
402 0 : && !backend[_detected_protocol]->is_rx_active()
403 0 : && now - added.last_config_change_ms > 1000) {
404 0 : added.opened = false;
405 : }
406 : }
407 :
408 243357861 : void AP_RCProtocol::update()
409 : {
410 243357861 : check_added_uart();
411 243357861 : }
412 :
413 : // explicitly investigate a backend for data, as opposed to feeding
414 : // the backend a byte (or pulse-train) at a time and having them make
415 : // an "add_input" callback):
416 137208537 : bool AP_RCProtocol::detect_async_protocol(rcprotocol_t protocol)
417 : {
418 137208537 : auto *p = backend[protocol];
419 137208537 : if (p == nullptr) {
420 : // backend is not allocated?!
421 0 : return false;
422 : }
423 :
424 137208537 : if (_detected_protocol == protocol) {
425 : // we are using this protocol already, see if there is new
426 : // data. Caller will handle the case where we stop presenting
427 : // data
428 36079553 : return p->new_input();
429 : }
430 :
431 : // we are not the currently in-use protocol.
432 101128984 : const uint32_t now = AP_HAL::millis();
433 :
434 : // see if another backend is providing data:
435 101128984 : if (!should_search(now)) {
436 : // apparently, yes
437 100768205 : return false;
438 : }
439 :
440 : #if AP_RC_CHANNEL_ENABLED
441 360779 : rc_protocols_mask = rc().enabled_protocols();
442 : #endif
443 :
444 360779 : if (!protocol_enabled(protocol)) {
445 0 : return false;
446 : }
447 :
448 : // nobody is providing data; can we provide data?
449 360779 : if (!p->new_input()) {
450 : // we can't provide data
451 358690 : return false;
452 : }
453 :
454 : // we can provide data, change the detected protocol to be us:
455 2089 : _detected_protocol = protocol;
456 2089 : return true;
457 : }
458 :
459 5029 : const char *AP_RCProtocol::detected_protocol_name() const
460 : {
461 5029 : switch (_detected_protocol) {
462 : #if AP_RCPROTOCOL_IOMCU_ENABLED
463 : case AP_RCProtocol::IOMCU:
464 : return ((AP_RCProtocol_IOMCU*)(backend[AP_RCProtocol::IOMCU]))->get_rc_protocol();
465 : #endif
466 : default:
467 5029 : return protocol_name_from_protocol(_detected_protocol);
468 : }
469 : }
470 :
471 3010 : void AP_RCProtocol::announce_detected()
472 : {
473 : const char *src;
474 3010 : const char *name = detected_protocol_name();
475 3010 : if (name == nullptr) {
476 921 : return;
477 : }
478 2089 : switch (_detected_protocol) {
479 : #if AP_RCPROTOCOL_IOMCU_ENABLED
480 : case AP_RCProtocol::IOMCU:
481 : src = "IOMCU";
482 : break;
483 : #endif
484 : default:
485 2089 : src = using_uart() ? "Bytes" : "Pulses";
486 : }
487 : (void)src; // iofirmware doesn't use this
488 : (void)name; // iofirmware doesn't use this
489 2089 : GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "RCInput: decoding %s (%s)", name, src);
490 : }
491 :
492 36177494 : bool AP_RCProtocol::new_input()
493 : {
494 : // if we have an extra UART from a SERIALn_PROTOCOL then check it for data
495 36177494 : check_added_uart();
496 :
497 : // run update function on backends
498 723549880 : for (uint8_t i = 0; i < ARRAY_SIZE(backend); i++) {
499 687372386 : if (backend[i] != nullptr) {
500 651194892 : backend[i]->update();
501 : }
502 : }
503 :
504 : // iterate through backends which don't do either of pulse or uart
505 : // input, and thus won't update_new_input
506 36177494 : const rcprotocol_t pollable[] {
507 : #if AP_RCPROTOCOL_DRONECAN_ENABLED
508 : AP_RCProtocol::DRONECAN,
509 : #endif
510 : #if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED
511 : AP_RCProtocol::MAVLINK_RADIO,
512 : #endif
513 : #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
514 : AP_RCProtocol::JOYSTICK_SFML,
515 : #endif
516 : #if AP_RCPROTOCOL_UDP_ENABLED
517 : AP_RCProtocol::UDP,
518 : #endif
519 : #if AP_RCPROTOCOL_FDM_ENABLED
520 : AP_RCProtocol::FDM,
521 : #endif
522 : #if AP_RCPROTOCOL_RADIO_ENABLED
523 : AP_RCProtocol::RADIO,
524 : #endif
525 : #if AP_RCPROTOCOL_IOMCU_ENABLED
526 : AP_RCProtocol::IOMCU,
527 : #endif // AP_RCPROTOCOL_IOMCU_ENABLED
528 : #if AP_RCPROTOCOL_EMLID_RCIO_ENABLED
529 : AP_RCProtocol::EMLID_RCIO,
530 : #endif
531 : };
532 165884592 : for (const auto protocol : pollable) {
533 137208537 : if (!detect_async_protocol(protocol)) {
534 129707098 : continue;
535 : }
536 7501439 : _new_input = true;
537 7501439 : _last_input_ms = AP_HAL::millis();
538 7501439 : break;
539 : }
540 :
541 : #if AP_RCPROTOCOL_IOMCU_ENABLED
542 : // IOMCU takes precedence over serial-port-type-23 input etc:
543 : if (((AP_RCProtocol_IOMCU*)(backend[AP_RCProtocol::IOMCU]))->active()) {
544 : _detected_protocol = AP_RCProtocol::IOMCU;
545 : }
546 : #endif
547 :
548 : // announce protocol changes:
549 72351978 : if (_detected_protocol != _last_detected_protocol ||
550 36174484 : using_uart() != _last_detected_using_uart) {
551 3010 : _last_detected_protocol = _detected_protocol;
552 3010 : _last_detected_using_uart = using_uart();
553 3010 : announce_detected();
554 : }
555 :
556 36177494 : bool ret = _new_input;
557 36177494 : _new_input = false;
558 36177494 : return ret;
559 : }
560 :
561 1311839 : uint8_t AP_RCProtocol::num_channels()
562 : {
563 1311839 : if (_detected_protocol != AP_RCProtocol::NONE) {
564 1310180 : return backend[_detected_protocol]->num_channels();
565 : }
566 1659 : return 0;
567 : }
568 :
569 119958672 : uint16_t AP_RCProtocol::read(uint8_t chan)
570 : {
571 119958672 : if (_detected_protocol != AP_RCProtocol::NONE) {
572 119953024 : return backend[_detected_protocol]->read(chan);
573 : }
574 5648 : return 0;
575 : }
576 :
577 0 : void AP_RCProtocol::read(uint16_t *pwm, uint8_t n)
578 : {
579 0 : if (_detected_protocol != AP_RCProtocol::NONE) {
580 0 : backend[_detected_protocol]->read(pwm, n);
581 : }
582 0 : }
583 :
584 0 : int16_t AP_RCProtocol::get_RSSI(void) const
585 : {
586 0 : if (_detected_protocol != AP_RCProtocol::NONE) {
587 0 : return backend[_detected_protocol]->get_RSSI();
588 : }
589 0 : return -1;
590 : }
591 0 : int16_t AP_RCProtocol::get_rx_link_quality(void) const
592 : {
593 0 : if (_detected_protocol != AP_RCProtocol::NONE) {
594 0 : return backend[_detected_protocol]->get_rx_link_quality();
595 : }
596 0 : return -1;
597 : }
598 : /*
599 : ask for bind start on supported receivers (eg spektrum satellite)
600 : */
601 0 : void AP_RCProtocol::start_bind()
602 : {
603 0 : for (uint8_t i = 0; i < ARRAY_SIZE(backend); i++) {
604 0 : if (backend[i] != nullptr && protocol_enabled((rcprotocol_t)i)) {
605 0 : backend[i]->start_bind();
606 : }
607 : }
608 0 : }
609 :
610 : #endif // AP_RCPROTOCOL_ENABLED
611 :
612 : /*
613 : return protocol name
614 : */
615 5029 : const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol)
616 : {
617 5029 : switch (protocol) {
618 : #if AP_RCPROTOCOL_PPMSUM_ENABLED
619 0 : case PPMSUM:
620 0 : return "PPM";
621 : #endif
622 : #if AP_RCPROTOCOL_IBUS_ENABLED
623 0 : case IBUS:
624 0 : return "IBUS";
625 : #endif
626 : #if AP_RCPROTOCOL_SBUS_ENABLED
627 0 : case SBUS:
628 0 : return "SBUS";
629 : #endif
630 : #if AP_RCPROTOCOL_SBUS_NI_ENABLED
631 0 : case SBUS_NI:
632 0 : return "SBUS";
633 : #endif
634 : #if AP_RCPROTOCOL_FASTSBUS_ENABLED
635 0 : case FASTSBUS:
636 0 : return "FastSBUS";
637 : #endif
638 : #if AP_RCPROTOCOL_DSM_ENABLED
639 0 : case DSM:
640 0 : return "DSM";
641 : #endif
642 : #if AP_RCPROTOCOL_SUMD_ENABLED
643 0 : case SUMD:
644 0 : return "SUMD";
645 : #endif
646 : #if AP_RCPROTOCOL_SRXL_ENABLED
647 0 : case SRXL:
648 0 : return "SRXL";
649 : #endif
650 : #if AP_RCPROTOCOL_SRXL2_ENABLED
651 0 : case SRXL2:
652 0 : return "SRXL2";
653 : #endif
654 : #if AP_RCPROTOCOL_CRSF_ENABLED
655 0 : case CRSF:
656 0 : return "CRSF";
657 : #endif
658 : #if AP_RCPROTOCOL_ST24_ENABLED
659 0 : case ST24:
660 0 : return "ST24";
661 : #endif
662 : #if AP_RCPROTOCOL_FPORT_ENABLED
663 0 : case FPORT:
664 0 : return "FPORT";
665 : #endif
666 : #if AP_RCPROTOCOL_FPORT2_ENABLED
667 0 : case FPORT2:
668 0 : return "FPORT2";
669 : #endif
670 : #if AP_RCPROTOCOL_DRONECAN_ENABLED
671 0 : case DRONECAN:
672 0 : return "DroneCAN";
673 : #endif
674 : #if AP_RCPROTOCOL_GHST_ENABLED
675 0 : case GHST:
676 0 : return "GHST";
677 : #endif
678 : #if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED
679 0 : case MAVLINK_RADIO:
680 0 : return "MAVRadio";
681 : #endif
682 : #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
683 : case JOYSTICK_SFML:
684 : return "SFML";
685 : #endif
686 : #if AP_RCPROTOCOL_UDP_ENABLED
687 4098 : case UDP:
688 4098 : return "UDP";
689 : #endif
690 : #if AP_RCPROTOCOL_FDM_ENABLED
691 0 : case FDM:
692 0 : return "FDM";
693 : #endif
694 : #if AP_RCPROTOCOL_RADIO_ENABLED
695 : case RADIO:
696 : return "Radio";
697 : #endif
698 : #if AP_RCPROTOCOL_IOMCU_ENABLED
699 : case IOMCU:
700 : return "IOMCU";
701 : #endif // AP_RCPROTOCOL_IOMCU_ENABLED
702 : #if AP_RCPROTOCOL_EMLID_RCIO_ENABLED
703 : case EMLID_RCIO:
704 : return "Emlid RCIO";
705 : #endif
706 931 : case NONE:
707 931 : break;
708 : }
709 931 : return nullptr;
710 : }
711 :
712 : #if AP_RCPROTOCOL_ENABLED
713 : /*
714 : add a uart to decode
715 : */
716 1 : void AP_RCProtocol::add_uart(AP_HAL::UARTDriver* uart)
717 : {
718 1 : added.uart = uart;
719 1 : added.uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
720 1 : }
721 :
722 : // return true if a specific protocol is enabled
723 360779 : bool AP_RCProtocol::protocol_enabled(rcprotocol_t protocol) const
724 : {
725 360779 : if ((rc_protocols_mask & 1U) != 0) {
726 : // all protocols enabled
727 360779 : return true;
728 : }
729 0 : return ((1U<<(uint8_t(protocol)+1)) & rc_protocols_mask) != 0;
730 : }
731 :
732 : #if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED
733 0 : void AP_RCProtocol::handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet)
734 : {
735 0 : if (backend[AP_RCProtocol::MAVLINK_RADIO] == nullptr) {
736 0 : return;
737 : }
738 :
739 0 : backend[AP_RCProtocol::MAVLINK_RADIO]->update_radio_rc_channels(packet);
740 : };
741 : #endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED
742 :
743 : namespace AP {
744 403010575 : AP_RCProtocol &RC()
745 : {
746 403010575 : static AP_RCProtocol rcprot;
747 403010575 : return rcprot;
748 : }
749 : };
750 :
751 : #endif // AP_RCPROTOCOL_ENABLED
|
