From dab5b5626d6515c922819eadfa1cf0dabca810bd Mon Sep 17 00:00:00 2001 From: johan9107 Date: Mon, 31 Oct 2016 09:58:04 +0100 Subject: [PATCH] PID new test changes, cast I term to float --- UAV-ControlSystem/src/Flight/pid.c | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index c595d16..219a66d 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -18,6 +18,8 @@ #include "scheduler/scheduler.h" #include #include "drivers/failsafe_toggles.h" +#include "drivers/motormix.h" + #define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/ #define ITERM_SCALE 0.244381f /*I-term used as a scale value to the PID controller*/ @@ -29,7 +31,7 @@ #define GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/ #define COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/ -#define PID_MAX_I 256 /*Constrains ITerm*/ +#define PID_MAX_I 256/2 /*Constrains ITerm*/ #define PID_MAX_D 512 /*Constrains DTerm*/ /*Struct that belongs to a certain PID controller*/ @@ -117,8 +119,8 @@ float constrainf(float amt, int low, int high) float oldSensorValue[2] = {0}; -float oldSensorValueRoll[50] = {0}; -float oldSensorValuePitch[50] = {0}; +float oldSensorValueRoll[20] = {0}; +float oldSensorValuePitch[20] = {0}; int i = 0; @@ -164,7 +166,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) float RollValue = 0; float PitchValue = 0; - for (int ii = 0; ii < 50; ii++) + for (int ii = 0; ii < 20; ii++) { RollValue = RollValue + oldSensorValueRoll[ii]; PitchValue = PitchValue + oldSensorValuePitch[ii]; @@ -172,13 +174,13 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) } - i = (i < 49)? i + 1:0; + i = (i < 19)? i + 1:0; - sensorValues[ROLL] = RollValue/50; - sensorValues[PITCH] = PitchValue/50; + sensorValues[ROLL] = RollValue/20; + sensorValues[PITCH] = PitchValue/20; - sensorValues[ROLL] = alpha*RollValue/50 + (1-alpha)*oldSensorValue[0]; - sensorValues[PITCH] = alpha*PitchValue/50 + (1-alpha)*oldSensorValue[1]; + sensorValues[ROLL] = alpha*RollValue/20 + (1-alpha)*oldSensorValue[0]; + sensorValues[PITCH] = alpha*PitchValue/20 + (1-alpha)*oldSensorValue[1]; // oldSensorValue[0] = sensorValues[ROLL]; oldSensorValue[1] = sensorValues[PITCH]; @@ -291,7 +293,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, /* -----calculate I component ---- */ - float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * pidProfile->I[axis]; + float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * (float)pidProfile->I[axis]; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I) moved before integration to make limiting independent from PID settings @@ -341,6 +343,11 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, /*----PID OUT----*/ + if(!flags_IsSet_ID(systemFlags_armed_id) || (rc_input.Throttle < mixerConfig.minCheck)) + { + ITerm = 0; + pidProfileBuff->lastITerm[axis] = 0; + } pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -pidProfile->pid_out_limit, pidProfile->pid_out_limit); }