/*====================================================================================================*/
void SysTick_Handler( void )
{
  u16 MOTOR[4] = {0};
  s16 Pitch = 0, Roll = 0, Yaw = 0;

  static u16 SysTick_Cnt = 0;
  static s16 *FIFO_X, *FIFO_Y, *FIFO_Z;
  static s16 FIFO_ACC[3][16] = {0}, FIFO_GYR[3][16] = {0}/*, FIFO_MAG[3][16] = {0}*/;
  static u32 Correction_Time = 0;

  /* Time Count */
  if(SysTick_Cnt == SampleRateFreg) {
    SysTick_Cnt = 0;
    Time_Sec++;
    if(Time_Sec == 60) {	// 0~59
      Time_Sec = 0;
      Time_Min++;
      if(Time_Sec == 60)
        Time_Min = 0;
    }
  }
  SysTick_Cnt++;

  /* 500Hz, Read Accelerometer, Gyroscope, Magnetometer */
  Sensor_Read(SampleRateFreg);

  /* Offset */
  Acc.X -= Acc.OffsetX;
  Acc.Y -= Acc.OffsetY;
  Acc.Z -= Acc.OffsetZ;
  Gyr.X -= Gyr.OffsetX;
  Gyr.Y -= Gyr.OffsetY;
  Gyr.Z -= Gyr.OffsetZ;

  #define MAFIFO_SIZE 250
  switch(SEN_STATE) {

    /************************** CorrectSelect ***********************************/
    case SEN_CORR:
//      SEN_STATE = (KEY == KEY_ON) ? SEN_GYR : SEN_NUMQ;
      SEN_STATE = SEN_GYR;
      break;

    /************************** CorrectGyr **************************************/
    case SEN_GYR:
      switch((u16)(Correction_Time/SampleRateFreg)) {
        case 0:   // 分配記憶體給 MaveAve 使用
          FIFO_X = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
          FIFO_Y = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
          FIFO_Z = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
          memset(FIFO_X, 0, MAFIFO_SIZE*sizeof(s16));
          memset(FIFO_Y, 0, MAFIFO_SIZE*sizeof(s16));
          memset(FIFO_Z, 0, MAFIFO_SIZE*sizeof(s16));
          Correction_Time = SampleRateFreg;
          break;
        case 1:   // 等待 FIFO 填滿靜態資料
          /* 移動平均 Simple Moving Average */
          Gyr.X = (s16)MoveAve_SMA(Gyr.X, FIFO_X, MAFIFO_SIZE);
          Gyr.Y = (s16)MoveAve_SMA(Gyr.Y, FIFO_Y, MAFIFO_SIZE);
          Gyr.Z = (s16)MoveAve_SMA(Gyr.Z, FIFO_Z, MAFIFO_SIZE);
          Correction_Time++;
          break;
        case 2:   // 釋放記憶體 & 計算陀螺儀偏移量
          free(FIFO_X);
          free(FIFO_Y);
          free(FIFO_Z);
          Gyr.OffsetX += (Gyr.X - GYR_X_OFFSET);  // 角速度為 0dps
          Gyr.OffsetY += (Gyr.Y - GYR_Y_OFFSET);  // 角速度為 0dps
          Gyr.OffsetZ += (Gyr.Z - GYR_Z_OFFSET);  // 角速度為 0dps
          Correction_Time = 0;
          SEN_STATE = SEN_ACC;
          break;
      }
      break;

    /************************** CorrectAcc **************************************/
    case SEN_ACC:
      switch((u16)(Correction_Time/SampleRateFreg)) {
        case 0:   // 分配記憶體給 MaveAve 使用
          FIFO_X = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
          FIFO_Y = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
          FIFO_Z = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
          memset(FIFO_X, 0, MAFIFO_SIZE*sizeof(s16));
          memset(FIFO_Y, 0, MAFIFO_SIZE*sizeof(s16));
          memset(FIFO_Z, 0, MAFIFO_SIZE*sizeof(s16));
          Correction_Time = SampleRateFreg;
          break;
        case 1:   // 等待 FIFO 填滿靜態資料
          /* 移動平均 Simple Moving Average */
          Acc.X = (s16)MoveAve_SMA(Acc.X, FIFO_X, MAFIFO_SIZE);
          Acc.Y = (s16)MoveAve_SMA(Acc.Y, FIFO_Y, MAFIFO_SIZE);
          Acc.Z = (s16)MoveAve_SMA(Acc.Z, FIFO_Z, MAFIFO_SIZE);
          Correction_Time++;
          break;
        case 2:   // 釋放記憶體 & 計算加速度計偏移量
          free(FIFO_X);
          free(FIFO_Y);
          free(FIFO_Z);
          Acc.OffsetX += (Acc.X - ACC_X_OFFSET);  // 重力加速度為 0g
          Acc.OffsetY += (Acc.Y - ACC_Y_OFFSET);  // 重力加速度為 0g
          Acc.OffsetZ += (Acc.Z - ACC_Z_OFFSET);  // 重力加速度為 1g
          Correction_Time = 0;

//          SEN_STATE = SEN_MAG;
          SEN_STATE = SEN_NUMQ;
          break;
      }
      break;

    /************************** CorrectMag **************************************/
    case SEN_MAG:
      SEN_STATE = SEN_NUMQ;
      break;

    /************************** Quaternion **************************************/
    case SEN_NUMQ:
      switch((u16)(Correction_Time/SampleRateFreg)) {
        case 0:   // 等待 FIFO 填滿靜態資料
          /* 加權移動平均法 Weighted Moving Average */
          Acc.X = (s16)MoveAve_WMA(Acc.X, FIFO_ACC[0], 16);
          Acc.Y = (s16)MoveAve_WMA(Acc.Y, FIFO_ACC[1], 16);
          Acc.Z = (s16)MoveAve_WMA(Acc.Z, FIFO_ACC[2], 16);
          Gyr.X = (s16)MoveAve_WMA(Gyr.X, FIFO_GYR[0], 16);
          Gyr.Y = (s16)MoveAve_WMA(Gyr.Y, FIFO_GYR[1], 16);
          Gyr.Z = (s16)MoveAve_WMA(Gyr.Z, FIFO_GYR[2], 16);

          Correction_Time++;
          break;
        case 1:

          /* 加權移動平均法 Weighted Moving Average */
          Acc.X = (s16)MoveAve_WMA(Acc.X, FIFO_ACC[0], 16);
          Acc.Y = (s16)MoveAve_WMA(Acc.Y, FIFO_ACC[1], 16);
          Acc.Z = (s16)MoveAve_WMA(Acc.Z, FIFO_ACC[2], 16);
          Gyr.X = (s16)MoveAve_WMA(Gyr.X, FIFO_GYR[0], 16);
          Gyr.Y = (s16)MoveAve_WMA(Gyr.Y, FIFO_GYR[1], 16);
          Gyr.Z = (s16)MoveAve_WMA(Gyr.Z, FIFO_GYR[2], 16);

          /* To Physical */
          Acc.TrueX  = Acc.X*MPU9250A_4g;       // g/LSB
          Acc.TrueY  = Acc.Y*MPU9250A_4g;       // g/LSB
          Acc.TrueZ  = Acc.Z*MPU9250A_4g;       // g/LSB
          Gyr.TrueX  = Gyr.X*MPU9250G_2000dps;  // dps/LSB
          Gyr.TrueY  = Gyr.Y*MPU9250G_2000dps;  // dps/LSB
          Gyr.TrueZ  = Gyr.Z*MPU9250G_2000dps;  // dps/LSB

          AngE.Pitch = toDeg(atan2f(Acc.TrueY, Acc.TrueZ));
          AngE.Roll  = toDeg(-asinf(Acc.TrueX));
//          AngE.Yaw   = toDeg(atan2f(Ellipse[3], Ellipse[4]))+180.0f;

          Quaternion_ToNumQ(&NumQ, &AngE);

          Correction_Time = 0;
          SEN_STATE = SEN_ALG;
          break;
      }
      break;

    /************************** Algorithm ***************************************/
    case SEN_ALG:

      /* 加權移動平均法 Weighted Moving Average */
      Acc.X = (s16)MoveAve_WMA(Acc.X, FIFO_ACC[0], 8);
      Acc.Y = (s16)MoveAve_WMA(Acc.Y, FIFO_ACC[1], 8);
      Acc.Z = (s16)MoveAve_WMA(Acc.Z, FIFO_ACC[2], 8);
      Gyr.X = (s16)MoveAve_WMA(Gyr.X, FIFO_GYR[0], 8);
      Gyr.Y = (s16)MoveAve_WMA(Gyr.Y, FIFO_GYR[1], 8);
      Gyr.Z = (s16)MoveAve_WMA(Gyr.Z, FIFO_GYR[2], 8);

      /* To Physical */
      Acc.TrueX = Acc.X*MPU9250A_4g;       // g/LSB
      Acc.TrueY = Acc.Y*MPU9250A_4g;       // g/LSB
      Acc.TrueZ = Acc.Z*MPU9250A_4g;       // g/LSB
      Gyr.TrueX = Gyr.X*MPU9250G_2000dps;  // dps/LSB
      Gyr.TrueY = Gyr.Y*MPU9250G_2000dps;  // dps/LSB
      Gyr.TrueZ = Gyr.Z*MPU9250G_2000dps;  // dps/LSB

      /* Get Attitude */
      AHRS_Update();

      if((KEY_LL == 1) && (KEY_RR == 1))  {	PID_Roll.Kp += 0.001f;	PID_Pitch.Kp += 0.001f;  }
      if((KEY_LL == 1) && (KEY_RP == 1))  {	PID_Roll.Kp -= 0.001f;	PID_Pitch.Kp -= 0.001f;  }
//      if((KEY_LL == 1) && (KEY_RR == 1))  {	PID_Roll.Ki += 0.0001f;	PID_Pitch.Ki += 0.0001f; }
//      if((KEY_LL == 1) && (KEY_RR == 1))  {	PID_Roll.Ki -= 0.0001f;	PID_Pitch.Ki -= 0.0001f; }
      if((KEY_LR == 1) && (KEY_RR == 1))  {	PID_Roll.Kd += 0.0001f;	PID_Pitch.Kd += 0.0001f; }
      if((KEY_LR == 1) && (KEY_RP == 1))  {	PID_Roll.Kd -= 0.0001f;	PID_Pitch.Kd -= 0.0001f; }
//      if((KEY_RR == 1) && (KEY_RR == 1))  {	PID_Yaw.Kd += 0.0001f; }
//      if((KEY_RR == 1) && (KEY_RR == 1))  {	PID_Yaw.Kd -= 0.0001f; }

      /* Get ZeroErr */
      PID_Pitch.ZeroErr = (fp32)((s16)EXP_PITCH/4.5f);
      PID_Roll.ZeroErr  = (fp32)((s16)EXP_ROLL/4.5f);
      PID_Yaw.ZeroErr   = (fp32)((s16)EXP_YAW)+180.0f;

      /* PID */
      Roll  = (s16)PID_AHRS_Cal(&PID_Roll,  AngE.Roll,  Gyr.TrueX);
      Pitch = (s16)PID_AHRS_Cal(&PID_Pitch, AngE.Pitch, Gyr.TrueY);
      Yaw   = (s16)(PID_Yaw.Kd*Gyr.TrueZ);

//      Roll  = 0;
//      Pitch = 0;
      Yaw   = 0;

      /* Motor Ctrl */
      MOTOR[0] = BasicThr + Pitch + Roll + Yaw;
      MOTOR[1] = BasicThr - Pitch + Roll - Yaw;
      MOTOR[2] = BasicThr - Pitch - Roll + Yaw;
      MOTOR[3] = BasicThr + Pitch - Roll - Yaw;

      /* Check Connection */
      #define NoSignal 1  // 1 sec
      if(KEY_RL == 1) {
        // Close Thr
        PID_Pitch.SumErr = 0.0f;
        PID_Roll.SumErr  = 0.0f;
        PID_Yaw.SumErr   = 0.0f;
        Ctrl_MotorTHR(MOTOR_THR_MIN, MOTOR_THR_MIN, MOTOR_THR_MIN, MOTOR_THR_MIN);
      }
      else if(((Time_Sec-RF_RecvData.Time.Sec)>NoSignal) || ((Time_Sec-RF_RecvData.Time.Sec)<-NoSignal))
        Ctrl_MotorTHR(MOTOR_THR_MIN, MOTOR_THR_MIN, MOTOR_THR_MIN, MOTOR_THR_MIN);
      else {
        // Thr Ctrl
        Ctrl_MotorTHR(MOTOR[0], MOTOR[1], MOTOR[2], MOTOR[3]);
      }
      break;

    /************************** Error *******************************************/
    default:
      while(1);
  }
}
Example #2
0
void flightControl_task()
{
	//Waiting for system finish initialize
	while (sys_status == SYSTEM_UNINITIALIZED);
	sys_status = SYSTEM_FLIGHT_CONTROL;
	while (1) {
		GPIO_ToggleBits(GPIOC, GPIO_Pin_7);
		uint8_t IMU_Buf[20] = {0};

		int16_t Final_M1 = 0;
		int16_t Final_M2 = 0;
		int16_t Final_M3 = 0;
		int16_t Final_M4 = 0;

		int16_t Thr = 0, Pitch = 0, Roll = 0, Yaw = 0;
		int16_t Exp_Thr = 0, Exp_Pitch = 0, Exp_Roll = 0, Exp_Yaw = 0;
		uint8_t safety = 0;


		static uint8_t BaroCnt = 0;



		/* 500Hz, Read Sensor ( Accelerometer, Gyroscope, Magnetometer ) */
		MPU9150_Read(IMU_Buf);

		/* 100Hz, Read Barometer */
		BaroCnt++;

		if (BaroCnt == SampleRateFreg / 100) {
			MS5611_Read(&Baro, MS5611_D1_OSR_4096);
			BaroCnt = 0;
		}

		Acc.X  = (s16)((IMU_Buf[0]  << 8) | IMU_Buf[1]);
		Acc.Y  = (s16)((IMU_Buf[2]  << 8) | IMU_Buf[3]);
		Acc.Z  = (s16)((IMU_Buf[4]  << 8) | IMU_Buf[5]);
		Temp.T = (s16)((IMU_Buf[6]  << 8) | IMU_Buf[7]);
		Gyr.X  = (s16)((IMU_Buf[8]  << 8) | IMU_Buf[9]);
		Gyr.Y  = (s16)((IMU_Buf[10] << 8) | IMU_Buf[11]);
		Gyr.Z  = (s16)((IMU_Buf[12] << 8) | IMU_Buf[13]);
		Mag.X  = (s16)((IMU_Buf[15] << 8) | IMU_Buf[14]);
		Mag.Y  = (s16)((IMU_Buf[17] << 8) | IMU_Buf[16]);
		Mag.Z  = (s16)((IMU_Buf[19] << 8) | IMU_Buf[18]);

		/* Offset */
		Acc.X -= Acc.OffsetX;
		Acc.Y -= Acc.OffsetY;
		Acc.Z -= Acc.OffsetZ;
		Gyr.X -= Gyr.OffsetX;
		Gyr.Y -= Gyr.OffsetY;
		Gyr.Z -= Gyr.OffsetZ;
		Mag.X *= Mag.AdjustX;
		Mag.Y *= Mag.AdjustY;
		Mag.Z *= Mag.AdjustZ;

		if (SensorMode == Mode_Algorithm) {

			/* 加權移動平均法 Weighted Moving Average */
			Acc.X = (s16)MoveAve_WMA(Acc.X, ACC_FIFO[0], 8);
			Acc.Y = (s16)MoveAve_WMA(Acc.Y, ACC_FIFO[1], 8);
			Acc.Z = (s16)MoveAve_WMA(Acc.Z, ACC_FIFO[2], 8);
			Gyr.X = (s16)MoveAve_WMA(Gyr.X, GYR_FIFO[0], 8);
			Gyr.Y = (s16)MoveAve_WMA(Gyr.Y, GYR_FIFO[1], 8);
			Gyr.Z = (s16)MoveAve_WMA(Gyr.Z, GYR_FIFO[2], 8);
			Mag.X = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], 64);
			Mag.Y = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], 64);
			Mag.Z = (s16)MoveAve_WMA(Mag.Z, MAG_FIFO[2], 64);

			/* To Physical */
			Acc.TrueX = Acc.X * MPU9150A_4g;      // g/LSB
			Acc.TrueY = Acc.Y * MPU9150A_4g;      // g/LSB
			Acc.TrueZ = Acc.Z * MPU9150A_4g;      // g/LSB
			Gyr.TrueX = Gyr.X * MPU9150G_2000dps; // dps/LSB
			Gyr.TrueY = Gyr.Y * MPU9150G_2000dps; // dps/LSB
			Gyr.TrueZ = Gyr.Z * MPU9150G_2000dps; // dps/LSB
			Mag.TrueX = Mag.X * MPU9150M_1200uT;  // uT/LSB
			Mag.TrueY = Mag.Y * MPU9150M_1200uT;  // uT/LSB
			Mag.TrueZ = Mag.Z * MPU9150M_1200uT;  // uT/LSB
			Temp.TrueT = Temp.T * MPU9150T_85degC; // degC/LSB

			/* Get Attitude Angle */
			AHRS_Update();
			global_var[TRUE_ROLL].param = AngE.Roll;
			global_var[TRUE_PITCH].param = AngE.Pitch;
			global_var[TRUE_YAW].param = AngE.Yaw;

			/*Get RC Control*/
			Update_RC_Control(&Exp_Roll, &Exp_Pitch, &Exp_Yaw, &Exp_Thr, &safety);
			global_var[RC_EXP_THR].param  = Exp_Thr;
			global_var[RC_EXP_ROLL].param = Exp_Roll;
			global_var[RC_EXP_PITCH].param = Exp_Pitch;
			global_var[RC_EXP_YAW].param = Exp_Yaw;
			/* Get ZeroErr */
			PID_Pitch.ZeroErr = (float)((s16)Exp_Pitch);
			PID_Roll.ZeroErr  = (float)((s16)Exp_Roll);
			PID_Yaw.ZeroErr   = (float)((s16)Exp_Yaw) + 180.0f;

			/* PID */
			Roll  = (s16)PID_AHRS_Cal(&PID_Roll,   AngE.Roll,  Gyr.TrueX);
			Pitch = (s16)PID_AHRS_Cal(&PID_Pitch,  AngE.Pitch, Gyr.TrueY);
