diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h index 39728c7..7213f7d 100644 --- a/UAV-ControlSystem/inc/Flight/pid.h +++ b/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 * diff --git a/UAV-ControlSystem/inc/drivers/motormix.h b/UAV-ControlSystem/inc/drivers/motormix.h index 85522b9..fc999e7 100644 --- a/UAV-ControlSystem/inc/drivers/motormix.h +++ b/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 * diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 97a78f8..87370fd 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/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; + } diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 1261837..0def06d 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/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)) diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index f678bab..2c9dfaf 100644 --- a/UAV-ControlSystem/src/main.c +++ b/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();