Fixed the problem with windup

UAV-ControlSystem/inc/Flight/pid.h

@@ -67,8 +67,7 @@ extern float accPitchFineTune;
 
 extern accel_t accelProfile;												/*Struct profile for input data from sensor*/
 
-/*Is set in motor mix and used in pidUAVcore and mix */
+
-bool motorLimitReached;
 
 /**************************************************************************
 * BRIEF: Initializes PID profiles			 					          *

UAV-ControlSystem/inc/drivers/motormix.h

@@ -46,6 +46,9 @@ typedef struct  {
 /* Global mixerConfig to bee available to EEPROM */
 extern mixerConfig_s mixerConfig;
 
+/*Is set in motor mix and used in pidUAVcore and mix */
+extern bool motorLimitReached;
+
 /***********************************************************************
 * BRIEF: The motormixer                                                *
 * INFORMATION: Sums the output from all control loops and adapts the   *

UAV-ControlSystem/src/Flight/pid.c

@@ -62,6 +62,8 @@ pt1Filter_t accelFilter[2] = {0};
 float accRollFineTune = 0;
 float accPitchFineTune = 0;
 
+
+
 /**************************************************************************
 * BRIEF: Calculates angle from accelerometer   		                      *
 * INFORMATION:                                                            *
@@ -324,6 +326,12 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
 //		pidProfileBuff->ITermLimit[axis] = abs(ITerm);
 //	}
 
+		if (motorLimitReached)
+		{
+			ITerm = pidProfileBuff->lastITerm[axis];
+		}
+
+
 	pidProfileBuff->lastITerm[axis] = ITerm;
 
 
@@ -611,7 +619,6 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
 void pidInit()
 {
 	mpu6000_init(&gyroProfile,&accelProfile); /*Init gyro and accelerometer*/
-	motorLimitReached = false;
 
 	pidUAVInitBuff(&PidProfileBuff[PID_ID_GYRO], PID_ID_GYRO);
 	pidUAVInitBuff(&PidProfileBuff[PID_ID_ACCELEROMETER], PID_ID_ACCELEROMETER);
@@ -636,6 +643,8 @@ void pidEproom(void)
 	PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000;
 	PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000;
 
+	PidProfile[PID_ID_GYRO].I[YAW] = 40;
+
 
 
 }

UAV-ControlSystem/src/drivers/motormix.c

@@ -42,6 +42,9 @@
 /* An array containing the calculated motor outputs */
 uint16_t motor_output[MOTOR_COUNT];
 
+/* Bool to see if motors are maxed out. Stops windup in PID implementation */
+bool motorLimitReached = false;
+
 /* Default values for the mixerConfig */
 // TODO: Implement in EEPROM
 mixerConfig_s mixerConfig = {
@@ -102,8 +105,6 @@ void mix()
 	int16_t RPY_Mix_Max = 0;			// Maximum desired command for any motor
 	int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE];	 // Import throttle value from remote
 
-	// Might be used for some debug if necessary
-	//static bool motorLimitReached;
 
 	/* Mixer Full Scale enabled */
 	if (flags_IsSet_ID(systemFlags_mixerfullscale_id))

UAV-ControlSystem/src/main.c

@@ -60,7 +60,7 @@ void init_system()
 	pidEproom();
 
 	//initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble
-	cliInit(USART6);
+	cliInit(USART3);
 
 	//init sbus, using USART1
 	sbus_init();