Пример #1
0
/*=====================================================================================================*/
void Sensor_Read( u16 ReadFreg )
{
  u8 ReadBuf[20] = {0};

  static s8 ReadCount = 0;
  static s32 Baro_Buf[2] = {0}; // 沒加 static 資料會有問題

  MPU9150_Read(ReadBuf);

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

  if(ReadCount == 0) {
    MS5611_Read(Baro_Buf, MS5611_D1_OSR_4096);
    Baro.Temp   = (fp32)(Baro_Buf[0]*0.01f);
    Baro.Press  = (fp32)(Baro_Buf[1]*0.01f);
    Baro.Height = (fp32)((Baro_Buf[1]-101333)*9.5238f);
    ReadCount = (u16)(ReadFreg/MS5611_RespFreq_4096)+1;
  }
  ReadCount--;
}
Пример #2
0
void sensor_read()
{	// 讀取IMU數值

#if USE_MPU9150_and_MS5611
	uint8_t IMU_Buf[20] = {0};
	static uint8_t BaroCnt = 0;

	/* 500Hz, Read Sensor ( Accelerometer, Gyroscope, Magnetometer ) */
	MPU9150_Read(IMU_Buf);
	// 讀取加速規、陀螺儀、電子羅盤之值

	BaroCnt++;//100Hz, Read Barometer

	if (BaroCnt == SampleRateFreg / 100) {
		MS5611_Read(&Baro, MS5611_D1_OSR_4096);
		BaroCnt = 0;
	}// 每讀取前述三個感測器 "SampleRateFreg / 100" 次,讀取氣壓計一次
	
		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]);
		//從buffer讀取IMU之值	

		/* 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;
#endif

#if	USE_GY86
	uint8_t IMU_Buf[14] = {0};
	static uint8_t BaroCnt = 0;

	/* 500Hz, Read Sensor ( Accelerometer, Gyroscope, Magnetometer ) */
	MPU9150_Read(IMU_Buf);
	// 讀取加速規、陀螺儀、電子羅盤之值

	BaroCnt++;//100Hz, Read Barometer

	if (BaroCnt == SampleRateFreg / 100) {
		MS5611_Read(&Baro, MS5611_D1_OSR_4096);
		BaroCnt = 0;
	}// 每讀取前述三個感測器 "SampleRateFreg / 100" 次,讀取氣壓計一次
	
		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]);

		//從buffer讀取IMU之值	

		/* 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;

#endif




}
Пример #3
0
void correction_task()
{
	ErrorStatus sensor_correct = ERROR;

	while (sys_status == SYSTEM_UNINITIALIZED);

	while ( sensor_correct == ERROR ) {

		while (SensorMode != Mode_Algorithm) {
			uint8_t IMU_Buf[20] = {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;

			correct_sensor();

			vTaskDelay(2);
		}

		if( (AngE.Roll < 0.1) && (AngE.Pitch < 0.1) && (NumQ.q0<1)
		     && (NumQ.q1<1) && (NumQ.q2<1) && (NumQ.q3<1) ) {
			sensor_correct = SUCCESS ; 
		}
		else {
			SensorMode = Mode_GyrCorrect;
			sensor_correct = ERROR ;
		}

		LED_G = ~LED_G ;
		vTaskDelay(200);
	}
	LED_G = 0;
	vTaskResume(FlightControl_Handle);
	vTaskDelete(NULL);
}
Пример #4
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);


		}

	}
}
Пример #5
0
/*=====================================================================================================*/
void SysTick_Handler( void )
{
    s16 IMU_Buf[10] = {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;

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

    /* 100Hz, Read Barometer */
    if((SysTick_Cnt%(SampleRateFreg/100)) == 0)
        MS5611_Read(&Baro, MS5611_D1_OSR_4096);

    /* Offset */
    Acc.X  = IMU_Buf[0] - Acc.OffsetX;
    Acc.Y  = IMU_Buf[1] - Acc.OffsetY;
    Acc.Z  = IMU_Buf[2] - Acc.OffsetZ;
    Gyr.X  = IMU_Buf[3] - Gyr.OffsetX;
    Gyr.Y  = IMU_Buf[4] - Gyr.OffsetY;
    Gyr.Z  = IMU_Buf[5] - Gyr.OffsetZ;
    Mag.X  = IMU_Buf[6] * Mag.AdjustX;
    Mag.Y  = IMU_Buf[7] * Mag.AdjustY;
    Mag.Z  = IMU_Buf[8] * Mag.AdjustZ;
    Temp.T = IMU_Buf[9];

#define MAFIFO_SIZE 250
    switch(SEN_STATE) {

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

    /************************** CorrectGyr **************************************/
    case SEN_GYR:
        LED_R = !LED_R;
        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:
        LED_R = !LED_R;
        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;
            break;
        }
        break;

    /************************** CorrectMag **************************************/
    case SEN_MAG:
        LED_R = !LED_R;
        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], 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);
            Mag.X = (s16)MoveAve_WMA(Mag.X, FIFO_MAG[0], 16);
            Mag.Y = (s16)MoveAve_WMA(Mag.Y, FIFO_MAG[1], 16);
            Mag.Z = (s16)MoveAve_WMA(Mag.Z, FIFO_MAG[2], 16);
            Correction_Time++;
            break;
        case 1:

            /* 加權移動平均法 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);
            Mag.X = (s16)MoveAve_WMA(Mag.X, FIFO_MAG[0], 16);
            Mag.Y = (s16)MoveAve_WMA(Mag.Y, FIFO_MAG[1], 16);
            Mag.Z = (s16)MoveAve_WMA(Mag.Z, FIFO_MAG[2], 16);

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

//          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);

            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);
        Mag.X = (s16)MoveAve_WMA(Mag.X, FIFO_MAG[0], 16);
        Mag.Y = (s16)MoveAve_WMA(Mag.Y, FIFO_MAG[1], 16);
        Mag.Z = (s16)MoveAve_WMA(Mag.Z, FIFO_MAG[2], 16);

        /* 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();

        break;

    /************************** Error *******************************************/
    default:
        LED_R = 1;
        LED_G = 1;
        LED_B = 1;
        while(1) {
            LED_R = !LED_R;
            Delay_100ms(10);
        }
    }
}
Пример #6
0
/*=====================================================================================================*/
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;
  }
}