首页 › M5Stack › 【搬运】通过经典蓝牙发送M5StickC的IMU数据并进行四元运算

vany5921 发表于 2019-11-15 20:15

【搬运】通过经典蓝牙发送M5StickC的IMU数据并进行四元运算

    经常碰到有人问，如何通过加速计和陀螺仪计算角度？有没有写好的现成的程序？要实现角度的读取需要进行融合运算，本例使用四元数运算来得到角度数据。最常见的示例就是控制Unity3D中的动画视角，以下程序通过M5StickC进行运算，利用SPP发送数据至上位机。项目地址https://github.com/naninunenoy/AxisOrange/tree/master/src 关键代码在.h文件内


#include <M5StickC.h>#include "ImuReader.h"
#include "BluetoothSerial.h"

#define TASK_DEFAULT_CORE_ID 1
#define TASK_STACK_DEPTH 4096UL
#define TASK_NAME_IMU "IMUTask"
#define TASK_NAME_SESSION "SessionTask"
#define TASK_SLEEP_IMU 5 // = 1000 / 200
#define TASK_SLEEP_SESSION 40 // = 1000 / 25
#define MUTEX_DEFAULT_WAIT 1000UL

static void ImuLoop(void* arg);
static void SessionLoop(void* arg);

imu::ImuReader* imuReader;
imu::ImuData imuData;
const float offsetGyroX = -1.76546F;
const float offsetGyroY = -6.8906F;
const float offsetGyroZ = 14.59196F;
static SemaphoreHandle_t imuDataMutex = NULL;
BluetoothSerial btSpp;

void setup() {
M5.begin();
// imu
imuReader = new imu::ImuReader(M5.Imu);
imuReader->initialize();
imuReader->writeGyroOffset(offsetGyroX, offsetGyroY, offsetGyroZ);
// lcd
M5.Lcd.setRotation(3);
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextSize(1);
M5.Lcd.setCursor(40, 0);
M5.Lcd.println("AxisOrange");
// bluetooth serial
btSpp.begin("AxisOrange");
// task
imuDataMutex = xSemaphoreCreateMutex();
xTaskCreatePinnedToCore(ImuLoop, TASK_NAME_IMU, TASK_STACK_DEPTH,
    NULL, 2, NULL, TASK_DEFAULT_CORE_ID);
xTaskCreatePinnedToCore(SessionLoop, TASK_NAME_SESSION, TASK_STACK_DEPTH,
    NULL, 1, NULL, TASK_DEFAULT_CORE_ID);
}

void loop() { /* Do Nothing */ }

static void ImuLoop(void* arg) {
while (1) {
    uint32_t entryTime = millis();
    if (xSemaphoreTake(imuDataMutex, MUTEX_DEFAULT_WAIT) == pdTRUE) {
      imuReader->update();
      imuReader->read(imuData);
    }
    xSemaphoreGive(imuDataMutex);
    // idle
    int32_t sleep = TASK_SLEEP_IMU - (millis() - entryTime);
    vTaskDelay((sleep > 0) ? sleep : 0);
}
}

static void SessionLoop(void* arg) {
while (1) {
    uint32_t entryTime = millis();
    if (xSemaphoreTake(imuDataMutex, MUTEX_DEFAULT_WAIT) == pdTRUE) {
      // uint32_t t = imuData.timestamp;
      // float* a = imuData.acc;
      // float* g = imuData.gyro;
      // float* q = imuData.quat;
      // btSpp.printf("%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",
      //   t, a, a, a, g, g, g, q, q, q, q);
      btSpp.write((uint8_t*)&imuData, imu::ImuDataLen);
    }
    xSemaphoreGive(imuDataMutex);
    // idle
    int32_t sleep = TASK_SLEEP_SESSION - (millis() - entryTime);
    vTaskDelay((sleep > 0) ? sleep : 0);
}
}

查看完整版本: 【搬运】通过经典蓝牙发送M5StickC的IMU数据并进行四元运算