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