|
|
经常碰到有人问，如何通过加速计和陀螺仪计算角度？有没有写好的现成的程序？要实现角度的读取需要进行融合运算，本例使用四元数运算来得到角度数据。最常见的示例就是控制Unity3D中的动画视角，以下程序通过M5StickC进行运算，利用SPP发送数据至上位机。项目地址https://github.com/naninunenoy/AxisOrange/tree/master/src 关键代码在.h文件内
[mw_shl_code=arduino,true]#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[ms] / 200[Hz]
#define TASK_SLEEP_SESSION 40 // = 1000[ms] / 25[Hz]
#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[0], a[1], a[2], g[0], g[1], g[2], q[0], q[1], q[2], q[3]);
btSpp.write((uint8_t*)&imuData, imu::ImuDataLen);
}
xSemaphoreGive(imuDataMutex);
// idle
int32_t sleep = TASK_SLEEP_SESSION - (millis() - entryTime);
vTaskDelay((sleep > 0) ? sleep : 0);
}
}[/mw_shl_code]
main.zip
(6.9 KB, 下载次数: 19)
|
|
发表于 2019-11-15 20:15