/******************** (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 #include #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; } }