/*====================================================================================================*/
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 SysTick_Handler( void )
{
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++;
/* 100Hz, Read Barometer */
/* 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;
Mag.X *= Mag.AdjustX;
Mag.Y *= Mag.AdjustY;
Mag.Z *= Mag.AdjustZ;
Temp.T -= Temp.OffsetT;
#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:
LED_G = !LED_G;
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 = LED_OFF;
LED_G = LED_OFF;
LED_B = LED_OFF;
while(1) {
LED_R = !LED_R;
Delay_100ms(10);
}
}
}
