philip/MDH-UAV-Control-System
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

PID added new lowpas filter and average calculations to Accelerometer values

Browse Source
This commit is contained in:
johan9107 2016-10-27 14:30:26 +02:00
parent c3e969e982
commit 3d3d16cead
1 changed files with 28 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
UAV-ControlSystem/src/Flight/pid.c
View File

@ -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];