diff --git a/datasheets/nucleo/nucleo_f767zi_morpho_left_2020_3_30.png b/datasheets/nucleo/nucleo_f767zi_morpho_left.png similarity index 100% rename from datasheets/nucleo/nucleo_f767zi_morpho_left_2020_3_30.png rename to datasheets/nucleo/nucleo_f767zi_morpho_left.png diff --git a/datasheets/nucleo/nucleo_f767zi_morpho_right_2020_3_30.png b/datasheets/nucleo/nucleo_f767zi_morpho_right.png similarity index 100% rename from datasheets/nucleo/nucleo_f767zi_morpho_right_2020_3_30.png rename to datasheets/nucleo/nucleo_f767zi_morpho_right.png diff --git a/datasheets/nucleo/nucleo_f767zi_zio_left_2020_3_30.png b/datasheets/nucleo/nucleo_f767zi_zio_left.png similarity index 100% rename from datasheets/nucleo/nucleo_f767zi_zio_left_2020_3_30.png rename to datasheets/nucleo/nucleo_f767zi_zio_left.png diff --git a/datasheets/nucleo/nucleo_f767zi_zio_right_2020_3_30.png b/datasheets/nucleo/nucleo_f767zi_zio_right.png similarity index 100% rename from datasheets/nucleo/nucleo_f767zi_zio_right_2020_3_30.png rename to datasheets/nucleo/nucleo_f767zi_zio_right.png diff --git a/firmware/mcal/cli/periph/can.hpp b/firmware/mcal/cli/periph/can.hpp index 1939dd3d5..fbd2fdc9f 100644 --- a/firmware/mcal/cli/periph/can.hpp +++ b/firmware/mcal/cli/periph/can.hpp @@ -3,14 +3,12 @@ #pragma once -#include -#include #include #include #include #include -#include +#include #include #include "shared/comms/can/msg.hpp" @@ -26,8 +24,9 @@ class CanBase : public shared::periph::CanBase { std::cout << "can interface: " << iface_ << std::endl; } - void Send(const shared::can::RawMessage& msg) { - std::cout << std::format("{} {}", iface_, msg) << std::endl; + void Send(const shared::can::RawMessage& msg) override { + std::cout << std::format("CanBase {}: Sending\n| {}", iface_, msg) + << std::endl; } private: diff --git a/firmware/mcal/linux/CMakeLists.txt b/firmware/mcal/linux/CMakeLists.txt index 1bd55f91c..68812e1cf 100644 --- a/firmware/mcal/linux/CMakeLists.txt +++ b/firmware/mcal/linux/CMakeLists.txt @@ -1,6 +1,6 @@ -add_library(mcal-linux STATIC) +add_library(mcal-linux INTERFACE) -target_include_directories(mcal-linux PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../..) +target_include_directories(mcal-linux INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/../..) add_subdirectory(periph) target_link_libraries(mcal-linux INTERFACE shared) diff --git a/firmware/mcal/linux/periph/CMakeLists.txt b/firmware/mcal/linux/periph/CMakeLists.txt index 987ed1410..8be9a4e82 100644 --- a/firmware/mcal/linux/periph/CMakeLists.txt +++ b/firmware/mcal/linux/periph/CMakeLists.txt @@ -1,5 +1,5 @@ target_sources(mcal-linux - PRIVATE + INTERFACE adc.cc can.cc digital_input.cc @@ -7,4 +7,4 @@ target_sources(mcal-linux ) add_subdirectory(vcan) -target_link_libraries(mcal-linux PUBLIC vcan pthread) \ No newline at end of file +target_link_libraries(mcal-linux INTERFACE vcan pthread) \ No newline at end of file diff --git a/firmware/mcal/linux/periph/adc.cc b/firmware/mcal/linux/periph/adc.cc index 215b5396c..40118f88e 100644 --- a/firmware/mcal/linux/periph/adc.cc +++ b/firmware/mcal/linux/periph/adc.cc @@ -2,7 +2,7 @@ #include #include -#include "adc.h" +#include "adc.hpp" namespace mcal::lnx::periph { diff --git a/firmware/mcal/linux/periph/can.cc b/firmware/mcal/linux/periph/can.cc index 9fe536bf0..3d1eedda8 100644 --- a/firmware/mcal/linux/periph/can.cc +++ b/firmware/mcal/linux/periph/can.cc @@ -1,17 +1,14 @@ #include -#include #include #include #include -#include #include -#include #include -#include "can.h" +#include "can.hpp" #include "shared/comms/can/msg.hpp" -#include "vcan/vcan.h" +#include "vcan/vcan.hpp" static can_frame to_can_frame(const shared::can::RawMessage& msg) { struct can_frame frame{ @@ -53,8 +50,11 @@ void CanBase::StartReading() { exit(1); } - shared::can::RawMessage raw_msg(frame.can_id, frame.can_dlc, + shared::can::RawMessage raw_msg(frame.can_id, true, frame.can_dlc, frame.data); + std::cout << std::format("CanBase {}: Received\n| {}", + socket_.GetIface(), raw_msg) + << std::endl; AddToBus(raw_msg); } diff --git a/firmware/mcal/linux/periph/can.hpp b/firmware/mcal/linux/periph/can.hpp index 85728d69a..2ef44f0c7 100644 --- a/firmware/mcal/linux/periph/can.hpp +++ b/firmware/mcal/linux/periph/can.hpp @@ -4,7 +4,7 @@ #include "mcal/linux/periph/vcan/vcan.hpp" #include "shared/periph/can.hpp" -#include "vcan/vcan.h" +#include "vcan/vcan.hpp" namespace mcal::lnx::periph { diff --git a/firmware/mcal/linux/periph/digital_input.cc b/firmware/mcal/linux/periph/digital_input.cc index e38f43fa5..12d5d0076 100644 --- a/firmware/mcal/linux/periph/digital_input.cc +++ b/firmware/mcal/linux/periph/digital_input.cc @@ -2,7 +2,7 @@ #include #include -#include "digital_input.h" +#include "digital_input.hpp" namespace mcal::lnx::periph { diff --git a/firmware/mcal/linux/periph/digital_output.cc b/firmware/mcal/linux/periph/digital_output.cc index 13375ab57..ee195b7a9 100644 --- a/firmware/mcal/linux/periph/digital_output.cc +++ b/firmware/mcal/linux/periph/digital_output.cc @@ -2,7 +2,7 @@ #include #include -#include "digital_output.h" +#include "digital_output.hpp" namespace mcal::lnx::periph { diff --git a/firmware/mcal/linux/periph/vcan/vcan.cc b/firmware/mcal/linux/periph/vcan/vcan.cc index b8cfa433b..d8a921d72 100644 --- a/firmware/mcal/linux/periph/vcan/vcan.cc +++ b/firmware/mcal/linux/periph/vcan/vcan.cc @@ -11,7 +11,7 @@ #include #include -#include "vcan.h" +#include "vcan.hpp" namespace mcal::lnx::periph::vcan { diff --git a/firmware/mcal/stm32f767/periph/CMakeLists.txt b/firmware/mcal/stm32f767/periph/CMakeLists.txt index d06d8ccf4..3a2813838 100644 --- a/firmware/mcal/stm32f767/periph/CMakeLists.txt +++ b/firmware/mcal/stm32f767/periph/CMakeLists.txt @@ -1 +1 @@ -# Empty file - implicitly adds this folder as an include directory \ No newline at end of file +target_sources(mcal-stm32f767 INTERFACE can.cc) \ No newline at end of file diff --git a/firmware/mcal/stm32f767/periph/can.cc b/firmware/mcal/stm32f767/periph/can.cc new file mode 100644 index 000000000..0eb3dde8a --- /dev/null +++ b/firmware/mcal/stm32f767/periph/can.cc @@ -0,0 +1,141 @@ +#include "stm32f7xx_hal.h" +#ifdef HAL_CAN_MODULE_ENABLED + +#include + +#include "can.hpp" + +// 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 + +/// 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); + } + + void Handle(CAN_HandleTypeDef* hcan) { + int index = HandleToIndex(hcan); + if (index == -1) return; + + auto can_base = can_bases[index]; + if (can_base != nullptr) { + can_bases[index]->Receive(); + } + } + +private: + CanBase* can_bases[3] = {nullptr, nullptr, nullptr}; + + int HandleToIndex(CAN_HandleTypeDef* hcan) { + // CANx are not contiguous in memory, so we can't use a simple array + if (hcan->Instance == CAN1) { + return 0; + } else if (hcan->Instance == CAN2) { + return 1; + } else if (hcan->Instance == CAN3) { + return 2; + } else { + return -1; + } + } +}; +} // namespace + +static InterruptHandler interrupt_handler; + +extern "C" { +void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan) { + // Overrides the "weak" callback defined by CubeMX + interrupt_handler.Handle(hcan); +} +} + +namespace mcal::stm32f767::periph { + +CanBase::CanBase(CAN_HandleTypeDef* hcan) : hcan_{hcan} {} + +void CanBase::Setup() { + ConfigFilters(); + interrupt_handler.RegisterCanBase(hcan_, this); + HAL_CAN_Start(hcan_); +} + +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; + } + + 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; + + if (msg.is_extended_frame) { + stm_tx_header.IDE = CAN_ID_EXT; + stm_tx_header.ExtId = msg.id; + } else { + stm_tx_header.IDE = CAN_ID_STD; + stm_tx_header.StdId = msg.id; + } + + HAL_CAN_AddTxMessage(hcan_, &stm_tx_header, msg.data, &tx_mailbox_addr_); +} + +void CanBase::Receive() { + shared::can::RawMessage rx_msg; + + CAN_RxHeaderTypeDef stm_rx_header; + HAL_CAN_GetRxMessage(hcan_, kCanFifo, &stm_rx_header, rx_msg.data); + + rx_msg.is_extended_frame = stm_rx_header.IDE == CAN_ID_EXT; + rx_msg.data_length = static_cast(stm_rx_header.DLC); + if (rx_msg.is_extended_frame) { + rx_msg.id = stm_rx_header.ExtId; + } else { + rx_msg.id = stm_rx_header.StdId; + } + + AddToBus(rx_msg); +} + +inline uint32_t CanBase::GetTimestamp() const { + return HAL_GetTick(); +} + +void CanBase::ConfigFilters() { + // Currently we don't support filtering - all messages are accepted + CAN_FilterTypeDef filter_config{ + .FilterIdHigh = 0x0000, + .FilterIdLow = 0x0000, + .FilterMaskIdHigh = 0x0000, + .FilterMaskIdLow = 0x0000, + .FilterFIFOAssignment = kCanFifo, + .FilterBank = 0, + .FilterMode = CAN_FILTERMODE_IDMASK, + .FilterScale = CAN_FILTERSCALE_32BIT, + .FilterActivation = CAN_FILTER_ENABLE, + .SlaveStartFilterBank = 14, + }; + + HAL_CAN_ConfigFilter(hcan_, &filter_config); +} + +} // namespace mcal::stm32f767::periph + +#endif // HAL_CAN_MODULE_ENABLED \ No newline at end of file diff --git a/firmware/mcal/stm32f767/periph/can.hpp b/firmware/mcal/stm32f767/periph/can.hpp index 90c58446f..0f823dce2 100644 --- a/firmware/mcal/stm32f767/periph/can.hpp +++ b/firmware/mcal/stm32f767/periph/can.hpp @@ -1,176 +1,37 @@ -/// @author Samuel Parent -/// @date 2023-01-12 +/// @author Blake Freer +/// @date 2024-11 #pragma once -#include +#include "stm32f7xx_hal.h" +#ifdef HAL_CAN_MODULE_ENABLED + +#include -#include "shared/comms/can/raw_can_msg.hpp" +#include "shared/comms/can/msg.hpp" #include "shared/periph/can.hpp" -#include "shared/util/data_structures/circular_queue.hpp" -#include "stm32f7xx_hal.h" -#include "stm32f7xx_hal_can.h" namespace mcal::stm32f767::periph { class CanBase : public shared::periph::CanBase { public: - CanBase(CAN_HandleTypeDef* hcan) : hcan_(hcan) {}; - - void Setup() { - ConfigFilters(); - HAL_CAN_Start(hcan_); - HAL_CAN_ActivateNotification(hcan_, kCanRxActiveInterruptFifo0); - } - - void Send(const shared::can::RawCanMsg& can_tx_msg) override { - uint32_t tx_mailboxes_free_level = - HAL_CAN_GetTxMailboxesFreeLevel(hcan_); - if (tx_mailboxes_free_level < 1) { - dropped_tx_frames_ += 1; - return; - } - - CAN_TxHeaderTypeDef stm_tx_header = - pack_stm_tx_header(can_tx_msg.header); - - HAL_CAN_AddTxMessage(hcan_, &stm_tx_header, can_tx_msg.data, - &tx_mailbox_addr_); - } - - void ReadQueue(shared::can::RawCanMsg can_rx_msgs[], size_t len) override { - uint16_t msg_idx = 0; - { - while (!can_queue_.is_empty() && (msg_idx < len)) { - can_rx_msgs[msg_idx] = can_queue_.Dequeue(); - msg_idx++; - } - } - } - - void AddRxMessageToQueue() { - if (can_queue_.is_full()) { - dropped_rx_frames_ += 1; - return; - } + CanBase(CAN_HandleTypeDef* hcan); - uint32_t fifo0_fill_level = - HAL_CAN_GetRxFifoFillLevel(hcan_, kCanRxFifo0); - if (fifo0_fill_level == 0) { - false_add_rx_msg_to_queue_ += 1; - return; - } - - CAN_RxHeaderTypeDef stm_rx_header; - shared::can::RawCanMsg raw_rx_msg; - - raw_rx_msg.tick_timestamp = get_tick_ms(); - - HAL_CAN_GetRxMessage(hcan_, kCanRxFifo0, &stm_rx_header, - raw_rx_msg.data); - - raw_rx_msg.header = unpack_stm_rx_header(stm_rx_header); - - can_queue_.Enqueue(raw_rx_msg); - } + void Setup(); + void Send(const shared::can::RawMessage& can_tx_msg) override; + void Receive(); private: - static constexpr uint8_t kMaxMsgBytes = 8; - static constexpr size_t kMsgQueueLen = 20; - static constexpr uint32_t kCanRxFifo0 = CAN_RX_FIFO0; - static constexpr uint32_t kCanRxActiveInterruptFifo0 = - CAN_IT_RX_FIFO0_MSG_PENDING; - - static constexpr uint32_t kDefaultFilterIdHigh = 0x0000; - static constexpr uint32_t kDefaultFilterIdLow = 0x0000; - static constexpr uint32_t kDefaultFilterMaskIdHigh = 0x0000; - static constexpr uint32_t kDefaultFilterMaskIdLow = 0x0000; - - static constexpr uint32_t kFilterScale = CAN_FILTERSCALE_32BIT; - static constexpr uint32_t kFilterEnable = CAN_FILTER_ENABLE; - static constexpr uint32_t kFilterMode = CAN_FILTERMODE_IDMASK; - - static constexpr uint32_t kDefaultSlaveStartFilterBank = 14; - static constexpr uint32_t kFilterBankCan2 = 14; - static constexpr uint32_t kFilterBankCan1Can3 = 0; - - static constexpr CAN_TxHeaderTypeDef pack_stm_tx_header( - const shared::can::CanHeader& header) { - CAN_TxHeaderTypeDef ret_stm_tx_header; - - // Set frame id based on standard or extended frame id - if (header.is_extended_frame) { - ret_stm_tx_header.IDE = CAN_ID_EXT; - ret_stm_tx_header.ExtId = header.id; - } else { - ret_stm_tx_header.IDE = CAN_ID_STD; - ret_stm_tx_header.StdId = header.id; - } - - // Sending a data frame - ret_stm_tx_header.RTR = CAN_RTR_DATA; - - // Set data length - ret_stm_tx_header.DLC = header.data_len; - - return ret_stm_tx_header; - } - - static constexpr shared::can::CanHeader unpack_stm_rx_header( - const CAN_RxHeaderTypeDef& stm_rx_header) { - shared::can::CanHeader ret_rx_header; - - if (stm_rx_header.IDE == CAN_ID_EXT) { - ret_rx_header.is_extended_frame = true; - ret_rx_header.id = stm_rx_header.ExtId; - } else { // Otherwise assume standard ID - ret_rx_header.is_extended_frame = false; - ret_rx_header.id = stm_rx_header.StdId; - } - - ret_rx_header.data_len = static_cast(stm_rx_header.DLC); - - return ret_rx_header; - } - - inline uint32_t get_tick_ms() { - return HAL_GetTick() * HAL_GetTickFreq(); - } - - void ConfigFilters() { - CAN_FilterTypeDef filter_config; - - filter_config.FilterIdHigh = kDefaultFilterIdHigh; - filter_config.FilterIdLow = kDefaultFilterIdLow; - filter_config.FilterMaskIdHigh = kDefaultFilterMaskIdHigh; - filter_config.FilterMaskIdLow = kDefaultFilterMaskIdLow; - - filter_config.FilterFIFOAssignment = kCanRxFifo0; - filter_config.FilterScale = kFilterScale; - filter_config.FilterActivation = kFilterEnable; - filter_config.FilterMode = kFilterMode; - - filter_config.SlaveStartFilterBank = kDefaultSlaveStartFilterBank; - if (hcan_->Instance == CAN2) { - filter_config.FilterBank = kFilterBankCan2; - } else { // CAN1, CAN3 - filter_config.FilterBank = kFilterBankCan1Can3; - } - - HAL_CAN_ConfigFilter(hcan_, &filter_config); - } + uint32_t GetTimestamp() const override; + void ConfigFilters(); /// @todo broadcast these over a can message uint32_t dropped_tx_frames_ = 0; - uint32_t dropped_rx_frames_ = 0; - uint32_t false_add_rx_msg_to_queue_ = 0; CAN_HandleTypeDef* hcan_; - uint32_t tx_mailbox_addr_ = 0; - - /// @todo make this an etl lockable queue. - shared::util::CircularQueue - can_queue_{}; + uint32_t tx_mailbox_addr_; }; } // namespace mcal::stm32f767::periph + +#endif // HAL_CAN_MODULE_ENABLED \ No newline at end of file diff --git a/firmware/projects/Demo/CAN/README.md b/firmware/projects/Demo/CAN/README.md index d5adae06f..481ab0709 100644 --- a/firmware/projects/Demo/CAN/README.md +++ b/firmware/projects/Demo/CAN/README.md @@ -2,7 +2,7 @@ Control an LED with a button over CAN. -The `Send` node reads a button input then sends the input over CAN to the `Receive` node which then sets turns on an LED if the button is pressed. +The `Send` node reads a button input and controls the `Receive` node's LED over CAN. ## DBC @@ -14,9 +14,15 @@ C++ code for the DBC is generated using `scripts/cangen`. The code is placed int ## Platforms +### stm32f767 Nucleo + +The `Send` node polls its built-in button and sends a message to the `Receive` node which sets its on-board LED to match the button. This looks like the `Send` button controlling the `Receive` LED. + +Both Send and Receive have CAN RX and TX on pins PB3 and PB4, respectively. See the Nucleo pinouts (`datasheets/nucleo/nucleo_f767zi_zio_right.png`) for the physical pin. For a complete circuit, you will also need 2 CAN drivers like the Analog Devices MAX33042E (see datasheets folder). + ### Linux -The projects communicate over virtual CAN (vcan). If using WSL, you need to recompile your kernel to enable vcan (instructions coming soon). +The projects communicate over virtual CAN (vcan). If using WSL, you need to recompile your kernel to enable vcan. See . Run `source vcan_setup.sh` to load the required modules. @@ -49,9 +55,3 @@ CanBase vcan0: Received | [3AC] 00 DigitalOutput "Indicator" => false ``` - -### stm32f767 Nucleo - -Both Send and Receive have CAN RX and TX on pins PB3 and PB4, respectively. See the nucleo pinouts (`racecar/datasheets/nucleo/`) for the physical pin. For a complete circuit, you will also need 2 CAN drivers like the AD MAX33042E (see datasheets folder). - -The `Send` node polls its built-in button every 50 ms and sends a message to the `Receive` node which sets its on-board LED to match the button. diff --git a/firmware/projects/Demo/CAN/Receive/main.cc b/firmware/projects/Demo/CAN/Receive/main.cc index faa0c3b4f..ca37a0ce2 100644 --- a/firmware/projects/Demo/CAN/Receive/main.cc +++ b/firmware/projects/Demo/CAN/Receive/main.cc @@ -3,8 +3,8 @@ #include -#include "bindings.h" -#include "generated/can/demobus_bus.pp" +#include "bindings.hpp" +#include "generated/can/demobus_bus.hpp" #include "generated/can/demobus_messages.hpp" #include "shared/comms/can/bus.hpp" diff --git a/firmware/projects/Demo/CAN/Receive/platforms/linux/CMakeLists.txt b/firmware/projects/Demo/CAN/Receive/platforms/linux/CMakeLists.txt index 58bff247f..8ecab6906 100644 --- a/firmware/projects/Demo/CAN/Receive/platforms/linux/CMakeLists.txt +++ b/firmware/projects/Demo/CAN/Receive/platforms/linux/CMakeLists.txt @@ -1,2 +1,2 @@ -target_sources(bindings PRIVATE bindings.cc) -target_link_libraries(bindings PRIVATE mcal-linux) \ No newline at end of file +target_sources(main PRIVATE bindings.cc) +target_link_libraries(main PRIVATE mcal-linux) \ No newline at end of file diff --git a/firmware/projects/Demo/CAN/Receive/platforms/stm32f767/bindings.cc b/firmware/projects/Demo/CAN/Receive/platforms/stm32f767/bindings.cc index 418aa08bd..1b9e78259 100644 --- a/firmware/projects/Demo/CAN/Receive/platforms/stm32f767/bindings.cc +++ b/firmware/projects/Demo/CAN/Receive/platforms/stm32f767/bindings.cc @@ -24,12 +24,12 @@ void SystemClock_Config(); namespace mcal { using namespace stm32f767::periph; -CanBase veh_can_base{&hcan3}; +CanBase demo_can_base{&hcan3}; DigitalOutput indicator{LedPin_GPIO_Port, LedPin_Pin}; } // namespace mcal namespace bindings { -shared::periph::CanBase& veh_can_base = mcal::veh_can_base; +shared::periph::CanBase& demo_can_base = mcal::demo_can_base; shared::periph::DigitalOutput& indicator = mcal::indicator; void Initialize() { @@ -37,15 +37,7 @@ void Initialize() { HAL_Init(); MX_GPIO_Init(); MX_CAN3_Init(); - mcal::veh_can_base.Setup(); -} - -extern "C" { -void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan) { - if (hcan == &hcan3) { - mcal::veh_can_base.AddRxMessageToQueue(); - } -} + mcal::demo_can_base.Setup(); } } // namespace bindings diff --git a/firmware/projects/Demo/CAN/Send/main.cc b/firmware/projects/Demo/CAN/Send/main.cc index 28ffb84ed..fa57891cf 100644 --- a/firmware/projects/Demo/CAN/Send/main.cc +++ b/firmware/projects/Demo/CAN/Send/main.cc @@ -3,7 +3,7 @@ #include -#include "bindings.h" +#include "bindings.hpp" #include "generated/can/demobus_bus.hpp" #include "generated/can/demobus_messages.hpp" @@ -13,7 +13,7 @@ DemobusBus can_bus{bindings::demo_can_base}; int main(void) { bindings::Initialize(); - uint32_t interval_ms = 50; + uint32_t interval_ms = 5; while (true) { TxButtonStatus msg{ diff --git a/firmware/projects/Demo/CAN/Send/platforms/stm32f767/bindings.cc b/firmware/projects/Demo/CAN/Send/platforms/stm32f767/bindings.cc index 452ddef04..ad42da8a2 100644 --- a/firmware/projects/Demo/CAN/Send/platforms/stm32f767/bindings.cc +++ b/firmware/projects/Demo/CAN/Send/platforms/stm32f767/bindings.cc @@ -24,12 +24,12 @@ void SystemClock_Config(); namespace mcal { using namespace stm32f767::periph; -CanBase veh_can_base{&hcan3}; +CanBase demo_can_base{&hcan3}; DigitalInput button{ButtonPin_GPIO_Port, ButtonPin_Pin}; } // namespace mcal namespace bindings { -shared::periph::CanBase& veh_can_base = mcal::veh_can_base; +shared::periph::CanBase& demo_can_base = mcal::demo_can_base; shared::periph::DigitalInput& button = mcal::button; void Initialize() { @@ -37,19 +37,11 @@ void Initialize() { HAL_Init(); MX_GPIO_Init(); MX_CAN3_Init(); - mcal::veh_can_base.Setup(); + mcal::demo_can_base.Setup(); } void TickBlocking(uint32_t ticks) { HAL_Delay(ticks); } -extern "C" { -void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan) { - if (hcan == &hcan3) { - mcal::veh_can_base.AddRxMessageToQueue(); - } -} -} - } // namespace bindings diff --git a/firmware/shared/CMakeLists.txt b/firmware/shared/CMakeLists.txt index 21e9fae67..ec7336ca3 100644 --- a/firmware/shared/CMakeLists.txt +++ b/firmware/shared/CMakeLists.txt @@ -1,6 +1,6 @@ -add_library(shared STATIC) +add_library(shared INTERFACE) -target_include_directories(shared PUBLIC ${CMAKE_SOURCE_DIR}) +target_include_directories(shared INTERFACE ${CMAKE_SOURCE_DIR}) add_subdirectory(comms) add_subdirectory(periph) \ No newline at end of file diff --git a/firmware/shared/comms/can/CMakeLists.txt b/firmware/shared/comms/can/CMakeLists.txt index a1b0c955f..d8761e05e 100644 --- a/firmware/shared/comms/can/CMakeLists.txt +++ b/firmware/shared/comms/can/CMakeLists.txt @@ -1 +1 @@ -target_sources(shared PRIVATE msg.cc bus.cc) \ No newline at end of file +target_sources(shared INTERFACE msg.cc bus.cc) \ No newline at end of file diff --git a/firmware/shared/comms/can/bus.cc b/firmware/shared/comms/can/bus.cc index 19871d13f..8cf9d7015 100644 --- a/firmware/shared/comms/can/bus.cc +++ b/firmware/shared/comms/can/bus.cc @@ -1,5 +1,5 @@ -#include "bus.h" -#include "shared/periph/can.h" +#include "bus.hpp" +#include "shared/periph/can.hpp" namespace shared::can { diff --git a/firmware/shared/comms/can/bus.h b/firmware/shared/comms/can/bus.hpp similarity index 88% rename from firmware/shared/comms/can/bus.h rename to firmware/shared/comms/can/bus.hpp index e5004ccdc..f51578884 100644 --- a/firmware/shared/comms/can/bus.h +++ b/firmware/shared/comms/can/bus.hpp @@ -1,7 +1,7 @@ #pragma once -#include "shared/comms/can/msg.h" -#include "shared/periph/can.h" +#include "shared/comms/can/msg.hpp" +#include "shared/periph/can.hpp" namespace shared::can { diff --git a/firmware/shared/comms/can/msg.cc b/firmware/shared/comms/can/msg.cc index c8d42ccce..1910f5953 100644 --- a/firmware/shared/comms/can/msg.cc +++ b/firmware/shared/comms/can/msg.cc @@ -1,12 +1,14 @@ #include -#include "msg.h" +#include "msg.hpp" namespace shared::can { -RawMessage::RawMessage(uint32_t id_, uint8_t data_length_, - const uint8_t data_[8]) - : id(id_), data_length(data_length_) { +RawMessage::RawMessage(uint32_t id_, bool is_extended_frame_, + uint8_t data_length_, const uint8_t data_[8]) + : id(id_), + is_extended_frame(is_extended_frame_), + data_length(data_length_) { std::memcpy(data, data_, data_length); } diff --git a/firmware/shared/comms/can/msg.h b/firmware/shared/comms/can/msg.hpp similarity index 50% rename from firmware/shared/comms/can/msg.h rename to firmware/shared/comms/can/msg.hpp index dc9ee7be5..a1f6a9064 100644 --- a/firmware/shared/comms/can/msg.h +++ b/firmware/shared/comms/can/msg.hpp @@ -7,9 +7,12 @@ namespace shared::can { struct RawMessage { - RawMessage(uint32_t id_, uint8_t data_length_, const uint8_t data_[8]); + RawMessage() = default; + RawMessage(uint32_t id_, bool is_extended_frame_, uint8_t data_length_, + const uint8_t data_[8]); uint32_t id; + bool is_extended_frame; uint8_t data_length; uint8_t data[8]; }; @@ -20,6 +23,27 @@ concept TxMessage = requires(const T msg) { { msg.pack() } -> std::same_as; }; +// Used in the generated code to unpack from / pack into a RawMessage +template +inline T unpack_right_shift(uint8_t value, uint8_t shift, uint8_t mask) { + return static_cast(static_cast(value & mask) >> shift); +} + +template +inline T unpack_left_shift(uint8_t value, uint8_t shift, uint8_t mask) { + return static_cast(static_cast(value & mask) << shift); +} + +template +inline uint8_t pack_left_shift(T value, uint8_t shift, uint8_t mask) { + return static_cast(static_cast(value << shift) & mask); +} + +template +inline uint8_t pack_right_shift(T value, uint8_t shift, uint8_t mask) { + return static_cast(static_cast(value >> shift) & mask); +} + } // namespace shared::can // Convert RawMessage to string with `std::format` diff --git a/firmware/shared/periph/CMakeLists.txt b/firmware/shared/periph/CMakeLists.txt index 5cb3c9c0b..228ecd12b 100644 --- a/firmware/shared/periph/CMakeLists.txt +++ b/firmware/shared/periph/CMakeLists.txt @@ -1 +1 @@ -target_sources(shared PRIVATE can.cc) \ No newline at end of file +target_sources(shared INTERFACE can.cc) \ No newline at end of file diff --git a/firmware/shared/periph/can.cc b/firmware/shared/periph/can.cc index 8a50d9b44..fe8ccf343 100644 --- a/firmware/shared/periph/can.cc +++ b/firmware/shared/periph/can.cc @@ -1,6 +1,6 @@ -#include "can.h" -#include "shared/comms/can/bus.h" -#include "shared/comms/can/msg.h" +#include "can.hpp" +#include "shared/comms/can/bus.hpp" +#include "shared/comms/can/msg.hpp" namespace shared::periph { diff --git a/firmware/shared/periph/can.hpp b/firmware/shared/periph/can.hpp index 0fc888658..2843c2b5c 100644 --- a/firmware/shared/periph/can.hpp +++ b/firmware/shared/periph/can.hpp @@ -8,6 +8,10 @@ #include "shared/comms/can/msg.hpp" #include "shared/util/peripheral.hpp" +namespace shared::can { +class Bus; // forward declare to avoid circular dependency +} // namespace shared::can + namespace shared::periph { class CanBase : public util::Peripheral { diff --git a/firmware/shared/util/buffer.hpp b/firmware/shared/util/buffer.hpp new file mode 100644 index 000000000..ca66d417f --- /dev/null +++ b/firmware/shared/util/buffer.hpp @@ -0,0 +1,76 @@ +// Blake Freer +// November 2024 + +#pragma once + +#include +#include + +namespace shared::util { + +/// @brief Thread & interrupt safe SPSC buffer. +/// `Get` and `Pop` return an `std::optional` since the buffer may be empty. +/// Calling `Pop` will clear the buffer. +/// +/// Assumes that `Get` and `Pop` may be interrupted by `Add`, but `Add` will not +/// be interrupted by `Get` or `Pop`. Intended for use where the producer +/// calling `Add` is an interrupt. +/// +/// @tparam T Value type to hold +/// @warning Data race will occur if a single `Pop` call is interrupted twice +/// by `Add` since `Pop` will clear the second written message. +template +class AtomicBuffer { +public: + AtomicBuffer() : read_pos_(0) { + buffer_[0] = std::nullopt; + buffer_[1] = std::nullopt; + } + + /// Get the latest item + std::optional Get(); + + /// Get the latest item and clear the buffer + std::optional Pop(); + + /// Add an item to the buffer + void Add(const T& new_item); + + /// Move an rvalue item into the buffer + void Add(T&& new_item); + +private: + std::atomic read_pos_; + std::optional buffer_[2]; +}; + +template +std::optional AtomicBuffer::Get() { + int read_pos = read_pos_.load(std::memory_order_acquire); + return buffer_[read_pos]; +} + +template +std::optional AtomicBuffer::Pop() { + int read_pos = read_pos_.load(std::memory_order_acquire); + auto val = std::move(buffer_[read_pos]); + buffer_[read_pos] = std::nullopt; + return val; +} + +template +void AtomicBuffer::Add(const T& new_item) { + // write to the "other" side of the buffer + int write_pos = 1 - read_pos_.load(std::memory_order_relaxed); + buffer_[write_pos] = new_item; + read_pos_.store(write_pos, std::memory_order_release); +} + +template +void AtomicBuffer::Add(T&& new_item) { + int write_pos = 1 - read_pos_.load(std::memory_order_relaxed); + buffer_[write_pos] = std::move(new_item); + read_pos_.store(write_pos, std::memory_order_release); +} + +} // namespace shared::util \ No newline at end of file diff --git a/scripts/cangen/cangen/can_generator.py b/scripts/cangen/cangen/can_generator.py index 1746d07d2..16bb423ad 100644 --- a/scripts/cangen/cangen/can_generator.py +++ b/scripts/cangen/cangen/can_generator.py @@ -201,6 +201,7 @@ def _generate_code(bus: Bus, output_dir: str): "unpack_info": _get_masks_shifts(rx_msgs), "pack_info": _get_masks_shifts(tx_msgs), "bus_name": bus.bus_name, + "node_name": bus.node, } logger.debug("Generating code for can messages and msg registry.") diff --git a/scripts/cangen/cangen/templates/bus.h.jinja2 b/scripts/cangen/cangen/templates/bus.h.jinja2 deleted file mode 100644 index 9153e34fd..000000000 --- a/scripts/cangen/cangen/templates/bus.h.jinja2 +++ /dev/null @@ -1,67 +0,0 @@ -/// @author Samuel Parent, Blake Freer -/// @date November 2024 - -// WARNING: DO NOT MODIFY THIS CODE. THIS IS AN AUTOGENERATED FILE. - -// clang-format off - -#include - -#include "shared/comms/can/bus.h" -#include "shared/comms/can/msg.h" -#include "shared/periph/can.h" -#include "{{bus_name | lower}}_messages.h" -#include "etl/mutex.h" - -namespace generated::can { - -{% set bus = bus_name+"Bus" %} -class {{ bus }} : public shared::can::Bus { -public: - {{ bus }}(shared::periph::CanBase& can_base) : shared::can::Bus(can_base) {} - - {% for msg in rx_msgs %} - {% set msg_title = "Rx"+msg.name %} - - // Get the latest message of type {{msg_title}} - auto Get{{msg_title}}() -> std::optional<{{msg_title}}> { - etl::lock_guard lock(msg_guard_); - return {{ msg.name | camel_to_snake }}_msg_; - } - - // Get the latest message of type {{msg_title}} and clear it - auto Pop{{msg_title}}() -> std::optional<{{msg_title}}> { - etl::lock_guard lock(msg_guard_); - auto temp = {{ msg.name | camel_to_snake }}_msg_; - {{ msg.name | camel_to_snake }}_msg_ = std::nullopt; - return temp; - } - {% endfor %} - -private: - etl::mutex msg_guard_; - - void AddMessage(const shared::can::RawMessage& msg, uint32_t timestamp) override { - etl::lock_guard lock(msg_guard_); - switch(msg.id) { - {% for msg in rx_msgs %} - case Rx{{msg.name}}::id: - {{ msg.name | camel_to_snake }}_msg_ = Rx{{msg.name}}(msg, timestamp); - break; - {% endfor %} - default: - break; // ignore messages not intended for this node on this bus - } - } - - // All messages start empty and will be filled when the first instance arrives. - {% for msg in rx_msgs %} - std::optional {{msg.name | camel_to_snake}}_msg_ = std::nullopt; - {% endfor %} - - friend class Base; -}; - -} // namespace generated::can - -// clang-format on \ No newline at end of file diff --git a/scripts/cangen/cangen/templates/bus.hpp.jinja2 b/scripts/cangen/cangen/templates/bus.hpp.jinja2 new file mode 100644 index 000000000..b19552442 --- /dev/null +++ b/scripts/cangen/cangen/templates/bus.hpp.jinja2 @@ -0,0 +1,70 @@ +/// @author Blake Freer +/// @date November 2024 + +// WARNING: DO NOT MODIFY THIS CODE. THIS IS AN AUTOGENERATED FILE. + +// clang-format off + +#pragma once + +#include + +#include "shared/comms/can/bus.hpp" +#include "shared/comms/can/msg.hpp" +#include "shared/periph/can.hpp" +#include "shared/util/buffer.hpp" +#include "{{bus_name | lower}}_messages.hpp" + +namespace generated::can { + +{% set bus = bus_name+"Bus" %} +class {{ bus }} : public shared::can::Bus { +public: + {{ bus }}(shared::periph::CanBase& can_base) : shared::can::Bus(can_base) {} + + {% for msg in rx_msgs %} + {% set msg_title = "Rx"+msg.name %} + // Get the latest message of type {{msg_title}} + inline auto Get{{msg_title}}() -> std::optional<{{msg_title}}> { + return {{ msg.name | camel_to_snake }}_buffer_.Get(); + } + + // Get the latest message of type {{msg_title}} and clear the buffer. + inline auto Pop{{msg_title}}() -> std::optional<{{msg_title}}> { + return {{ msg.name | camel_to_snake }}_buffer_.Pop(); + } + {% endfor %} + +private: + void AddMessage(const shared::can::RawMessage& msg, uint32_t timestamp) override { + {% if rx_msgs %} + switch(msg.id) { + {% for msg in rx_msgs %} + case Rx{{msg.name}}::id: + {{ msg.name | camel_to_snake }}_buffer_.Add(Rx{{msg.name}}(msg, timestamp)); + break; + {% endfor %} + default: + break; // ignore messages not intended for this node on this bus + } + {% else %} + // The "{{node_name}}" CAN node does not receive any messages. + // If this is a mistake, check that the node is: + // 1. Set correctly in the project's config.yaml under the "{{bus_name}}" bus. + // 2. Listed as a signal receiver in the DBC file. + (void)msg; + (void)timestamp; + {% endif %} + } + + {% if rx_msgs %} + // All messages start empty and will be filled when the first instance arrives. + {% for msg in rx_msgs %} + shared::util::AtomicBuffer {{msg.name | camel_to_snake}}_buffer_; + {% endfor %} + {% endif %} +}; + +} // namespace generated::can + +// clang-format on \ No newline at end of file diff --git a/scripts/cangen/cangen/templates/messages.h.jinja2 b/scripts/cangen/cangen/templates/messages.hpp.jinja2 similarity index 79% rename from scripts/cangen/cangen/templates/messages.h.jinja2 rename to scripts/cangen/cangen/templates/messages.hpp.jinja2 index 3a6ebe24c..3976752ca 100644 --- a/scripts/cangen/cangen/templates/messages.h.jinja2 +++ b/scripts/cangen/cangen/templates/messages.hpp.jinja2 @@ -13,26 +13,6 @@ #include #include "shared/comms/can/msg.hpp" -template -static inline T unpack_right_shift(uint8_t value, uint8_t shift, uint8_t mask) { - return static_cast(static_cast(value & mask) >> shift); -} - -template -static inline T unpack_left_shift(uint8_t value, uint8_t shift, uint8_t mask) { - return static_cast(static_cast(value & mask) << shift); -} - -template -static inline uint8_t pack_left_shift(T value, uint8_t shift, uint8_t mask) { - return static_cast(static_cast(value << shift) & mask); -} - -template -static inline uint8_t pack_right_shift(T value, uint8_t shift, uint8_t mask) { - return static_cast(static_cast(value >> shift) & mask); -} - namespace generated::can { {% set bus = bus_name+"Bus" %} @@ -101,9 +81,9 @@ private: {% set shift = shifts[i] %} {% if mask != 0 %} {% if shift >= 0 %} - {{ temp_sig_var }} |= unpack_left_shift<{{ temp_sig_var_type }}>(raw_msg.data[{{ i }}], {{ shift }}U, {{ mask | decimal_to_hex }}U); + {{ temp_sig_var }} |= shared::can::unpack_left_shift<{{ temp_sig_var_type }}>(raw_msg.data[{{ i }}], {{ shift }}U, {{ mask | decimal_to_hex }}U); {% else %} - {{ temp_sig_var }} |= unpack_right_shift<{{ temp_sig_var_type }}>(raw_msg.data[{{ i }}], {{ -shift }}U, {{ mask | decimal_to_hex }}U); + {{ temp_sig_var }} |= shared::can::unpack_right_shift<{{ temp_sig_var_type }}>(raw_msg.data[{{ i }}], {{ -shift }}U, {{ mask | decimal_to_hex }}U); {% endif %} {% endif %} {% endfor %} @@ -147,20 +127,21 @@ public: {% set shift = shifts[i] %} {% if mask != 0 %} {% if shift >= 0 %} - data[{{ i }}] |= pack_right_shift(temp_{{ sig_var }}, {{ shift }}U, {{ mask | decimal_to_hex }}U); + data[{{ i }}] |= shared::can::pack_right_shift(temp_{{ sig_var }}, {{ shift }}U, {{ mask | decimal_to_hex }}U); {% else %} - data[{{ i }}] |= pack_left_shift(temp_{{ sig_var }}, {{ -(shift) }}U, {{ mask | decimal_to_hex }}U); + data[{{ i }}] |= shared::can::pack_left_shift(temp_{{ sig_var }}, {{ -(shift) }}U, {{ mask | decimal_to_hex }}U); {% endif %} {% endif %} {% endfor %} {% endfor %} - return shared::can::RawMessage(id, data_length, data); + return shared::can::RawMessage(id, extended_frame, data_length, data); } private: static constexpr uint32_t id = {{ msg.frame_id }}; static constexpr uint8_t data_length = {{ msg.length }}; + constexpr static uint8_t extended_frame = {{ msg.is_extended_frame | lower }}; // Signal properties {% for sig in msg.signals %}