u8 get_dmp() { dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more); if (sensors & INV_WXYZ_QUAT) { q0 = (float)quat[0] / q30; q1 = (float)quat[1] / q30; q2 = (float)quat[2] / q30; q3 = (float)quat[3] / q30; mpu_gryo_pitch = MoveAve_WMA(gyro[0], MPU6050_GYR_FIFO[0], 8); mpu_gryo_roll = MoveAve_WMA(gyro[1], MPU6050_GYR_FIFO[1], 8); mpu_gryo_yaw = MoveAve_WMA(gyro[2], MPU6050_GYR_FIFO[2], 8); ahrs.gryo_pitch = -mpu_gryo_pitch * gyroscale / 32767.0f; ahrs.gryo_roll = -mpu_gryo_roll * gyroscale / 32767.0f; ahrs.gryo_yaw = -mpu_gryo_yaw * gyroscale / 32767.0f; ahrs.degree_roll = -asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3f + settings.angle_diff_roll; //+ Pitch_error; // pitch ahrs.degree_pitch = -atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3f + settings.angle_diff_pitch; //+ Roll_error; // roll if (!has_hmc5883) ahrs.degree_yaw = atan2(2 * (q1*q2 + q0*q3), q0*q0 + q1*q1 - q2*q2 - q3*q3) * 57.3f; //+ Yaw_error; ahrs.time_span = Timer4_GetSec(); if (en_out_ahrs) rt_kprintf("%d,%d,%d %d\n", (s32)(ahrs.degree_pitch), (s32)(ahrs.degree_roll), (s32)(ahrs.degree_yaw), (u32)(1.0f / ahrs.time_span)); rt_event_send(&ahrs_event, AHRS_EVENT_Update); dmp_retry = 0; return 1; } lost: dmp_retry++; return 0; }
/*====================================================================================================*/ 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 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 correct_sensor() { float Ellipse[5] = {0}; #define MovegAveFIFO_Size 250 switch (SensorMode) { /************************** Mode_CorrectGyr **************************************/ case Mode_GyrCorrect: /* 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 == SampleRateFreg) { 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_AccCorrect; } break; /************************** Mode_CorrectAcc **************************************/ case Mode_AccCorrect: /* 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 == SampleRateFreg) { 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 // DelayTime : SampleRate * 600 case Mode_MagCorrect: Correction_Time++; switch ((u16)(Correction_Time / MagCorrect_Delay)) { case 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: 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: 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: 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: 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: 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: 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: 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: 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: /* 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; } }
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; } }
void control_thread_entry(void* parameter) { float yaw_inc = 0; float yaw_exp = 0; u8 i; u8 take_off = 0; p_rate_pid.expect = 0; r_rate_pid.expect = 0; y_rate_pid.expect = 0; p_angle_pid.expect = 0; r_angle_pid.expect = 0; LED2(5); for (i = 0;i < 15;i++) { u8 j; for (j = 0;j < 200;j++) { get_dmp(); rt_thread_delay(2); } } rt_kprintf("start control\n"); while (1) { LED2(pwmcon * 3); if (pwmcon) { if (PWM3_Time <= settings.th_max&&PWM3_Time >= settings.th_min) throttle = (PWM3_Time - settings.th_min) * 1000 / (settings.th_max - settings.th_min); else throttle = 0; if (PWM1_Time <= settings.roll_max&&PWM1_Time >= settings.roll_min) { roll = MoveAve_WMA(PWM1_Time, roll_ctl, 16) - settings.roll_mid; if (roll > 5) PID_SetTarget(&r_angle_pid, -roll / (float)(settings.roll_max - settings.roll_mid)*45.0f); else if (roll < -5) PID_SetTarget(&r_angle_pid, -roll / (float)(settings.roll_mid - settings.roll_min)*45.0f); else PID_SetTarget(&r_angle_pid, 0); } if (PWM2_Time <= settings.pitch_max&&PWM2_Time >= settings.pitch_min) { pitch = MoveAve_WMA(PWM2_Time, pitch_ctl, 16) - settings.pitch_mid; if (pitch > 5) PID_SetTarget(&p_angle_pid, -pitch / (float)(settings.pitch_max - settings.pitch_mid)*30.0f); else if (pitch < -5) PID_SetTarget(&p_angle_pid, -pitch / (float)(settings.pitch_mid - settings.pitch_min)*30.0f); else PID_SetTarget(&p_angle_pid, 0); } if (PWM4_Time <= settings.yaw_max&&PWM4_Time >= settings.yaw_min) { yaw_inc = MoveAve_WMA(PWM4_Time, yaw_ctl, 16) - settings.yaw_mid; if (has_hmc5883) { if (yaw_inc > 5) yaw_exp -= yaw_inc / (float)(settings.yaw_max - settings.yaw_mid)*0.5f; else if (yaw_inc < -5) yaw_exp -= yaw_inc / (float)(settings.yaw_mid - settings.yaw_min)*0.5f; if (yaw_exp > 360.0f)yaw_exp -= 360.0f; else if (yaw_exp < 0.0f)yaw_exp += 360.0f; PID_SetTarget(&y_angle_pid, 0); } else { if (yaw > 5) PID_SetTarget(&y_rate_pid, -yaw_inc / (float)(settings.yaw_max - settings.yaw_mid)*100.0f); else if (yaw < -5) PID_SetTarget(&y_rate_pid, -yaw_inc / (float)(settings.yaw_mid - settings.yaw_min)*100.0f); else PID_SetTarget(&y_rate_pid, 0); } } if (!balence) Motor_Set(throttle, throttle, throttle, throttle); } else if (PWM3_Time > settings.th_min&&PWM3_Time < settings.th_min + 40 && PWM5_Time < 1700 && PWM5_Time>500) { //set pwm middle if (!pwmcon) { settings.roll_mid = PWM1_Time; settings.pitch_mid = PWM2_Time; settings.yaw_mid = PWM4_Time; } pwmcon = 1; balence = 1; p_rate_pid.iv = 0; r_rate_pid.iv = 0; y_rate_pid.iv = 0; take_off = 1; yaw_exp = ahrs.degree_yaw; } if (get_dmp() && balence) { rt_uint32_t dump; if (throttle > 60 && abs(ahrs.degree_pitch) < 40 && abs(ahrs.degree_roll) < 40) { if (PWM5_Time < 1200 && PWM5_Time>800 && sonar_state) { if (rt_event_recv(&ahrs_event, AHRS_EVENT_SONAR, RT_EVENT_FLAG_AND | RT_EVENT_FLAG_CLEAR, RT_WAITING_NO, &dump) == RT_EOK) { PID_SetTarget(&h_pid, 60.0); PID_xUpdate(&h_pid, sonar_h); h_pid.out = RangeValue(h_pid.out, -200, 200); } LED4(4); throttle = 500; if (has_adns3080&& PWM7_Time > 1500) { if (!poscon) { poscon = 1; pos_X = opx; pos_y = opy; } if (rt_event_recv(&ahrs_event, AHRS_EVENT_ADNS3080, RT_EVENT_FLAG_AND | RT_EVENT_FLAG_CLEAR, RT_WAITING_NO, &dump) == RT_EOK) { PID_SetTarget(&x_d_pid, pos_X); PID_SetTarget(&y_d_pid, pos_y); PID_xUpdate(&x_d_pid, opx); PID_xUpdate(&y_d_pid, opy); PID_SetTarget(&x_v_pid, -RangeValue(x_d_pid.out, -10, 10)); PID_SetTarget(&y_v_pid, -RangeValue(y_d_pid.out, -10, 10)); PID_xUpdate(&x_v_pid, optc_dx); PID_xUpdate(&y_v_pid, optc_dy); } LED4(2); PID_SetTarget(&r_angle_pid, -RangeValue(x_v_pid.out, -10, 10)); PID_SetTarget(&p_angle_pid, +RangeValue(y_v_pid.out, -10, 10)); } else poscon = 0; } else { h_pid.out = 0; LED4(0); } PID_xUpdate(&p_angle_pid, ahrs.degree_pitch); PID_SetTarget(&p_rate_pid, -RangeValue(p_angle_pid.out, -80, 80)); PID_xUpdate(&p_rate_pid, ahrs.gryo_pitch); PID_xUpdate(&r_angle_pid, ahrs.degree_roll); PID_SetTarget(&r_rate_pid, -RangeValue(r_angle_pid.out, -80, 80)); PID_xUpdate(&r_rate_pid, ahrs.gryo_roll); if (has_hmc5883) { if (rt_event_recv(&ahrs_event, AHRS_EVENT_HMC5883, RT_EVENT_FLAG_AND | RT_EVENT_FLAG_CLEAR, RT_WAITING_NO, &dump) == RT_EOK) { yaw = ahrs.degree_yaw - yaw_exp; if (yaw > 180.0f)yaw -= 360.0f; if (yaw < -180.0f)yaw += 360.0f; PID_xUpdate(&y_angle_pid, yaw); PID_SetTarget(&y_rate_pid, -RangeValue(y_angle_pid.out, -100, 100)); } } PID_xUpdate(&y_rate_pid, ahrs.gryo_yaw); Motor_Set1(throttle - p_rate_pid.out - r_rate_pid.out + y_rate_pid.out - h_pid.out); Motor_Set2(throttle - p_rate_pid.out + r_rate_pid.out - y_rate_pid.out - h_pid.out); Motor_Set3(throttle + p_rate_pid.out - r_rate_pid.out - y_rate_pid.out - h_pid.out); Motor_Set4(throttle + p_rate_pid.out + r_rate_pid.out + y_rate_pid.out - h_pid.out); } else Motor_Set(60, 60, 60, 60); } else LED4(0); if (PWM5_Time > 1700) { Motor_Set(0, 0, 0, 0); p_rate_pid.iv = 0; r_rate_pid.iv = 0; y_rate_pid.iv = 0; balence = 0; pwmcon = 0; } if (dmp_retry > 200) { Motor_Set(0, 0, 0, 0); balence = 0; pwmcon = 0; LED3(2); break; } if (en_out_pwm) { rt_kprintf("\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n", PWM1_Time, PWM2_Time, PWM3_Time, PWM4_Time, PWM5_Time, PWM6_Time, PWM7_Time, PWM8_Time); // rt_kprintf("\t%d\t%d\t%d\t%d\t%d\t%d\n", // Motor1,Motor2,Motor3, // Motor4,Motor5,Motor6); } rt_thread_delay(2); } }