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

Merge remote-tracking branch 'refs/remotes/origin/PID' into PID_from_eeprom

Browse Source
This commit is contained in:
Lennart Eriksson 2016-10-14 15:54:33 +02:00
commit 679040f9e7
1 changed files with 56 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -14,17 +14,21 @@
* **************************************************************************/
#include "Flight/pid.h"
#include "drivers/accel_gyro.h"
#include <math.h>
#define BAROMETER_RANGE 2000
#define RADIO_RANGE 1000
#define ACCELEROMETER_RANGE 4000
#define GYRO_RANGE 32
#define ACCELEROMETER_RANGE 90 // Accelerometer takes int to max 16 G
#define GYRO_RANGE 2000
#define COMPASS_RANGE 360
control_data_t RawRcCommand = {0};
pidProfile_t PidGyroProfile = {0}, PidAccelerometerProfile = {0};
pidProfile_t PidCompassProfile = {0}, PidBarometerProfile = {0};
accel_t accelProfile;
gyro_t gyroProfile;
/**************************************************************************
* BRIEF: Constrain float values within a defined limit *
@ -46,27 +50,29 @@ float constrainf(float amt, int low, int high)
**************************************************************************/
void getCurrentValues(float *sensorValues, uint8_t ID_profile)
{
//*Do something smart*//
switch (ID_profile)
{
case 1:
sensorValues[ROLL] = 0;
sensorValues[PITCH] = 0;
sensorValues[YAW] = 0;
case PID_ID_GYRO:
mpu6000_read_gyro(&gyroProfile);
sensorValues[ROLL] = gyroProfile.gyroY;
sensorValues[PITCH] = gyroProfile.gyroX;
sensorValues[YAW] = gyroProfile.gyroZ;
break;
case 2:
case PID_ID_ACCELEROMETER:
mpu6000_read_accel(&accelProfile);
/*May need Low pass filter since the accelerometer may drift*/
sensorValues[ROLL] = atan2(accelProfile.accelXconv,accelProfile.accelZconv)*180*M_PI;//acos(accelProfile.accelXconv/R)*180/M_PI;
sensorValues[PITCH] = atan2(-accelProfile.accelXconv, sqrt(accelProfile.accelYconv*accelProfile.accelYconv + accelProfile.accelZconv*accelProfile.accelZconv))*180/M_PI;
break;
case 3:
case PID_ID_COMPASS:
break;
case 4:
break;
case 5:
case PID_ID_BAROMETER:
break;
default:
break;
}
};
float convertData(int inputRange, int outputRange, int offset, float value)
@ -84,9 +90,28 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile)
switch(ID_profile)
{
case PID_ID_GYRO:
desiredCommand[ROLL] = PidAccelerometerProfile.PID_Out[ROLL];
desiredCommand[PITCH] = PidAccelerometerProfile.PID_Out[PITCH];
desiredCommand[YAW] = PidCompassProfile.PID_Out[0];
if (!PidAccelerometerProfile.pidEnabled)
{
desiredCommand[ROLL] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[ROLL]);
desiredCommand[PITCH] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[PITCH]);
}
else
{
desiredCommand[ROLL] = convertData(ACCELEROMETER_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[ROLL]);
desiredCommand[PITCH] = convertData(ACCELEROMETER_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[PITCH]);
}
if (!PidCompassProfile.pidEnabled)
{
desiredCommand[YAW] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidCompassProfile.PID_Out[0]);
}
else
{
desiredCommand[YAW] = convertData(ACCELEROMETER_RANGE, GYRO_RANGE, 0, PidCompassProfile.PID_Out[0]);
}
break;
case PID_ID_ACCELEROMETER:
desiredCommand[ROLL] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, RawRcCommand.Roll);
@ -178,8 +203,10 @@ void pidUAVInit(pidProfile_t *pidProfile)
**************************************************************************/
void pidInit()
{
int test;
pidUAVInit(&PidGyroProfile);
pidUAVInit(&PidAccelerometerProfile);
pidUAVInit(&PidCompassProfile);
pidUAVInit(&PidBarometerProfile);
}
/**************************************************************************
@ -290,7 +317,16 @@ void pidRun(uint8_t ID)
case PID_ID_GYRO:
pidUAV(&PidGyroProfile);
if (!PidGyroProfile.pidEnabled)
{
pidUAV(&PidGyroProfile);
}
else
{
PidGyroProfile.PID_Out[ROLL] = RawRcCommand.Roll;
PidGyroProfile.PID_Out[PITCH] = RawRcCommand.Pitch;
PidGyroProfile.PID_Out[YAW] = RawRcCommand.Yaw;
}
break;
case PID_ID_ACCELEROMETER:
@ -326,8 +362,6 @@ void pidRun(uint8_t ID)
}
else
{
//convertData(1000, 3000, 0, );
pidUAV(&PidBarometerProfile);
}