Adds a Tx Buffer to avoid data loss when bursts of messages are sent …

…together.

firmware/mcal/stm32f767/periph/can.cc

@@ -4,37 +4,45 @@
 #include <cstdint>
 
 #include "can.hpp"
+#include "etl/queue_spsc_atomic.h"
 
 // FIFO0 is the "high priority" queue on stm32f7. We do not use FIFO1
 constexpr uint32_t kCanFifo = CAN_RX_FIFO0;
 constexpr uint32_t kCanInterrupt = CAN_IT_RX_FIFO0_MSG_PENDING;
 
-namespace {  // InterruptHandler is private to this file
+namespace mcal::stm32f767::periph::priv {
 
 /// Prior to this class, we spent several hours debugging an issue where the
 /// interrupt was activated but the handler was not being called, causing the
 /// program to get repeatedly executed the interupt callback and stall the rest
 /// of the program.
 /// This class ensures that interrupts activated and handled together.
 class InterruptHandler {
-    using CanBase = mcal::stm32f767::periph::CanBase;
-
 public:
     void RegisterCanBase(CAN_HandleTypeDef* hcan, CanBase* can_base) {
         int index = HandleToIndex(hcan);
         if (index == -1) return;
 
         can_bases[index] = can_base;
         HAL_CAN_ActivateNotification(hcan, kCanInterrupt);
+        HAL_CAN_ActivateNotification(hcan, CAN_IT_TX_MAILBOX_EMPTY);
     }
 
-    void Handle(CAN_HandleTypeDef* hcan) {
-        int index = HandleToIndex(hcan);
-        if (index == -1) return;
+    void Receive(CAN_HandleTypeDef* hcan) {
+        auto can_base = HandleToCanBase(hcan);
+        if (can_base == nullptr) return;
+
+        can_base->Receive();
+    }
 
-        auto can_base = can_bases[index];
-        if (can_base != nullptr) {
-            can_bases[index]->Receive();
+    void LoadTx(CAN_HandleTypeDef* hcan) {
+        auto can_base = HandleToCanBase(hcan);
+        if (can_base == nullptr) return;
+
+        shared::can::RawMessage msg;
+        auto msg_loaded = can_base->tx_queue_.pop(msg);
+        if (msg_loaded) {
+            can_base->SendLL(msg);
         }
     }
 
@@ -53,15 +61,36 @@ class InterruptHandler {
             return -1;
         }
     }
+
+    CanBase* HandleToCanBase(CAN_HandleTypeDef* hcan) {
+        int index = HandleToIndex(hcan);
+        if (index == -1) return nullptr;
+
+        return can_bases[index];
+    }
 };
-}  // namespace
+}  // namespace mcal::stm32f767::periph::priv
 
-static InterruptHandler interrupt_handler;
+// Define the HAL Interrupt callbacks, overriding the "weak" callbacks
+// defined by CubeMX. By statically declaring the callbacks, the firmware
+// applications can "just use" CanBase without needing to set up interrupts.
+static mcal::stm32f767::periph::priv::InterruptHandler interrupt_handler;
 
 extern "C" {
 void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan) {
-    // Overrides the "weak" callback defined by CubeMX
-    interrupt_handler.Handle(hcan);
+    // Message was received
+    interrupt_handler.Receive(hcan);
+}
+
+// All tx empty callbacks are the same.
+void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef* hcan) {
+    interrupt_handler.LoadTx(hcan);
+}
+void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef* hcan) {
+    interrupt_handler.LoadTx(hcan);
+}
+void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef* hcan) {
+    interrupt_handler.LoadTx(hcan);
 }
 }
 