//      Yaw   = (s16)PID_AHRS_CalYaw(&PID_Yaw, AngE.Yaw,   Gyr.TrueZ);
			Yaw   = (s16)(PID_Yaw.Kd * Gyr.TrueZ) + 3 * (s16)Exp_Yaw;
			Thr   = (s16)Exp_Thr;

			global_var[PID_ROLL].param = Roll;
			global_var[PID_PITCH].param = Pitch;
			global_var[PID_YAW].param = Yaw;


			/* Motor Ctrl */
			Final_M1 = Thr + Pitch - Roll - Yaw;
			Final_M2 = Thr + Pitch + Roll + Yaw;
			Final_M3 = Thr - Pitch + Roll - Yaw;
			Final_M4 = Thr - Pitch - Roll + Yaw;

			global_var[MOTOR1].param = Final_M1;
			global_var[MOTOR2].param = Final_M2;
			global_var[MOTOR3].param = Final_M3;
			global_var[MOTOR4].param = Final_M4;

			Bound(Final_M1, PWM_MOTOR_MIN, PWM_MOTOR_MAX);
			Bound(Final_M2, PWM_MOTOR_MIN, PWM_MOTOR_MAX);
			Bound(Final_M3, PWM_MOTOR_MIN, PWM_MOTOR_MAX);
			Bound(Final_M4, PWM_MOTOR_MIN, PWM_MOTOR_MAX);


			if (safety == 1) {
				Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);

			} else {
				Motor_Control(Final_M1, Final_M2, Final_M3, Final_M4);
			}

			vTaskDelay(2);


		}

	}
}
void AHRS_and_RC_updata(int16_t *Thr, int16_t *Pitch, int16_t *Roll, int16_t *Yaw, int16_t *safety)
{
	int16_t Exp_Thr = 0, Exp_Pitch = 0, Exp_Roll = 0, Exp_Yaw = 0;
	int16_t Safety = *safety ;


	/* 加權移動平均法 Weighted Moving Average */
	Acc.X = (s16)MoveAve_WMA(Acc.X, ACC_FIFO[0], 8);
	Acc.Y = (s16)MoveAve_WMA(Acc.Y, ACC_FIFO[1], 8);
	Acc.Z = (s16)MoveAve_WMA(Acc.Z, ACC_FIFO[2], 8);
	Gyr.X = (s16)MoveAve_WMA(Gyr.X, GYR_FIFO[0], 8);
	Gyr.Y = (s16)MoveAve_WMA(Gyr.Y, GYR_FIFO[1], 8);
	Gyr.Z = (s16)MoveAve_WMA(Gyr.Z, GYR_FIFO[2], 8);

#if USE_MPU9150_and_MS5611
	Mag.X = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], 64);
	Mag.Y = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], 64);
	Mag.Z = (s16)MoveAve_WMA(Mag.Z, MAG_FIFO[2], 64);
