← 代码层索引 · bsp · service · algorithm · tasks
职责:把某个具体设备「绑定到哪个 BSP、注册到哪个节点、什么时候 init、怎么给指令」封成一个类。
不负责:底层时序(→ bsp/)、帧字节布局(→ device/motor/emmv5_protocol)、PID 数学(→ algorithm/)。
目录按设备种类分:motor/、sensor/、remote/、actuator/。只有头文件的是模板(按总线实例类型实例化)。
| 文件 | 设备 | 依赖 | 关键类型 |
|---|---|---|---|
motor/dji_motor.hpp |
DJI M3508 / M2006 / GM6020 | BspCanT、CanRxNodeT、CanTxNodeT |
DjiMotor<DjiMotorType, CanT> |
motor/dm_motor.hpp |
达妙 J4310 / J4340P / J8009P(MIT / 位置速度 / 速度) | BspCanT、CanRxNodeT |
DmMotor<CanT>、DmModel、DmMotorFeedback |
motor/device_motor.hpp/.cpp |
直流减速电机 + 编码器 + PID | BspPwm、BspEncoder、PID |
DeviceMotor |
motor/device_emmv5_group.hpp |
Emm_V5 步进闭环(一条 485 上多个地址) | BspUartT、emmv5_protocol |
DeviceEmmv5GroupT<Uart> |
motor/emmv5_protocol.hpp/.cpp |
Emm_V5 帧编解码(纯函数,不碰 HAL / bsp) | — | emmv5::Frame、*Scanner |
sensor/device_bmi088.hpp/.cpp |
BMI088 IMU + VQF 姿态 | BspSpiDevice、VQF |
DeviceBmi088、Bmi088RawData |
sensor/device_wt102.hpp |
WT102 单轴陀螺仪(Z 轴加速度 / 角速度 / 航向角) | BspUartT |
DeviceWt102T<Uart> |
remote/device_r9ds.hpp |
R9DS 遥控接收机(SBUS) | BspUartT |
DeviceR9dsT<Uart> |
actuator/device_servo.hpp/.cpp |
舵机(脉宽控制) | BspPwm |
DeviceServo |
actuator/device_buzzer.hpp/.cpp |
无源蜂鸣器 | BspPwm |
DeviceBuzzer |
设备类的构造都不是 constexpr(构造时就要绑外设、查参数):实例推荐定义在宿主 board_cfg.cpp、头文件里 extern;下面示例里写成 inline 的也能用(动态初始化,只初始化一次)。init() 在 board_init() 里逐个 ICBSP_CHECK。
enum class DjiMotorType : uint8_t { M3508 = 0x00, GM6020 = 0x01, M2006 = 0x02 };
inline DjiMotor<DjiMotorType::M3508, decltype(bsp_can1)> chassis_lf(bsp_can1, 1U); // 1 = 电调 ID- 模板参数
type决定:合法 ID 范围、反馈解析、控制帧映射、限幅、默认减速比 - 构造后必须
init():内部会CanRxNodeT::regist()注册反馈帧、CanTxNodeT申请发送槽位 DjiMotorControlMode::VOLTAGE/CURRENT只对 GM6020 有效(M3508/C620、M2006/C610 固定电流协议)- 跨任务读反馈用
snapshot(MotorData &, DjiMotorRawData *)(临界区内拷贝同一帧);get_data()是实时引用,can_rx_task 随时在改。 原来的互斥锁去掉了(读者拿不到锁,try-lock 失败时反而把帧当成没人认领,评审 F5) - 构造函数最后一个参数
online_timeout_ms(默认 30):多久收不到有效反馈判离线(评审 I4) - 多圈角度
angle_multi_round是 double,直接存双精度结果(评审 F11) - 对象不可复制/移动:在线检查节点与控制帧槽位靠对象地址标识
unpack(rx)由接收节点回调调用,fill_data(output)往发送槽位填值,一般业务只用get_status()和设置输出
协议来自 J4310 / J4340P / J8009P 说明书的 CAN 通信一章(清单与获取方式见 docs/sheet/README.md),三个系列逐项相同,所以不按型号做模板:
型号经 Config::model(DmModel::J4310 / J4340P / J4340P_V11 / J8009P)查 dm_model_info()(减速比、额定 / 峰值扭矩、空载转速),
MIT 前馈扭矩按峰值扭矩限幅。映射范围是驱动器里可改的寄存器,不能从型号推出:实测 J4340P 是 12.5 / 10 / 28,不是说明书的预设 12.5 / 30 / 10。
实板(阶段 3,2026-11-06):J4340P(MIT)读寄存器、sync_ranges()、使能 / 失能、零力矩 / 阻尼速度 / 位置控制都通过(宿主 M05 / M09);J4310P 只做了读寄存器与状态反馈(它的 CAN_ID 与同总线 GM6020 冲突,没有使能);J8009P 没有实物,只按说明书核对。
inline DmMotor<decltype(bsp_can2)> yaw_motor(bsp_can2, {.model = DmModel::J4340P, .can_id = 0x02, .master_id = 0x12,
.mode = DmMode::MIT}); // 映射范围留 0
ICBSP_CHECK(yaw_motor.init() == Status::OK); // board_init()
yaw_motor.sync_ranges(); // 任务里、system_start() 之后:从驱动器读 PMAX / VMAX / TMAX / 模式
yaw_motor.enable(); // 上电是失能态
yaw_motor.set_mit(p, v, kp, kd, t); // 控制任务每周期调用p_max/v_max/t_max:填和驱动器一致的值,或留 0、启动后sync_ranges()从驱动器读(未知时set_*()返回NOT_INIT)。填了但和驱动器不一致时sync_ranges()计入Diag::range_mismatch、在默认树节点记BAD_ARG,之后以驱动器为准;模式同理;Kp ∈ [0, 500]、Kd ∈ [0, 5],超范围限幅,非有限值返回BAD_ARG不发。- 控制帧不经过
CanTxNodeT:发送槽位过期清零后,MIT 帧全 0 映射成 p = −P_MAX、t = −T_MAX(满负扭矩)。控制帧由send_latest()直接发,调用方按控制周期持续调用;停发后靠电机自己的通讯丢失保护失能。使能 / 失能 / 清错误 / 设零点用send()排队发,不会被同 ID 的控制帧挤掉。 - 反馈是问询式的(电机收到发给自己的帧才回),不发控制帧时会判离线。每台电机的
master_id要不同;反馈里的 ID 只有低 4 位,本类只认can_id & 0x0F对得上的帧。 switch_mode()写 CTRL_MODE 寄存器(不存 flash);写寄存器的回复也走master_id,本类按挂起的那次写识别并吃掉,不当反馈解。- ID 冲突:驱动器只核对帧 ID 的低 8 位。和 DJI 电机同总线时
can_id的低 8 位要避开 0x010x0B(DJI 反馈 0x2010x20B)和 0xFE / 0xFF / 0x00(DJI 控制帧)。实测 CAN_ID 0x05(速度模式,控制帧 0x205)会把 GM6020 ID 1 的反馈 0x205 当成速度给定。 - 只需要
ICBSP_CAN_RX_ENABLE(控制帧不走发送节点),关了接收路径时包含dm_motor.hpp会#error。 - 默认树:节点挂在 CAN 总线节点下,掉线记
TIMEOUT,反馈进入故障态(ERR 不是 0 失能 / 1 使能:校准 / 传感器异常、超压 / 欠压 / 过流 / 过温 / 通讯丢失 / 过载)记IO_ERROR;read_register()读寄存器(不用使能,可用来核对 PMAX / VMAX / TMAX / 控制模式)。
inline DeviceMotor motor(
{.pwm_a = &bsp_pwm1, .pwm_b = &bsp_pwm2, .encoder = &bsp_encoder1,
.encoder_lines = 500.0f, .encoder_multiplier = 4.0f,
.gear_ratio = 32.0f, .wheel_diameter_cm = 7.0f,
.out_max = 100.0f, .deadband_cm_s = 0.4f,
.pid = {/* 速度环参数,默认全 0,靠实机标定 */}});| 方法 | 说明 |
|---|---|
Status init() |
校验三个绑定指针,计算 counts_per_cm |
void set_speed(float cm_s) |
设目标线速度 |
void set_speed_time(float cm_s, uint32_t ms) |
带时长的目标(到期自动停) |
bool update(float dt_s) |
每周期调用:算编码器速度 → PID → 写 PWM |
size_t get_pid_frame(uint8_t *buf, uint8_t fields = PID_ALL) const |
只组 JustFloat 帧,发不发、发到哪由 App 决定 |
void stop() / Mode::STOP |
两路 PWM 给满占空比(绕组短路刹车) |
get_pid_frame() 的字段位:
DeviceMotor::PID_TARGET | DeviceMotor::PID_FEEDBACK | DeviceMotor::PID_OUTPUT | DeviceMotor::PID_ALL帧尾固定 00 00 80 7F(JustFloat 结束符),返回值是实际写入的字节数。
为什么设备不持有串口:波形发给上位机是调试需求,不是电机的一部分。App 拿 get_pid_frame() 组好的缓冲
用哪个串口发都行,换传输方式也不用改设备。
一条 485 总线上挂多个地址的 Emm_V5:
Status enable(uint8_t addr, bool on);
Status set_velocity(uint8_t addr, emmv5::Direction dir, uint16_t rpm, uint8_t accel);
Status set_position(uint8_t addr, emmv5::Direction dir, uint16_t rpm, uint8_t accel,
int32_t pulses, emmv5::PositionMode mode);
Status stop(uint8_t addr);lock_bus() / unlock_bus() 用于多条指令的原子性(多任务共用一条 485 时必须包起来)。
每一帧的字节由 device/motor/emmv5_protocol.hpp 负责,设备层只管「发给谁、发什么参数、等不等回复」。
- 回复的读取:
take_position()/take_status()/take_in_position()读完就清(take_,N33);is_in_position(i)/is_online(addr)只查不清。 - 校验方式:
Config::checksum必须和驱动器菜单 Checksum 一致,支持emmv5::Checksum::FIXED_6B(默认)和XOR(未实测;回复帧两种都接受,因为说明书没写回复用哪种)。CRC-8 没有实现(说明书没给参数)。 - 帧缓冲跟随串口实例的
TX_SIZE(BspUartT::TX_CAPACITY),MMCL 子命令最多TX_SIZE − 5字节。
帧编解码(motor/emmv5_protocol)
纯函数式:没有状态机以外的成员,不 include HAL / bsp;改协议只动它,改收发方式只动设备类。
帧布局(EmmV5.0 说明书第 6 章):
普通命令: 地址(1) + 功能码(1) + 参数(...) + 校验(1)
MMCL 总帧:地址(1) + 0xAA(1) + 总长度(2, 大端,含头尾) + 子命令串(N) + 校验(1)
子命令 = 一条完整的普通命令(各自带校验字节)
- 地址 0x00 是广播(
emmv5::BROADCAST)。 - 校验默认是固定字节
0x6B(Checksum::FIXED_6B);驱动器菜单选 XOR 时用Checksum::XOR(前面全部字节的异或)。回复帧两种都接受。
组帧用现成的函数,每个返回一帧封好口的 emmv5::Frame(data[] / size 是成员,is_valid() 检查有没有溢出):
emmv5::Frame f = emmv5::enable(addr, true); // 也有 velocity / position / stop / zero_position /
// clear_protection / set_control_mode / quick_position* / read
f.apply_checksum(emmv5::Checksum::XOR); // 驱动器菜单选了 XOR 时
if (f.is_valid()) { uart.send(f.data, f.size); }
size_t n = emmv5::build_multi_frame(emmv5::BROADCAST, sub, sub_len, out, sizeof(out)); // MMCL 总帧;0 = 放不下解析回复(流式):回复是「到位」「位置」「状态」之类的通知帧,字节流可能粘连 / 截断,所以用扫描器、不一次性解析:
struct InPositionScanner { void reset(); bool feed(uint8_t addr, const uint8_t *data, size_t size); };
struct PositionScanner { void reset(); bool feed(uint8_t addr, const uint8_t *data, size_t size, int32_t &position); };
struct StatusScanner { void reset(); bool feed(uint8_t addr, const uint8_t *data, size_t size, uint8_t &status); };
emmv5::PositionScanner scanner;
if (scanner.feed(addr, buf, len, position)) { /* 凑满一帧:更新该地址的当前位置 */ }扫描器内部保存半帧状态,同一路总线的扫描器只能在一个任务里用。
// board_cfg.cpp(BspSpiDevice / DeviceBmi088 的构造不是 constexpr,不能写成 inline)
BspSpiDevice bmi088_acc_spi(bsp_spi2, {ACC_CS_GPIO_Port, ACC_CS_Pin, 0x80U, 1U}); // 读标志 0x80,加速度计多一个空字节
BspSpiDevice bmi088_gyro_spi(bsp_spi2, {GYRO_CS_GPIO_Port, GYRO_CS_Pin, 0x80U, 0U});
DeviceBmi088 imu(bmi088_acc_spi, bmi088_gyro_spi, {DeviceBmi088::AccRange::G24, DeviceBmi088::GyroRange::DPS2000});init()做自检与量程配置(加速度计和陀螺仪是两条独立片选)update(timeout_ms)读原始数据并跑 VQF,snapshot(ImuData&, Bmi088RawData*)取一致的一份快照- 诊断计数:
get_acc_saturation_count()、get_gyro_saturation_count()、get_update_fail_count()、get_attitude_skip_count() is_attitude_ready():姿态解算过至少一帧才为真;之前ImuData的四元数是单位四元数(不是全 0),姿态控制要先等它(评审 F1)- 运行中重新
init()与内部刷新任务互斥(刷新锁),不会和正在解算的那一帧并发改 VQF / 芯片(评审 F8) - 调度器启动前的等待用 DWT 忙等,不依赖 HAL 时基中断的优先级(评审 A3)
- 自动模式只按陀螺仪节奏刷新,加速度计 INT1 只计数:不需要时宿主可以不转调、或者不开对应 EXTI
attach_task(handle)可绑定任务句柄,让中断/错误能唤醒该任务
DeviceWt102T<decltype(bsp_uart7)> gyro_z(bsp_uart7); // board_cfg.cpp;串口在 CubeMX 里配 230400 8N1init()挂进默认树并兜底校验串口帧格式(不是 230400 8N1 返回BAD_ARG);可重复调用,会清掉数据和诊断计数update(timeout_ms)把串口里攒的字节全部解完;设备 500 Hz 发帧,要在某个任务里周期调用(不建任务、不注册 Online)get_z_newest()/get_z_previous()取最近两帧的 Z 轴原始 int16(acc_z/gyro_z/yaw),单位换算:wz = gyro_z / 32768 × 2000 °/s、yaw = yaw / 32768 × 180 °、az = acc_z / 32768 × 16 gis_online()按 tick 现算(50 ms 没有校验通过的帧就离线);get_diag()区分“没字节 / 切不出帧 / 校验错”- 不是线程安全的:
update()和读数据要在同一个任务里
inline DeviceR9dsT<decltype(bsp_uart3)> rc(bsp_uart3);init()挂进默认树,并兜底校验串口帧格式(SBUS:100000 bit/s、8E2,不对返回BAD_ARG);SBUS 是反相电平,要么在 CubeMX 里开 RX 引脚反相(Advanced Features),要么板上有硬件反相器update(timeout_ms)把串口里攒的字节解成通道值,要在某个任务里周期调用(本类不建任务);控制逻辑读get_rc()(摇杆 / 旋钮 −100 ~ +100,开关给挡位)is_online()同时看三条:帧超时、失控保护位、连续丢帧,失控保护时怎么处置由上层决定;单看is_frame_lost()/is_failsafe()也可以update()在别的任务里跑时用snapshot(Rc &, uint16_t *raw)读(评审 F5);诊断计数byte_cnt/frame_cnt是uint32_t(N34)
inline DeviceServo servo({.pwm = &bsp_pwm_servo,
.pulse_min_us = 500.0f, // 角度 0 端的脉宽
.pulse_max_us = 2500.0f, // range_deg 端的脉宽
.range_deg = 270.0f, // 180° 舵机填 180
.max_speed_dps = 300.0f, // 0 = 不限速
.max_accel_dps2 = 1500.0f,// 0 = 不限加速度
.reversed = false,
.trim_us = 0.0f});init() 后 set_angle(deg) 设目标,update() 周期调用做缓动,off() 关输出。
角度按 range_deg 限幅,set_pulse_us(us) 可直接给脉宽。PWM 通道必须已配成 50 Hz / 20 ms。
inline DeviceBuzzer buzzer(bsp_pwm_buzzer);
buzzer.beep(2000, 100); // 2 kHz 响 100 ms
buzzer.tone(1000, 50); // 持续音,音量 50%挂在总线上的设备各有一个 EventState 节点,在自己的 init() 里挂到所用总线的节点下(get_state() 读取):
| 设备 | 挂在 | 记什么 |
|---|---|---|
DjiMotor |
CAN 总线节点 | 初始化失败;掉线(Online 由在线变离线)→ TIMEOUT |
DmMotor |
CAN 总线节点 | 初始化失败;掉线 → TIMEOUT;反馈进入故障态 → IO_ERROR |
DeviceEmmv5GroupT |
串口总线节点 | 初始化失败;任一台电机掉线 → TIMEOUT |
DeviceR9dsT |
串口总线节点 | 初始化失败;update() 发现由在线变离线 → TIMEOUT |
DeviceWt102T |
串口总线节点 | 初始化失败;update() 发现由在线变离线 → TIMEOUT |
DeviceBmi088 |
SPI 总线节点 | 初始化失败;读帧失败 → 读帧返回的错误码 |
DeviceMotor、DeviceServo、DeviceBuzzer 不在总线上(PWM / 编码器),没有节点。
| 现象 | 原因 |
|---|---|
DjiMotor::init() 返回 BAD_ARG |
电调 ID 超范围:M3508 / M2006 是 1 |
| GM6020 橙灯常亮、不转 | 电调在电流模式,却发了电压指令(0x1FF / 0x2FF);构造时传 DjiMotorControlMode::CURRENT(发 0x1FE / 0x2FE) |
| 达妙电机被“别人的帧”带动 | 驱动器只看帧 ID 低 8 位,和 DJI 电机同总线时 can_id 撞上了 0x01~0x0B 等(见上) |
| 电机抖动不动 | DeviceMotor 的 encoder_multiplier 与 CubeMX 编码器模式不匹配(TI12 = 4,TI1 = 2) |
| 电机低速嗡嗡响 | deadband_cm_s 太小,静止时的 ±1 计数被 P 放大成占空比抖动 |
| 多电机只动一个 | 同总线两台同 ID(DJI 电调拨码 / 达妙 can_id、master_id) |
| Emm_V5 偶发丢指令 | 多任务共用 485 却没 lock_bus() |
| BMI088 读数乱 / 芯片 ID 不对 | BspSpiDevice 的 read_dummy 配反(加速度计读寄存器多一个空字节 = 1,陀螺仪 = 0),或 CubeMX 没开 MasterKeepIOState |
| 波形发不出去 | get_pid_frame() 只组帧;要在 App 里调 bsp_uart1.send() |
| Emm_V5 不响应 | 地址与驱动器设置不一致,或校验方式和驱动器菜单(Checksum)不一致 |
| Emm_V5 偶发丢回复 | 用「读一次就解析」而不是扫描器,遇到半帧就丢 |
| Emm_V5 位置值跳变 | PositionScanner 的 addr 传错,把别的电机回复当成自己的 |
| Emm_V5 多任务抢 485 | 扫描器状态被两个任务共享;应在 DeviceEmmv5GroupT 上 lock_bus() |