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

Added velocity compensation to baromter loop instead of controling height

Browse Source
This commit is contained in:
johan9107 2016-11-15 09:15:15 +01:00
parent 7333665fc6
commit 497e00aa2f
1 changed files with 11 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -29,7 +29,7 @@
#define BAROMETER_SCALE 11
#define RADIO_RANGE 500 /*Radio range input*/
#define BAROMETER_RANGE 3 /*Determines the range of the maximum height (limits the rc input)*/
#define BAROMETER_RANGE 10 /*Determines the range of the maximum height (limits the rc input)*/
#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)*/
@ -90,7 +90,6 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
{
static float last_micros = 0;
static float oldHeightValue = 0;
static uint8_t counter = 0;
switch (ID_profile)
{
@ -124,34 +123,16 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
break;
case PID_ID_BAROMETER:
sensorValues[0] = barometer_GetCurrentAveragedtAltitude();
float current_micros = clock_get_us()/1000000;
float delta_t = (current_micros - last_micros);
float current_height = barometer_GetCurrentAveragedtAltitude();
counter += 1;
last_micros = current_micros;
if (counter > 2)
{
sensorValues[0] = ((current_height - oldHeightValue)/delta_t);
sensorValues[0] = (sensorValues[0] < - 6 || sensorValues[0] > 6)? sensorValues[0]:0;
float current_micros = clock_get_us()/1000000;
float delta_t = (current_micros - last_micros);
last_micros = current_micros;
counter = 0;
float current_velocity = (sensorValues[0] - oldHeightValue)/delta_t;
if( current_velocity<= -10)
{
VelocityCompensation = ABS_FLOAT(current_velocity*3);
}
else
{
VelocityCompensation = 0;
}
}
oldHeightValue = sensorValues[0];
oldHeightValue = current_height;
sensorValues[0]*=BAROMETER_SCALE;
break;
@ -212,9 +193,9 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile)
break;
case PID_ID_BAROMETER:
//desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle*throttleRate);
desiredCommand[THROTTLE] = DESIRED_HEIGHT*BAROMETER_SCALE;
float currentThrottle = rc_input.Throttle - 1500;
float velocity = (currentThrottle < - 10 || currentThrottle > 10 )? currentThrottle:0;
desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle)*BAROMETER_SCALE;
break;
default: