rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

Get simple attitude estimation working on F4 INS. Also make MPU6050 return

Browse Source 
scaling that casts to degrees.
This commit is contained in:
James Cotton 2011-11-16 11:36:04 -06:00
parent 8e79f86716
commit 1deb799b6f
2 changed files with 70 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
flight/Modules/Attitude/revolution/attitude.c
View File

@ -58,10 +58,11 @@
#include "CoordinateConversions.h"
// Private constants
#define STACK_SIZE_BYTES 540
#define STACK_SIZE_BYTES 1540
#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
#define PI_MOD(x) (fmod(x + M_PI, M_PI * 2) - M_PI)
#define F_PI 3.14159265358979323846f
#define PI_MOD(x) (fmod(x + F_PI, F_PI * 2) - F_PI)
// Private types
// Private variables
@ -81,7 +82,6 @@ static float accelKp = 0;
static float yawBiasRate = 0;
static float gyroGain = 0.42;
static int16_t accelbias[3];
static float q[4] = {1,0,0,0};
static float R[3][3];
static int8_t rotate = 0;
static bool zero_during_arming = false;
@ -127,10 +127,6 @@ int32_t AttitudeInitialize(void)
gyro_correct_int[1] = 0;
gyro_correct_int[2] = 0;
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
for(uint8_t i = 0; i < 3; i++)
for(uint8_t j = 0; j < 3; j++)
R[i][j] = 0;
@ -200,7 +196,7 @@ static void AttitudeTask(void *parameters)
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
else {
// Only update attitude when sensor data is good
//updateAttitude(&attitudeRaw);
updateAttitude(&attitudeRaw);
AttitudeRawSet(&attitudeRaw);
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
@ -253,8 +249,8 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
// Not the swaping of channel orders
scaling = PIOS_BMA180_GetScale() / accel_samples;
attitudeRaw->accels[ATTITUDERAW_ACCELS_X] = accel_accum[0] * scaling;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] = accel_accum[1] * scaling;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = accel_accum[2] * scaling;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] = -accel_accum[1] * scaling;
attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = -accel_accum[2] * scaling;
@ -331,12 +327,19 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
return 0;
}
float accel_mag;
float qmag;
static void updateAttitude(AttitudeRawData * attitudeRaw)
{
float dT;
portTickType thisSysTime = xTaskGetTickCount();
static portTickType lastSysTime = 0;
float q[4];
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
dT = (thisSysTime == lastSysTime) ? 0.001 : (portMAX_DELAY & (thisSysTime - lastSysTime)) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
@ -346,60 +349,62 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
gyro[1] = attitudeRaw->gyros[1];
gyro[2] = attitudeRaw->gyros[2];
{
float * accels = attitudeRaw->accels;
float grot[3];
float accel_err[3];
float accels[3];
accels[0] = attitudeRaw->accels[0];
accels[1] = attitudeRaw->accels[1];
accels[2] = attitudeRaw->accels[2];
float grot[3];
float accel_err[3];
// Get the current attitude estimate
quat_copy(&attitudeActual.q1, q);
// Rotate gravity to body frame and cross with accels
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
CrossProduct((const float *) accels, (const float *) grot, accel_err);
// Rotate gravity to body frame and cross with accels
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
CrossProduct((const float *) accels, (const float *) grot, accel_err);
// Account for accel magnitude
float accel_mag = sqrt(accels[0]*accels[0] + accels[1]*accels[1] + accels[2]*accels[2]);
accel_err[0] /= accel_mag;
accel_err[1] /= accel_mag;
accel_err[2] /= accel_mag;
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
gyro_correct_int[0] += accel_err[0] * accelKi;
gyro_correct_int[1] += accel_err[1] * accelKi;
//gyro_correct_int[2] += accel_err[2] * settings.AccelKI * dT;
// Correct rates based on error, integral component dealt with in updateSensors
gyro[0] += accel_err[0] * accelKp / dT;
gyro[1] += accel_err[1] * accelKp / dT;
gyro[2] += accel_err[2] * accelKp / dT;
}
{ // scoping variables to save memory
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]) * dT * M_PI / 180 / 2;
qdot[1] = (q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * M_PI / 180 / 2;
qdot[2] = (q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * M_PI / 180 / 2;
qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2;
// Take a time step
q[0] = q[0] + qdot[0];
q[1] = q[1] + qdot[1];
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
// Account for accel magnitude
accel_mag = accels[0]*accels[0] + accels[1]*accels[1] + accels[2]*accels[2];
accel_mag = sqrtf(accel_mag);
accel_err[0] /= accel_mag;
accel_err[1] /= accel_mag;
accel_err[2] /= accel_mag;
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
gyro_correct_int[0] += accel_err[0] * accelKi;
gyro_correct_int[1] += accel_err[1] * accelKi;
if(q[0] < 0) {
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
}
// Correct rates based on error, integral component dealt with in updateSensors
gyro[0] += accel_err[0] * accelKp / dT;
gyro[1] += accel_err[1] * accelKp / dT;
gyro[2] += accel_err[2] * accelKp / dT;
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]) * dT * F_PI / 180 / 2;
qdot[1] = (q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * F_PI / 180 / 2;
qdot[2] = (q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * F_PI / 180 / 2;
qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * F_PI / 180 / 2;
// Take a time step
q[0] = q[0] + qdot[0];
q[1] = q[1] + qdot[1];
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
if(q[0] < 0) {
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
// Renomalize
float qmag = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
@ -407,16 +412,13 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if((fabs(qmag) < 1e-3) || (qmag != qmag)) {
if((fabs(qmag) < 1.0e-3f) || (qmag != qmag)) {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
quat_copy(q, &attitudeActual.q1);
// Convert into eueler degrees (makes assumptions about RPY order)

18
flight/PiOS/STM32F4xx/pios_mpu6050.c
View File

@ -267,13 +267,13 @@ float PIOS_MPU6050_GetScale()
{
switch (cfg->gyro_range) {
case PIOS_MPU6050_SCALE_250_DEG:
return DEG_TO_RAD / 131.0;
return 1.0f / 131.0f;
case PIOS_MPU6050_SCALE_500_DEG:
return DEG_TO_RAD / 65.5;
return 1.0f / 65.5f;
case PIOS_MPU6050_SCALE_1000_DEG:
return DEG_TO_RAD / 32.8;
return 1.0f / 32.8f;
case PIOS_MPU6050_SCALE_2000_DEG:
return DEG_TO_RAD / 16.4;
return 1.0f / 16.4f;
}
return 0;
}
@ -336,16 +336,6 @@ void PIOS_MPU6050_IRQHandler(void)
cb_not_ready++;
return;
}
/*// If at least one read doesnt succeed then the irq not reset and we will stall
if(PIOS_MPU6050_Read(PIOS_MPU6050_FIFO_CNT_MSB, (uint8_t *) &fifo_level_data, sizeof(fifo_level_data)) != 0)
return;
fifo_level = (fifo_level_data[0] << 8) + fifo_level_data[1];
PIOS_DELAY_WaituS(10);
*/
// Leave footer in buffer
PIOS_MPU6050_Read_Callback(PIOS_MPU6050_FIFO_REG, mpu6050_read_buffer, sizeof(mpu6050_read_buffer), MPU6050_callback);