From 3d3d16ceadd4dc842c9e9abb3d6d55768971c0c5 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Thu, 27 Oct 2016 14:30:26 +0200 Subject: [PATCH] PID added new lowpas filter and average calculations to Accelerometer values --- UAV-ControlSystem/src/Flight/pid.c | 30 ++++++++++++++++++++++++++++-- 1 file changed, 28 insertions(+), 2 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 98ffe07..d263d18 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -115,12 +115,19 @@ float constrainf(float amt, int low, int high) float oldSensorValue[2] = {0}; +float oldSensorValueRoll[50] = {0}; +float oldSensorValuePitch[50] = {0}; + +int i = 0; + /************************************************************************** * BRIEF: Update current sensor values * * INFORMATION: * **************************************************************************/ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) { + + switch (ID_profile) { case PID_ID_GYRO: @@ -144,9 +151,28 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); - sensorValues[ROLL] = alpha*X_roll + (1-alpha)*oldSensorValue[0]; - sensorValues[PITCH] = alpha*X_pitch + (1-alpha)*oldSensorValue[1]; + oldSensorValueRoll[i] = X_roll; + oldSensorValuePitch[i] = X_pitch; + float RollValue = 0; + float PitchValue = 0; + + for (int ii = 0; ii < 50; ii++) + { + RollValue = RollValue + oldSensorValueRoll[ii]; + PitchValue = PitchValue + oldSensorValuePitch[ii]; + + } + + + i = (i < 49)? i + 1:0; + + sensorValues[ROLL] = RollValue/50; + sensorValues[PITCH] = PitchValue/50; + + sensorValues[ROLL] = alpha*RollValue/50 + (1-alpha)*oldSensorValue[0]; + sensorValues[PITCH] = alpha*PitchValue/50 + (1-alpha)*oldSensorValue[1]; +// oldSensorValue[0] = sensorValues[ROLL]; oldSensorValue[1] = sensorValues[PITCH];