-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[FEAT] Add first draft on CAN bus driver
WARNING: this is untested
- Loading branch information
1 parent
0d3c28f
commit a3c2278
Showing
8 changed files
with
313 additions
and
67 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,175 @@ | ||
| /* USER CODE BEGIN Header */ | ||
| /* | ||
| * 401 Ballon OBC | ||
| * Copyright (C) 2024 Bluesat and contributors. | ||
| * | ||
| * This program is free software: you can redistribute it and/or modify it | ||
| * under the terms of the GNU General Public License as published by the Free | ||
| * Software Foundation, either version 3 of the License, or (at your option) | ||
| * any later version. | ||
| * | ||
| * This program is distributed in the hope that it will be useful, but WITHOUT | ||
| * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or | ||
| * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for | ||
| * more details. | ||
| * | ||
| * You should have received a copy of the GNU General Public License along with | ||
| * this program. If not, see <https://www.gnu.org/licenses/>. | ||
| */ | ||
| /* USER CODE END Header */ | ||
|
|
||
| #define FDCAN1 | ||
| #include <algorithm> | ||
| #include <array> | ||
| #include <mutex> | ||
| #include <optional> | ||
| #include <span> | ||
| #include <tuple> | ||
|
|
||
| #include <stm32h7xx_hal_fdcan.h> | ||
| #include <units/time.h> | ||
|
|
||
| #include "bus.hpp" | ||
| #include "ipc/mutex.hpp" | ||
| #include "scheduling/delay.hpp" | ||
| #include "scheduling/task.hpp" | ||
|
|
||
| namespace obc::bus { | ||
| /** | ||
| * To prevent this driver from locking up the entire system in an ISR, the | ||
| * implementation elects to use a polling approach. | ||
| * | ||
| * todo(evan): The more efficient approach would be to register an interupt | ||
| * which sets the ready flag on this task. This avoids excessive polling. | ||
| */ | ||
| class CanFd : public scheduling::StackTask<>, bus::ListenBusMixin<> { | ||
| public: | ||
| static constexpr size_t kMaxPayloadSize = 64; | ||
|
|
||
| // TODO(evan): Switch to error type with string repr | ||
| enum class SendError { kTimeout, kInvalidSize }; | ||
|
|
||
| // handle is init by the autogenerated code | ||
| inline CanFd(FDCAN_HandleTypeDef* handle) | ||
| : StackTask("CAN-FD Driver"), m_handle {handle} {} | ||
|
|
||
| inline auto Send( | ||
| const BasicMessage& msg, const units::milliseconds<float> timeout | ||
| ) -> std::expected<std::monostate, SendError> { | ||
| std::lock_guard lock {m_send_lock}; | ||
|
|
||
| // Wait for a free slot in the message queue | ||
| scheduling::Timeout timeout_block(timeout); | ||
| if (!timeout_block.Poll([&]() { | ||
| return HAL_FDCAN_GetTxFifoFreeLevel(m_handle); | ||
| })) | ||
| return std::unexpected {SendError::kTimeout}; | ||
|
|
||
| // Pad the payload so that it fits within a valid DLC size if required | ||
| auto [dlc, padding] { | ||
| UNWRAP_TAGGED(EncodeDlc(msg.data.size()), SendError::kInvalidSize) | ||
| }; | ||
| // Don't waste time zeroing memory if this buffer is not used | ||
| // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast) | ||
| std::array<std::byte, kMaxPayloadSize> padded_payload; | ||
| uint8_t* payload_ptr {reinterpret_cast<uint8_t*>( | ||
| padding ? ({ | ||
| std::ranges::copy(msg.data, padded_payload.begin()); | ||
| std::ranges::fill_n( | ||
| padded_payload.begin() + msg.data.size(), padding, | ||
| std::byte {0} | ||
| ); | ||
| padded_payload.data(); | ||
| }) | ||
| : msg.data.data() | ||
| )}; | ||
|
|
||
| FDCAN_TxHeaderTypeDef header { | ||
| .Identifier {msg.address}, | ||
| .IdType {FDCAN_EXTENDED_ID}, | ||
| .TxFrameType {FDCAN_DATA_FRAME}, | ||
| .DataLength {}, | ||
| .ErrorStateIndicator {FDCAN_ESI_ACTIVE}, | ||
| .BitRateSwitch {FDCAN_BRS_ON}, | ||
| .FDFormat {FDCAN_FD_CAN}, | ||
| .TxEventFifoControl {FDCAN_STORE_TX_EVENTS}, | ||
| .MessageMarker {0}, | ||
| }; | ||
|
|
||
| // The call can only fail if the queue is full or the can bus is not | ||
| // started, both of these are our fault. | ||
| utils::CheckOrPanic( | ||
| HAL_FDCAN_AddMessageToTxFifoQ(m_handle, &header, payload_ptr), | ||
| utils::IsHalOk | ||
| ); | ||
| return {}; | ||
| } | ||
|
|
||
| protected: | ||
| inline auto Run() -> void override { | ||
| for (const auto& fifo : {FDCAN_RX_FIFO0, FDCAN_RX_FIFO1}) { | ||
| // Does this FIFO have any incoming messages? | ||
| if (!HAL_FDCAN_GetRxFifoFillLevel(m_handle, fifo)) continue; | ||
|
|
||
| FDCAN_RxHeaderTypeDef header {}; | ||
| std::array<std::byte, kMaxPayloadSize> payload {}; | ||
| // The call can only fail if the queue is full or the can bus is not | ||
| // started, both of these are our fault. | ||
| utils::CheckOrPanic( | ||
| HAL_FDCAN_GetRxMessage( | ||
| m_handle, fifo, &header, | ||
| reinterpret_cast<uint8_t*>(payload.data()) | ||
| ), | ||
| utils::IsHalOk | ||
| ); | ||
| FeedListeners( | ||
| {.address = header.Identifier, | ||
| .data = std::span<std::byte>( | ||
| payload.data(), | ||
| // Only well formed CAN frames should exist in the FIFO | ||
| utils::UnwrapOrPanic(DecodeDlc(header.DataLength)) | ||
| )} | ||
| ); | ||
| } | ||
| } | ||
|
|
||
| private: | ||
| static constexpr std::array<std::pair<uint32_t, size_t>, 15> kDlcSizeMap { | ||
| std::pair { FDCAN_DLC_BYTES_0, 0}, | ||
| std::pair { FDCAN_DLC_BYTES_1, 1}, | ||
| std::pair { FDCAN_DLC_BYTES_2, 2}, | ||
| std::pair { FDCAN_DLC_BYTES_3, 3}, | ||
| std::pair { FDCAN_DLC_BYTES_4, 4}, | ||
| std::pair { FDCAN_DLC_BYTES_5, 5}, | ||
| std::pair { FDCAN_DLC_BYTES_6, 6}, | ||
| std::pair { FDCAN_DLC_BYTES_7, 7}, | ||
| std::pair { FDCAN_DLC_BYTES_8, 8}, | ||
| std::pair {FDCAN_DLC_BYTES_12, 12}, | ||
| std::pair {FDCAN_DLC_BYTES_16, 16}, | ||
| std::pair {FDCAN_DLC_BYTES_24, 24}, | ||
| std::pair {FDCAN_DLC_BYTES_32, 32}, | ||
| std::pair {FDCAN_DLC_BYTES_48, 48}, | ||
| std::pair {FDCAN_DLC_BYTES_64, 64}, | ||
| }; | ||
|
|
||
| [[nodiscard]] inline static constexpr auto DecodeDlc(uint32_t dlc) | ||
| -> std::optional<size_t> { | ||
| for (auto& [map_dlc, map_size] : kDlcSizeMap) | ||
| if (dlc == map_dlc) return map_size; | ||
| return std::nullopt; | ||
| } | ||
|
|
||
| [[nodiscard]] inline static constexpr auto EncodeDlc(size_t size) | ||
| -> std::optional<std::pair<uint32_t, size_t>> { | ||
| for (auto& [map_dlc, map_size] : kDlcSizeMap) | ||
| if (size <= map_size) | ||
| return { | ||
| {map_dlc, map_size - size} | ||
| }; | ||
| return std::nullopt; | ||
| } | ||
|
|
||
| FDCAN_HandleTypeDef* m_handle; | ||
| ipc::Mutex m_send_lock {}; | ||
| }; | ||
| } // namespace obc::bus |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.