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

PID changed buffer to filter

Browse Source
This commit is contained in:
johan9107 2016-10-31 14:44:34 +01:00
parent f787b19572
commit dcf36fc542
1 changed files with 14 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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