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 oldSensorValue[2] = {0};
float oldSensorValueRoll[50] = {0};
float oldSensorValuePitch[50] = {0};
int i = 0;
/************************************************************************** /**************************************************************************
* BRIEF: Update current sensor values * * BRIEF: Update current sensor values *
* INFORMATION: * * INFORMATION: *
**************************************************************************/ **************************************************************************/
void getCurrentValues(float sensorValues[3], uint8_t ID_profile) void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
{ {
switch (ID_profile) switch (ID_profile)
{ {
case PID_ID_GYRO: 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_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
float X_pitch = calcAngle(PITCH, 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]; oldSensorValueRoll[i] = X_roll;
sensorValues[PITCH] = alpha*X_pitch + (1-alpha)*oldSensorValue[1]; 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[0] = sensorValues[ROLL];
oldSensorValue[1] = sensorValues[PITCH]; oldSensorValue[1] = sensorValues[PITCH];