From 0f273776cfecc3eebe14df66656fb3129e264d46 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 15 Jul 2023 19:06:06 +0100 Subject: [PATCH 1/6] AP_HAL: uart frame API --- libraries/AP_HAL/UARTDriver.cpp | 64 ++++++++++++++++++++++++++++++++- libraries/AP_HAL/UARTDriver.h | 45 ++++++++++++++++++++++- 2 files changed, 107 insertions(+), 2 deletions(-) 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; From b77be50bee8dfc53902e84326ccfdee33d1ec7ea Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 15 Jul 2023 19:06:21 +0100 Subject: [PATCH 2/6] AP_HAL_ChibiOS: uart frame API implementation by using IDLE to demarcate end of frames ensure that DMA transfers triggered by IDLE line occur before framing start and stop serial device when changing config --- libraries/AP_HAL_ChibiOS/UARTDriver.cpp | 172 +++++++++++++++++++++++- libraries/AP_HAL_ChibiOS/UARTDriver.h | 22 ++- 2 files changed, 185 insertions(+), 9 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index a04537fdbddaf..e7290694e5287 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -310,6 +310,9 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) } if (clear_buffers) { + _frame.idle = false; + _frame.bounce_len[0] = _frame.bounce_len[1] = 0; + _frame.bounce_idx = _frame.read_bounce_idx = 0; _readbuf.clear(); } @@ -444,19 +447,22 @@ 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); + _frame.idle = false; sdStart((SerialDriver*)sdef.serial, &sercfg); #ifndef HAL_UART_NODMA @@ -496,6 +502,93 @@ 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; + _frame.idle = 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; + } + + const uint8_t bounce_idx = _frame.read_bounce_idx; + _frame.read_bounce_idx ^= 1; + + 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; + + 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 +614,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 +630,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 +638,49 @@ 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; + } + + _frame.bounce_len[frame_bounce_idx] = _readbuf.read(_frame.bounce_buf[frame_bounce_idx], fsize); + _frame.bounce_idx ^= 1; + _frame.idle = false; +} + +// 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) { +#endif + // this is only ever called if IDLEIE is set + if (uart_drv->_framing_initialised) { + uart_drv->transfer_frame(); + } +#ifndef HAL_UART_NODMA return; } + + uart_drv->_frame.idle = true; + #if defined(STM32F7) || defined(STM32H7) //disable dma, triggering DMA transfer complete interrupt uart_drv->rxdma->stream->CR &= ~STM32_DMA_CR_EN; @@ -572,13 +697,14 @@ 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 /* 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 +712,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; @@ -605,6 +732,10 @@ __RAMFUNC__ void UARTDriver::rxbuff_full_irq(void* self, uint32_t flags) uart_drv->receive_timestamp_update(); } + if (uart_drv->_framing_initialised && uart_drv->_frame.idle) { + uart_drv->transfer_frame(); + } + if (uart_drv->_wait.thread_ctx && uart_drv->_readbuf.available() >= uart_drv->_wait.n) { chSysLockFromISR(); chEvtSignalI(uart_drv->_wait.thread_ctx, EVT_DATA); @@ -637,6 +768,8 @@ void UARTDriver::_end() _readbuf.set_size(0); _writebuf.set_size(0); + + _end_framing(); } void UARTDriver::_flush() @@ -685,7 +818,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 +856,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 +1545,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); @@ -1432,8 +1577,13 @@ void UARTDriver::configure_parity(uint8_t v) break; } + // restarting the serial device results in a spurious IDLE interrupt + _frame.idle = false; + 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 +1617,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); @@ -1482,7 +1634,13 @@ void UARTDriver::set_stop_bits(int n) } sercfg.cr2 = _cr2_options; + _frame.idle = false; + 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..a1bd107245401 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; + volatile bool idle; + 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,15 @@ 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(); }; // access to usb init for stdio.cpp From 41d652e73c7a8b3a0dc8c604afef9e49ec2d1b3b Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 15 Jul 2023 20:57:57 +0100 Subject: [PATCH 3/6] AP_RCProtocol: use framing API to decode CRSF check sync byte for GHST and CRSF to avoid confusing the two protocols do not change baudrate when scanning GHST protocol unless specifically configured --- libraries/AP_RCProtocol/AP_RCProtocol.cpp | 62 ++++++++++++++++--- libraries/AP_RCProtocol/AP_RCProtocol.h | 1 + .../AP_RCProtocol/AP_RCProtocol_Backend.h | 8 +++ .../AP_RCProtocol/AP_RCProtocol_CRSF.cpp | 50 +++++++++++++++ libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h | 4 ++ .../AP_RCProtocol/AP_RCProtocol_GHST.cpp | 10 ++- 6 files changed, 125 insertions(+), 10 deletions(-) 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..69bcd76c27b88 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -99,6 +99,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 +137,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 +145,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..ed6817dc69f27 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) { + 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 (AP_RCProtocol_Backend::frame_input_enabled()) { + return false; + } + if (baudrate > CRSF_BAUDRATE_2MBIT) { return false; } 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..ea6295049fbc9 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,17 @@ 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) { + || (get_rc_protocols_mask() & (1U<<(uint8_t(AP_RCProtocol::GHST)+1))) == 0) { return; } From 6b28c8f892028bb7278c3b0bd4a02a0bf0e2fa95 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 1 Feb 2024 21:27:09 +0000 Subject: [PATCH 4/6] AP_RCProtocol: allow CRSFv3 at 1Mbps with framing --- libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index ed6817dc69f27..db4966f2c29fa 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -302,7 +302,7 @@ void AP_RCProtocol_CRSF::frame_input_enabled(AP_HAL::UARTDriver* uart, bool onof } // due to bugs in CRSFv3, framing is not possible at higher baudrates - if (onoff && uart->get_baud_rate() > CRSF_BAUDRATE) { + if (onoff && uart->get_baud_rate() > CRSF_BAUDRATE_1MBIT) { return; } @@ -644,7 +644,7 @@ bool AP_RCProtocol_CRSF::change_baud_rate(uint32_t baudrate) } #endif - if (AP_RCProtocol_Backend::frame_input_enabled()) { + if (baudrate > CRSF_BAUDRATE_1MBIT && AP_RCProtocol_Backend::frame_input_enabled()) { return false; } From ac73a218caa93afbd8ed46a1acd938d5e75e61c6 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 1 Feb 2024 13:51:02 +0000 Subject: [PATCH 5/6] AP_RCProtocol: discriminate GHST and CRSF based on first byte do not change to GHST baudrate unless it has been specifically configured --- libraries/AP_RCProtocol/AP_RCProtocol_Backend.h | 4 ++++ libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp | 2 +- libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp | 3 ++- 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index 69bcd76c27b88..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; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index db4966f2c29fa..615b3453655d2 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -667,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_GHST.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp index ea6295049fbc9..a51c9dd795c88 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp @@ -435,7 +435,8 @@ void AP_RCProtocol_GHST::process_handshake(uint32_t baudrate) || baudrate != CRSF_BAUDRATE || baudrate == GHST_BAUDRATE || uart->get_baud_rate() == GHST_BAUDRATE - || (get_rc_protocols_mask() & (1U<<(uint8_t(AP_RCProtocol::GHST)+1))) == 0) { + || !protocol_enabled(AP_RCProtocol::GHST) + || protocol_enabled(AP_RCProtocol::CRSF)) { return; } From f715476cd5835db9b662ca881c584fe15d742b2d Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 10 Feb 2024 11:32:18 +0000 Subject: [PATCH 6/6] AP_HAL_ChibiOS: asynchronous frame transfer --- libraries/AP_HAL_ChibiOS/UARTDriver.cpp | 57 +++++++++++++++---------- libraries/AP_HAL_ChibiOS/UARTDriver.h | 3 +- 2 files changed, 37 insertions(+), 23 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index e7290694e5287..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,7 +316,6 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) } if (clear_buffers) { - _frame.idle = false; _frame.bounce_len[0] = _frame.bounce_len[1] = 0; _frame.bounce_idx = _frame.read_bounce_idx = 0; _readbuf.clear(); @@ -462,7 +467,6 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) sercfg.ctx = (void*)this; sdStop((SerialDriver*)sdef.serial); - _frame.idle = false; sdStart((SerialDriver*)sdef.serial, &sercfg); #ifndef HAL_UART_NODMA @@ -537,7 +541,6 @@ void UARTDriver::_end_framing() _frame.bounce_len[0] = _frame.bounce_len[1] = 0; _frame.bounce_idx = _frame.read_bounce_idx = 0; _framing_initialised = false; - _frame.idle = false; } bool UARTDriver::_frame_available() @@ -570,8 +573,10 @@ ssize_t UARTDriver::_read_frame(uint8_t *buffer, uint16_t buf_size) return -1; } + //hal.gpio->toggle(53); + //hal.gpio->toggle(53); + const uint8_t bounce_idx = _frame.read_bounce_idx; - _frame.read_bounce_idx ^= 1; uint16_t flen = _frame.bounce_len[bounce_idx]; if (flen == 0) { @@ -580,7 +585,7 @@ ssize_t UARTDriver::_read_frame(uint8_t *buffer, uint16_t buf_size) memcpy(buffer, _frame.bounce_buf[bounce_idx], MIN(buf_size, flen)); - _frame.bounce_len[bounce_idx] = 0; + _frame.bounce_len[bounce_idx] = 0; // frame has been consumed if (!_rts_is_active) { update_rts_line(); @@ -653,9 +658,13 @@ void UARTDriver::transfer_frame() 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; - _frame.idle = false; } // UART idle ISR, only ever called if IDLEIE has been set @@ -670,16 +679,15 @@ void UARTDriver::rx_irq_cb(void* self) #ifndef HAL_UART_NODMA if (!uart_drv->rx_dma_enabled) { -#endif - // this is only ever called if IDLEIE is set - if (uart_drv->_framing_initialised) { - uart_drv->transfer_frame(); - } -#ifndef HAL_UART_NODMA return; } - uart_drv->_frame.idle = true; + 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 @@ -701,6 +709,20 @@ void UARTDriver::rx_irq_cb(void* self) #endif // HAL_USE_SERIAL } +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 */ @@ -732,10 +754,6 @@ __RAMFUNC__ void UARTDriver::rxbuff_full_irq(void* self, uint32_t flags) uart_drv->receive_timestamp_update(); } - if (uart_drv->_framing_initialised && uart_drv->_frame.idle) { - uart_drv->transfer_frame(); - } - if (uart_drv->_wait.thread_ctx && uart_drv->_readbuf.available() >= uart_drv->_wait.n) { chSysLockFromISR(); chEvtSignalI(uart_drv->_wait.thread_ctx, EVT_DATA); @@ -1577,9 +1595,6 @@ void UARTDriver::configure_parity(uint8_t v) break; } - // restarting the serial device results in a spurious IDLE interrupt - _frame.idle = false; - sdStart((SerialDriver*)sdef.serial, &sercfg); _framing_initialised = frminit; @@ -1634,8 +1649,6 @@ void UARTDriver::set_stop_bits(int n) } sercfg.cr2 = _cr2_options; - _frame.idle = false; - sdStart((SerialDriver*)sdef.serial, &sercfg); // restarting the serial device results in a spurious IDLE interrupt diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index a1bd107245401..79b927854fccf 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -185,7 +185,7 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { struct { volatile uint8_t bounce_idx; volatile uint8_t read_bounce_idx; - volatile bool idle; + virtual_timer_t rx_timeout; uint8_t *bounce_buf[2]; uint16_t bounce_len[2]; } _frame; @@ -293,6 +293,7 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { private: void transfer_frame(); + static void transfer_frame(virtual_timer_t* vt, void *p); }; // access to usb init for stdio.cpp