@@ -76,12 +105,17 @@ void CanBase::Setup() {
 }
 
 void CanBase::Send(const shared::can::RawMessage& msg) {
-    uint32_t tx_mailboxes_free_level = HAL_CAN_GetTxMailboxesFreeLevel(hcan_);
-    if (tx_mailboxes_free_level < 1) {
-        dropped_tx_frames_ += 1;
-        return;
+    if (HAL_CAN_GetTxMailboxesFreeLevel(hcan_) > 0) {
+        SendLL(msg);
+    } else {
+        auto msg_dropped = !tx_queue_.push(msg);
+        if (msg_dropped) {  // Queue is full
+            dropped_tx_frames_ += 1;
+        }
     }
+}
 
+void CanBase::SendLL(const shared::can::RawMessage& msg) {
     CAN_TxHeaderTypeDef stm_tx_header;
     stm_tx_header.RTR = CAN_RTR_DATA;  // we only support data frames currently
     stm_tx_header.DLC = msg.data_length;
@@ -94,6 +128,7 @@ void CanBase::Send(const shared::can::RawMessage& msg) {
         stm_tx_header.StdId = msg.id;
     }
 
+    uint32_t tx_mailbox_addr_;  // Which mailbox the msg is placed in. Unused.
     HAL_CAN_AddTxMessage(hcan_, &stm_tx_header, msg.data, &tx_mailbox_addr_);
 }
 

firmware/mcal/stm32f767/periph/can.hpp

@@ -8,28 +8,39 @@
 
 #include <cstdint>
 
+#include "etl/queue_spsc_atomic.h"
 #include "shared/comms/can/msg.hpp"
 #include "shared/periph/can.hpp"
 
 namespace mcal::stm32f767::periph {
 
+namespace priv {
+class InterruptHandler;
+}  // namespace priv
+
 class CanBase : public shared::periph::CanBase {
 public:
     CanBase(CAN_HandleTypeDef* hcan);
 
     void Setup();
     void Send(const shared::can::RawMessage& can_tx_msg) override;
-    void Receive();
 
 private:
     uint32_t GetTimestamp() const override;
     void ConfigFilters();
+    void Receive();
+    void SendLL(const shared::can::RawMessage& msg);
+
+    CAN_HandleTypeDef* hcan_;
+
+    /// The STM32F7 CAN peripheral only has 3 Tx mailboxes. This queue provides
+    /// more space to ensure bursts of `.Send()` don't overflow them.
+    etl::queue_spsc_atomic<shared::can::RawMessage, 16> tx_queue_;
 
     /// @todo broadcast these over a can message
     uint32_t dropped_tx_frames_ = 0;
 
-    CAN_HandleTypeDef* hcan_;
-    uint32_t tx_mailbox_addr_;
+    friend class priv::InterruptHandler;
 };
 
 }  // namespace mcal::stm32f767::periph

firmware/projects/Demo/CAN/Receive/platforms/stm32f767/cubemx/board_config.ioc

@@ -36,6 +36,7 @@ MxCube.Version=6.12.0
 MxDb.Version=DB.6.0.120
 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false
 NVIC.CAN3_RX0_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
+NVIC.CAN3_TX_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false
 NVIC.ForceEnableDMAVector=true
 NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false

firmware/projects/Demo/CAN/Send/platforms/stm32f767/cubemx/board_config.ioc

@@ -36,6 +36,7 @@ MxCube.Version=6.12.0
 MxDb.Version=DB.6.0.120
 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false
 NVIC.CAN3_RX0_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
+NVIC.CAN3_TX_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false
 NVIC.ForceEnableDMAVector=true
 NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false

firmware/projects/FrontController/platforms/stm32f767/cubemx/board_config.ioc

@@ -93,7 +93,9 @@ MxCube.Version=6.12.0
 MxDb.Version=DB.6.0.120
 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.CAN1_RX0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
+NVIC.CAN1_TX_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.CAN3_RX0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
+NVIC.CAN3_TX_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.ForceEnableDMAVector=true
 NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false

firmware/projects/LVController/platforms/stm32f767/cubemx/board_config.ioc

@@ -77,6 +77,7 @@ MxCube.Version=6.12.0
 MxDb.Version=DB.6.0.120
 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.CAN3_RX0_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
+NVIC.CAN3_TX_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.ForceEnableDMAVector=true
 NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false

firmware/projects/TMS/platforms/stm32f767/cubemx/board_config.ioc

@@ -60,6 +60,7 @@ MxCube.Version=6.12.0
 MxDb.Version=DB.6.0.120
 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false\:false
 NVIC.CAN2_RX0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
+NVIC.CAN2_TX_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false\:false
 NVIC.ForceEnableDMAVector=true
 NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false\:false