#include "imu.h"



#define DCM_KP_ACC      0.400f    //加速度补偿陀螺仪PI参数
#define DCM_KI_ACC      0.001f

#define DCM_KP_MAG			1.000f		//磁力计补偿陀螺仪PI参数
#define DCM_KI_MAG			0.000f


#define IMU_SMALL_ANGLE    15.0f   //满足水平状态的最小角度（单位deg）

#define SPIN_RATE_LIMIT     20    //旋转速率


//float Kp = 0.4f;		/*比例增益*/
//float Ki = 0.001f;		/*积分增益*/
//float exInt = 0.0f;
//float eyInt = 0.0f;
//float ezInt = 0.0f;		/*积分误差累计*/


float imuAttitudeYaw;//范围：-180~180，用于上传到匿名上位机（支持范围-180~180）
static float q0 = 1.0f, q1 = 1.0f, q2 = 0.0f, q3 = 0.0f, q4 = 0.0f;//四元数
static float rMat[3][3];//四元数的旋转矩阵
static float smallAngleCosZ;//水平最小角余弦值


//旋转机体坐标系加速度到NEU坐标系
void imuTransformVectorBodyToEarth(Axis3f *v)
{
    const float x = rMat[0][0] * v->x + rMat[0][1] * v->y + rMat[0][2] * v->z;
    const float y = rMat[1][0] * v->x + rMat[1][1] * v->y + rMat[1][2] * v->z;
    const float z = rMat[2][0] * v->x + rMat[2][1] * v->y + rMat[2][2] * v->z;

    v->x = x;
    v->y = -y;//
    v->z = z;
}

//NEU坐标系加速度到NEU坐标系
void imuTransformVectorEarthToBody(Axis3f *v)
{
    v->y = -v->y;

    /* From earth frame to body frame */
    const float x = rMat[0][0] * v->x + rMat[1][0] * v->y + rMat[2][0] * v->z;
    const float y = rMat[0][1] * v->x + rMat[1][1] * v->y + rMat[2][1] * v->z;
    const float z = rMat[0][2] * v->x + rMat[1][2] * v->y + rMat[2][2] * v->z;

    v->x = x;
    v->y = y;
    v->z = z;
}


float degreesToRadians(int16_t degrees1)
{
    return degrees1 * RAD;
}


//计算四元数的旋转矩阵
static void imuComputeRotationMatrix(void)
{
    float q1q1 = q1 * q1;
    float q2q2 = q2 * q2;
    float q3q3 = q3 * q3;

    float q0q1 = q0 * q1;
    float q0q2 = q0 * q2;
    float q0q3 = q0 * q3;
    float q1q2 = q1 * q2;
    float q1q3 = q1 * q3;
    float q2q3 = q2 * q3;

    rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
    rMat[0][1] = 2.0f * (q1q2 + -q0q3);
    rMat[0][2] = 2.0f * (q1q3 - -q0q2);

    rMat[1][0] = 2.0f * (q1q2 - -q0q3);
    rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
    rMat[1][2] = 2.0f * (q2q3 + -q0q1);

    rMat[2][0] = 2.0f * (q1q3 + -q0q2);
    rMat[2][1] = 2.0f * (q2q3 - -q0q1);
    rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
}


void imuInit(void)
{

    smallAngleCosZ = cos_approx(degreesToRadians(IMU_SMALL_ANGLE));//最小倾角余弦值
    imuComputeRotationMatrix();//计算四元数的旋转矩阵
}

static float invSqrt(float x)
{
    return 1.0f / sqrtf(x);
}


//磁力计快速增益
static float imuMagFastPGainSaleFactor(void)
{
	//四轴上电后磁力计融合需要一段时间
	//为了快速融合，前100次使用快速增益
	static int32_t FastPGainCount = 100;
	if(FastPGainCount>0)//
	{
		FastPGainCount--;
		return 10.0f;	
	}
	else
		return 1.0f;
}


