基于RT-Thread的RoboMaster电控框架设计

由于 RT-Thread 稳定高效的内核，丰富的文档教程，积极活跃的社区氛围，以及设备驱动框架、Kconfig、Scons、日志系统、海量的软件包……很难不选择 RT-Thread 进行项目开发。但也正是因为这些优点的覆盖面较广，很多初学者会觉得无从下手，但只要步入 RT-Thread 的大门，你就发现她的美好。这系列文档将作为本人基于 RT-Thread 开发 RoboMaster 电控框架的记录与分享，希望能帮助到更多初识 RT-Thread 的小伙伴，也欢迎大家交流分享，指正不足，共同进步。

背景
Robomaster 机器人比赛包含多个兵种，为了提高研发效率，模块化尤为重要，使用 RT-Thread 有助于面对对象思想开发；通过配备的 Kconfig，Scons 等工具可以实现工程的灵活配置；软件定时器可用作各电机等模块监控，RingBuffer 可以实现传感器信息的高效处理 …….

使用的开发板为大疆的 RoboMaster-C 型开发板，基础工程为 rt-thread>bsp>stm32f407-robomaster-c

电机模块开发
使用电机和电调均为大疆官方出品，如 2006，3508，6020 等，采用 CAN 通讯方式。

构建对象
首先我们根据使用的电机特性，构建一个通用的电机对象

/**

@brief DJI intelligent motor typedef
/
typedef struct dji_motor_object
{
rt_device_t can_dev; // 电机CAN实例
dji_motor_measure_t measure; // 电机测量值
uint32_t tx_id; // 发送id(主发)
uint32_t rx_id; // 接收id(主收)
/ 分组发送设置 /
uint8_t send_group; // 同一帧报文分组
uint8_t message_num; // 一帧报文中位置
motor_type_e motor_type; // 电机类型
motor_working_type_e stop_flag; // 启停标志
/ 监控线程相关 /
rt_timer_t timer; // 电机监控定时器
/ 电机控制相关 */
void *controller; // 电机控制器
int16_t (*control)(dji_motor_measure_t measure); // 控制电机的接口 用户可以自定义,返回值为16位的电压或电流值
} dji_motor_object_t;
因为这些电机我们均使用 CAN 方式进行驱动，是 CAN 设备的延申，于是将 rt_device_t can_dev 父类结构体对象内嵌。

dji_motor_measure_t 结构体中为，电机控制时需要用到的一些反馈值，包括电调直接反馈的数据以及进一步解算的得出的：

/**

@brief DJI motor feedback
/
typedef struct
{
/ 以下是处理得出的数据 /
float angle_single_round; // 单圈角度
float speed_aps; // 角速度,单位为:度/秒
float total_angle; // 总角度,注意方向
int32_t total_round; // 总圈数,注意方向
float target; // 目标值(输出轴扭矩矩/速度/角度(单位度))
/ 以下是电调直接回传的数据 */
uint16_t ecd; // 0-8191
uint16_t last_ecd; // 上一次读取的编码器值
int16_t speed_rpm; //电机的转速值
int16_t real_current; // 实际转矩电流
uint8_t temperature; // Celsius
} dji_motor_measure_t;
注册实例
通过 dji_motor_object_t *dji_motor_register(motor_config_t *config, void *controller) 注册对应的电机实例，用户通过 motor_config_t *config 对实例进行灵活配置：

/**
@brief 电机初始化,返回一个电机实例

@param config 电机配置
@return dji_motor_object_t* 电机实例指针
*/
dji_motor_object_t *dji_motor_register(motor_config_t *config, void *controller)
{
dji_motor_object_t *object = (dji_motor_object_t )rt_malloc(sizeof(dji_motor_object_t));
rt_memset(object, 0, sizeof(dji_motor_object_t));
// 对接用户配置的 motor_config
object->motor_type = config->motor_type; // 6020 or 2006 or 3508
object->rx_id = config->rx_id; // 电机接收报文的ID
object->control = controller; // 电机控制器
/ 查找 CAN 设备 /
object->can_dev = rt_device_find(config->can_name);
// 电机分组,因为至多4个电机可以共用一帧CAN控制报文
motor_send_grouping(object, config);
// 电机离线检测定时器相关
object->timer = rt_timer_create("motor1",
motor_lost_callback,
object, 20,
RT_TIMER_FLAG_PERIODIC);
rt_timer_start(object->timer);
dji_motor_enable(object);
dji_motor_obj[idx++] = object;
return object;
}
/ 电机配置结构体 */
typedef struct
{
motor_type_e motor_type;
const char *can_name;
uint32_t tx_id; // 发送id(主发)
uint32_t rx_id; // 接收id(主收)
void *controller; // 电机控制器
} motor_config_t;
motor_config_t 结构体中的 void *controller 为电机所使用到的控制器集合，是一个控制器类型为其成员的结构体变量，如下：

static struct chassis_controller_t{
pid_object_t *speed_pid;
}chassis_controller;
static struct gimbal_controller_t{
pid_object_t *speed_pid;
pid_object_t *angle_pid;
}gimbal_controlelr;
调用 dji_motor_object_t *dji_motor_register 时传入的 void *controller 为电机对应的控制器具体实现，如进行 pid 计算，滤波等，会赋值给电机对象对应的函数指针，在进行电机控制计算时被执行，如下：

