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