//快速增益
static float imuACCFastPGainSaleFactor(void)
{
	//四轴上电后ACC融合需要一段时间
	//为了快速融合，前100次使用快速增益
	static int32_t FastPGainCount = 100;
	if(FastPGainCount>0)//
	{
		FastPGainCount--;
		return 100.0f;	
	}
	else
		return 1.0f;
}



//计算四元数和旋转矩阵
void imuMahonyAHRSupdate(float gx, float gy, float gz,float ax, float ay, float az,float mx, float my, float mz,bool useMag,float dt)
{
  static float integralAccX = 0.0f,  integralAccY = 0.0f, integralAccZ = 0.0f;    //加速度积分误差
	static float integralMagX = 0.0f,  integralMagY = 0.0f, integralMagZ = 0.0f;    //磁力计积分误差
  float ex, ey, ez;

    //计算旋转速率(rad/s)
    const float spin_rate_sq = sq(gx) + sq(gy) + sq(gz);

    //Step 1: Yaw correction
    if (useMag) 
	  {
				const float magMagnitudeSq = mx * mx + my * my + mz * mz;
				float kpMag = DCM_KP_MAG * imuMagFastPGainSaleFactor();
				
				if (magMagnitudeSq > 0.01f) 
				{
					//单位化磁力计测量值
					const float magRecipNorm = invSqrt(magMagnitudeSq);
					mx *= magRecipNorm;
					my *= magRecipNorm;
					mz *= magRecipNorm;
				
//					printf("   mx:%f    ",mx);
//					printf("   my:%f    ",my);
//					printf("   mz:%f    ",mz);
//					printf("   magRecipNorm:%f    ",magRecipNorm);

//					printf(" \r\n ");						
					
					
					
					//计算X\Y方向的磁通，磁北方向磁通
					const float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
					const float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
					const float bx = sqrtf(hx * hx + hy * hy);

					//磁力计误差是估计磁北和测量磁北之间的交叉乘积
					const float ez_ef = -(hy * bx);

					//旋转误差到机体坐标系
					ex = rMat[2][0] * ez_ef;
					ey = rMat[2][1] * ez_ef;
					ez = rMat[2][2] * ez_ef;
				}
				else 
				{
					ex = 0;
					ey = 0;
					ez = 0;
				}

				//累计误差补偿
				if (DCM_KI_MAG > 0.0f) 
				{
					//如果旋转速率大于限制值则停止积分
					if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) 
					{
						integralMagX += DCM_KI_MAG * ex * dt;
						integralMagY += DCM_KI_MAG * ey * dt;
						integralMagZ += DCM_KI_MAG * ez * dt;

						gx += integralMagX;
						gy += integralMagY;
						gz += integralMagZ;
					}
				}	
				
				//误差补偿
				gx += kpMag * ex;
				gy += kpMag * ey;
				gz += kpMag * ez;
	}	
	
	
	
    //Step 2: Roll and pitch correction
  if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))//加速度计输出有效时,利用加速度计补偿陀螺仪
  {
    //单位化加速计测量值
    const float accRecipNorm = invSqrt(ax * ax + ay * ay + az * az);
    ax *= accRecipNorm;
    ay *= accRecipNorm;
    az *= accRecipNorm;

    //加速计读取的方向与重力加速计方向的差值，用向量叉乘计算
    ex = (ay * rMat[2][2] - az * rMat[2][1]);
    ey = (az * rMat[2][0] - ax * rMat[2][2]);
    ez = (ax * rMat[2][1] - ay * rMat[2][0]);

    //累计误差补偿
    if (DCM_KI_ACC > 0.0f) 
    {
      //如果旋转速率大于限制值则停止积分
      if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)))
      {
        /*误差累计，与积分常数相乘*/
        integralAccX += DCM_KI_ACC * ex * dt;
        integralAccY += DCM_KI_ACC * ey * dt;
        integralAccZ += DCM_KI_ACC * ez * dt;

        gx += integralAccX;
        gy += integralAccY;
        gz += integralAccZ;
      }
    }

    //误差补偿 用叉积误差来做PI修正陀螺零偏，即抵消陀螺读数中的偏移量
		float ak = imuACCFastPGainSaleFactor();
		//printf(" ak:%f ",ak);
    gx += DCM_KP_ACC * ex * ak;
    gy += DCM_KP_ACC * ey * ak;
    gz += DCM_KP_ACC * ez * ak;
		
  }
  
  //一阶近似算法，四元数运动学方程的离散化形式和积分
    gx *= (0.5f * dt);
    gy *= (0.5f * dt);
    gz *= (0.5f * dt);

    const float qa = q0;
    const float qb = q1;
    const float qc = q2;
    q0 += (-qb * gx - qc * gy - q3 * gz);
    q1 += (qa * gx + qc * gz - q3 * gy);
    q2 += (qa * gy - qb * gz + q3 * gx);
    q3 += (qa * gz + qb * gy - qc * gx);

  //单位化四元数
    const float quatRecipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= quatRecipNorm;
    q1 *= quatRecipNorm;
    q2 *= quatRecipNorm;
    q3 *= quatRecipNorm;

    //计算四元数的旋转矩阵
    imuComputeRotationMatrix();
}