rt_int16_t chassis_control(dji_motor_measure_t measure){
static rt_int16_t set = 0;
set = pid_calculate(chassis_controller.speed_pid, measure.speed_rpm, 1000);
return set;
}
数据处理
电机对象离不开对数据稳定快速的收发和解析计算，接下来展开讨论使用 RT-Thread 的 CAN 设备驱动收发数据的思路。

首先是数据的接收，stm32f4 拥有 2 个 CAN 外设，所有电机和使用 CAN 总线的设备都挂载在这两条总线上，但 RT-Thread 的每个 CAN 总线只能通过 rt_device_set_rx_indicate(can_dev, can_rx_call); 注册一个对应的接收回调函数。但不同类型电机，不同 CAN 设备的数据解析处理都是不一样的，我这里的解决思路是：首先创建了一个 usr_callback 文件，用于统一管理 CAN、串口等设备可能用到的用户接收对调函数；将一个大的设备类型回调函数注册到对应 CAN 设备，其中再细分各挂载设备的数据解析，实现如下：

#ifdef BSP_USING_CAN
rt_err_t can_rx_call(rt_device_t dev, rt_size_t size)
{
struct rt_can_msg rxmsg = {0};
uint8_t rxbuff = rxmsg.data;
/ 从 CAN 读取一帧数据 /
rt_device_read(dev, 0, &rxmsg, sizeof(rxmsg));
/ CAN 接收到数据后产生中断，调用此回调函数，然后发送接收信号量 /
#ifdef BSP_USING_DJI_MOTOR
dji_motot_rx_callback(rxmsg.id, rxbuff);
#endif / BSP_USING_DJI_MOTOR /
return RT_EOK;
}
#endif / BSP_USING_CAN */
但是这其中也有一点问题，rt_err_t can_rx_call(rt_device_t dev, rt_size_t size) 传入的参数无法判断具体的 CAN 设备来源，因此所有使用到的 CAN 外设数据处理函数都会被调用，但目前问题不大，因为同一条总线上不会挂载相同 ID 的设备，这也是一开始就应该避免的错误。

接下来是 CAN 报文的发送，调用 rt_device_write 发送填充好的 CAN 报文帧即可。

离线检测
这里使用 RT-Thread 的软件定时器对电机进行离线检测，当超过定时间没有接收到对应电机反馈报文，则进入超时回调，并输出警告日志：

/**

@brief 电机定时器超时回调函数
@param motor_ptr
*/
static void motor_lost_callback(void *motor_ptr)
{
dji_motor_object_t *motor = (dji_motor_object_t *)motor_ptr;
// dji_motor_stop(motor);
LOG_W("[dji_motor] Motor lost, can bus [%s] , id 0x[%x]", motor->can_dev->parent.name, motor->rx_id);
}
使用实例
封装完成的电机模块使用示例如下：

static struct chassis_controller_t{
pid_object_t *speed_pid;
}chassis_controller;
static struct gimbal_controller_t{
pid_object_t *speed_pid;
pid_object_t *angle_pid;
}gimbal_controlelr;
static dji_motor_object_t *chassis_motor;
static dji_motor_object_t *gimbal_motor;
rt_int16_t chassis_control(dji_motor_measure_t measure){
static rt_int16_t set = 0;
set = pid_calculate(chassis_controller.speed_pid, measure.speed_rpm, 1000);
return set;
}
rt_int16_t gimbal_control(dji_motor_measure_t measure){
static rt_int16_t set = 0;
set = pid_calculate(gimbal_controlelr.speed_pid, measure.speed_rpm, 0);
return set;
}
static void example_init()
{
pid_config_t chassis_speed_config = {
.Kp = 10, // 4.5
.Ki = 0, // 0
.Kd = 0, // 0
.IntegralLimit = 3000,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.MaxOut = 12000,
};
pid_config_t gimbal_speed_config = {
.Kp = 50, // 50
.Ki = 200, // 200
.Kd = 0,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 3000,
.MaxOut = 20000,
};
chassis_controller.speed_pid = pid_register(&chassis_speed_config);
gimbal_controlelr.speed_pid = pid_register(&gimbal_speed_config);
motor_config_t chassis_motor_config = {
.motor_type = M3508,
.can_name = CAN_CHASSIS,
.rx_id = 0x201,
.controller = &chassis_controller,
};
motor_config_t gimbal_motor_config = {
.motor_type = GM6020,
.can_name = CAN_GIMBAL,
.rx_id = 0x206,
.controller = &gimbal_controlelr,
};
chassis_motor = dji_motor_register(&chassis_motor_config, chassis_control);
gimbal_motor = dji_motor_register(&gimbal_motor_config, gimbal_control);
}
到此就可以方便且灵活的配置和使用电机模块啦。

存在问题及优化方向
目前 rt-thread 下 stm32 can驱动似乎仅支持 FIFO0 ，但 stm32f4 系列 can 具备两个 FIFO，如能同时使能所有 FIFO，应该能有效提高性能和稳定性。
电机的离线回调可以增加相应的声光报警。
后续考虑能不能也优化为，read，write，control 等形式。