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