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

PID, added correct calculations to accelerometer data

Browse Source 
PID, added correct calculations to accelerometer data
This commit is contained in:
johan9107 2016-10-24 14:37:01 +02:00
parent 339f3538eb
commit c0cb3ffe17
2 changed files with 34 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -25,7 +25,7 @@
#define RADIO_RANGE 500 /*Radio range input*/ #define RADIO_RANGE 500 /*Radio range input*/
#define BAROMETER_RANGE 2000 /*Determines the range of the maximum height (limits the rc input)*/ #define BAROMETER_RANGE 2000 /*Determines the range of the maximum height (limits the rc input)*/
#define ACCELEROMETER_RANGE 90 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/ #define ACCELEROMETER_RANGE 20 /*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 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)*/ #define COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/
@ -56,6 +56,31 @@ accel_t accelProfile; /*Struct profile for input data from sensor*/
gyro_t gyroProfile; /*Struct profile for input data from sensor*/ gyro_t gyroProfile; /*Struct profile for input data from sensor*/
/**************************************************************************
* BRIEF: Calculates angle from accelerometer *
* INFORMATION: *
**************************************************************************/
float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, const float z_axis)
{
float angle;
switch (axis)
{
case ROLL:
angle = atan2(z_axis, sqrt(x_axis*x_axis + y_axis*y_axis))*180/M_PI - 90;
angle = (x_axis < 0) ? -angle : angle; /*CW (right, form behind) = pos angle*/
break;
case PITCH:
angle = atan2(sqrt(x_axis*x_axis + z_axis*z_axis), y_axis)*180/M_PI - 90; /*down (the front down against ground) = pos angle*/
break;
default:
angle = 0;
break;
}
return angle;
}
/************************************************************************** /**************************************************************************
* BRIEF: Scales data from input range to output range * * BRIEF: Scales data from input range to output range *
* INFORMATION: * * INFORMATION: *
@ -83,7 +108,7 @@ float constrainf(float amt, int low, int high)
* BRIEF: Update current sensor values * * BRIEF: Update current sensor values *
* INFORMATION: * * INFORMATION: *
**************************************************************************/ **************************************************************************/
void getCurrentValues(float *sensorValues, uint8_t ID_profile) void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
{ {
switch (ID_profile) switch (ID_profile)
{ {
@ -101,8 +126,8 @@ void getCurrentValues(float *sensorValues, uint8_t ID_profile)
mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/ mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/
/*May need Low pass filter since the accelerometer may drift*/ /*May need Low pass filter since the accelerometer may drift*/
sensorValues[ROLL] = atan2(accelProfile.accelXconv,accelProfile.accelZconv)*180*M_PI; sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
sensorValues[PITCH] = atan2(-accelProfile.accelXconv, sqrt(accelProfile.accelYconv*accelProfile.accelYconv + accelProfile.accelZconv*accelProfile.accelZconv))*180/M_PI; sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
break; break;
case PID_ID_COMPASS: case PID_ID_COMPASS:
@ -434,9 +459,9 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
break; break;
case PID_ID_ACCELEROMETER: case PID_ID_ACCELEROMETER:
PidProfile[ID].pidEnabled = false; PidProfile[ID].pidEnabled = true;
//PidProfile[ID].dterm_lpf = 90; PidProfile[ID].dterm_lpf = 90;
//PidProfile[ID].pid_out_limit = 3000; PidProfile[ID].pid_out_limit = 1000;
break; break;
case PID_ID_COMPASS: case PID_ID_COMPASS:

2
UAV-ControlSystem/src/drivers/accel_gyro.c
View File

@ -180,6 +180,8 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel)
if(!mpu6000_transmit(reg, 2)) if(!mpu6000_transmit(reg, 2))
return false; return false;
HAL_Delay(60);
mpu6000_read_offset(gyro, accel); mpu6000_read_offset(gyro, accel);
return true; return true;