void IMU_Comput(SixAxis cache) {
static float g_q0 = 1, g_q1 = 0, g_q2 = 0, g_q3 = 0; //Quaternion
static float g_exInt = 0, g_eyInt = 0, g_ezInt = 0;
float norm; //模
float vx, vy, vz;
float ex, ey, ez;
norm = _sqrt(cache.aX*cache.aX + cache.aY*cache.aY + cache.aZ*cache.aZ); //取模
//向量化
cache.aX = cache.aX / norm;
cache.aY = cache.aY / norm;
cache.aZ = cache.aZ / norm;
//估计方向的重力
vx = 2 * (g_q1 * g_q3 - g_q0 * g_q2);
vy = 2 * (g_q0 * g_q1 + g_q2 * g_q3);
vz = g_q0*g_q0 - g_q1*g_q1 - g_q2*g_q2 + g_q3*g_q3;
//错误的领域和方向传感器测量参考方向几件的交叉乘积的总和
ex = (cache.aY * vz - cache.aZ * vy);
ey = (cache.aZ * vx - cache.aX * vz);
ez = (cache.aX * vy - cache.aY * vx);
//积分误差比例积分增益
g_exInt += ex * Ki;
g_eyInt += ey * Ki;
g_ezInt += ez * Ki;
//调整后的陀螺仪测量
cache.gX += Kp * ex + g_exInt;
cache.gY += Kp * ey + g_eyInt;
cache.gZ += Kp * ez + g_ezInt;
//整合四元数率和正常化
g_q0 += (-g_q1 * cache.gX - g_q2 * cache.gY - g_q3 * cache.gZ) * halfT;
g_q1 += (g_q0 * cache.gX + g_q2 * cache.gZ - g_q3 * cache.gY) * halfT;
g_q2 += (g_q0 * cache.gY - g_q1 * cache.gZ + g_q3 * cache.gX) * halfT;
g_q3 += (g_q0 * cache.gZ + g_q1 * cache.gY - g_q2 * cache.gX) * halfT;
//正常化四元
norm = _sqrt(g_q0*g_q0 + g_q1*g_q1 + g_q2*g_q2 + g_q3*g_q3);
g_q0 = g_q0 / norm;
g_q1 = g_q1 / norm;
g_q2 = g_q2 / norm;
g_q3 = g_q3 / norm;
g_Pitch = _asin(-2 * g_q1 * g_q3 + 2 * g_q0 * g_q2) * 57.3;
g_Roll = _atan2(2 * g_q2 * g_q3 + 2 * g_q0 * g_q1, -2 * g_q1*g_q1 - 2 * g_q2*g_q2 + 1) * 57.3;
g_Yaw = _atan2(2 * (g_q1 * g_q2 + g_q0 * g_q3), g_q0*g_q0 + g_q1*g_q1 - g_q2*g_q2 - g_q3*g_q3) * 57.3;
}
void GetEstimatedAttitude(imu_t* imu_ptr)
{
uint8_t axis;
int32_t accMag = 0;
float scale, deltaGyroAngle[3];
uint8_t validAcc;
static uint16_t previousT;
uint16_t currentT = micros();
scale = (currentT - previousT) * GYRO_SCALE; // GYRO_SCALE unit: radian/microsecond
previousT = currentT;
// Initialization
for (axis = 0; axis < 3; axis++)
{
deltaGyroAngle[axis] = imu_ptr->gyroADC[axis] * scale; // radian
accLPF32[axis] -= accLPF32[axis]>>ACC_LPF_FACTOR;
accLPF32[axis] += imu_ptr->accADC[axis];
imu_ptr->accSmooth[axis] = accLPF32[axis]>>ACC_LPF_FACTOR;
accMag += (int32_t)imu_ptr->accSmooth[axis]*imu_ptr->accSmooth[axis] ;
}
rotateV(&EstG.V,deltaGyroAngle);
rotateV(&EstM.V,deltaGyroAngle);
accMag = accMag*100/((int32_t)ACC_1G*ACC_1G);
validAcc = 72 < (uint16_t)accMag && (uint16_t)accMag < 133;
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
for (axis = 0; axis < 3; axis++)
{
if ( validAcc )
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + imu_ptr->accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
EstG32.A[axis] = EstG.A[axis]; //int32_t cross calculation is a little bit faster than float
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + imu_ptr->magADC[axis]) * INV_GYR_CMPFM_FACTOR;
EstM32.A[axis] = EstM.A[axis];
}
// Attitude of the estimated vector
int32_t sqGX_sqGZ = sq(EstG32.V.X) + sq(EstG32.V.Z);
invG = InvSqrt(sqGX_sqGZ + sq(EstG32.V.Y));
att.angle[ROLL] = _atan2(EstG32.V.X , EstG32.V.Z);
att.angle[PITCH] = _atan2(EstG32.V.Y , InvSqrt(sqGX_sqGZ)*sqGX_sqGZ);
att.heading = _atan2(
EstM32.V.Z * EstG32.V.X - EstM32.V.X * EstG32.V.Z,
(EstM.V.Y * sqGX_sqGZ - (EstM32.V.X * EstG32.V.X + EstM32.V.Z * EstG32.V.Z) * EstG.V.Y)*invG );
att.heading /= 10;
}
// ===========================================================================================
void decomposeR(mat33_t *Rz, const mat33_t *R)
{
real_t cl = _atan2(R->m[7],R->m[6]);
vec3_t rpy;
vec3_assign(&rpy,0,0,cl);
rpyMat(Rz,&rpy);
}
// ===========================================================================================
void rpyAng(vec3_t *angs, const mat33_t *R)
{
const real_t sinB = -(R->m[6]);
const real_t cosB = _sqrt(R->m[0]*R->m[0] + R->m[3]*R->m[3]);
if(_abs(cosB) > (real_t)(1E-15))
{
const real_t sinA = R->m[3] / cosB;
const real_t cosA = R->m[0] / cosB;
const real_t sinC = R->m[7] / cosB;
const real_t cosC = R->m[8] / cosB;
vec3_assign(angs,_atan2(sinC,cosC),_atan2(sinB,cosB),_atan2(sinA,cosA));
}
else
{
const real_t sinC = (R->m[1] - R->m[5]) * 0.5;
const real_t cosC = (R->m[4] - R->m[2]) * 0.5;
vec3_assign(angs,_atan2(sinC,cosC),CONST_PI_OVER_2, 0.0);
}
}
void angleCalculate()
{
/// Дальше идет суровая математика, в силу которой мы получаем тангаж, крен, а также курс,
/// ... Причем в расчете курса учавствует магнетометр.
// Attitude of the estimated vector
int32_t sqGZ = sq(EstG32.V.Z);
int32_t sqGX = sq(EstG32.V.X);
int32_t sqGY = sq(EstG32.V.Y);
int32_t sqGX_sqGZ = sqGX + sqGZ;
float invmagXZ = InvSqrt(sqGX_sqGZ);
invG = InvSqrt(sqGX_sqGZ + sqGY);
angle[ROLL] = _atan2(EstG32.V.X ,EstG32.V.Z);
angle[PITCH] = - _atan2(EstG32.V.Y , invmagXZ*sqGX_sqGZ);
heading = _atan2(
EstM32.V.Z * EstG32.V.X - EstM32.V.X * EstG32.V.Z,
EstM32.V.Y * invG * sqGX_sqGZ - (EstM32.V.X * EstG32.V.X + EstM32.V.Z * EstG32.V.Z) * invG * EstG32.V.Y );
heading = heading /10;
// vectorMultiply(&EstM32,&EstG32,&headvect);
// heading2=
// axisDekl=_atan2(EstG32.V.X , EstG32.V.Y);
// deklination=_atan(EstG32.V.Z * InvSqrt(sqGX+sqGY));
}
void getEstimatedAttitude(){
uint8_t axis;
int32_t accMag = 0;
#ifndef MAYHONY
static t_fp_vector EstG;
#if MAG
static t_fp_vector EstM;
#endif
#endif
#if defined(MG_LPF_FACTOR)
static int16_t mgSmooth[3];
#endif
#if defined(ACC_LPF_FACTOR)
static float accLPF[3];
#endif
static uint32_t previousT;
uint32_t currentT = micros();
#ifndef MAYHONY
float scale;
float deltaGyroAngle[3];
scale = (currentT - previousT) * GYRO_SCALE;
#else
float GyroRate[3];
samplePeriod = (currentT-previousT)/1000000.0f;
#endif
previousT = currentT;
// Initialization
for (axis = 0; axis < 3; axis++) {
#ifndef MAYHONY
deltaGyroAngle[axis] = gyroADC[axis] * scale;
#endif
#if defined(ACC_LPF_FACTOR)
accLPF[axis] = accLPF[axis] * (1.0f - (1.0f/ACC_LPF_FACTOR)) + accADC[axis] * (1.0f/ACC_LPF_FACTOR);
accSmooth[axis] = accLPF[axis];
#define ACC_VALUE accSmooth[axis]
#else
accSmooth[axis] = accADC[axis];
#define ACC_VALUE accADC[axis]
#endif
accMag += (ACC_VALUE * 10 / (int16_t)acc_1G) * (ACC_VALUE * 10 / (int16_t)acc_1G);
accMag += (int32_t)ACC_VALUE*ACC_VALUE ;
#if MAG
#if defined(MG_LPF_FACTOR)
mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR; // LPF for Magnetometer values
#define MAG_VALUE mgSmooth[axis]
#else
#define MAG_VALUE magADC[axis]
#endif
#endif
}
accMag = accMag*100/((int32_t)acc_1G*acc_1G);
#if defined(MAYHONY)
if(calibratingA == 0 && calibratingG == 0)
{
for(axis = 0; axis < 3; axis++)
{
GyroRate[axis] = gyroAHRS[axis]*GYRO_SCALE_MAYHONY;
}
#ifdef MAG
MayhonyAHRSupdate(GyroRate[Xaxis], GyroRate[Yaxis], GyroRate[Zaxis], (float)accSmooth[Xaxis], (float)accSmooth[Yaxis], (float)accSmooth[Zaxis], (float)magADC[Xaxis], (float)magADC[Yaxis], (float)magADC[Zaxis]);
#else
MayhonyAHRSupdateIMU(GyroRate[Xaxis], GyroRate[Yaxis], GyroRate[Zaxis], (float)accSmooth[Xaxis], (float)accSmooth[Yaxis], (float)accSmooth[Zaxis]);
#endif
}
#else
rotateV(&EstG.V,deltaGyroAngle);
#if MAG
rotateV(&EstM.V,deltaGyroAngle);
#endif
if ( fabs(accSmooth[ROLL])<acc_25deg && fabs(accSmooth[PITCH])<acc_25deg && accSmooth[YAW]>0) {
f.SMALL_ANGLES_25 = 1;
} else {
f.SMALL_ANGLES_25 = 0;
}
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
if ( ( 36 < accMag && accMag < 196 ) || f.SMALL_ANGLES_25 )
for (axis = 0; axis < 3; axis++) {
int16_t acc = ACC_VALUE;
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + acc) * INV_GYR_CMPF_FACTOR;
}
#if MAG
for (axis = 0; axis < 3; axis++)
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + MAG_VALUE) * INV_GYR_CMPFM_FACTOR;
#endif
// Attitude of the estimated vector
angle[ROLL] = _atan2(EstG.V.X , EstG.V.Z) ;
angle[PITCH] = _atan2(EstG.V.Y , EstG.V.Z) ;
#if MAG
// Attitude of the cross product vector GxM
heading = _atan2( EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X , EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z );
heading += MAG_DECLINIATION * 10; //add declination
heading = heading /10;
if ( heading > 180) heading = heading - 360;
else if (heading < -180) heading = heading + 360;
#endif
#endif//multiwii fusion
}
