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

Possible fix to the accelerometer problem

Browse Source
This commit is contained in:
Jonas Holmberg 2016-11-07 10:43:43 +01:00
parent 8bd9dc986d
commit 280240815d
5 changed files with 68 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
UAV-ControlSystem/inc/utilities.h
View File

@ -83,6 +83,11 @@ uint32_t accumulate(uint32_t list[], int length);
***********************************************************************/
void Error_Handler(void);
/**************************************************************************
* BRIEF: Constrain float values within a defined limit *
* INFORMATION: Used in PID loop to limit values *
**************************************************************************/
float constrainf(float amt, int low, int high);
uint8_t reverse(uint8_t byte);

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

@ -110,19 +110,6 @@ float convertData(int inputRange, int outputRange, int offset, float value)
return ((float)outputRange/(float)inputRange)*(value-(float)offset);
}
/**************************************************************************
* BRIEF: Constrain float values within a defined limit *
* INFORMATION: Used in PID loop to limit values *
**************************************************************************/
float constrainf(float amt, int low, int high)
{
if (amt < (float)low)
return (float)low;
else if (amt > (float)high)
return (float)high;
else
return amt;
}
float oldSensorValue[2] = {0};

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

@ -9,6 +9,7 @@
#include "drivers/spi.h"
#include "utilities.h"
#include "math.h"
#include "drivers/system_clock.h"
spi_profile mpu6000_spi_profile;
uint8_t num_failed_receive = 0;
@ -518,6 +519,17 @@ bool mpu6000_who_am_i()
}
/* Set the Gyro Weight for Gyro/Acc complementary filter
Increasing this value would reduce and delay Acc influence on the output of the filter*/
#define GYRO_ACC_DIV_FACTOR (2^6) // that means a CMP_FACTOR of 1024 (2^10)
#define GetMagnitude(x) (x*x)
#define Low_Magnitude (GetMagnitude(0.85))
#define High_Magnitude (GetMagnitude(1.15))
/* Returns the angle. If magnitude of acc is out of range we calculate on integrated gyro data */
void mpu6000_read_angle(accel_t* accel, gyro_t* gyro)
{
@ -526,61 +538,58 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro)
uint32_t delta_t = current_micros - last_micros;
last_micros = current_micros;
float deltaGyroAngleFloat[3] = {0};
static float lpf_Acc[3] = {0};
static float smooth[3] = {0};
float sign[3] = {0};
float magnitude = 0;
/* We read the accelerometer to get fresh data */
mpu6000_read_accel(accel);
float accelConv[3] = {accel->accelXconv, accel->accelYconv, accel->accelZconv};
/* Filter part */
/* Filter part, go thorugh each axis */
for (int i = 0; i < 3; i ++)
{
//Calculate a new smooth value based on a factor of the LPF value
smooth[i] = lpf_Acc[i] / 16;
//Save the sign(+/-) of the value
sign[i] = (accelConv[i]< 0) ? -1 : 1;
//Calculate the new LPF value based on the raw sensor data - the smoothing value
lpf_Acc[i] += sign[i]*sqrtf(ABS_FLOAT(accelConv[i])) - smooth[i];
//recalculate the accelerometer data based on the smooth value, since we had to take the square root off the original value we square it to get in the original size
accelConv[i] = smooth[i] * smooth[i] * sign[i];
//calculate the magnitude of the gravitation for all axis
magnitude += ABS_FLOAT(accelConv[i]) * ABS_FLOAT(accelConv[i]);
}
sign[0] = (accel->accelXconv< 0) ? -1 : 1;
lpf_Acc[0] += sign[0]*sqrtf(ABS_FLOAT(accel->accelXconv)) - smooth[0];
//Calculate the approximate angle increase based on the gyros probable movement since the last invoke
deltaGyroAngleFloat[0] = (delta_t * (float)gyro->gyroX / 1000000.0);
deltaGyroAngleFloat[1] = (delta_t * (float)gyro->gyroY / 1000000.0) ;
deltaGyroAngleFloat[2] = (delta_t * (float)gyro->gyroZ / 1000000.0);
//First integrate the gyro and add that to the angle calculation
accel->rollAngle += deltaGyroAngleFloat[1];
accel->pitchAngle += deltaGyroAngleFloat[0];
sign[1] = (accel->accelYconv< 0) ? -1 : 1;
lpf_Acc[1] += sign[1]*sqrtf(ABS_FLOAT(accel->accelYconv)) - smooth[1];
sign[2] = (accel->accelZconv< 0) ? -1 : 1;
lpf_Acc[2] += sign[2]*sqrtf(ABS_FLOAT(accel->accelZconv)) - smooth[2];
accel->accelXconv = smooth[0] * smooth[0] * sign[0];
accel->accelYconv = smooth[1] * smooth[1] * sign[1];
accel->accelZconv = smooth[2] * smooth[2] * sign[2];
/* The total magnitude of the acceleration */
float magnitude = sqrtf( \
ABS_FLOAT(accel->accelXconv) * ABS_FLOAT(accel->accelXconv) + \
ABS_FLOAT(accel->accelYconv) * ABS_FLOAT(accel->accelYconv) + \
ABS_FLOAT(accel->accelZconv) * ABS_FLOAT(accel->accelZconv) \
);
/* Everything is normal. No outer forces */
if (0.85 < magnitude && magnitude < 1.15)
//If the g forces of the accelerometer is within the given magnitude we will also add accelerometer data to the calculation
if (Low_Magnitude < magnitude && magnitude < High_Magnitude)
{
// Roll
accel->rollAngle = -atan2(accel->accelXconv, sqrt(accel->accelYconv*accel->accelYconv + accel->accelZconv*accel->accelZconv))*180/M_PI;
//Calculate the pure angle given by the accelerometer data
float a_RollAngle = -atan2(accelConv[0], sqrt(accelConv[1]*accelConv[1] + accelConv[2]*accelConv[2]))*180/M_PI;
float a_PitchAngle = atan2( accelConv[1], sqrt(accelConv[2]*accelConv[2] + accelConv[0]*accelConv[0]))*180/M_PI;
// Pitch
accel->pitchAngle = atan2( accel->accelYconv, sqrt(accel->accelZconv*accel->accelZconv + accel->accelXconv*accel->accelXconv))*180/M_PI;
//Check how much the accelerometer angle differs from the current calculated ange with the gyro. Add calculated factor to the real angle value
accel->rollAngle += (a_RollAngle - accel->rollAngle) / GYRO_ACC_DIV_FACTOR;
accel->pitchAngle += (a_PitchAngle - accel->pitchAngle) / GYRO_ACC_DIV_FACTOR;
}
/* Too big forces to calculate on ACC data. Fallback on gyro integration */
else
{
accel->rollAngle += (float)(delta_t * gyro->gyroY) / 1000000.0;
accel->pitchAngle += (float)(delta_t * gyro->gyroX) / 1000000.0;
//Always constran the angular values within the possible ranges 90 to -90
accel->rollAngle = constrainf(accel->rollAngle, -90,90);
accel->pitchAngle = constrainf(accel->pitchAngle, -90,90);
}
}

1
UAV-ControlSystem/src/tasks_main.c
View File

@ -64,6 +64,7 @@ void systemTaskGyroPid(void)
void systemTaskAccelerometer(void)
{
flags_Set_ID(systemFlags_flightmode_acceleromter_id);
pidRun(PID_ID_ACCELEROMETER);
//update the accelerometer data
// uint8_t c = 97;

14
UAV-ControlSystem/src/utilities.c
View File

@ -228,3 +228,17 @@ int16_t constrain(int16_t value, int16_t min, int16_t max)
else
return value;
}
/**************************************************************************
* BRIEF: Constrain float values within a defined limit *
* INFORMATION: Used in PID loop to limit values *
**************************************************************************/
float constrainf(float amt, int low, int high)
{
if (amt < (float)low)
return (float)low;
else if (amt > (float)high)
return (float)high;
else
return amt;
}