c板姿态解算.md

c板姿态解算

简介

本例程是一个FreeRTOS任务，演示如何使用本库中的BMI088封装和Mahony姿态解算算法实现姿态解算。

代码

#include "cmsis_os.h"
#include "spi.h"

#include "librm.hpp"

using rm::device::sensor::BMI088;
using rm::modules::algorithm::MahonyAhrs;

// 注意！！：不要在全局作用域中创建传感器对象，因为这样会导致对象的构造函数在main函数之前调用，而HAL库此时还未初始化
// 如果需要全局访问，可以将对象指针声明为全局变量，然后在任务函数中new一个对象绑定到指针上
// BMI088 *bmi088;
// void task() {
//   bmi088 = new BMI088(...);
// }

extern "C" {

void ExampleAttitudeTask(const void *pv_arg) {
  BMI088 bmi088(hspi1, CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, CS1_GYRO_GPIO_Port,
                CS1_GYRO_Pin);
  // 传入采样率
  MahonyAhrs mahony(1000.0f);

  for (;;) {
    bmi088.Update();
    mahony.Update(rm::modules::algorithm::ImuData6Dof{
        bmi088.gyro_x(), bmi088.gyro_y(), bmi088.gyro_z(), bmi088.accel_x(),
        bmi088.accel_y(), bmi088.accel_z()});

    // 获取姿态数据(欧拉角，弧度)
    mahony.euler_angle().pitch;
    mahony.euler_angle().roll;
    mahony.euler_angle().yaw;
    // 或者获取四元数
    mahony.quaternion().w;
    mahony.quaternion().x;
    mahony.quaternion().y;
    mahony.quaternion().z;
    osDelay(1);
  }
}
}