#endif
	/* To Physical */
	Acc.TrueX = Acc.X * MPU9150A_4g;      // g/LSB
	Acc.TrueY = Acc.Y * MPU9150A_4g;      // g/LSB
	Acc.TrueZ = Acc.Z * MPU9150A_4g;      // g/LSB
	Gyr.TrueX = Gyr.X * MPU9150G_2000dps; // dps/LSB
	Gyr.TrueY = Gyr.Y * MPU9150G_2000dps; // dps/LSB
	Gyr.TrueZ = Gyr.Z * MPU9150G_2000dps; // dps/LSB

#if USE_MPU9150_and_MS5611
	Mag.TrueX = Mag.X * MPU9150M_1200uT;  // uT/LSB
	Mag.TrueY = Mag.Y * MPU9150M_1200uT;  // uT/LSB
	Mag.TrueZ = Mag.Z * MPU9150M_1200uT;  // uT/LSB
	Temp.TrueT = Temp.T * MPU9150T_85degC; // degC/LSB
#endif
	system.variable[ACCX].value = Acc.TrueX;
	system.variable[ACCY].value = Acc.TrueY;
	system.variable[ACCZ].value = Acc.TrueZ;

	system.variable[GYROX].value = Gyr.TrueX;
	system.variable[GYROY].value = Gyr.TrueY;
	system.variable[GYROZ].value = Gyr.TrueZ;
	
	system.variable[MAGX].value = Mag.TrueX;
	system.variable[MAGY].value = Mag.TrueY;
	system.variable[MAGZ].value = Mag.TrueZ;

