From b1641740379787f6fcc1ab2a0b9e361e6be0534e Mon Sep 17 00:00:00 2001 From: JacobCrabill Date: Mon, 3 Aug 2020 18:04:53 -0700 Subject: [PATCH] uavcan: stm32h7: Fix improper loopback handling Fixes #15305 --- .../stm32h7/driver/include/uavcan_stm32h7/can.hpp | 2 +- .../stm32h7/driver/src/uc_stm32h7_can.cpp | 11 +++++++++-- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp index 3253cef688..4b0be0fd22 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp @@ -94,7 +94,7 @@ class CanIface : public uavcan::ICanIface, uavcan::Noncopyable uavcan::MonotonicTime deadline; uavcan::CanFrame frame; uavcan::uint8_t index; - //bool pending; + bool pending; bool loopback; bool abort_on_error; diff --git a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp index edc1fe2034..fdae6246e5 100644 --- a/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp +++ b/src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_can.cpp @@ -470,6 +470,7 @@ uavcan::int16_t CanIface::send(const uavcan::CanFrame &frame, uavcan::MonotonicT txi.loopback = (flags & uavcan::CanIOFlagLoopback) != 0; txi.abort_on_error = (flags & uavcan::CanIOFlagAbortOnError) != 0; txi.index = index; + txi.pending = true; return 1; } @@ -705,9 +706,13 @@ void CanIface::handleTxInterrupt(const uavcan::uint64_t utc_usec) for (uint8_t i = 0; i < NumTxMailboxes; i++) { if ((can_->TXBTO & (1 << i)) > 0) { // Transmission Occurred in buffer i - if (pending_tx_[i].loopback && had_activity_) { - rx_queue_.push(pending_tx_[i].frame, utc_usec, uavcan::CanIOFlagLoopback); + TxItem &txi = pending_tx_[i]; + + if (txi.loopback && txi.pending) { + rx_queue_.push(txi.frame, utc_usec, uavcan::CanIOFlagLoopback); } + + txi.pending = false; } } @@ -818,6 +823,7 @@ void CanIface::pollErrorFlagsFromISR() if (txi.abort_on_error && ((1 << txi.index) & can_->TXBRP)) { // Request to Cancel Tx item can_->TXBCR = (1 << txi.index); + txi.pending = false; error_cnt_++; served_aborts_cnt_++; } @@ -835,6 +841,7 @@ void CanIface::discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time) if (((1 << txi.index) & can_->TXBRP) && txi.deadline < current_time) { // Request to Cancel Tx item can_->TXBCR = (1 << txi.index); + txi.pending = false; error_cnt_++; } }