/* * This file 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 file 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 . * * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include #include "AP_RCProtocol.h" #include "AP_RCProtocol_PPMSum.h" #include "AP_RCProtocol_DSM.h" #include "AP_RCProtocol_IBUS.h" #include "AP_RCProtocol_SBUS.h" #include "AP_RCProtocol_SUMD.h" #include "AP_RCProtocol_SRXL.h" #ifndef IOMCU_FW #include "AP_RCProtocol_SRXL2.h" #endif #include "AP_RCProtocol_CRSF.h" #include "AP_RCProtocol_ST24.h" #include "AP_RCProtocol_FPort.h" #include "AP_RCProtocol_FPort2.h" #include #include extern const AP_HAL::HAL& hal; void AP_RCProtocol::init() { backend[AP_RCProtocol::PPM] = new AP_RCProtocol_PPMSum(*this); backend[AP_RCProtocol::IBUS] = new AP_RCProtocol_IBUS(*this); backend[AP_RCProtocol::SBUS] = new AP_RCProtocol_SBUS(*this, true, 100000); #if AP_RCPROTOCOL_FASTSBUS_ENABLED backend[AP_RCProtocol::FASTSBUS] = new AP_RCProtocol_SBUS(*this, true, 200000); #endif backend[AP_RCProtocol::DSM] = new AP_RCProtocol_DSM(*this); backend[AP_RCProtocol::SUMD] = new AP_RCProtocol_SUMD(*this); #if AP_RCPROTOCOL_SRXL_ENABLED backend[AP_RCProtocol::SRXL] = new AP_RCProtocol_SRXL(*this); #endif #ifndef IOMCU_FW backend[AP_RCProtocol::SBUS_NI] = new AP_RCProtocol_SBUS(*this, false, 100000); backend[AP_RCProtocol::SRXL2] = new AP_RCProtocol_SRXL2(*this); backend[AP_RCProtocol::CRSF] = new AP_RCProtocol_CRSF(*this); #if AP_RCPROTOCOL_FPORT2_ENABLED backend[AP_RCProtocol::FPORT2] = new AP_RCProtocol_FPort2(*this, true); #endif #endif backend[AP_RCProtocol::ST24] = new AP_RCProtocol_ST24(*this); #if AP_RCPROTOCOL_FPORT_ENABLED backend[AP_RCProtocol::FPORT] = new AP_RCProtocol_FPort(*this, true); #endif } AP_RCProtocol::~AP_RCProtocol() { for (uint8_t i = 0; i < ARRAY_SIZE(backend); i++) { if (backend[i] != nullptr) { delete backend[i]; backend[i] = nullptr; } } } bool AP_RCProtocol::should_search(uint32_t now_ms) const { #if !defined(IOMCU_FW) && !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) if (_detected_protocol != AP_RCProtocol::NONE && !rc().multiple_receiver_support()) { return false; } #else // on IOMCU don't allow protocol to change once detected if (_detected_protocol != AP_RCProtocol::NONE) { return false; } #endif return (now_ms - _last_input_ms >= 200); } void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1) { uint32_t now = AP_HAL::millis(); bool searching = should_search(now); #ifndef IOMCU_FW rc_protocols_mask = rc().enabled_protocols(); #endif if (_detected_protocol != AP_RCProtocol::NONE && !protocol_enabled(_detected_protocol)) { _detected_protocol = AP_RCProtocol::NONE; } if (_detected_protocol != AP_RCProtocol::NONE && _detected_with_bytes && !searching) { // we're using byte inputs, discard pulses return; } // first try current protocol if (_detected_protocol != AP_RCProtocol::NONE && !searching) { backend[_detected_protocol]->process_pulse(width_s0, width_s1); if (backend[_detected_protocol]->new_input()) { _new_input = true; _last_input_ms = now; } return; } // otherwise scan all protocols for (uint8_t i = 0; i < ARRAY_SIZE(backend); i++) { if (_disabled_for_pulses & (1U << i)) { // this protocol is disabled for pulse input continue; } if (backend[i] != nullptr) { if (!protocol_enabled(rcprotocol_t(i))) { continue; } const uint32_t frame_count = backend[i]->get_rc_frame_count(); const uint32_t input_count = backend[i]->get_rc_input_count(); backend[i]->process_pulse(width_s0, width_s1); const uint32_t frame_count2 = backend[i]->get_rc_frame_count(); if (frame_count2 > frame_count) { if (requires_3_frames((rcprotocol_t)i) && frame_count2 < 3) { continue; } _new_input = (input_count != backend[i]->get_rc_input_count()); _detected_protocol = (enum AP_RCProtocol::rcprotocol_t)i; for (uint8_t j = 0; j < ARRAY_SIZE(backend); j++) { if (backend[j]) { backend[j]->reset_rc_frame_count(); } } _last_input_ms = now; _detected_with_bytes = false; break; } } } } /* process an array of pulses. n must be even */ void AP_RCProtocol::process_pulse_list(const uint32_t *widths, uint16_t n, bool need_swap) { if (n & 1) { return; } while (n) { uint32_t widths0 = widths[0]; uint32_t widths1 = widths[1]; if (need_swap) { uint32_t tmp = widths1; widths1 = widths0; widths0 = tmp; } widths1 -= widths0; process_pulse(widths0, widths1); widths += 2; n -= 2; } } bool AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate) { uint32_t now = AP_HAL::millis(); bool searching = should_search(now); #ifndef IOMCU_FW rc_protocols_mask = rc().enabled_protocols(); #endif if (_detected_protocol != AP_RCProtocol::NONE && !protocol_enabled(_detected_protocol)) { _detected_protocol = AP_RCProtocol::NONE; } if (_detected_protocol != AP_RCProtocol::NONE && !_detected_with_bytes && !searching) { // we're using pulse inputs, discard bytes return false; } // first try current protocol if (_detected_protocol != AP_RCProtocol::NONE && !searching) { backend[_detected_protocol]->process_byte(byte, baudrate); if (backend[_detected_protocol]->new_input()) { _new_input = true; _last_input_ms = now; } return true; } // otherwise scan all protocols for (uint8_t i = 0; i < ARRAY_SIZE(backend); i++) { if (backend[i] != nullptr) { if (!protocol_enabled(rcprotocol_t(i))) { continue; } const uint32_t frame_count = backend[i]->get_rc_frame_count(); const uint32_t input_count = backend[i]->get_rc_input_count(); backend[i]->process_byte(byte, baudrate); const uint32_t frame_count2 = backend[i]->get_rc_frame_count(); if (frame_count2 > frame_count) { if (requires_3_frames((rcprotocol_t)i) && frame_count2 < 3) { continue; } _new_input = (input_count != backend[i]->get_rc_input_count()); _detected_protocol = (enum AP_RCProtocol::rcprotocol_t)i; _last_input_ms = now; _detected_with_bytes = true; for (uint8_t j = 0; j < ARRAY_SIZE(backend); j++) { if (backend[j]) { backend[j]->reset_rc_frame_count(); } } // stop decoding pulses to save CPU hal.rcin->pulse_input_enable(false); break; } } } return false; } // handshake if nothing else has succeeded so far void AP_RCProtocol::process_handshake( uint32_t baudrate) { // if we ever succeeded before then do not handshake if (_detected_protocol != AP_RCProtocol::NONE || _last_input_ms > 0) { return; } // otherwise handshake all protocols for (uint8_t i = 0; i < ARRAY_SIZE(backend); i++) { if (backend[i] != nullptr) { backend[i]->process_handshake(baudrate); } } } /* check for bytes from an additional uart. This is used to support RC protocols from SERIALn_PROTOCOL */ void AP_RCProtocol::SerialConfig::apply_to_uart(AP_HAL::UARTDriver *uart) const { uart->configure_parity(parity); uart->set_stop_bits(stop_bits); if (invert_rx) { uart->set_options(uart->get_options() | AP_HAL::UARTDriver::OPTION_RXINV); } else { uart->set_options(uart->get_options() & ~AP_HAL::UARTDriver::OPTION_RXINV); } uart->begin(baud, 128, 128); } static const AP_RCProtocol::SerialConfig serial_configs[] { // BAUD PRTY STOP INVERT-RX // inverted and uninverted 115200 8N1: { 115200, 0, 1, false }, { 115200, 0, 1, true }, // SBUS settings, even parity, 2 stop bits: { 100000, 2, 2, true }, #if AP_RCPROTOCOL_FASTSBUS_ENABLED // FastSBUS: { 200000, 2, 2, true }, #endif // CrossFire: { 416666, 0, 1, false }, }; static_assert(ARRAY_SIZE(serial_configs) > 1, "must have at least one serial config"); void AP_RCProtocol::check_added_uart(void) { if (!added.uart) { return; } uint32_t now = AP_HAL::millis(); bool searching = should_search(now); if (!searching && !_detected_with_bytes) { // not using this uart return; } if (!added.opened) { added.opened = true; added.last_config_change_ms = AP_HAL::millis(); serial_configs[added.config_num].apply_to_uart(added.uart); } #ifndef IOMCU_FW rc_protocols_mask = rc().enabled_protocols(); #endif const uint32_t current_baud = serial_configs[added.config_num].baud; process_handshake(current_baud); uint32_t n = added.uart->available(); n = MIN(n, 255U); for (uint8_t i=0; iread(); if (b >= 0) { process_byte(uint8_t(b), current_baud); } } if (searching) { if (now - added.last_config_change_ms > 1000) { // change configs if not detected once a second added.config_num++; if (added.config_num >= ARRAY_SIZE(serial_configs)) { added.config_num = 0; } added.opened = false; } // power loss on CRSF requires re-bootstrap because the baudrate is reset to the default. The CRSF side will // drop back down to 416k if it has received 200 incorrect characters (or none at all) } else if (_detected_protocol != AP_RCProtocol::NONE // protocols that want to be able to renegotiate should return false in is_rx_active() && !backend[_detected_protocol]->is_rx_active() && now - added.last_config_change_ms > 1000) { added.opened = false; } } void AP_RCProtocol::update() { check_added_uart(); } bool AP_RCProtocol::new_input() { bool ret = _new_input; _new_input = false; // if we have an extra UART from a SERIALn_PROTOCOL then check it for data check_added_uart(); // run update function on backends for (uint8_t i = 0; i < ARRAY_SIZE(backend); i++) { if (backend[i] != nullptr) { backend[i]->update(); } } return ret; } uint8_t AP_RCProtocol::num_channels() { if (_detected_protocol != AP_RCProtocol::NONE) { return backend[_detected_protocol]->num_channels(); } return 0; } uint16_t AP_RCProtocol::read(uint8_t chan) { if (_detected_protocol != AP_RCProtocol::NONE) { return backend[_detected_protocol]->read(chan); } return 0; } void AP_RCProtocol::read(uint16_t *pwm, uint8_t n) { if (_detected_protocol != AP_RCProtocol::NONE) { backend[_detected_protocol]->read(pwm, n); } } int16_t AP_RCProtocol::get_RSSI(void) const { if (_detected_protocol != AP_RCProtocol::NONE) { return backend[_detected_protocol]->get_RSSI(); } return -1; } int16_t AP_RCProtocol::get_rx_link_quality(void) const { if (_detected_protocol != AP_RCProtocol::NONE) { return backend[_detected_protocol]->get_rx_link_quality(); } return -1; } /* ask for bind start on supported receivers (eg spektrum satellite) */ void AP_RCProtocol::start_bind(void) { for (uint8_t i = 0; i < ARRAY_SIZE(backend); i++) { if (backend[i] != nullptr) { backend[i]->start_bind(); } } } /* return protocol name */ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol) { switch (protocol) { case PPM: return "PPM"; case IBUS: return "IBUS"; case SBUS: case SBUS_NI: return "SBUS"; #if AP_RCPROTOCOL_FASTSBUS_ENABLED case FASTSBUS: return "FastSBUS"; #endif case DSM: return "DSM"; case SUMD: return "SUMD"; #if AP_RCPROTOCOL_SRXL_ENABLED case SRXL: return "SRXL"; #endif case SRXL2: return "SRXL2"; case CRSF: return "CRSF"; case ST24: return "ST24"; #if AP_RCPROTOCOL_FPORT_ENABLED case FPORT: return "FPORT"; #endif #if AP_RCPROTOCOL_FPORT2_ENABLED case FPORT2: return "FPORT2"; #endif case NONE: break; } return nullptr; } /* return protocol name */ const char *AP_RCProtocol::protocol_name(void) const { return protocol_name_from_protocol(_detected_protocol); } /* add a uart to decode */ void AP_RCProtocol::add_uart(AP_HAL::UARTDriver* uart) { added.uart = uart; added.uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); } // return true if a specific protocol is enabled bool AP_RCProtocol::protocol_enabled(rcprotocol_t protocol) const { if ((rc_protocols_mask & 1) != 0) { // all protocols enabled return true; } return ((1U<<(uint8_t(protocol)+1)) & rc_protocols_mask) != 0; } namespace AP { AP_RCProtocol &RC() { static AP_RCProtocol rcprot; return rcprot; } };