logzhan
/
PDR
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
PDR/1.Software/PDR 1.04/src/AHRS.c

423 lines
15 KiB
C
Raw Normal View History

优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : pdr_ahrs.c
* Department : Sensor Algorithm Team
* Current Version : V1.2
* Author : logzhan &
*
*
* Date of Issued : 2020.7.3
* Comments : PDR AHRS <EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <math.h>
#include <string.h>
#include "pdr_base.h"
#include "AHRS.h"
#include "pdr_detector.h"
#include "Filter.h"
#include "buffer.h"
#include "Kalman.h"
#include "Utils.h"
#include "Quaternion.h"
/* Macro Definition ----------------------------------------------------------*/
#define AHRS_BUF_LEN 256
#define FLT_THRES 0.000001f // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>ֵ
//<2F><><EFBFBD><EFBFBD>ģֵ
#define GET_MOL(v) sqrt((v)[0] * (v)[0] + (v)[1] * (v)[1] + (v)[2] * (v)[2])
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD>鳤<EFBFBD><E9B3A4>
#define ARR_LEN(arr) (sizeof(arr) / sizeof(*arr))
#define SET_BIT(msk) (g_ahrs.status |= AHRS_STATUS_##msk)
#define CLR_BIT(msk) (g_ahrs.status &= ~AHRS_STATUS_##msk)
#define TST_BIT(msk) (g_ahrs.status & AHRS_STATUS_##msk)
#define INC_PTR(buf, ptr) (((ptr) + 1) % ((buf).length + 1))
#define GRV_BUF_NME(i) "grav_buf_##i"
#define GYR_BUF_NME(i) "gyro_buf_##i"
#define MAG_BUF_NME(i) "magn_buf_##i"
/* Global Variable Definition ------------------------------------------------*/
/* Macro Definition ----------------------------------------------------------*/
/* Global Variable Definition ------------------------------------------------*/
static IMU g_imu;
AHRS_t g_ahrs;
static uint64_t g_ticker;
static BUFFER_LONG g_erro_buf;
static BUFFER_LONG g_grav_buf[3];
static BUFFER_LONG g_gyro_buf[3];
static BUFFER_LONG g_magn_buf[3];
static float dt = 1.0f / AHRS_SAMPLE_FREQ; // sample interval in second
static float Kp = 0; // 2 * proportional gain (Kp)
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // <20><>ʼ<EFBFBD><CABC>Ԫ<EFBFBD><D4AA>
static const float g_x_axis[] = {1, 0, 0};
static const float g_y_axis[] = {0, 1, 0};
static const float g_z_axis[] = {0, 0, 1};
/**----------------------------------------------------------------------
* Function : MahonyUpdateAHRS
* Description : Mahony<EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨
* Date : 2020/02/18 logzhan :
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>AHRS<EFBFBD>ȶ<EFBFBD><EFBFBD>Ա<EFBFBD>־λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>AHRS<EFBFBD>ȶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ټ<EFBFBD><EFBFBD><EFBFBD>error, <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PDR<EFBFBD><EFBFBD><EFBFBD><EFBFBD>10%<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
*---------------------------------------------------------------------**/
static void MahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, float az,
float mx, float my, float mz) {
// <20><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƺͼ<C6BA><CDBC>ٶȼ<D9B6>
pdr_v3Norm(&ax, &ay, &az);
float wx = 0.0f, wy = 0.0f, wz = 0.0f;
//if (!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
// pdr_v3Norm(&mx, &my, &mz);
// // <20>Ѵų<D1B4><C5B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵת<CFB5><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ h = R^-1 * (mx,my,mz)
// float hx = 2.0f * (mx * (0.5f - q2 * q2 - q3 * q3) + my * (q1 * q2 - q0 * q3) + mz * (q1 * q3 + q0 * q2));
// float hy = 2.0f * (mx * (q1 * q2 + q0 * q3) + my * (0.5f - q1 * q1 - q3 * q3) + mz * (q2 * q3 - q0 * q1));
// float hz = 2.0f * (mx * (q1 * q3 - q0 * q2) + my * (q2 * q3 + q0 * q1) + mz * (0.5f - q1 * q1 - q2 * q2));
// // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>bx = 0, by ָ<><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>ֻ<EFBFBD>IMU<4D><55><EFBFBD><EFBFBD>ʹ<EFBFBD>õ<EFBFBD><C3B5>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>(x,y,z)<29><><EFBFBD><EFBFBD>ϵ
// float by = (float)sqrt(hx * hx + hy * hy);
// float bz = hz;
// wx = by * (q0 * q3 + q1 * q2) + bz * (q1 * q3 - q0 * q2);
// wy = by * (0.5f - q1 * q1 - q3 * q3) + bz * (q0 * q1 + q2 * q3);
// wz = by * (q2 * q3 - q0 * q1) + bz * (0.5f - q1 * q1 - q2 * q2);
//}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ v = R * (0,0,1)
float vx = q1*q3 - q0*q2;
float vy = q0*q1 + q2*q3;
float vz = q0*q0 - 0.5f + q3*q3;
// <20><><EFBFBD><EFBFBD>ʸ<EFBFBD><CAB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float ex = ay*vz - az*vy + my*wz - mz*wy;
float ey = az*vx - ax*vz + mz*wx - mx*wz;
float ez = ax*vy - ay*vx + mx*wy - my*wx;
// Apply proportional feedback
gx += Kp * ex;
gy += Kp * ey;
gz += Kp * ez;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ռ<EFBFBD>˴<EFBFBD><CBB4><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
if (g_ahrs.stable == PDR_FALSE) {
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
BufferPush((Buffer_t*)&g_erro_buf, (float)(2 * sqrt(ex * ex + ey * ey + ez * ez)));
BufferMean((Buffer_t*)&g_erro_buf, &g_ahrs.error);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>
float qa = q0; float qb = q1; float qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz) * 0.5f * (dt);
q1 += ( qa * gx + qc * gz - q3 * gy) * 0.5f * (dt);
q2 += ( qa * gy - qb * gz + q3 * gx) * 0.5f * (dt);
q3 += ( qa * gz + qb * gy - qc * gx) * 0.5f * (dt);
// <20><>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
QuaternionNorm(&q0, &q1, &q2, &q3);
// <20><>Ԫ<EFBFBD><D4AA>תŷ<D7AA><C5B7><EFBFBD><EFBFBD>
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
g_ahrs.Pitch = (float)asin(-2.0f * (q3*q1 - q0*q2)) * (180.0f / 3.141592f);
g_ahrs.Yaw = (float)atan2(q2*q1 + q0*q3, 0.5f - q2*q2 - q3*q3) * (180.0f / 3.141592f);
g_ahrs.Roll = (float)atan2(q2*q3 + q0*q1, 0.5f - q2*q2 - q1*q1) * (180.0f / 3.141592f);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
}
/**---------------------------------------------------------------------
* Function : AHRS_sensorFrame2EarthFrame
* Description : ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȼ<EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
* Date : 2020/7/20 logzhan
*---------------------------------------------------------------------**/
void AHRS_sensorFrame2EarthFrame(float e[], float s[]) {
float quaVec[4];
float quaTmp[4];
float quaCnj[4];
quaVec[0] = 0;
quaVec[1] = s[0];
quaVec[2] = s[1];
quaVec[3] = s[2];
QuaternProd(quaTmp, g_ahrs.qnb, quaVec);
QuaternConj(quaCnj, g_ahrs.qnb);
QuaternProd(quaVec, quaTmp, quaCnj);
e[0] = quaVec[1];
e[1] = quaVec[2];
e[2] = quaVec[3];
}
void AHRS_Init(void) {
int i;
g_ahrs.stable = PDR_FALSE;
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
BufferInit((Buffer_t *)&g_erro_buf, "erro_buf", BUFFER_TYPE_QUEUE, AHRS_BUF_LEN);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
for (i = 0; i < 3; ++i) {
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
BufferInit((Buffer_t *)&g_grav_buf[i], GRV_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN);
BufferInit((Buffer_t *)&g_gyro_buf[i], GYR_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN);
BufferInit((Buffer_t *)&g_magn_buf[i], MAG_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
}
ResetARHS();
}
void ResetARHS(void) {
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
BufferClear((Buffer_t *)&g_erro_buf);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
for (uchar i = 0; i < 3; ++i) {
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
BufferClear((Buffer_t *)&g_grav_buf[i]);
BufferClear((Buffer_t *)&g_gyro_buf[i]);
BufferClear((Buffer_t *)&g_magn_buf[i]);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
}
g_ticker = 0;
q0 = 1.0f;
q1 = 0.0f;
q2 = 0.0f;
q3 = 0.0f;
memset(&g_ahrs, 0, sizeof(g_ahrs));
g_ahrs.status = (uint8_t)AHRS_STATUS_RESET_PHASE_0;
g_ahrs.error = -1;
g_ahrs.qnb[0] = 1.0f;
g_ahrs.qnb[1] = 0.0f;
g_ahrs.qnb[2] = 0.0f;
g_ahrs.qnb[3] = 0.0f;
g_ahrs.gravity[0] = 0.0f;
g_ahrs.gravity[1] = 0.0f;
g_ahrs.gravity[2] = 9.8f;
g_ahrs.x_axis[0] = 1.0f;
g_ahrs.x_axis[1] = 0.0f;
g_ahrs.x_axis[2] = 0.0f;
g_ahrs.y_axis[0] = 0.0f;
g_ahrs.y_axis[1] = 1.0f;
g_ahrs.y_axis[2] = 0.0f;
g_ahrs.z_axis[0] = 0.0f;
g_ahrs.z_axis[1] = 0.0f;
g_ahrs.z_axis[2] = 1.0f;
}
/**---------------------------------------------------------------------
* Function : estimate_sensor_vector
* Description : <EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD>ǰ<EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>q<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD>ǰ<EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD>grav
* input : float eVec[], float gyro[]
* output : float sVec[]
* Date : 2020/7/20 logzhan
*---------------------------------------------------------------------**/
static void estimate_sensor_vector(float sVec[], float eVec[], float gyro[]) {
float q[4];
float gx, gy, gz;
float qa, qb, qc;
float recipNorm;
float quaVec[4];
float quaTmp[4];
float quaCnj[4];
/* estimate */
gx = gyro[0] * (0.5f * (dt));
gy = gyro[1] * (0.5f * (dt));
gz = gyro[2] * (0.5f * (dt));
qa = q0;
qb = q1;
qc = q2;
q[0] = q0 + (-qb * gx - qc * gy - q3 * gz);
q[1] = q1 + (qa * gx + qc * gz - q3 * gy);
q[2] = q2 + (qa * gy - qb * gz + q3 * gx);
q[3] = q3 + (qa * gz + qb * gy - qc * gx);
recipNorm = InvSqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
q[0] *= recipNorm;
q[1] *= recipNorm;
q[2] *= recipNorm;
q[3] *= recipNorm;
/* project */
QuaternConj(q, q);
quaVec[0] = 0;
quaVec[1] = eVec[0];
quaVec[2] = eVec[1];
quaVec[3] = eVec[2];
QuaternProd(quaTmp, q, quaVec);
QuaternConj(quaCnj, q);
QuaternProd(quaVec, quaTmp, quaCnj);
sVec[0] = quaVec[1];
sVec[1] = quaVec[2];
sVec[2] = quaVec[3];
}
/**----------------------------------------------------------------------
* Function : pdr_imuUpdateAhrs
* Description : 1<EFBFBD><EFBFBD>AHRS<EFBFBD>ںϽ<EFBFBD><EFBFBD><EFBFBD>
* 2<EFBFBD><EFBFBD>Error<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* 3<EFBFBD><EFBFBD>PCA<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* Date : 2020/6/30 logzhan
*---------------------------------------------------------------------**/
int UpdateAHRS(IMU_t* imu) {
float gyro[3], acce[3], magn[3], grav[3], mean[2];
int index;
int count;
int i;
for (uchar i = 0; i < 3; ++i) {
gyro[i] = imu->gyr.s[i];
acce[i] = imu->acc.s[i];
magn[i] = imu->mag.s[i];
}
//g_ahrs.status(<28><>ʼ״̬0x80) <20><>λ<EFBFBD><CEBB> AHRS_STATUS_RESET_PHASE_0(0x80)
if (g_ahrs.status & AHRS_STATUS_RESET_PHASE_0) {
// ARHS BUF <20>д洢<D0B4><E6B4A2>256<35><36>sensor<6F>Ļ<EFBFBD><C4BB><EFBFBD>
if (g_ticker <= AHRS_BUF_LEN) {
for (i = 0; i < 3; ++i) {
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
BufferPush((Buffer_t *)&g_grav_buf[i], acce[i]);
BufferPush((Buffer_t *)&g_gyro_buf[i], gyro[i]);
BufferPush((Buffer_t *)&g_magn_buf[i], magn[i]);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
}
}
// ֻ<>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>ż<EFBFBD><C5BC><EFBFBD>
if (g_ticker >= AHRS_BUF_LEN) {
//int index;
index = (g_magn_buf[0]._top + AHRS_BUF_LEN + 1 - AHRS_BUF_LEN / 2) % (g_magn_buf[0].length + 1);
for (i = 0; i < 3; ++i) {
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
BufferMean((Buffer_t *)&g_grav_buf[i], &grav[i]);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
magn[i] = g_magn_buf[i].data[index];
g_grav_buf[i]._top = g_grav_buf[i]._bottom;
}
Kp = (float)(2 * 2);
/* <20><><EFBFBD><EFBFBD>ѭ<EFBFBD><D1AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1000<30><30>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ<EFBFBD><C8B7>̬ */
for (i = 0; i < 1000; ++i) {
MahonyUpdateAHRS(0, 0, 0, grav[0], grav[1], grav[2], magn[0], magn[1], magn[2]);
}
AHRS_sensorFrame2EarthFrame(g_ahrs.gravity, grav);
Kp = (float)(2 * 2);
for (index = INC_PTR(g_gyro_buf[0], index); index != g_gyro_buf[0]._top; index = INC_PTR(g_gyro_buf[0], index)) {
for (i = 0; i < 3; ++i) {
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
BufferPush((Buffer_t *)&g_grav_buf[i], g_ahrs.gravity[i]);
BufferMean((Buffer_t *)&g_grav_buf[i], &grav[i]);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
gyro[i] = g_gyro_buf[i].data[index];
acce[i] = g_grav_buf[i].data[index];
magn[i] = g_magn_buf[i].data[index];
}
estimate_sensor_vector(grav, grav, gyro);
// AHRS <20><><EFBFBD><EFBFBD>
MahonyUpdateAHRS(gyro[0], gyro[1], gyro[2], grav[0], grav[1], grav[2], magn[0],
magn[1], magn[2]);
AHRS_sensorFrame2EarthFrame(g_ahrs.gravity, acce);
}
g_ahrs.qnb[0] = q0;
g_ahrs.qnb[1] = q1;
g_ahrs.qnb[2] = q2;
g_ahrs.qnb[3] = q3;
CLR_BIT(RESET_PHASE_0); /*#define CLR_BIT(msk) (g_ahrs.status &= ~AHRS_STATUS_##msk)<29><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>g_ahrs.status*/
SET_BIT(RESET_PHASE_1); /*#define SET_BIT(msk) (g_ahrs.status |= AHRS_STATUS_##msk)<29><><EFBFBD><EFBFBD>ֵg_ahrs.status<75><73><EFBFBD><EFBFBD>phase1<65><31>ͬ*/
}
++g_ticker;
//return;
//g_ahrs.status(0x80) <20><>λ<EFBFBD><CEBB> AHRS_STATUS_RESET_PHASE_1(0x40)
}else if (g_ahrs.status & AHRS_STATUS_RESET_PHASE_1) {
//PDR_LOGD("<22>׶<EFBFBD>1");
// Ӧ<><D3A6><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>PCA<43><41><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (g_ahrs.error < 0.3) {
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
BufferCount((Buffer_t *)&g_erro_buf, &count);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
memset((void *)mean, 0, sizeof(mean));
for (index = g_erro_buf._bottom, i = 0; index != g_erro_buf._top; index = INC_PTR(g_erro_buf, index), ++i) {
int c = count >> 1;
if (i < c ) {
mean[0] += g_erro_buf.data[index];
}
else {
mean[1] += g_erro_buf.data[index];
}
}
int a = count >> 1;
int b = count - (count >> 1);
mean[0] /= a;
mean[1] /= b;
if (fabs(mean[0] - mean[1]) < 0.025) {
Kp = (float)(2 * AHRS_KP);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־λ<D6BE><CEBB><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>PCA<43><41><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
CLR_BIT(RESET_PHASE_1);
SET_BIT(STABLE);
g_ahrs.stable = PDR_TRUE;
}
}
for (i = 0; i < 3; ++i) {
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
BufferPush((Buffer_t *)&g_grav_buf[i], g_ahrs.gravity[i]);
BufferMean((Buffer_t *)&g_grav_buf[i], &grav[i]);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
}
estimate_sensor_vector(grav, grav, gyro);
MahonyUpdateAHRS(gyro[0], gyro[1], gyro[2],
grav[0], grav[1], grav[2],
magn[0], magn[1], magn[2]);
g_ahrs.qnb[0] = q0;
g_ahrs.qnb[1] = q1;
g_ahrs.qnb[2] = q2;
g_ahrs.qnb[3] = q3;
AHRS_sensorFrame2EarthFrame(g_ahrs.gravity, acce);
AHRS_sensorFrame2EarthFrame(g_ahrs.x_axis, (float*)g_x_axis);
AHRS_sensorFrame2EarthFrame(g_ahrs.y_axis, (float*)g_y_axis);
AHRS_sensorFrame2EarthFrame(g_ahrs.z_axis, (float*)g_z_axis);
ComputePCA(&g_ahrs);
++g_ticker;
}
else {
// Buffer_mean<61>ļ<EFBFBD><C4BC><EFBFBD>ʱ<EFBFBD><CAB1>̫<EFBFBD><CCAB><EFBFBD>ˣ<EFBFBD>10ms<6D><73><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD><EFBFBD>256<35><36><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5>̫<EFBFBD><CCAB>
//for (i = 0; i < 3; ++i) {
// Buffer_push((BUFFER *)&g_grav_buf[i], g_ahrs.gravity[i]);
// Buffer_mean((BUFFER *)&g_grav_buf[i], &grav[i]);
//}
//estimate_sensor_vector(grav, grav, gyro);
// logzhan 21/02/06 : <20>о<EFBFBD><D0BE><EFBFBD>acc<63><63><EFBFBD>뾫<EFBFBD>Ȼ<EFBFBD><C8BB><EFBFBD>һЩ
MahonyUpdateAHRS(gyro[0], gyro[1], gyro[2],
acce[0], acce[1], acce[2],
magn[0], magn[1], magn[2]);
g_ahrs.qnb[0] = q0;
g_ahrs.qnb[1] = q1;
g_ahrs.qnb[2] = q2;
g_ahrs.qnb[3] = q3;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>ļ<EFBFBD><C4BC>ٶ<EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ,
AHRS_sensorFrame2EarthFrame(g_ahrs.gravity, acce);
// <20>ⲿ<EFBFBD>ִ<EFBFBD><D6B4>붼<EFBFBD>ǿ<EFBFBD><C7BF>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȵģ<C8B5><C4A3><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
AHRS_sensorFrame2EarthFrame(g_ahrs.x_axis, (float*)g_x_axis);
AHRS_sensorFrame2EarthFrame(g_ahrs.y_axis, (float*)g_y_axis);
AHRS_sensorFrame2EarthFrame(g_ahrs.z_axis, (float*)g_z_axis);
++g_ticker;
return PDR_TRUE;
}
return PDR_FALSE;
}
/**---------------------------------------------------------------------
* Function : fv3Norm
* Description : <EFBFBD><EFBFBD>ά<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD>
* Date : 2021/01/26 logzhan
*---------------------------------------------------------------------**/
void fv3Norm(float* vx, float* vy, float* vz)
{
float norm = sqrtf(((*vx) * (*vx) + (*vy) * (*vy) +
(*vz) * (*vz)));
// <20><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD>ģΪ0<CEAA><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (norm > FLT_THRES) {
norm = 1 / norm;
(*vx) *= norm; (*vy) *= norm; (*vz) *= norm;
}
}