diff --git a/libraries/AP_HAL/UARTDriver.cpp b/libraries/AP_HAL/UARTDriver.cpp index b3d06c4f450ff..2d96dcc8d1f71 100644 --- a/libraries/AP_HAL/UARTDriver.cpp +++ b/libraries/AP_HAL/UARTDriver.cpp @@ -9,7 +9,7 @@ void AP_HAL::UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace // silently fail return; } - return _begin(baud, rxSpace, txSpace); + _begin(baud, rxSpace, txSpace); } void AP_HAL::UARTDriver::begin(uint32_t baud) @@ -17,6 +17,39 @@ void AP_HAL::UARTDriver::begin(uint32_t baud) return begin(baud, 0, 0); } +/* + begin in frame-based mode. the framing API works on top of the + existing UART infrastructure and framing can be started at any + time after the UART has been started. Consumed frames can be at most frame_size + bytes long, frames longer than this will be considered corrupt and the + framing buffer will be reset. The framing buffer holds at most + frames_in_buffer frames, the frames are stored in a ring buffer and so + older frames that have not been read will be overwritten. + Returns true if the framing API could be started, false otherwise + */ +bool AP_HAL::UARTDriver::begin_framing(uint16_t max_frame_size) +{ + if (lock_write_key != 0) { + // silently fail + return false; + } + + const bool ret = _begin_framing(max_frame_size); + frame_size = max_frame_size; + return ret; +} + +void AP_HAL::UARTDriver::end_framing() +{ + if (lock_write_key != 0) { + // silently fail + return; + } + + _end_framing(); + frame_size = 1; +} + /* lock the uart for exclusive use by write_locked() and read_locked() with the right key */ @@ -111,6 +144,27 @@ bool AP_HAL::UARTDriver::read(uint8_t &b) return n > 0; } +/* + frame-based read. An entire frame will be read into buffer. count indicates the size of the + buffer and if this is less than the size of the frame to be read, the remaining bytes in the frame will + be discarded. returns the size of the frame that was read or 0 if no frames are available to + be read. +*/ +ssize_t AP_HAL::UARTDriver::read_frame(uint8_t *buf, uint16_t buf_size) +{ + if (lock_read_key != 0) { + return 0; + } + ssize_t ret = _read_frame(buf, buf_size); +#if AP_UART_MONITOR_ENABLED + auto monitor = _monitor_read_buffer; + if (monitor != nullptr && ret > 0) { + monitor->write(buf, ret); + } +#endif + return ret; +} + int16_t AP_HAL::UARTDriver::read(void) { uint8_t b; @@ -129,6 +183,14 @@ uint32_t AP_HAL::UARTDriver::available() return _available(); } +bool AP_HAL::UARTDriver::frame_available() +{ + if (lock_read_key != 0) { + return 0; + } + return _frame_available(); +} + void AP_HAL::UARTDriver::end() { if (lock_read_key != 0 || lock_write_key != 0) { diff --git a/libraries/AP_HAL/UARTDriver.h b/libraries/AP_HAL/UARTDriver.h index d876de6a93d4e..88c400f807b40 100644 --- a/libraries/AP_HAL/UARTDriver.h +++ b/libraries/AP_HAL/UARTDriver.h @@ -34,6 +34,18 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { */ void begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace); + /* + begin in frame-based mode. the framing API works on top of the + existing UART infrastructure and framing can be started at any + time after the UART has been started. Consumed frames can be at most frame_size + bytes long, frames longer than this will be considered corrupt and the + framing buffer will be reset. The framing buffer holds at most + frames_in_buffer frames, the frames are stored in a ring buffer and so + older frames that have not been read will be overwritten. + Returns true if the framing API could be started, false otherwise + */ + bool begin_framing(uint16_t max_frame_size); + /* begin for use when the port is write locked. Note that this does not lock the port, an existing write_key from lock_port() must @@ -54,8 +66,17 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { int16_t read(void) override; bool read(uint8_t &b) override WARN_IF_UNUSED; ssize_t read(uint8_t *buffer, uint16_t count) override; - + + /* + frame-based read. An entire frame will be read into buffer. count indicates the size of the + buffer and if this is less than the size of the frame to be read, the remaining bytes in the frame will + be discarded. returns the size of the frame that was read or 0 if no frames are available to + be read. + */ + ssize_t read_frame(uint8_t *buffer, uint16_t count); + void end(); + void end_framing(); void flush(); virtual bool is_initialized() = 0; @@ -67,6 +88,7 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { // check data available for read, return 0 is not available uint32_t available() override; uint32_t available_locked(uint32_t key); + bool frame_available(); // discard any pending input bool discard_input() override; @@ -189,12 +211,22 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { // key for a locked port uint32_t lock_write_key; uint32_t lock_read_key; + uint16_t frame_size = 1; /* backend begin method */ virtual void _begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) = 0; + /* + backend begin in frame-based mode. see the documentation for begin_framing() + */ + virtual bool _begin_framing(uint16_t max_frame_size) { + return false; + } + + virtual void _end_framing() { } + /* backend write method */ @@ -205,6 +237,14 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { */ virtual ssize_t _read(uint8_t *buffer, uint16_t count) WARN_IF_UNUSED = 0; + /* + frame-based read. see the documentation for read_frame() + */ + virtual ssize_t _read_frame(uint8_t *buffer, uint16_t buf_size) { + // a simplistic implementation that simply reads bytes using _read() + return _read(buffer, buf_size < frame_size ? buf_size : frame_size); + } + /* end control of the port, freeing buffers */ @@ -218,6 +258,9 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { // check available data on the port virtual uint32_t _available() = 0; + // check for available frames + virtual bool _frame_available() { return available() != 0; } + // discard incoming data on the port virtual bool _discard_input(void) = 0; diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index a04537fdbddaf..984333fcb5699 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -106,6 +106,7 @@ _baudrate(57600) { osalDbgAssert(serial_num < UART_MAX_DRIVERS, "too many SERIALn drivers"); serial_drivers[serial_num] = this; + chVTObjectInit(&_frame.rx_timeout); } /* @@ -124,6 +125,11 @@ void UARTDriver::uart_thread() hal.scheduler->delay_microseconds(1000); } + hal.gpio->pinMode(51,1); + hal.gpio->pinMode(52,1); + hal.gpio->pinMode(53,1); + hal.gpio->pinMode(54,1); + while (true) { eventmask_t mask = chEvtWaitAnyTimeout(EVT_TRANSMIT_DATA_READY | EVT_TRANSMIT_END | EVT_TRANSMIT_UNBUFFERED, chTimeMS2I(1)); uint32_t now = AP_HAL::micros(); @@ -310,6 +316,8 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) } if (clear_buffers) { + _frame.bounce_len[0] = _frame.bounce_len[1] = 0; + _frame.bounce_idx = _frame.read_bounce_idx = 0; _readbuf.clear(); } @@ -444,19 +452,21 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) #ifndef HAL_UART_NODMA if (rx_dma_enabled) { - sercfg.cr1 |= USART_CR1_IDLEIE; sercfg.cr3 |= USART_CR3_DMAR; } if (tx_dma_enabled) { sercfg.cr3 |= USART_CR3_DMAT; } - sercfg.irq_cb = rx_irq_cb; #endif // HAL_UART_NODMA + sercfg.cr1 |= USART_CR1_IDLEIE; + sercfg.irq_cb = rx_irq_cb; + if (!(sercfg.cr2 & USART_CR2_STOP2_BITS)) { sercfg.cr2 |= USART_CR2_STOP1_BITS; } sercfg.ctx = (void*)this; + sdStop((SerialDriver*)sdef.serial); sdStart((SerialDriver*)sdef.serial, &sercfg); #ifndef HAL_UART_NODMA @@ -496,6 +506,94 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) } } +/* + begin framing. Data from _readbuf is read into _frame.readbuf when an idle ISR is detected. + _frame.reabounce_buf is sized to be able to accommodate at frames of at most max_frame_size + */ +bool UARTDriver::_begin_framing(uint16_t max_frame_size) +{ + // disallow framing from a different thread + if (_uart_owner_thd != chThdGetSelfX()) { + return false; + } + + if (!_framing_initialised || frame_size != max_frame_size) { + hal.util->free_type(_frame.bounce_buf[0], max_frame_size, AP_HAL::Util::MEM_DMA_SAFE); + hal.util->free_type(_frame.bounce_buf[1], max_frame_size, AP_HAL::Util::MEM_DMA_SAFE); + _frame.bounce_buf[0] = (uint8_t *)hal.util->malloc_type(max_frame_size, AP_HAL::Util::MEM_DMA_SAFE); + _frame.bounce_buf[1] = (uint8_t *)hal.util->malloc_type(max_frame_size, AP_HAL::Util::MEM_DMA_SAFE); + } + _frame.bounce_len[0] = 0; + _frame.bounce_len[1] = 0; + _frame.bounce_idx = _frame.read_bounce_idx = 0; + + _framing_initialised = true; + + return true; +} + +void UARTDriver::_end_framing() +{ + // clean up frame read buffer + hal.util->free_type(_frame.bounce_buf[0], frame_size, AP_HAL::Util::MEM_DMA_SAFE); + hal.util->free_type(_frame.bounce_buf[1], frame_size, AP_HAL::Util::MEM_DMA_SAFE); + _frame.bounce_buf[0] = _frame.bounce_buf[1] = nullptr; + _frame.bounce_len[0] = _frame.bounce_len[1] = 0; + _frame.bounce_idx = _frame.read_bounce_idx = 0; + _framing_initialised = false; +} + +bool UARTDriver::_frame_available() +{ + if (!_framing_initialised) { + return false; + } + + // if there is any data then a frame must be available + return _frame.bounce_len[_frame.read_bounce_idx] > 0; +} + +/* + read a frame from _frame.readbuf into buffer. if the size of the frame to be + read exceeds buf_size then remaining bytes are discarded. returns the number of + read bytes (usually the size of the frame) or 0 if no frames are available + or -1 if an error occurs + */ +ssize_t UARTDriver::_read_frame(uint8_t *buffer, uint16_t buf_size) +{ + if (_uart_owner_thd != chThdGetSelfX()){ + return -1; + } + + if (!_rx_initialised) { + return -1; + } + + if (!_framing_initialised) { + return -1; + } + + //hal.gpio->toggle(53); + //hal.gpio->toggle(53); + + const uint8_t bounce_idx = _frame.read_bounce_idx; + + uint16_t flen = _frame.bounce_len[bounce_idx]; + if (flen == 0) { + return 0; + } + + memcpy(buffer, _frame.bounce_buf[bounce_idx], MIN(buf_size, flen)); + + _frame.bounce_len[bounce_idx] = 0; // frame has been consumed + + if (!_rts_is_active) { + update_rts_line(); + } + + return MIN(buf_size, flen); +} + #ifndef HAL_UART_NODMA void UARTDriver::dma_tx_allocate(Shared_DMA *ctx) { @@ -521,7 +619,6 @@ void UARTDriver::dma_tx_allocate(Shared_DMA *ctx) #endif // HAL_USE_SERIAL } -#ifndef HAL_UART_NODMA void UARTDriver::dma_rx_enable(void) { uint32_t dmamode = STM32_DMA_CR_DMEIE | STM32_DMA_CR_TEIE; @@ -538,7 +635,6 @@ void UARTDriver::dma_rx_enable(void) STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE); dmaStreamEnable(rxdma); } -#endif void UARTDriver::dma_tx_deallocate(Shared_DMA *ctx) { @@ -547,15 +643,52 @@ void UARTDriver::dma_tx_deallocate(Shared_DMA *ctx) txdma = nullptr; chSysUnlock(); } +#endif -#ifndef HAL_UART_NODMA +// transfer a frame from _readbuf to _frame.bounce_buf by reading all of +// the available bytes. the API only works if _readbuf contains +// only a single complete frame. if any kind of data errors occurs _readbuf and +// _frame.buf are reset. +void UARTDriver::transfer_frame() +{ + const uint32_t fsize = MIN(_readbuf.available(), frame_size); + const uint8_t frame_bounce_idx = _frame.bounce_idx; + // no data + if (fsize == 0 || _frame.bounce_buf[frame_bounce_idx] == nullptr) { + return; + } + + //hal.gpio->toggle(52); + //hal.gpio->toggle(52); + + _frame.bounce_len[frame_bounce_idx] = _readbuf.read(_frame.bounce_buf[frame_bounce_idx], fsize); + _readbuf.clear(); // discard anything left over + _frame.read_bounce_idx = frame_bounce_idx; // fresh data + _frame.bounce_idx ^= 1; +} + +// UART idle ISR, only ever called if IDLEIE has been set +// this callback is always executed in order to support the +// framing API. Note that start and stopping the serial device +// will also generate an IDLE interrupt even though there is no +// data. void UARTDriver::rx_irq_cb(void* self) { #if HAL_USE_SERIAL == TRUE UARTDriver* uart_drv = (UARTDriver*)self; + +#ifndef HAL_UART_NODMA if (!uart_drv->rx_dma_enabled) { return; } + + if (uart_drv->_framing_initialised) { + chSysLockFromISR(); + chVTSetI(&uart_drv->_frame.rx_timeout, chTimeUS2I(10), transfer_frame, self); + chSysUnlockFromISR(); + } + + #if defined(STM32F7) || defined(STM32H7) //disable dma, triggering DMA transfer complete interrupt uart_drv->rxdma->stream->CR &= ~STM32_DMA_CR_EN; @@ -572,13 +705,28 @@ void UARTDriver::rx_irq_cb(void* self) uart_drv->rxdma->stream->CR &= ~STM32_DMA_CR_EN; } #endif // STM32F7 +#endif // HAL_UART_NODMA #endif // HAL_USE_SERIAL } -#endif + +void UARTDriver::transfer_frame(virtual_timer_t* vt, void *p) +{ + chSysLockFromISR(); + UARTDriver *uart = (UARTDriver *)p; + + if (uart->_readbuf.available() && dmaStreamGetTransactionSize(uart->rxdma) == RX_BOUNCE_BUFSIZE) { + hal.gpio->toggle(51); + hal.gpio->toggle(51); + uart->transfer_frame(); + } + + chSysUnlockFromISR(); +} /* handle a RX DMA full interrupt */ + #ifndef HAL_UART_NODMA __RAMFUNC__ void UARTDriver::rxbuff_full_irq(void* self, uint32_t flags) { #if HAL_USE_SERIAL == TRUE @@ -586,6 +734,7 @@ __RAMFUNC__ void UARTDriver::rxbuff_full_irq(void* self, uint32_t flags) if (!uart_drv->rx_dma_enabled) { return; } + uint16_t len = RX_BOUNCE_BUFSIZE - dmaStreamGetTransactionSize(uart_drv->rxdma); const uint8_t bounce_idx = uart_drv->rx_bounce_idx; @@ -637,6 +786,8 @@ void UARTDriver::_end() _readbuf.set_size(0); _writebuf.set_size(0); + + _end_framing(); } void UARTDriver::_flush() @@ -685,7 +836,11 @@ uint32_t UARTDriver::_available() } #endif } - return _readbuf.available(); + if (_framing_initialised) { + return _frame.bounce_len[_frame.read_bounce_idx]; + } else { + return _readbuf.available(); + } } uint32_t UARTDriver::txspace() @@ -719,10 +874,15 @@ ssize_t UARTDriver::_read(uint8_t *buffer, uint16_t count) if (_uart_owner_thd != chThdGetSelfX()){ return -1; } + if (!_rx_initialised) { return -1; } + if (_framing_initialised) { + return -1; + } + const uint32_t ret = _readbuf.read(buffer, count); if (ret == 0) { return 0; @@ -1403,6 +1563,9 @@ void UARTDriver::configure_parity(uint8_t v) return; } #if HAL_USE_SERIAL == TRUE + const bool frminit = _framing_initialised; + _framing_initialised = false; + // stop and start to take effect sdStop((SerialDriver*)sdef.serial); @@ -1434,6 +1597,8 @@ void UARTDriver::configure_parity(uint8_t v) sdStart((SerialDriver*)sdef.serial, &sercfg); + _framing_initialised = frminit; + #if CH_CFG_USE_EVENTS == TRUE if (parity_enabled) { chEvtUnregister(chnGetEventSource((SerialDriver*)sdef.serial), &ev_listener); @@ -1467,6 +1632,8 @@ void UARTDriver::set_stop_bits(int n) return; } #if HAL_USE_SERIAL + const bool frminit = _framing_initialised; + _framing_initialised = false; // stop and start to take effect sdStop((SerialDriver*)sdef.serial); @@ -1483,6 +1650,10 @@ void UARTDriver::set_stop_bits(int n) sercfg.cr2 = _cr2_options; sdStart((SerialDriver*)sdef.serial, &sercfg); + + // restarting the serial device results in a spurious IDLE interrupt + _framing_initialised = frminit; + #ifndef HAL_UART_NODMA if (rx_dma_enabled) { //Configure serial driver to skip handling RX packets diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index 1a045fa9e7e81..79b927854fccf 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -180,6 +180,17 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { #endif ByteBuffer _readbuf{0}; ByteBuffer _writebuf{0}; + + // A/B buffers for reading UART frames + struct { + volatile uint8_t bounce_idx; + volatile uint8_t read_bounce_idx; + virtual_timer_t rx_timeout; + uint8_t *bounce_buf[2]; + uint16_t bounce_len[2]; + } _frame; + + volatile bool _framing_initialised; HAL_Semaphore _write_mutex; #ifndef HAL_UART_NODMA const stm32_dma_stream_t* rxdma; @@ -235,9 +246,7 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { bool parity_enabled; #endif -#ifndef HAL_UART_NODMA static void rx_irq_cb(void* sd); -#endif static void rxbuff_full_irq(void* self, uint32_t flags); static void tx_complete(void* self, uint32_t flags); @@ -275,6 +284,16 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { ssize_t _read(uint8_t *buffer, uint16_t count) override; uint32_t _available() override; bool _discard_input() override; + + // frame based operations + bool _begin_framing(uint16_t frame_size) override; + void _end_framing() override; + ssize_t _read_frame(uint8_t *buffer, uint16_t buf_size) override; + bool _frame_available() override; + +private: + void transfer_frame(); + static void transfer_frame(virtual_timer_t* vt, void *p); }; // access to usb init for stdio.cpp diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index d2d11cfb304bf..eba84a6343c08 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -147,6 +147,7 @@ void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1) // this protocol is disabled for pulse input continue; } + if (backend[i] != nullptr) { if (!protocol_enabled(rcprotocol_t(i))) { continue; @@ -197,6 +198,33 @@ void AP_RCProtocol::process_pulse_list(const uint32_t *widths, uint16_t n, bool } } +bool AP_RCProtocol::process_frame(const uint8_t* buf, ssize_t nbytes) +{ + uint32_t now = AP_HAL::millis(); + bool searching = should_search(now); + +#if AP_RC_CHANNEL_ENABLED + rc_protocols_mask = rc().enabled_protocols(); +#endif + + if (_detected_protocol != AP_RCProtocol::NONE && + !protocol_enabled(_detected_protocol)) { + _detected_protocol = AP_RCProtocol::NONE; + } + + // first try current protocol + if (_detected_protocol != AP_RCProtocol::NONE && !searching) { + backend[_detected_protocol]->process_frame(buf, nbytes); + if (backend[_detected_protocol]->new_input()) { + _new_input = true; + _last_input_ms = now; + } + return true; + } + + return false; +} + bool AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate) { uint32_t now = AP_HAL::millis(); @@ -226,12 +254,14 @@ bool AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate) 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); @@ -309,7 +339,7 @@ static const AP_RCProtocol::SerialConfig serial_configs[] { #endif }; -static_assert(ARRAY_SIZE(serial_configs) > 1, "must have at least one serial config"); +static_assert(ARRAY_SIZE(serial_configs) > 0, "must have at least one serial config"); void AP_RCProtocol::check_added_uart(void) { @@ -333,14 +363,32 @@ void AP_RCProtocol::check_added_uart(void) 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); + // processing in frame-based mode + if (!searching && backend[_detected_protocol]->frame_input_enabled()) { + uint8_t buf[PROTOCOL_MAX_FRAME_SIZE]; + while (added.uart->frame_available()) { + ssize_t nbytes = added.uart->read_frame(buf, PROTOCOL_MAX_FRAME_SIZE); + process_frame(buf, nbytes); + } + } else { // processing in byte-based mode + if (_detected_protocol != AP_RCProtocol::NONE + && backend[_detected_protocol]->frame_input_enabled()) { + // searching again, make sure we are in byte mode + backend[_detected_protocol]->frame_input_enabled(added.uart, false); + } + + uint32_t n = added.uart->available(); + n = MIN(n, 255U); + for (uint8_t i=0; iread(); + if (b >= 0) { + if (process_byte(uint8_t(b), current_baud)) { + backend[_detected_protocol]->frame_input_enabled(added.uart, true); + } + } } } + if (searching) { if (now - added.last_config_change_ms > 1000) { // change configs if not detected once a second diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index 8889fdce89ee4..4ac741cf896fa 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -94,6 +94,7 @@ class AP_RCProtocol { void process_pulse(uint32_t width_s0, uint32_t width_s1); void process_pulse_list(const uint32_t *widths, uint16_t n, bool need_swap); bool process_byte(uint8_t byte, uint32_t baudrate); + bool process_frame(const uint8_t* buf, ssize_t nbytes); void process_handshake(uint32_t baudrate); void update(void); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index e0a128dc30a3c..d8d14354fbf6c 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -67,6 +67,10 @@ class AP_RCProtocol_Backend { return frontend.rc_protocols_mask; } + bool protocol_enabled(enum AP_RCProtocol::rcprotocol_t protocol) const { + return frontend.protocol_enabled(protocol); + } + // get RSSI int16_t get_RSSI(void) const { return rssi; @@ -99,6 +103,11 @@ class AP_RCProtocol_Backend { return frontend._detected_protocol != AP_RCProtocol::NONE && frontend.backend[frontend._detected_protocol] == this; } + // framing API + virtual void frame_input_enabled(AP_HAL::UARTDriver* uart, bool onoff) { _framing_enabled = onoff; } + bool frame_input_enabled() const { return _framing_enabled; } + virtual void process_frame(const uint8_t* buffer, uint16_t buflen) { } + #if AP_VIDEOTX_ENABLED // called by static methods to confiig video transmitters: static void configure_vtx(uint8_t band, uint8_t channel, uint8_t power, uint8_t pitmode); @@ -132,6 +141,7 @@ class AP_RCProtocol_Backend { uint32_t rc_input_count; uint32_t last_rc_input_count; uint32_t rc_frame_count; + bool _framing_enabled; uint16_t _pwm_values[MAX_RCIN_CHANNELS]; uint8_t _num_channels; @@ -139,4 +149,6 @@ class AP_RCProtocol_Backend { int16_t rx_link_quality = -1; }; +#define PROTOCOL_MAX_FRAME_SIZE 64 + #endif // AP_RCPROTOCOL_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index b9d68b63900b5..615b3453655d2 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -231,6 +231,16 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte) _start_frame_time_us = timestamp_us; } + _process_raw_byte(timestamp_us, byte); +} + +void AP_RCProtocol_CRSF::_process_raw_byte(uint32_t timestamp_us, uint8_t byte) +{ + // overflow check + if (_frame_ofs >= CRSF_FRAMELEN_MAX) { + _frame_ofs = 0; + } + add_to_buffer(_frame_ofs++, byte); // need a header to get the length @@ -238,6 +248,10 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte) return; } + if (_frame.device_address != DeviceAddress::CRSF_ADDRESS_FLIGHT_CONTROLLER) { + return; + } + // parse the length if (_frame_ofs == CSRF_HEADER_TYPE_LEN) { _frame_crc = crc8_dvb_s2(0, _frame.type); @@ -280,6 +294,34 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte) } } +void AP_RCProtocol_CRSF::frame_input_enabled(AP_HAL::UARTDriver* uart, bool onoff) +{ + // nothing to do + if (onoff == AP_RCProtocol_Backend::frame_input_enabled()) { + return; + } + + // due to bugs in CRSFv3, framing is not possible at higher baudrates + if (onoff && uart->get_baud_rate() > CRSF_BAUDRATE_1MBIT) { + return; + } + + AP_RCProtocol_Backend::frame_input_enabled(uart, onoff); + if (onoff) { + uart->begin_framing(CRSF_FRAMELEN_MAX); + } else { + uart->end_framing(); + } +} + +void AP_RCProtocol_CRSF::process_frame(const uint8_t* buffer, uint16_t buflen) +{ + uint32_t now_us = AP_HAL::micros(); + for (uint8_t i = 0; i < buflen; i++) { + _process_raw_byte(now_us, buffer[i]); + } +} + void AP_RCProtocol_CRSF::update(void) { // if we are in standalone mode, process data from the uart @@ -412,6 +454,9 @@ bool AP_RCProtocol_CRSF::decode_crsf_packet() hal.scheduler->delay(4); // change the baud rate uart->begin(_new_baud_rate); + if (!AP_RCProtocol_Backend::frame_input_enabled()) { + uart->begin_framing(CRSF_FRAMELEN_MAX); + } } _new_baud_rate = 0; } @@ -598,6 +643,11 @@ bool AP_RCProtocol_CRSF::change_baud_rate(uint32_t baudrate) return false; } #endif + + if (baudrate > CRSF_BAUDRATE_1MBIT && AP_RCProtocol_Backend::frame_input_enabled()) { + return false; + } + if (baudrate > CRSF_BAUDRATE_2MBIT) { return false; } @@ -617,7 +667,7 @@ void AP_RCProtocol_CRSF::process_handshake(uint32_t baudrate) || baudrate != CRSF_BAUDRATE || baudrate == get_bootstrap_baud_rate() || uart->get_baud_rate() == get_bootstrap_baud_rate() - || (get_rc_protocols_mask() & ((1U<<(uint8_t(AP_RCProtocol::CRSF)+1))+1)) == 0) { + || !protocol_enabled(AP_RCProtocol::CRSF)) { return; } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h index c063175e01f3e..6693b7ef152e5 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h @@ -54,6 +54,9 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend { #endif } + void frame_input_enabled(AP_HAL::UARTDriver* uart, bool onoff) override; + void process_frame(const uint8_t* buffer, uint16_t buflen) override; + // is the receiver active, used to detect power loss and baudrate changes bool is_rx_active() const override { // later versions of CRSFv3 will send link rate frames every 200ms @@ -298,6 +301,7 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend { static AP_RCProtocol_CRSF* _singleton; void _process_byte(uint32_t timestamp_us, uint8_t byte); + void _process_raw_byte(uint32_t timestamp_us, uint8_t byte); bool decode_crsf_packet(); bool process_telemetry(bool check_constraint = true); void process_link_stats_frame(const void* data); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp index 9a8a68f871383..a51c9dd795c88 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp @@ -152,6 +152,10 @@ void AP_RCProtocol_GHST::_process_byte(uint32_t timestamp_us, uint8_t byte) return; } + if (_frame.device_address != DeviceAddress::GHST_ADDRESS_FLIGHT_CONTROLLER) { + return; + } + // parse the length if (_frame_ofs == GHST_HEADER_TYPE_LEN) { _frame_crc = crc8_dvb_s2(0, _frame.type); @@ -421,17 +425,18 @@ void AP_RCProtocol_GHST::process_byte(uint8_t byte, uint32_t baudrate) _process_byte(AP_HAL::micros(), byte); } -// change the bootstrap baud rate to ELRS standard if configured +// change the bootstrap baud rate to Ghost standard if configured void AP_RCProtocol_GHST::process_handshake(uint32_t baudrate) { AP_HAL::UARTDriver *uart = get_current_UART(); - // only change the baudrate if we are bootstrapping CRSF + // only change the baudrate if we are specifically bootstrapping Ghost if (uart == nullptr || baudrate != CRSF_BAUDRATE || baudrate == GHST_BAUDRATE || uart->get_baud_rate() == GHST_BAUDRATE - || (get_rc_protocols_mask() & ((1U<<(uint8_t(AP_RCProtocol::GHST)+1))+1)) == 0) { + || !protocol_enabled(AP_RCProtocol::GHST) + || protocol_enabled(AP_RCProtocol::CRSF)) { return; }