//更新欧拉角
static void imuUpdateEulerAngles(attitude_t *attitude)
{
  attitude->roll = RADIANS_TO_DEGREES(atan2_approx(rMat[2][1], rMat[2][2]));
  attitude->pitch = RADIANS_TO_DEGREES((0.5f * M_PIf) - acos_approx(-rMat[2][0]));//arcsin = 0.5PI - arccos
  attitude->yaw = RADIANS_TO_DEGREES(atan2_approx(rMat[1][0], rMat[0][0]));

  imuAttitudeYaw = attitude->yaw;

  if (attitude->yaw < 0.0f)//转换位0~360
    attitude->yaw += 360.0f;

  //更新最小倾角状态
  if (rMat[2][2] > smallAngleCosZ) 
    ENABLE_STATE(SMALL_ANGLE);
  else 
    DISABLE_STATE(SMALL_ANGLE);
    
}


//四元数和欧拉角计算
void imuUpdateAttitude(const sensorData_t *sensorData, state_t *state, float dt)
{
  bool useMag = 0;//compassIsHealthy();	
	
	Axis3f gyro = sensorData->gyro;
	Axis3f acc  = sensorData->acc;
	Axis3f mag  = sensorData->mag;
   


	//角速度单位由度转为弧度
	gyro.x = gyro.x * DEG2RAD;
	gyro.y = gyro.y * DEG2RAD;
	gyro.z = gyro.z * DEG2RAD;

	
	//计算四元数和旋转矩阵
    imuMahonyAHRSupdate(gyro.x, gyro.y, gyro.z,
                        acc.x, acc.y, acc.z,
                        mag.x, mag.y, mag.z,
						useMag,dt);
	
    //计算欧拉角               
    imuUpdateEulerAngles(&state->attitude);
    
}


 
float beta = 0.2;//0.04 加速计和磁力计补偿因子，数值越大纠正越快，越不稳定
 
