From facfcb7023c5c1adfc461a4e55ddae26bfb2f168 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Tue, 25 Oct 2016 10:13:49 +0200 Subject: [PATCH] PID accelerometer added low pass filter --- UAV-ControlSystem/src/Flight/pid.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 082ef17..2ddce64 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -56,6 +56,7 @@ pidProfileBuff_t PidProfileBuff[PID_COUNT] = {0}; accel_t accelProfile; /*Struct profile for input data from sensor*/ gyro_t gyroProfile; /*Struct profile for input data from sensor*/ +pt1Filter_t accelFilter[2] = {0}; /************************************************************************** * BRIEF: Calculates angle from accelerometer * @@ -127,10 +128,19 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/ + + /*May need Low pass filter since the accelerometer may drift*/ + sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); + //float sensorRoll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); + //sensorValues[ROLL] = pt1FilterApply4(&accelFilter[0], sensorRoll, 90, PidProfileBuff[ROLL].dT); + + //float sensorPitch = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv); + //sensorValues[PITCH] = pt1FilterApply4(&accelFilter[1], sensorPitch, 90, PidProfileBuff[PITCH].dT); + break; case PID_ID_COMPASS: case PID_ID_BAROMETER: