You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
154 lines
2.8 KiB
154 lines
2.8 KiB
13 years ago
|
/* dcm.c */
|
||
|
|
||
|
#ifdef WE_HAVE_SQRT
|
||
|
#include <math.h>
|
||
|
#endif
|
||
|
#include "matrix.h"
|
||
|
#include "dcm.h"
|
||
|
#include "uart.h"
|
||
|
|
||
|
/* Implementation of the DCM IMU concept as described by Premerlani
|
||
|
* and Bizard
|
||
|
*/
|
||
|
|
||
|
float dcm[3*3] = {1, 0, 0,
|
||
|
0, 1, 0,
|
||
|
0, 0, 1};
|
||
|
|
||
|
float delta_t = 0.01;
|
||
|
|
||
|
void dcm_update(float omega_x, float omega_y, float omega_z)
|
||
|
{
|
||
|
float tx = delta_t * omega_x;
|
||
|
float ty = delta_t * omega_y;
|
||
|
float tz = delta_t * omega_z;
|
||
|
|
||
|
float update_matrix[3*3] = { 0, -tz, ty,
|
||
|
tz, 0, -tx,
|
||
|
-ty, tx, 0};
|
||
|
float temp_matrix[3*3];
|
||
|
|
||
|
matrix_multiply(temp_matrix, dcm, update_matrix, 3, 3, 3);
|
||
|
matrix_add(dcm, dcm, temp_matrix, 3, 3);
|
||
|
|
||
|
dcm_normalise();
|
||
|
}
|
||
|
|
||
|
void dcm_normalise(void)
|
||
|
{
|
||
|
float error;
|
||
|
float tmp[6];
|
||
|
int i;
|
||
|
|
||
|
/* dot product of first two rows */
|
||
|
error = dcm[0]*dcm[3] + dcm[1]*dcm[4] + dcm[2]*dcm[5];
|
||
|
|
||
|
/* printf("error is %f\n", error); */
|
||
|
|
||
|
tmp[0] = dcm[3];
|
||
|
tmp[1] = dcm[4];
|
||
|
tmp[2] = dcm[5];
|
||
|
tmp[3] = dcm[0];
|
||
|
tmp[4] = dcm[1];
|
||
|
tmp[5] = dcm[2];
|
||
|
|
||
|
for (i = 0; i < 6; i++)
|
||
|
dcm[i] = dcm[i] - (tmp[i] * (0.5f * error));
|
||
|
|
||
|
/* third row = cross product of first two rows */
|
||
|
dcm[6] = dcm[1]*dcm[5] - dcm[2]*dcm[4];
|
||
|
dcm[7] = dcm[2]*dcm[3] - dcm[0]*dcm[5];
|
||
|
dcm[8] = dcm[0]*dcm[4] - dcm[1]*dcm[3];
|
||
|
|
||
|
if (!(dcm_renormalise(dcm+0) &&
|
||
|
dcm_renormalise(dcm+3) &&
|
||
|
dcm_renormalise(dcm+6))) {
|
||
|
/* Shit. I've been shot. */
|
||
|
dcm[0] = dcm[4] = dcm[8] = 1.0f;
|
||
|
dcm[1] = dcm[2] = dcm[3] = 0.0f;
|
||
|
dcm[5] = dcm[6] = dcm[7] = 0.0f;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
bool dcm_renormalise(float *v)
|
||
|
{
|
||
|
float f = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
|
||
|
|
||
|
/* printf("f is %f\n", f); */
|
||
|
|
||
|
if (f < 1.5625f && f > 0.64f) {
|
||
|
f = 0.5 * (3 - f);
|
||
|
#ifdef WE_HAVE_SQRT
|
||
|
} else if (f < 100.0f && f > 0.01f) {
|
||
|
f = 1.0 / sqrt(f);
|
||
|
/* XXX log this event? */
|
||
|
#endif
|
||
|
} else {
|
||
|
putstr("problem\r\n");
|
||
|
return FALSE;
|
||
|
}
|
||
|
|
||
|
v[0] = v[0] * f;
|
||
|
v[1] = v[1] * f;
|
||
|
v[2] = v[2] * f;
|
||
|
|
||
|
return TRUE;
|
||
|
}
|
||
|
|
||
|
void dcm_dump(void)
|
||
|
{
|
||
|
putstr("dcm: ");
|
||
|
putint_s((int)(dcm[0] * 100000.0f));
|
||
|
putstr(", ");
|
||
|
putint_s((int)(dcm[1] * 100000.0f));
|
||
|
putstr(", ");
|
||
|
putint_s((int)(dcm[2] * 100000.0f));
|
||
|
putstr("\r\n ");
|
||
|
putint_s((int)(dcm[3] * 100000.0f));
|
||
|
putstr(", ");
|
||
|
putint_s((int)(dcm[4] * 100000.0f));
|
||
|
putstr(", ");
|
||
|
putint_s((int)(dcm[5] * 100000.0f));
|
||
|
putstr("\r\n ");
|
||
|
putint_s((int)(dcm[6] * 100000.0f));
|
||
|
putstr(", ");
|
||
|
putint_s((int)(dcm[7] * 100000.0f));
|
||
|
putstr(", ");
|
||
|
putint_s((int)(dcm[8] * 100000.0f));
|
||
|
putstr("\r\n");
|
||
|
}
|
||
|
|
||
|
void puthexfloat(float f)
|
||
|
{
|
||
|
union {
|
||
|
float f;
|
||
|
unsigned int i;
|
||
|
} u;
|
||
|
|
||
|
u.f = f;
|
||
|
puthex(u.i);
|
||
|
}
|
||
|
|
||
|
void dcm_send_packet(void)
|
||
|
{
|
||
|
putstr("D:(");
|
||
|
puthexfloat(dcm[0]);
|
||
|
putstr(",");
|
||
|
puthexfloat(dcm[1]);
|
||
|
putstr(",");
|
||
|
puthexfloat(dcm[2]);
|
||
|
putstr(",");
|
||
|
puthexfloat(dcm[3]);
|
||
|
putstr(",");
|
||
|
puthexfloat(dcm[4]);
|
||
|
putstr(",");
|
||
|
puthexfloat(dcm[5]);
|
||
|
putstr(",");
|
||
|
puthexfloat(dcm[6]);
|
||
|
putstr(",");
|
||
|
puthexfloat(dcm[7]);
|
||
|
putstr(",");
|
||
|
puthexfloat(dcm[8]);
|
||
|
putstr(")\r\n");
|
||
|
}
|