Ejemplos de Update_RC_Control en C++ (Cpp)

Ejemplo n.º 1

Archivo: IMU_correct.c Proyecto: saya222777/STM32F429_Quadrotor

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

Ejemplo n.º 2

Archivo: main.c Proyecto: pcedison/QuadcopterFlightControl

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


		}

	}
}