From 03be1e6b40f45f22c30ded815d4d26a747c315ec Mon Sep 17 00:00:00 2001 From: johan9107 Date: Thu, 17 Nov 2016 10:59:23 +0100 Subject: [PATCH] Deleted oscilation when enableing acc --- UAV-ControlSystem/src/Flight/pid.c | 25 ++++++++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 31027e9..18ceb86 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -325,6 +325,27 @@ void pidUAV(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff) } } +uint8_t flagAccBuff = 0; + +void pidAccelerometer(void) +{ + static uint8_t counterAcc = 0; + + counterAcc = ((flagAccBuff == 0))? 0: counterAcc; + + pidUAV(&PidProfile[PID_ID_ACCELEROMETER], &PidProfileBuff[PID_ID_ACCELEROMETER]); + + if (counterAcc > 50) + { + PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll; + PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch; + } + else + { + counterAcc +=1; + } +} + /************************************************************************** * BRIEF: Runs a certain PID Controller * * INFORMATION: * @@ -354,10 +375,12 @@ void pidRun(uint8_t ID) { PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll; PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch; + flagAccBuff = 0; } else { - pidUAV(&PidProfile[PID_ID_ACCELEROMETER], &PidProfileBuff[PID_ID_ACCELEROMETER]); + pidAccelerometer(); + flagAccBuff = 1; } break;