PDR/1.Software/PDR 1.06/src/AHRS.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 <EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <math.h>
#include <string.h>
#include "PDRBase.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 FLT_THRES                           0.000001f                // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>ֵ
#define ENABLE_MAG                          0
/* Global Variable Definition ------------------------------------------------*/
AHRS_t Ahrs;

/**----------------------------------------------------------------------
* 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) {

    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 = asinf(-2.0f * (q[3]*q[1] - q[0]*q[2])) * (180.0f / 3.141592f);
    Ahrs.Yaw   = atan2f(q[2]*q[1] + q[0]*q[3], 0.5f - q[2]*q[2] - q[3]*q[3]) * (180.0f / 3.141592f);
    Ahrs.Roll  = atan2f(q[2]*q[3] + q[0]*q[1], 0.5f - q[2]*q[2] - q[1]*q[1]) * (180.0f / 3.141592f);
}

void AHRS_Init(void) {
    memset(&Ahrs, 0, sizeof(Ahrs));
    Ahrs.Kp   = AHRS_KP;
    Ahrs.Dt   = 1.0f / AHRS_SAMPLE_FREQ;
    Ahrs.q[0] = 1.0f;
}

/**----------------------------------------------------------------------
* Function    : UpdateAHRS
* Description : AHRS<EFBFBD>ںϽ<EFBFBD><EFBFBD><EFBFBD>
* Date        : 2022/10/18 logzhan
*---------------------------------------------------------------------**/
int UpdateAHRS(IMU_t* imu) {

    // logzhan 21/02/06 : <20>о<EFBFBD><D0BE><EFBFBD>acc<63><63><EFBFBD>뾫<EFBFBD>Ȼ<EFBFBD><C8BB><EFBFBD>һЩ
	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;
}