void MadgwickQuaternionUpdate(Axis3f acc, Axis3f gyro, Axis3f mag, attitude_t *attitude , float dt)
{
	float norm;
	float hx, hy, _2bx, _2bz;
	float s1, s2, s3, s4;
	float qDot1, qDot2, qDot3, qDot4;
 
	
	//提前计算好下面要用到的数值防止重复计算，减少运算量
	float _2q1mx;
	float _2q1my;
	float _2q1mz;
	float _2q2mx;
	float _4bx;
	float _4bz;
	float _2q1 = 2.0f * q1;
	float _2q2 = 2.0f * q2;
	float _2q3 = 2.0f * q3;
	float _2q4 = 2.0f * q4;
	float _2q1q3 = 2.0f * q1 * q3;
	float _2q3q4 = 2.0f * q3 * q4;
	float q1q1 = q1 * q1;
	float q1q2 = q1 * q2;
	float q1q3 = q1 * q3;
	float q1q4 = q1 * q4;
	float q2q2 = q2 * q2;
	float q2q3 = q2 * q3;
	float q2q4 = q2 * q4;
	float q3q3 = q3 * q3;
	float q3q4 = q3 * q4;
	float q4q4 = q4 * q4;
 
			
    //度转弧度
	gyro.x = gyro.x * DEG2RAD;	/* 度转弧度 */
	gyro.y = gyro.y * DEG2RAD;
	gyro.z = gyro.z * DEG2RAD;
 
	//单位化加速度向量
	norm = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
	if (norm == 0.0f) return; // handle NaN
	norm = 1.0f/norm;
	acc.x *= norm;
	acc.y *= norm;
	acc.z *= norm;
 
	//单位化磁力计向量
	norm = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
	if (norm == 0.0f) return; // handle NaN
	norm = 1.0f/norm;
	mag.x *= norm;
	mag.y *= norm;
	mag.z *= norm;
 
	//提前计算好下面要用到的值
	_2q1mx = 2.0f * q1 * mag.x;
	_2q1my = 2.0f * q1 * mag.y;
	_2q1mz = 2.0f * q1 * mag.z;
	_2q2mx = 2.0f * q2 * mag.x;
	//磁力计从机体到地球
	hx = mag.x * q1q1 - _2q1my * q4 + _2q1mz * q3 + mag.x * q2q2 + _2q2 * mag.y * q3 + _2q2 * mag.z * q4 - mag.x * q3q3 - mag.x * q4q4;
	hy = _2q1mx * q4 + mag.y * q1q1 - _2q1mz * q2 + _2q2mx * q3 - mag.y * q2q2 + mag.y * q3q3 + _2q3 * mag.z * q4 - mag.y * q4q4;
	/*让导航坐标系中X轴指向正北方*/
	_2bx = sqrtf(hx * hx + hy * hy);
	_2bz = -_2q1mx * q3 + _2q1my * q2 + mag.z * q1q1 + _2q2mx * q4 - mag.z * q2q2 + _2q3 * mag.y * q4 - mag.z * q3q3 + mag.z * q4q4;
	_4bx = 2.0f * _2bx;
	_4bz = 2.0f * _2bz;
 
	//梯度下降法计算纠正误差
	s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q2 * (2.0f * q1q2 + _2q3q4 - acc.y) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
	s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q1 * (2.0f * q1q2 + _2q3q4 - acc.y) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - acc.z) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
	s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q4 * (2.0f * q1q2 + _2q3q4 - acc.y) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - acc.z) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
	s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q3 * (2.0f * q1q2 + _2q3q4 - acc.y) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
    //单位化计算好的误差向量
	norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    
	norm = 1.0f/norm;
	s1 *= norm;
	s2 *= norm;
	s3 *= norm;
	s4 *= norm;
 
	// 将计算好的误差向量补偿到四元数
	qDot1 = 0.5f * (-q2 * gyro.x - q3 * gyro.y - q4 * gyro.z) - beta * s1;
	qDot2 = 0.5f * (q1 * gyro.x + q3 * gyro.z - q4 * gyro.y) - beta * s2;
	qDot3 = 0.5f * (q1 * gyro.y - q2 * gyro.z + q4 * gyro.x) - beta * s3;
	qDot4 = 0.5f * (q1 * gyro.z + q2 * gyro.y - q3 * gyro.x) - beta * s4;
 
	//更新四元数的值
	q1 += qDot1 * dt;
	q2 += qDot2 * dt;
	q3 += qDot3 * dt;
	q4 += qDot4 * dt;
	norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
	norm = 1.0f/norm;
	q1 = q1 * norm;
	q2 = q2 * norm;
	q3 = q3 * norm;
	q4 = q4 * norm;
	
    //四元数转换为欧拉角	
	attitude->yaw   = atan2(2.0f * (q2 * q3 + q1 * q4), q1 * q1 + q2 * q2 - q3 * q3 - q4 * q4) * RAD2DEG;   
	attitude->pitch = -asin(2.0f * (q2 * q4 - q1 * q3)) * RAD2DEG;
	attitude->roll  = atan2(2.0f * (q1 * q2 + q3 * q4), q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4) * RAD2DEG;
	
}