Gavan Fantom
62732758a8
This brings in an implementation of some general matrix manipulation routines, not all of which are used at teh moment. Output the DCM on the UART at 50Hz for the host to display it usefully.
154 lines
2.8 KiB
C
154 lines
2.8 KiB
C
/* 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");
|
|
}
|