/* Get Attitude Angle */
	AHRS_Update();
	//計算四元數誤差以及將角度轉換為尤拉角,並使用互補式濾波器處理訊號
	system.variable[TRUE_ROLL].value = AngE.Roll;
	system.variable[TRUE_PITCH].value = AngE.Pitch;
	system.variable[TRUE_YAW].value = AngE.Yaw;

	/*Get RC Control*/
	Update_RC_Control(&Exp_Roll, &Exp_Pitch, &Exp_Yaw, &Exp_Thr, &Safety);
	//將輸入訊號依照遙控器型號不同分別處理,並計算出roll、pitch、yaw、throttle、safety訊號
	system.variable[RC_EXP_THR].value  = Exp_Thr;
	system.variable[RC_EXP_ROLL].value = Exp_Roll;
	system.variable[RC_EXP_PITCH].value = Exp_Pitch;
	system.variable[RC_EXP_YAW].value = Exp_Yaw;
	system.variable[TEST1].value = Safety;

	/* Get ZeroErr */
	PID_Pitch.ZeroErr = (float)((s16)Exp_Pitch);
	PID_Roll.ZeroErr = (float)((s16)Exp_Roll);
	PID_Yaw.ZeroErr = (float)((s16)Exp_Yaw) + 180.0f;

	/* PID */
	*Roll  = (s16)PID_AHRS_Cal(&PID_Roll,   AngE.Roll,  Gyr.TrueX);
	*Pitch = (s16)PID_AHRS_Cal(&PID_Pitch,  AngE.Pitch, Gyr.TrueY);
	*Yaw   = (s16)(PID_Yaw.Kd * Gyr.TrueZ) + 3 * (s16)Exp_Yaw;
	*Thr   = (s16)Exp_Thr;
	Bound(*Yaw, -90, 90);
   	system.variable[PID_ROLL].value = *Roll;
	system.variable[PID_PITCH].value = *Pitch;
	system.variable[PID_YAW].value = *Yaw;

	if (AngE.Yaw > 180.0f) 
		AngE.Yaw=AngE.Yaw-360;

	*safety = Safety;
}
/*=====================================================================================================*/
void SysTick_Handler( void )
{
  u8 IMU_Buf[20] = {0};

  u16 Final_M1 = 0;
  u16 Final_M2 = 0;
  u16 Final_M3 = 0;
  u16 Final_M4 = 0;

  s16 Thr = 0, Pitch = 0, Roll = 0, Yaw = 0;

  float Ellipse[5] = {0};

  static u8  BaroCnt = 0;

  static s16 ACC_FIFO[3][256] = {0};
  static s16 GYR_FIFO[3][256] = {0};
  static s16 MAG_FIFO[3][256] = {0};

  static s16 MagDataX[8] = {0};
  static s16 MagDataY[8] = {0};

  static u16 Correction_Time = 0;

  /* Time Count */
  SysTick_Cnt++;
  if(SysTick_Cnt == SampleRateFreg) {
    SysTick_Cnt = 0;
    Time_Sec++;
    if(Time_Sec == 60) {	// 0~59
      Time_Sec = 0;
      Time_Min++;
      if(Time_Sec == 60)
      Time_Min = 0;
    }
  }

  /* 400Hz, Read Sensor ( Accelerometer, Gyroscope, Magnetometer ) */
  MPU9150_Read(IMU_Buf);

  /* 100Hz, Read Barometer */
  BaroCnt++;
  if(BaroCnt == 4) {
    MS5611_Read(&Baro, MS5611_D1_OSR_4096);
    BaroCnt = 0;
  }

  Acc.X  = (s16)((IMU_Buf[0]  << 8) | IMU_Buf[1]);
  Acc.Y  = (s16)((IMU_Buf[2]  << 8) | IMU_Buf[3]);
  Acc.Z  = (s16)((IMU_Buf[4]  << 8) | IMU_Buf[5]);
  Temp.T = (s16)((IMU_Buf[6]  << 8) | IMU_Buf[7]);
  Gyr.X  = (s16)((IMU_Buf[8]  << 8) | IMU_Buf[9]);
  Gyr.Y  = (s16)((IMU_Buf[10] << 8) | IMU_Buf[11]);
  Gyr.Z  = (s16)((IMU_Buf[12] << 8) | IMU_Buf[13]);
  Mag.X  = (s16)((IMU_Buf[15] << 8) | IMU_Buf[14]);
  Mag.Y  = (s16)((IMU_Buf[17] << 8) | IMU_Buf[16]);
  Mag.Z  = (s16)((IMU_Buf[19] << 8) | IMU_Buf[18]);

  /* Offset */
  Acc.X -= Acc.OffsetX;
  Acc.Y -= Acc.OffsetY;
  Acc.Z -= Acc.OffsetZ;
  Gyr.X -= Gyr.OffsetX;
  Gyr.Y -= Gyr.OffsetY;
  Gyr.Z -= Gyr.OffsetZ;
  Mag.X *= Mag.AdjustX;
  Mag.Y *= Mag.AdjustY;
  Mag.Z *= Mag.AdjustZ;

  #define MovegAveFIFO_Size 250
  switch(SensorMode) {

  /************************** Mode_CorrectGyr **************************************/
    case Mode_GyrCorrect:
      LED_R = !LED_R;
      /* Simple Moving Average */
      Gyr.X = (s16)MoveAve_SMA(Gyr.X, GYR_FIFO[0], MovegAveFIFO_Size);
      Gyr.Y = (s16)MoveAve_SMA(Gyr.Y, GYR_FIFO[1], MovegAveFIFO_Size);
      Gyr.Z = (s16)MoveAve_SMA(Gyr.Z, GYR_FIFO[2], MovegAveFIFO_Size);

      Correction_Time++;  // 等待 FIFO 填滿空值 or 填滿靜態資料
      if(Correction_Time == 400) {
        Gyr.OffsetX += (Gyr.X - GYR_X_OFFSET);  // 角速度為 0dps
        Gyr.OffsetY += (Gyr.Y - GYR_Y_OFFSET);  // 角速度為 0dps
        Gyr.OffsetZ += (Gyr.Z - GYR_Z_OFFSET);  // 角速度為 0dps

        Correction_Time = 0;
        SensorMode = Mode_MagCorrect; // Mode_AccCorrect;
      }
      break;
 
  /************************** Mode_CorrectAcc **************************************/
    case Mode_AccCorrect:
      LED_R = ~LED_R;
      /* Simple Moving Average */
      Acc.X = (s16)MoveAve_SMA(Acc.X, ACC_FIFO[0], MovegAveFIFO_Size);
      Acc.Y = (s16)MoveAve_SMA(Acc.Y, ACC_FIFO[1], MovegAveFIFO_Size);
      Acc.Z = (s16)MoveAve_SMA(Acc.Z, ACC_FIFO[2], MovegAveFIFO_Size);

      Correction_Time++;  // 等待 FIFO 填滿空值 or 填滿靜態資料
      if(Correction_Time == 400) {
        Acc.OffsetX += (Acc.X - ACC_X_OFFSET);  // 重力加速度為 0g
        Acc.OffsetY += (Acc.Y - ACC_Y_OFFSET);  // 重力加速度為 0g
        Acc.OffsetZ += (Acc.Z - ACC_Z_OFFSET);  // 重力加速度為 1g

        Correction_Time = 0;
        SensorMode = Mode_Quaternion; // Mode_MagCorrect;
      }
      break;

  /************************** Mode_CorrectMag **************************************/
    #define MagCorrect_Ave    100
    #define MagCorrect_Delay  600   // 2.5ms * 600 = 1.5s
    case Mode_MagCorrect:
      LED_R = !LED_R;
      Correction_Time++;
      switch((u16)(Correction_Time/MagCorrect_Delay)) {
        case 0:
          LED_B = 0;
          MagDataX[0] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
          MagDataY[0] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
          break;
        case 1:
          LED_B = 1;
          MagDataX[1] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
          MagDataY[1] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
          break;
        case 2:
          LED_B = 0;
          MagDataX[2] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
          MagDataY[2] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
          break;
        case 3:
          LED_B = 1;
          MagDataX[3] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
          MagDataY[3] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
          break;
        case 4:
          LED_B = 0;
          MagDataX[4] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
          MagDataY[4] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
          break;
        case 5:
          LED_B = 1;
          MagDataX[5] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
          MagDataY[5] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
          break;
        case 6:
          LED_B = 0;
          MagDataX[6] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
          MagDataY[6] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
          break;
        case 7:
          LED_B = 1;
          MagDataX[7] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
          MagDataY[7] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
          break;
        default:
          LED_B = 1;
          EllipseFitting(Ellipse, MagDataX, MagDataY, 8);
          Mag.EllipseSita = Ellipse[0];
          Mag.EllipseX0   = Ellipse[1];
          Mag.EllipseY0   = Ellipse[2];
          Mag.EllipseA    = Ellipse[3];
          Mag.EllipseB    = Ellipse[4];

          Correction_Time = 0;
          SensorMode = Mode_Quaternion;
          break;
      }
      break;

  /************************** Algorithm Mode **************************************/
    case Mode_Quaternion:
      LED_R = !LED_R;

      /* To Physical */
      Acc.TrueX = Acc.X*MPU9150A_4g;        // g/LSB
      Acc.TrueY = Acc.Y*MPU9150A_4g;        // g/LSB
      Acc.TrueZ = Acc.Z*MPU9150A_4g;        // g/LSB
      Gyr.TrueX = Gyr.X*MPU9150G_2000dps;   // dps/LSB
      Gyr.TrueY = Gyr.Y*MPU9150G_2000dps;   // dps/LSB
      Gyr.TrueZ = Gyr.Z*MPU9150G_2000dps;   // dps/LSB
      Mag.TrueX = Mag.X*MPU9150M_1200uT;    // uT/LSB
      Mag.TrueY = Mag.Y*MPU9150M_1200uT;    // uT/LSB
      Mag.TrueZ = Mag.Z*MPU9150M_1200uT;    // uT/LSB
      Temp.TrueT = Temp.T*MPU9150T_85degC;  // degC/LSB

      Ellipse[3] = ( Mag.X*arm_cos_f32(Mag.EllipseSita)+Mag.Y*arm_sin_f32(Mag.EllipseSita))/Mag.EllipseB;
      Ellipse[4] = (-Mag.X*arm_sin_f32(Mag.EllipseSita)+Mag.Y*arm_cos_f32(Mag.EllipseSita))/Mag.EllipseA;

      AngE.Pitch = toDeg(atan2f(Acc.TrueY, Acc.TrueZ));
      AngE.Roll  = toDeg(-asinf(Acc.TrueX));
      AngE.Yaw   = toDeg(atan2f(Ellipse[3], Ellipse[4]))+180.0f;

      Quaternion_ToNumQ(&NumQ, &AngE);

      SensorMode = Mode_Algorithm;
      break;

  /************************** Algorithm Mode ****************************************/
    case Mode_Algorithm:

      /* 加權移動平均法 Weighted Moving Average */
      Acc.X = (s16)MoveAve_WMA(Acc.X, ACC_FIFO[0], 8);
      Acc.Y = (s16)MoveAve_WMA(Acc.Y, ACC_FIFO[1], 8);
      Acc.Z = (s16)MoveAve_WMA(Acc.Z, ACC_FIFO[2], 8);
      Gyr.X = (s16)MoveAve_WMA(Gyr.X, GYR_FIFO[0], 8);
      Gyr.Y = (s16)MoveAve_WMA(Gyr.Y, GYR_FIFO[1], 8);
      Gyr.Z = (s16)MoveAve_WMA(Gyr.Z, GYR_FIFO[2], 8);
      Mag.X = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], 64);
      Mag.Y = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], 64);
      Mag.Z = (s16)MoveAve_WMA(Mag.Z, MAG_FIFO[2], 64);

      /* To Physical */
      Acc.TrueX = Acc.X*MPU9150A_4g;        // g/LSB
      Acc.TrueY = Acc.Y*MPU9150A_4g;        // g/LSB
      Acc.TrueZ = Acc.Z*MPU9150A_4g;        // g/LSB
      Gyr.TrueX = Gyr.X*MPU9150G_2000dps;   // dps/LSB
      Gyr.TrueY = Gyr.Y*MPU9150G_2000dps;   // dps/LSB
      Gyr.TrueZ = Gyr.Z*MPU9150G_2000dps;   // dps/LSB
      Mag.TrueX = Mag.X*MPU9150M_1200uT;    // uT/LSB
      Mag.TrueY = Mag.Y*MPU9150M_1200uT;    // uT/LSB
      Mag.TrueZ = Mag.Z*MPU9150M_1200uT;    // uT/LSB
      Temp.TrueT = Temp.T*MPU9150T_85degC;  // degC/LSB

      /* Get Attitude Angle */
      AHRS_Update();

