Skip to content

Implemented rmcs_slave for dm_mc02 #1

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Smoaflie wants to merge 6 commits into
base: main
Choose a base branch
from
Open

Implemented rmcs_slave for dm_mc02 #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
24288f0
Implemented rmcs_slave for dm_mc02
Smoaflie Mar 5, 2025
c94df64
repair usb-HS support
Smoaflie Mar 6, 2025
b6d3562
adjust UART5 configuration to support dbus receive.
Smoaflie Mar 6, 2025
72abe46
add uart3 and can3 transmit device support
Smoaflie Mar 9, 2025
73fc3c1
disable CAN autoretransmission
Smoaflie May 22, 2025
88a1ad3
Update README.md
Smoaflie Jul 5, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion .gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,4 +2,5 @@
.vscode
.xmake
build
compile_commands.json
compile_commands.json
*.jdebug*
10 changes: 9 additions & 1 deletion README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,3 +1,11 @@
# 提醒

抽空对从机进行了通信速率测试，使用一收一发方式，测出速率仅**2.6~3.1Mbps**

* 该测试办法或许不够严谨，但**达妙板高负载下所需通信带宽远大于该值**

* 对于整车控制，在条件允许的情况下优先推荐多板方案

# RMCS Slave

[无下位机控制系统 RMCS（RoboMaster Control System）](https://github.com/Alliance-Algorithm/RMCS) 的下位机固件。
Expand Down Expand Up @@ -151,4 +159,4 @@ xmake

使用 `xmake -r` 可强制重新构建。

使用 `xmake -v` 可显示构建过程中的细节，如构建指令、资源占用等。
使用 `xmake -v` 可显示构建过程中的细节，如构建指令、资源占用等。
8 changes: 8 additions & 0 deletions app/app.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,17 +8,21 @@
#include "app/uart/uart.hpp"
#include "app/usb/cdc.hpp"

#include "app/logger/logger.hpp"
extern "C" {
void AppEntry() { app.init().main(); }
}

App::App() {
logger::logger.init().printf("Init...\n");
led::led.init();
usb::cdc.init();
can::can1.init();
can::can2.init();
can::can3.init();
uart::uart1.init();
uart::uart2.init();
uart::uart3.init();
uart::uart_dbus.init();
spi::bmi088::accelerometer.init();
spi::bmi088::gyroscope.init();
Expand All @@ -32,10 +36,14 @@ App::App() {
usb::cdc->try_transmit();
can::can2->try_transmit();
usb::cdc->try_transmit();
can::can3->try_transmit();
usb::cdc->try_transmit();
uart::uart1->try_transmit();
usb::cdc->try_transmit();
uart::uart2->try_transmit();
usb::cdc->try_transmit();
uart::uart3->try_transmit();
usb::cdc->try_transmit();
uart::uart_dbus->try_transmit();
}
}
9 changes: 5 additions & 4 deletions app/can/can.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,13 +1,14 @@
#include "app/can/can.hpp"
#include "app/usb/cdc.hpp"

#include <can.h>
#include <fdcan.h>

extern "C" {

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan) {
auto can = hcan == &hcan1 ? can::can1.get() : can::can2.get();
auto field_id = hcan == &hcan1 ? usb::field::UplinkId::CAN1_ : usb::field::UplinkId::CAN2_;
void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs) {
auto can = hfdcan == &hfdcan1 ? can::can1.get() : (hfdcan == &hfdcan2 ? can::can2.get() : can::can3.get());
auto field_id = hfdcan == &hfdcan1 ? usb::field::UplinkId::CAN1_ :
(hfdcan == &hfdcan2 ? usb::field::UplinkId::CAN2_ : usb::field::UplinkId::CAN3_);

can->read_device_write_buffer(usb::cdc->get_transmit_buffer(), field_id);
}
Expand Down
129 changes: 74 additions & 55 deletions app/can/can.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
#include <cstdint>
#include <cstring>

#include <can.h>
#include <fdcan.h>

#include "app/usb/field.hpp"
#include "app/usb/interrupt_safe_buffer.hpp"
Expand All @@ -17,9 +17,9 @@ namespace can {

class Can : private utility::Immovable {
public:
using Lazy = utility::Lazy<Can, CAN_HandleTypeDef*, uint32_t, uint32_t>;
using Lazy = utility::Lazy<Can, FDCAN_HandleTypeDef*, uint32_t, uint32_t>;

Can(CAN_HandleTypeDef* hal_can_handle, uint32_t hal_filter_bank,
Can(FDCAN_HandleTypeDef* hal_can_handle, uint32_t hal_filter_bank,
uint32_t hal_slave_start_filter_bank)
: hal_can_handle_(hal_can_handle) {
config_can(hal_filter_bank, hal_slave_start_filter_bank);
Expand All @@ -32,22 +32,22 @@ class Can : private utility::Immovable {
auto& header = *std::launder(reinterpret_cast<const FieldHeader*>(buffer));
buffer += sizeof(header);

// *modified from stm32h7xx_hal_fdcan.c FDCAN_CopyMessageToRAM
uint8_t can_data_length;
if (header.is_extended_can_id) {
auto& ext_id = *std::launder(reinterpret_cast<const CanExtendedId*>(buffer));
buffer += sizeof(ext_id);
mailbox.identifier = (ext_id.can_id << CAN_TI0R_EXID_Pos) | CAN_ID_EXT;
mailbox.identifier = (ext_id.can_id << 0) | FDCAN_EXTENDED_ID;
can_data_length = header.has_can_data ? ext_id.data_length + 1 : 0;
} else [[likely]] {
auto& std_id = *std::launder(reinterpret_cast<const CanStandardId*>(buffer));
buffer += sizeof(std_id);
mailbox.identifier = (std_id.can_id << CAN_TI0R_STID_Pos) | CAN_ID_STD;
mailbox.identifier = (std_id.can_id << 18) | FDCAN_STANDARD_ID;
can_data_length = header.has_can_data ? std_id.data_length + 1 : 0;
}
mailbox.identifier |= header.is_remote_transmission ? CAN_RTR_REMOTE : CAN_RTR_DATA;
mailbox.identifier |= CAN_TI0R_TXRQ;
mailbox.identifier |= header.is_remote_transmission ? FDCAN_REMOTE_FRAME : FDCAN_DATA_FRAME;
mailbox.data_length_and_timestamp = can_data_length;

// Always read full 8 bytes to reduce the number of if-branches for performance
// considerations (almost all CAN messages have a length of 8 bytes).
std::memcpy(mailbox.data, buffer, 8);
Expand All @@ -68,66 +68,85 @@ class Can : private utility::Immovable {
auto hcan = hal_can_handle_;

auto state = hcan->State;
assert_always((state == HAL_CAN_STATE_READY) || (state == HAL_CAN_STATE_LISTENING));

uint32_t tsr = hcan->Instance->TSR;
auto free_mailbox_count =
!!(tsr & CAN_TSR_TME0) + !!(tsr & CAN_TSR_TME1) + !!(tsr & CAN_TSR_TME2);
// fetch from HAL_FDCAN_AddMessageToTxFifoQ
assert_always(state == HAL_FDCAN_STATE_BUSY);

uint32_t txfqs = hcan->Instance->TXFQS;
auto free_mailbox_count = txfqs & FDCAN_TXFQS_TFFL;

return transmit_buffer_.pop_front_multi(
[this, hcan](TransmitMailboxData&& mailbox_data) {
auto target_mailbox_index =
(hcan->Instance->TSR & CAN_TSR_CODE) >> CAN_TSR_CODE_Pos;
assert_always(target_mailbox_index <= 2);

auto& target_mailbox = hal_can_handle_->Instance->sTxMailBox[target_mailbox_index];
target_mailbox.TDTR = mailbox_data.data_length_and_timestamp;
target_mailbox.TDLR = mailbox_data.data[0];
target_mailbox.TDHR = mailbox_data.data[1];
target_mailbox.TIR = mailbox_data.identifier;
auto put_index = ((hcan->Instance->TXFQS & FDCAN_TXFQS_TFQPI) >> FDCAN_TXFQS_TFQPI_Pos);

struct TxMailbox{
uint32_t TIR;
uint32_t TDTR;
uint32_t TDLR;
uint32_t TDHR;
};
auto target_mailbox = reinterpret_cast<TxMailbox*>(hcan->msgRam.TxBufferSA + (put_index * hcan->Init.TxElmtSize * 4U));

target_mailbox->TIR = mailbox_data.identifier;
target_mailbox->TDTR = mailbox_data.data_length_and_timestamp << 16;
target_mailbox->TDLR = mailbox_data.data[0];
target_mailbox->TDHR = mailbox_data.data[1];

hcan->Instance->TXBAR = ((uint32_t)1 << put_index);
hcan->LatestTxFifoQRequest = ((uint32_t)1 << put_index);

},
free_mailbox_count);
}

private:
friend void ::HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef*);
friend void ::HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs);

void config_can(uint32_t hal_filter_bank, uint32_t hal_slave_start_filter_bank) {
CAN_FilterTypeDef sFilterConfig;

sFilterConfig.FilterBank = hal_filter_bank;
sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
sFilterConfig.FilterIdHigh = 0x0000;
sFilterConfig.FilterIdLow = 0x0000;
sFilterConfig.FilterMaskIdHigh = 0x0000;
sFilterConfig.FilterMaskIdLow = 0x0000;
sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0;
sFilterConfig.FilterActivation = CAN_FILTER_ENABLE;
sFilterConfig.SlaveStartFilterBank = hal_slave_start_filter_bank;
// *midified config filter with HAL_FDCAN_ConfigFilter
FDCAN_FilterTypeDef sFilterConfig;

sFilterConfig.IdType = FDCAN_STANDARD_ID;
sFilterConfig.FilterIndex = hal_filter_bank;
sFilterConfig.FilterType = FDCAN_FILTER_MASK;
sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
sFilterConfig.FilterID1 = 0x0000;
sFilterConfig.FilterID2 = 0x0000;

constexpr auto ok = HAL_OK;
assert_always(HAL_CAN_ConfigFilter(hal_can_handle_, &sFilterConfig) == ok);
assert_always(HAL_CAN_Start(hal_can_handle_) == ok);
assert_always(
HAL_CAN_ActivateNotification(hal_can_handle_, CAN_IT_RX_FIFO0_MSG_PENDING) == ok);
assert_always(HAL_FDCAN_ConfigFilter(hal_can_handle_, &sFilterConfig) == ok);

sFilterConfig.IdType = FDCAN_EXTENDED_ID;
sFilterConfig.FilterIndex = hal_filter_bank+1;
sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;

assert_always(HAL_FDCAN_ConfigFilter(hal_can_handle_, &sFilterConfig) == ok);

assert_always(HAL_FDCAN_Start(hal_can_handle_) == ok);
assert_always(HAL_FDCAN_ActivateNotification(hal_can_handle_, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0) == ok);
}

bool read_device_write_buffer(
usb::InterruptSafeBuffer& buffer_wrapper, usb::field::UplinkId field_id) {
auto hal_can_state = hal_can_handle_->State;
auto hal_can_instance = hal_can_handle_->Instance;

assert_always(
(hal_can_state == HAL_CAN_STATE_READY) || (hal_can_state == HAL_CAN_STATE_LISTENING));
assert_always((hal_can_instance->RF0R & CAN_RF0R_FMP0) != 0U); // Assert if rx_fifo is empty
// modified from HAL_FDCAN_GetRxMessage
assert_always(hal_can_state == HAL_FDCAN_STATE_BUSY);
assert_always((hal_can_instance->RXF0S & FDCAN_RXF0S_F0FL) != 0U); // Assert if rx_fifo is empty

auto hal_can_instance_rir = hal_can_instance->sFIFOMailBox[CAN_RX_FIFO0].RIR;
auto hal_can_instance_rdtr = hal_can_instance->sFIFOMailBox[CAN_RX_FIFO0].RDTR;
struct RxMailbox{
uint32_t RIR;
uint32_t RDTR;
uint32_t RDLR;
uint32_t RDHR;
};
auto get_index = ((hal_can_instance->RXF0S & FDCAN_RXF0S_F0GI) >> FDCAN_RXF0S_F0GI_Pos);
auto rx_mailbox = reinterpret_cast<RxMailbox*>(hal_can_handle_->msgRam.RxFIFO0SA + (get_index * hal_can_handle_->Init.TxElmtSize * 4U));

bool is_extended_can_id = static_cast<bool>(CAN_RI0R_IDE & hal_can_instance_rir);
bool is_remote_transmission = static_cast<bool>(CAN_RI0R_RTR & hal_can_instance_rir);
size_t can_data_length = (CAN_RDT0R_DLC & hal_can_instance_rdtr) >> CAN_RDT0R_DLC_Pos;
bool is_extended_can_id = static_cast<bool>(rx_mailbox->RIR & 0x40000000U);
bool is_remote_transmission = static_cast<bool>(rx_mailbox->RIR & 0x20000000U);
size_t can_data_length = (rx_mailbox->RDTR & 0x000F0000U) >> 16;

// Calculate field size and try to allocate from buffer
std::byte* buffer = buffer_wrapper.allocate(
Expand All @@ -148,31 +167,30 @@ class Can : private utility::Immovable {
if (is_extended_can_id) {
auto& ext_id = *new (buffer) CanExtendedId{};
buffer += sizeof(CanExtendedId);
ext_id.can_id =
((CAN_RI0R_EXID | CAN_RI0R_STID) & hal_can_instance_rir) >> CAN_RI0R_EXID_Pos;
ext_id.can_id = rx_mailbox->RIR & 0x1FFFFFFFU;
ext_id.data_length = can_data_length - 1;
} else [[likely]] {
auto& std_id = *new (buffer) CanStandardId{};
buffer += sizeof(CanStandardId);
std_id.can_id = (CAN_RI0R_STID & hal_can_instance_rir) >> CAN_TI0R_STID_Pos;
std_id.can_id = (rx_mailbox->RIR & 0x1FFC0000U) >> 18;
std_id.data_length = can_data_length - 1;
}

// Write CAN data
uint32_t can_data[2];
can_data[0] = hal_can_instance->sFIFOMailBox[CAN_RX_FIFO0].RDLR;
can_data[1] = hal_can_instance->sFIFOMailBox[CAN_RX_FIFO0].RDHR;
can_data[0] = rx_mailbox->RDLR;
can_data[1] = rx_mailbox->RDHR;
std::memcpy(buffer, can_data, can_data_length);
buffer += can_data_length;
}

// Release the FIFO
hal_can_instance->RF0R |= CAN_RF0R_RFOM0;
hal_can_handle_->Instance->RXF0A = get_index;

return static_cast<bool>(buffer);
}

CAN_HandleTypeDef* hal_can_handle_;
FDCAN_HandleTypeDef* hal_can_handle_;

struct __attribute__((packed)) FieldHeader {
uint8_t field_id : 4;
Expand All @@ -199,7 +217,8 @@ class Can : private utility::Immovable {
utility::RingBuffer<TransmitMailboxData, 16> transmit_buffer_;
};

inline constinit Can::Lazy can1{&hcan1, 0, 14};
inline constinit Can::Lazy can2{&hcan2, 14, 14};
inline constinit Can::Lazy can1{&hfdcan1, 0, 14};
inline constinit Can::Lazy can2{&hfdcan2, 14, 14};
inline constinit Can::Lazy can3{&hfdcan3, 28, 14};

} // namespace can
2 changes: 1 addition & 1 deletion app/gpio/gpio.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
#include <cstdint>

#include <main.h>
#include <stm32f4xx_hal_gpio.h>
#include <stm32h7xx_hal_gpio.h>

#include "app/spi/bmi088/accel.hpp"
#include "app/spi/bmi088/gyro.hpp"
Expand Down
Loading