coolfactor/quadrotor
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Take interleaved accelerometer and gyro readings, and integrate them into

Browse Source 
a single attitude estimation.

A bunch of debug stuff, too.
This commit is contained in:
Gavan Fantom
2011-11-04 12:47:16 +00:00
parent 62732758a8
commit 2890007195
7 changed files with 223 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

113
dcm.c
View File

@@ -2,11 +2,20 @@
#ifdef WE_HAVE_SQRT
#include <math.h>
#else
#include "fisqrt.h"
#endif
#include "matrix.h"
#include "dcm.h"
#include "uart.h"
#define GRAVITY 9.80665f
#define KP_ROLLPITCH 0.05967
#define KI_ROLLPITCH 0.00001278
#define ERROR_LIMIT 1.17f
/* Implementation of the DCM IMU concept as described by Premerlani
* and Bizard
*/
@@ -15,10 +24,17 @@ float dcm[3*3] = {1, 0, 0,
0, 1, 0,
0, 0, 1};
float omega_p[3] = {0.0, 0.0, 0.0};
float omega_i[3] = {0.0, 0.0, 0.0};
float delta_t = 0.01;
void dcm_update(float omega_x, float omega_y, float omega_z)
void dcm_update(float x, float y, float z)
{
float omega_x = x + omega_i[0] + omega_p[0];
float omega_y = y + omega_i[1] + omega_p[1];
float omega_z = z + omega_i[2] + omega_p[2];
float tx = delta_t * omega_x;
float ty = delta_t * omega_y;
float tz = delta_t * omega_z;
@@ -34,6 +50,36 @@ void dcm_update(float omega_x, float omega_y, float omega_z)
dcm_normalise();
}
void dcm_setvector(float x, float y, float z)
{
/* We're given the Z axis */
dcm[6] = x;
dcm[7] = y;
dcm[8] = z;
/* Second row = cross product of unit X and third rows */
dcm[3] = 0.0;
dcm[4] = -dcm[8];
dcm[5] = dcm[7];
/* First row = cross product of third and second rows */
dcm[0] = dcm[7]*dcm[5] - dcm[8]*dcm[4];
dcm[1] = dcm[8]*dcm[3] - dcm[6]*dcm[5];
dcm[2] = dcm[6]*dcm[4] - dcm[7]*dcm[3];
/* Second row = cross product of third and first rows */
dcm[3] = dcm[7]*dcm[2] - dcm[8]*dcm[1];
dcm[4] = dcm[8]*dcm[0] - dcm[6]*dcm[2];
dcm[5] = dcm[6]*dcm[1] - dcm[7]*dcm[0];
dcm_renormalise(dcm+0);
dcm_renormalise(dcm+3);
dcm_renormalise(dcm+6);
#if 0
dcm_normalise();
#endif
}
void dcm_normalise(void)
{
float error;
@@ -78,11 +124,14 @@ bool dcm_renormalise(float *v)
if (f < 1.5625f && f > 0.64f) {
f = 0.5 * (3 - f);
#ifdef WE_HAVE_SQRT
} else if (f < 100.0f && f > 0.01f) {
#ifdef WE_HAVE_SQRT
f = 1.0 / sqrt(f);
/* XXX log this event? */
#else
f = fisqrt(f);
#endif
/* XXX log this event? */
putstr("sqrt\r\n");
} else {
putstr("problem\r\n");
return FALSE;
@@ -95,6 +144,64 @@ bool dcm_renormalise(float *v)
return TRUE;
}
void dcm_drift_correction(float x, float y, float z)
{
float mag;
float weight;
float error[3];
int i;
mag = (1.0/fisqrt(x*x+y*y+z*z)) / GRAVITY;
mag = 1-mag;
if (mag < 0.0)
mag = -mag;
weight = 1 - 3*mag;
if (weight < 0.0)
weight = 0.0;
if (weight > 1.0)
weight = 1.0;
/* error = cross product of dcm last row and acceleration vector */
/* third row = cross product of first two rows */
error[0] = dcm[7]*z - dcm[8]*y;
error[1] = dcm[8]*x - dcm[6]*z;
error[2] = dcm[6]*y - dcm[7]*x;
for (i = 0; i < 3; i++) {
if (error[i] > ERROR_LIMIT)
error[i] = ERROR_LIMIT;
if (error[i] < -ERROR_LIMIT)
error[i] = -ERROR_LIMIT;
}
for (i = 0; i < 3; i++) {
omega_p[i] = error[i] * (KP_ROLLPITCH * weight);
omega_i[i] += error[i] * (KI_ROLLPITCH * weight);
}
putstr("w: ");
putint_s((int)(weight * 100000.0f));
putstr("\r\n");
#if 0
putstr("p: ");
putint_s((int)(omega_p[0] * 100000.0f));
putstr(", ");
putint_s((int)(omega_p[1] * 100000.0f));
putstr(", ");
putint_s((int)(omega_p[2] * 100000.0f));
putstr(" i: ");
putint_s((int)(omega_i[0] * 100000.0f));
putstr(", ");
putint_s((int)(omega_i[1] * 100000.0f));
putstr(", ");
putint_s((int)(omega_i[2] * 100000.0f));
putstr("\r\n");
#endif
}
void dcm_dump(void)
{
putstr("dcm: ");