基于 RT-Thread 的 RoboMaster 电控框架（四）

基于 RT-Thread 的 RoboMaster 电控框架（四）

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

背景

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

IMU姿态解算

使用 BMI088 robomaster-c 开发板上集成的6轴 imu 进行姿态解算，使用的方案其实是RM中比较普遍成熟的一套，主要使用了四元数和卡尔曼滤波进行融合解算，解算频率为 1Khz。

        dt = dwt_get_delta(&ins_dwt);
        imu_ops.gyro_read(ins.gyro);
        imu_ops.accel_read(ins.accel);

        // 核心函数,EKF更新四元数
        IMU_QuaternionEKF_Update(ins.gyro[X], ins.gyro[Y], ins.gyro[Z], ins.accel[X], ins.accel[Y], ins.accel[Z], dt);

        memcpy(ins.q, QEKF_INS.q, sizeof(QEKF_INS.q));

        // 机体系基向量转换到导航坐标系，本例选取惯性系为导航系
        BodyFrameToEarthFrame(xb, ins.xn, ins.q);
        BodyFrameToEarthFrame(yb, ins.yn, ins.q);
        BodyFrameToEarthFrame(zb, ins.zn, ins.q);

        // 将重力从导航坐标系n转换到机体系b,随后根据加速度计数据计算运动加速度
        float gravity_b[3];
        EarthFrameToBodyFrame(gravity, gravity_b, ins.q);
        for (uint8_t i = 0; i < 3; ++i) // 同样过一个低通滤波
        {
            ins.motion_accel_b[i] = (ins.accel[i] - gravity_b[i]) * dt / (ins.accel_lpf + dt) + ins.motion_accel_b[i] * ins.accel_lpf / (ins.accel_lpf + dt);
        }
        BodyFrameToEarthFrame(ins.motion_accel_b, ins.motion_accel_b, ins.q); // 转换回导航系n

        ins.yaw = QEKF_INS.Yaw;
        ins.pitch = QEKF_INS.Pitch;
        ins.roll = QEKF_INS.Roll;
        ins.yaw_total_angle = QEKF_INS.YawTotalAngle;

恒温控制

C型开发板上的 BMI088 周围是又一圈加热电阻的，可以通过 PWM 控制加热功率，从而实现恒温控制，有助于抑制陀螺仪漂移，根据手册提示恒温控制在40摄氏度较好，

static pid_obj_t *imu_temp_pid;
static pid_config_t imu_temp_config = {
        .Kp = 50000,
        .Ki = 8000,
        .Kd = 0,
        .IntegralLimit = 50000,
        .Improve = PID_Integral_Limit,
        .MaxOut = 250000,
};

static rt_err_t temp_pwm_init(rt_uint32_t period, rt_uint32_t pulse)
{
    temp_pwm_dev = (struct rt_device_pwm *)rt_device_find(TEMP_PWM_DEV_NAME);
    if (temp_pwm_dev == RT_NULL)
    {
        LOG_E("Can't find %s device!", TEMP_PWM_DEV_NAME);
        return -RT_ERROR;
    }
    /* 设置PWM周期和脉冲宽度默认值 */
    rt_pwm_set(temp_pwm_dev, TEMP_PWM_DEV_CHANNEL, period, pulse);
    /* 使能设备 */
    rt_pwm_enable(temp_pwm_dev, TEMP_PWM_DEV_CHANNEL);
}

在 ins_task 中以 500hz 的频率进行恒温控制，需要注意，使用加热电阻外围电路需要给C板额外供电。

void ins_thread_entry(void *argument)
{
    static uint32_t count;

    temp_pwm_init(period, pulse);
    /* 注册 PID 实例 */
    imu_temp_pid = pid_register(&imu_temp_config);
    imu_ops.imu_init();
    LOG_I("Example Task Start");

    for (;;)
    {
        example_start = dwt_get_time_ms();
        imu_ops.gyro_read(gyro);
        imu_ops.accel_read(acc);
        
        if(count % 2 == 0){
            temp = imu_ops.temp_read();
            pulse = pid_calculate(imu_temp_pid, temp, IMU_TARGET_TEMP);
            rt_pwm_set_pulse(temp_pwm_dev, TEMP_PWM_DEV_CHANNEL, pulse);
        }
        count++;

        rt_thread_delay(1);
    }
}

实际效果测试如下：

• 不加恒温控制，室温下（23摄氏度左右）十分钟内，yaw 轴偏移近20度；
• 加入恒温控制后（40摄氏度），十分钟内，yaw 轴偏移10度

arm_math库使用

有一个小插曲就是arm_math库的移植使用，需要通过修改 Scons 文件，将 arm_math 库链接到工程中，并添加需要的宏定义：

#  使用 arm_math.h 需要添加相关内核定义
CPPDEFINES = ['ARM_MATH_CM4']
LIBPATH = [cwd + '/arm_math']
LIBS = ['libarm_cortexM4lf_math.a']
path += [cwd + '/arm_math']

group = DefineGroup('RM_Algorithms', src, depend = [''], CPPPATH = path, CPPDEFINES = CPPDEFINES, LIBS = LIBS, LIBPATH=LIBPATH)

到此就可以使用解算得到的欧拉角等数据去对云台等进行闭环控制啦

仓库地址放这里了 HNU_RM_SHARK_C ，觉得不错可以点个 Star ！

存在问题及优化方向

1. 虽然通过陀螺仪校准和恒温控制等有效抑制了零漂，但yaw的零飘依然存在；
2. 之后考虑通过融合磁力计数据，解决零飘问题；