423 lines
15 KiB
C
423 lines
15 KiB
C
/******************** (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 姿态解算相关
|
||
********************************************************************************/
|
||
/* 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 // 浮点数最小阈值
|
||
//计算模值
|
||
#define GET_MOL(v) sqrt((v)[0] * (v)[0] + (v)[1] * (v)[1] + (v)[2] * (v)[2])
|
||
//获取数组长度
|
||
#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; // 初始四元数
|
||
|
||
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姿态更新算法
|
||
* Date : 2020/02/18 logzhan :
|
||
* 增加了AHRS稳定性标志位，如果AHRS稳定后不再计算error, 降低运算
|
||
量。能提升PDR仿真10%的运行速度
|
||
*---------------------------------------------------------------------**/
|
||
static void MahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, float az,
|
||
float mx, float my, float mz) {
|
||
// 归一化磁力计和加速度计
|
||
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);
|
||
|
||
// // 把磁场从载体系转换到地理坐标系 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));
|
||
|
||
// // 理论上bx = 0, by 指向北向，因为手机IMU数据使用的是东北天(x,y,z)坐标系
|
||
// 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);
|
||
//}
|
||
|
||
// 把重力转换到地理坐标系 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;
|
||
|
||
// 计算矢量叉乘误差
|
||
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;
|
||
|
||
// 这两个函数占了大量的时间
|
||
if (g_ahrs.stable == PDR_FALSE) {
|
||
BufferPush((Buffer_t*)&g_erro_buf, (float)(2 * sqrt(ex * ex + ey * ey + ez * ez)));
|
||
BufferMean((Buffer_t*)&g_erro_buf, &g_ahrs.error);
|
||
}
|
||
|
||
// 龙格库塔法更新四元数
|
||
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);
|
||
|
||
// 四元数归一化
|
||
QuaternionNorm(&q0, &q1, &q2, &q3);
|
||
|
||
// 四元数转欧拉角
|
||
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);
|
||
}
|
||
|
||
/**---------------------------------------------------------------------
|
||
* Function : AHRS_sensorFrame2EarthFrame
|
||
* Description : 转换加速度计值到地球坐标系
|
||
* 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;
|
||
BufferInit((Buffer_t *)&g_erro_buf, "erro_buf", BUFFER_TYPE_QUEUE, AHRS_BUF_LEN);
|
||
for (i = 0; i < 3; ++i) {
|
||
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);
|
||
}
|
||
ResetARHS();
|
||
}
|
||
|
||
void ResetARHS(void) {
|
||
|
||
BufferClear((Buffer_t *)&g_erro_buf);
|
||
for (uchar i = 0; i < 3; ++i) {
|
||
BufferClear((Buffer_t *)&g_grav_buf[i]);
|
||
BufferClear((Buffer_t *)&g_gyro_buf[i]);
|
||
BufferClear((Buffer_t *)&g_magn_buf[i]);
|
||
}
|
||
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 : 利用当前四元数q与陀螺仪，计算在当前姿态下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、AHRS融合解算
|
||
* 2、Error分析
|
||
* 3、PCA方向计算
|
||
* 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(初始状态0x80) 按位与 AHRS_STATUS_RESET_PHASE_0(0x80)
|
||
if (g_ahrs.status & AHRS_STATUS_RESET_PHASE_0) {
|
||
|
||
// ARHS BUF 中存储了256个sensor的缓存
|
||
if (g_ticker <= AHRS_BUF_LEN) {
|
||
for (i = 0; i < 3; ++i) {
|
||
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]);
|
||
}
|
||
}
|
||
// 只有缓存满的时候才计算
|
||
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) {
|
||
BufferMean((Buffer_t *)&g_grav_buf[i], &grav[i]);
|
||
magn[i] = g_magn_buf[i].data[index];
|
||
g_grav_buf[i]._top = g_grav_buf[i]._bottom;
|
||
}
|
||
|
||
Kp = (float)(2 * 2);
|
||
/* 这里循环运行1000次,收敛，趋于正确姿态 */
|
||
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) {
|
||
BufferPush((Buffer_t *)&g_grav_buf[i], g_ahrs.gravity[i]);
|
||
BufferMean((Buffer_t *)&g_grav_buf[i], &grav[i]);
|
||
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 更新
|
||
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)；清零g_ahrs.status*/
|
||
SET_BIT(RESET_PHASE_1); /*#define SET_BIT(msk) (g_ahrs.status |= AHRS_STATUS_##msk)；赋值g_ahrs.status，与phase1相同*/
|
||
}
|
||
++g_ticker;
|
||
//return;
|
||
//g_ahrs.status(0x80) 按位与 AHRS_STATUS_RESET_PHASE_1(0x40)
|
||
}else if (g_ahrs.status & AHRS_STATUS_RESET_PHASE_1) {
|
||
//PDR_LOGD("阶段1");
|
||
// 应该是判断是否进入PCA的条件
|
||
if (g_ahrs.error < 0.3) {
|
||
|
||
BufferCount((Buffer_t *)&g_erro_buf, &count);
|
||
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);
|
||
|
||
// 这个标志位决定是否进入PCA方向估计
|
||
CLR_BIT(RESET_PHASE_1);
|
||
SET_BIT(STABLE);
|
||
g_ahrs.stable = PDR_TRUE;
|
||
}
|
||
}
|
||
|
||
for (i = 0; i < 3; ++i) {
|
||
BufferPush((Buffer_t *)&g_grav_buf[i], g_ahrs.gravity[i]);
|
||
BufferMean((Buffer_t *)&g_grav_buf[i], &grav[i]);
|
||
}
|
||
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的计算时间太长了，10ms更新来算，计算256个数的平均
|
||
// 计算量实在太大
|
||
//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 : 感觉是acc输入精度会高一些
|
||
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;
|
||
|
||
// 把载体系的加速度转换为地理坐标系,
|
||
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);
|
||
|
||
++g_ticker;
|
||
return PDR_TRUE;
|
||
}
|
||
return PDR_FALSE;
|
||
}
|
||
|
||
/**---------------------------------------------------------------------
|
||
* Function : fv3Norm
|
||
* Description : 三维浮点数向量归一化
|
||
* Date : 2021/01/26 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void fv3Norm(float* vx, float* vy, float* vz)
|
||
{
|
||
float norm = sqrtf(((*vx) * (*vx) + (*vy) * (*vy) +
|
||
(*vz) * (*vz)));
|
||
// 防止出现模为0的情况
|
||
if (norm > FLT_THRES) {
|
||
norm = 1 / norm;
|
||
(*vx) *= norm; (*vy) *= norm; (*vz) *= norm;
|
||
}
|
||
}
|