//      if(KEYL_U == 0)	{	PID_Roll.Kp += 0.001f;	PID_Pitch.Kp += 0.001f;  }
//      if(KEYL_L == 0)	{	PID_Roll.Kp -= 0.001f;	PID_Pitch.Kp -= 0.001f;  }
//      if(KEYL_R == 0)	{	PID_Roll.Ki += 0.0001f;	PID_Pitch.Ki += 0.0001f; }
//      if(KEYL_D == 0)	{	PID_Roll.Ki -= 0.0001f;	PID_Pitch.Ki -= 0.0001f; }
//      if(KEYR_R == 0)	{	PID_Roll.Kd += 0.0001f;	PID_Pitch.Kd += 0.0001f; }
//      if(KEYR_D == 0)	{	PID_Roll.Kd -= 0.0001f;	PID_Pitch.Kd -= 0.0001f; }
//      if(KEYR_L == 0)	{	PID_Roll.SumErr = 0.0f;	PID_Pitch.SumErr = 0.0f; }
//      if(KEYL_U == 0)	{	PID_Yaw.Kp += 0.001f;    }
//      if(KEYL_L == 0)	{	PID_Yaw.Kp -= 0.001f;    }
//      if(KEYL_R == 0)	{	PID_Yaw.Ki += 0.001f;	 }
//      if(KEYL_D == 0)	{	PID_Yaw.Ki -= 0.001f;	 }
//      if(KEYR_R == 0)	{	PID_Yaw.Kd += 0.0001f;	 }
//      if(KEYR_D == 0)	{	PID_Yaw.Kd -= 0.0001f;	 }
//      if(KEYR_L == 0)	{	PID_Roll.SumErr = 0.0f;	 }

      /* Get ZeroErr */
      PID_Pitch.ZeroErr = (float)((s16)Exp_Pitch/4.5f);
      PID_Roll.ZeroErr  = (float)((s16)Exp_Roll/4.5f);
      PID_Yaw.ZeroErr   = (float)((s16)Exp_Yaw)+180.0f;

      /* PID */
      Roll  = (s16)PID_AHRS_Cal(&PID_Roll,   AngE.Roll,  Gyr.TrueX);
      Pitch = (s16)PID_AHRS_Cal(&PID_Pitch,  AngE.Pitch, Gyr.TrueY);
