From dcf36fc542ac0677e9dbf6e692d778e41881eeaf Mon Sep 17 00:00:00 2001 From: johan9107 Date: Mon, 31 Oct 2016 14:44:34 +0100 Subject: [PATCH] PID changed buffer to filter --- UAV-ControlSystem/src/Flight/pid.c | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 31d130e..e538621 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -27,7 +27,7 @@ #define RADIO_RANGE 500 /*Radio range input*/ #define BAROMETER_RANGE 2000 /*Determines the range of the maximum height (limits the rc input)*/ -#define ACCELEROMETER_RANGE 35 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/ +#define ACCELEROMETER_RANGE 60 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/ #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)*/ @@ -92,6 +92,11 @@ float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, cons return angle; } +float calcGravity(accel_t profile ) //const float x_axis, const float y_axis, const float z_axis) +{ + return sqrt(profile.accelXconv*profile.accelXconv + profile.accelYconv*profile.accelYconv + profile.accelZconv*profile.accelZconv); +} + /************************************************************************** * BRIEF: Scales data from input range to output range * * INFORMATION: * @@ -118,8 +123,8 @@ float constrainf(float amt, int low, int high) float oldSensorValue[2] = {0}; -float oldSensorValueRoll[20] = {0}; -float oldSensorValuePitch[20] = {0}; +float oldSensorValueRoll[12] = {0}; +float oldSensorValuePitch[12] = {0}; int i = 0; @@ -147,7 +152,6 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/ - float alpha = 0.5; /*May need Low pass filter since the accelerometer may drift*/ @@ -165,7 +169,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) float RollValue = 0; float PitchValue = 0; - for (int ii = 0; ii < 20; ii++) + for (int ii = 0; ii < 12; ii++) { RollValue = RollValue + oldSensorValueRoll[ii]; PitchValue = PitchValue + oldSensorValuePitch[ii]; @@ -173,13 +177,13 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) } - i = (i < 19)? i + 1:0; + i = (i < 11)? i + 1:0; - sensorValues[ROLL] = RollValue/20; - sensorValues[PITCH] = PitchValue/20; + sensorValues[ROLL] = RollValue/12; + sensorValues[PITCH] = PitchValue/12; - sensorValues[ROLL] = alpha*RollValue/20 + (1-alpha)*oldSensorValue[0]; - sensorValues[PITCH] = alpha*PitchValue/20 + (1-alpha)*oldSensorValue[1]; + sensorValues[ROLL] = alpha*RollValue/12 + (1-alpha)*oldSensorValue[0]; + sensorValues[PITCH] = alpha*PitchValue/12 + (1-alpha)*oldSensorValue[1]; // oldSensorValue[0] = sensorValues[ROLL]; oldSensorValue[1] = sensorValues[PITCH];