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

First commit to new acc implementation

Browse Source
This commit is contained in:
philsson 2016-11-02 09:22:22 +01:00
parent 0152b14006
commit 88f09eef25
3 changed files with 117 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
UAV-ControlSystem/inc/drivers/accel_gyro.h
View File

@ -114,6 +114,7 @@ typedef struct accel_t {
int16_t accelXraw, accelYraw, accelZraw; /* Raw accelerometer data */
int16_t offsetX, offsetY, offsetZ; /* Accelerometer offset raw values */
uint16_t accel1G; /* Sensitivity factor */
float rollAngle, pitchAngle;
} accel_t;
/***********************************************************************
@ -164,6 +165,8 @@ int mpu6000_read_fifo(gyro_t* gyro, int16_t* data_out);
***********************************************************************/
bool mpu6000_who_am_i();
void mpu6000_read_angle(accel_t* accel, gyro_t* gyro);
#endif /* DRIVERS_ACCEL_GYRO_H_ */

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

@ -65,6 +65,7 @@ float accPitchFineTune = 0;
//TODO: Remove as implemented in accel_gyro
/**************************************************************************
* BRIEF: Calculates angle from accelerometer *
* INFORMATION: *
@ -151,56 +152,48 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
break;
case PID_ID_ACCELEROMETER:
mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/
//
// if (calcGravity(accelProfile) > 1.15)
// {
//
// sensorValues[ROLL] = gyroProfile.gyroY*PidProfileBuff[ROLL].dT;
// sensorValues[PITCH] = gyroProfile.gyroX*PidProfileBuff[PITCH].dT;
//
// }
// else
// {
mpu6000_read_angle(&accelProfile,&gyroProfile); /*Reads data from accelerometer*/
float alpha = 0.5;
/*May need Low pass filter since the accelerometer may drift*/
float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
// float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
// float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
/*TODO add finetune for roll and pitch*/
X_roll += accRollFineTune;
X_pitch += accPitchFineTune;
// X_roll += accRollFineTune;
// X_pitch += accPitchFineTune;
sensorValues[ROLL] = accelProfile.rollAngle + accRollFineTune;
sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune;
oldSensorValueRoll[i] = X_roll;
oldSensorValuePitch[i] = X_pitch;
float RollValue = 0;
float PitchValue = 0;
for (int ii = 0; ii < 12; ii++)
{
RollValue = RollValue + oldSensorValueRoll[ii];
PitchValue = PitchValue + oldSensorValuePitch[ii];
}
i = (i < 11)? i + 1:0;
sensorValues[ROLL] = RollValue/12;
sensorValues[PITCH] = PitchValue/12;
sensorValues[ROLL] = alpha*RollValue/12 + (1-alpha)*oldSensorValue[0];
sensorValues[PITCH] = alpha*PitchValue/12 + (1-alpha)*oldSensorValue[1];
// oldSensorValueRoll[i] = X_roll;
// oldSensorValuePitch[i] = X_pitch;
//
oldSensorValue[0] = sensorValues[ROLL];
oldSensorValue[1] = sensorValues[PITCH];
// float RollValue = 0;
// float PitchValue = 0;
//
// for (int ii = 0; ii < 12; ii++)
// {
// RollValue = RollValue + oldSensorValueRoll[ii];
// PitchValue = PitchValue + oldSensorValuePitch[ii];
//
// }
//
//
// i = (i < 11)? i + 1:0;
// sensorValues[ROLL] = RollValue/12;
// sensorValues[PITCH] = PitchValue/12;
//
// 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];
//
// sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);

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

@ -7,6 +7,8 @@
#include <drivers/accel_gyro.h>
#include "drivers/spi.h"
#include "utilities.h"
#include "math.h"
spi_profile mpu6000_spi_profile;
uint8_t num_failed_receive = 0;
@ -514,3 +516,83 @@ bool mpu6000_who_am_i()
return false;
}
/* 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)
{
static uint32_t last_micros = 0; // Static stores micros measured from last iteration
uint32_t current_micros = clock_get_us();
uint32_t delta_t = current_micros - last_micros;
last_micros = current_micros;
static float lpf_Acc[3] = {0};
static float smooth[3] = {0};
/* We read the accelerometer to get fresh data */
mpu6000_read_accel(accel);
/* Filter part */
for (int i = 0; i < 3; i ++)
smooth[i] = lpf_Acc[i] / 16;
lpf_Acc[0] += sqrtf(accel->accelXconv) - smooth[0];
lpf_Acc[1] += sqrtf(accel->accelYconv) - smooth[1];
lpf_Acc[2] += sqrtf(accel->accelZconv) - smooth[2];
accel->accelXconv = smooth[0] * smooth[0];
accel->accelYconv = smooth[1] * smooth[1];
accel->accelZconv = smooth[2] * smooth[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)
{
//TODO: JOHAN FIXAR! Kolla sĺ det är rätt här hela vägen
// Roll
accel->rollAngle = atan2(accel->accelXconv, sqrt(accel->accelYconv*accel->accelYconv + accel->accelZconv*accel->accelZconv))*180/M_PI;
if (accel->rollAngle > 0)
{
if (accel->accelYconv < 0 )
accel->rollAngle = 180 - accel->rollAngle;
}
else
{
if (accel->accelYconv < 0 )
accel->rollAngle = - 180 - accel->rollAngle;
}
accel->rollAngle = -accel->rollAngle;
// Pitch
accel->pitchAngle = atan2( accel->accelYconv, sqrt(accel->accelZconv*accel->accelZconv + accel->accelXconv*accel->accelXconv))*180/M_PI;
if (accel->pitchAngle > 0)
{
if (accel->accelYconv < 0)
accel->pitchAngle = 180 - accel->pitchAngle;
}
else
{
if (accel->accelYconv < 0 )
accel->pitchAngle = - 180 - accel->pitchAngle;
}
}
/* Too big forces to calculate on ACC data. Fallback on gyro integration */
else
{
accel->rollAngle += (float)(delta_t * gyro->gyroX) / 1000000.0;
accel->pitchAngle += (float)(delta_t * gyro->gyroY) / 1000000.0;
}
}