logzhan
/
PDR
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
PDR/1.Software/Simualator/Src/AHRS.cpp

123 lines
4.8 KiB
C++
Raw Normal View History

update.
2023-12-02 22:46:06 +08:00
/******************** (C) COPYRIGHT 2022 Geek************************************
* File Name : AHRS.cpp
* Current Version : V1.0
* Author : logzhan
* Date of Issued : 2022.09.24
* Comments : PDR<EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <string.h>
#include <math.h>
#include "AHRS.h"
#include "Quaternion.h"
/* Macro Definition ----------------------------------------------------------*/
#define FLT_THRES 0.000001f // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>ֵ
#define ENABLE_MAG 0
/* Global Variable Definition ------------------------------------------------*/
AHRS_t Ahrs;
/**----------------------------------------------------------------------
* Function : AHRS_Init
* Description : AHRS<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>
* Date : 2022/09/21 logzhan
*---------------------------------------------------------------------**/
void AHRS_Init()
{
memset(&Ahrs, 0, sizeof(Ahrs));
Ahrs.Kp = AHRS_KP;
Ahrs.Dt = 1.0f / AHRS_SAMPLE_FREQ;
Ahrs.q[0] = 1.0f;
}
/**---------------------------------------------------------------------
* Function : Vec3Norm
* Description : <EFBFBD><EFBFBD>ά<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD>
* Date : 2022/11/1 logzhan
*---------------------------------------------------------------------**/
void Vec3Norm(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;
}
}
/**----------------------------------------------------------------------
* Function : MahonyUpdateAHRS
* Description : Mahony<EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨
* Date : 2022/09/21 logzhan
*---------------------------------------------------------------------**/
void MahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, float az,
float mx, float my, float mz)
{
float* q = Ahrs.q;
// <20><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƺͼ<C6BA><CDBC>ٶȼ<D9B6>
Vec3Norm(&ax, &ay, &az);
float wx = 0.0f, wy = 0.0f, wz = 0.0f;
#if ENABLE_MAG
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 - q[3] * q[3]) + my * (q[1] * q2 - q[0] * q[3]) + mz * (q[1] * q[3] + q[0] * q2));
float hy = 2.0f * (mx * (q[1] * q2 + q[0] * q[3]) + my * (0.5f - q[1] * q[1] - q[3] * q[3]) + mz * (q2 * q[3] - q[0] * q[1]));
float hz = 2.0f * (mx * (q[1] * q[3] - q[0] * q2) + my * (q2 * q[3] + q[0] * q[1]) + mz * (0.5f - q[1] * q[1] - 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 * (q[0] * q[3] + q[1] * q2) + bz * (q[1] * q[3] - q[0] * q2);
wy = by * (0.5f - q[1] * q[1] - q[3] * q[3]) + bz * (q[0] * q[1] + q2 * q[3]);
wz = by * (q2 * q[3] - q[0] * q[1]) + bz * (0.5f - q[1] * q[1] - q2 * q2);
}
#endif
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ v = R * (0,0,1)
float vx = q[1] * q[3] - q[0] * q[2];
float vy = q[0] * q[1] + q[2] * q[3];
float vz = q[0] * q[0] - 0.5f + q[3] * q[3];
// <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 += Ahrs.Kp * ex;
gy += Ahrs.Kp * ey;
gz += Ahrs.Kp * ez;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>
float qa = q[0]; float qb = q[1]; float qc = q[2];
q[0] += (-qb * gx - qc * gy - q[3] * gz) * 0.5f * (Ahrs.Dt);
q[1] += (qa * gx + qc * gz - q[3] * gy) * 0.5f * (Ahrs.Dt);
q[2] += (qa * gy - qb * gz + q[3] * gx) * 0.5f * (Ahrs.Dt);
q[3] += (qa * gz + qb * gy - qc * gx) * 0.5f * (Ahrs.Dt);
// <20><>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
QuaternionNorm(&q[0], &q[1], &q[2], &q[3]);
// <20><>Ԫ<EFBFBD><D4AA>תŷ<D7AA><C5B7><EFBFBD><EFBFBD>
Ahrs.Pitch = (float)asin(-2.0f * (q[3] * q[1] - q[0] * q[2])) * (180.0f / 3.141592f);
Ahrs.Yaw = (float)atan2(q[2] * q[1] + q[0] * q[3], 0.5f - q[2] * q[2] - q[3] * q[3]) * (180.0f / 3.141592f);
Ahrs.Roll = (float)atan2(q[2] * q[3] + q[0] * q[1], 0.5f - q[2] * q[2] - q[1] * q[1]) * (180.0f / 3.141592f);
}
/**----------------------------------------------------------------------
* Function : UpdateAHRS
* Description : AHRS<EFBFBD>ںϽ<EFBFBD><EFBFBD><EFBFBD>
* Date : 2022/09/23 logzhan
*---------------------------------------------------------------------**/
int UpdateAHRS(IMU_t* imu)
{
MahonyUpdateAHRS(imu->gyr.s[0], imu->gyr.s[1], imu->gyr.s[2],
imu->acc.s[0], imu->acc.s[1], imu->acc.s[2],
imu->mag.s[0], imu->mag.s[1], imu->mag.s[2]);
return PDR_TRUE;
}