//      Yaw   = (s16)PID_AHRS_CalYaw(&PID_Yaw, AngE.Yaw,   Gyr.TrueZ);
      Yaw   = (s16)(PID_Yaw.Kd*Gyr.TrueZ);
      Thr   = (s16)Exp_Thr;

      /* Motor Ctrl */
      Final_M1 = PWM_M1 + Thr + Pitch + Roll + Yaw;
      Final_M2 = PWM_M2 + Thr - Pitch + Roll - Yaw;
      Final_M3 = PWM_M3 + Thr - Pitch - Roll + Yaw;
      Final_M4 = PWM_M4 + Thr + Pitch - Roll - Yaw;

      /* Check Connection */
      #define NoSignal 1  // 1 sec
      if(KEYR_L == 0)
        Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
      else if(((Time_Sec-RecvTime_Sec)>NoSignal) || ((Time_Sec-RecvTime_Sec)<-NoSignal))
        Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
      else
        Motor_Control(Final_M1, Final_M2, Final_M3, Final_M4);

      /* DeBug */
      Tmp_PID_KP = PID_Yaw.Kp*1000;
      Tmp_PID_KI = PID_Yaw.Ki*1000;
      Tmp_PID_KD = PID_Yaw.Kd*1000;
      Tmp_PID_Pitch = Yaw;

      break;
  }
}