jlpoole/MotionCal
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Experimental (and ultimately ineffective) fiddling with axes

Browse source
This commit is contained in:
PaulStoffregen 2018-01-23 06:14:51 -08:00
commit 8ee5e0bf55
2 changed files with 85 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

59
imuread.c
View file

@ -21,8 +21,67 @@ static void glut_display_callback(void)
glutSwapBuffers();
}
extern int invert_q0;
extern int invert_q1;
extern int invert_q2;
extern int invert_q3;
extern int invert_x;
extern int invert_y;
extern int invert_z;
static void print_invert_state(void)
{
printf("Invert: %s %s %s %s %s %s %s\n",
(invert_q0 ? "Q0" : " "),
(invert_q1 ? "Q1" : " "),
(invert_q2 ? "Q2" : " "),
(invert_q3 ? "Q3" : " "),
(invert_x ? "x'" : " "),
(invert_y ? "y'" : " "),
(invert_z ? "z'" : " ")
);
}
static void glut_keystroke_callback(unsigned char ch, int x, int y)
{
if (ch == '0') {
invert_q0 ^= 1;
print_invert_state();
return;
}
if (ch == '1') {
invert_q1 ^= 1;
print_invert_state();
return;
}
if (ch == '2') {
invert_q2 ^= 1;
print_invert_state();
return;
}
if (ch == '3') {
invert_q3 ^= 1;
print_invert_state();
return;
}
if (ch == 'x') {
invert_x ^= 1;
print_invert_state();
return;
}
if (ch == 'y') {
invert_y ^= 1;
print_invert_state();
return;
}
if (ch == 'z') {
invert_z ^= 1;
print_invert_state();
return;
}
if (magcal.FitError > 9.0) {
printf("Poor Calibration: ");
printf("soft iron fit error = %.1f%%\n", magcal.FitError);

30
visualize.c
View file

@ -46,6 +46,14 @@ static void rotate(const Point_t *in, Point_t *out, const float *rmatrix)
static GLuint spherelist;
static GLuint spherelowreslist;
int invert_q0=0;
int invert_q1=0;
int invert_q2=0;
int invert_q3=1;
int invert_x=0;
int invert_y=0;
int invert_z=0;
void display_callback(void)
{
int i;
@ -76,11 +84,22 @@ void display_callback(void)
memcpy(&orientation, &current_orientation, sizeof(orientation));
// TODO: this almost but doesn't perfectly seems to get the
// real & screen axes in sync....
//orientation.q0 *= -1.0f;
//orientation.q1 *= -1.0f;
//orientation.q2 *= -1.0f;
orientation.q3 *= -1.0f;
if (invert_q0) orientation.q0 *= -1.0f;
if (invert_q1) orientation.q1 *= -1.0f;
if (invert_q2) orientation.q2 *= -1.0f;
if (invert_q3) orientation.q3 *= -1.0f;
quad_to_rotation(&orientation, rotation);
//rotation[0] *= -1.0f;
//rotation[1] *= -1.0f;
//rotation[2] *= -1.0f;
//rotation[3] *= -1.0f;
//rotation[4] *= -1.0f;
//rotation[5] *= -1.0f;
//rotation[6] *= -1.0f;
//rotation[7] *= -1.0f;
//rotation[8] *= -1.0f;
for (i=0; i < MAGBUFFSIZE; i++) {
if (magcal.valid[i]) {
apply_calibration(magcal.BpFast[0][i], magcal.BpFast[1][i],
@ -91,6 +110,9 @@ void display_callback(void)
quality_update(&point);
rotate(&point, &draw, rotation);
glPushMatrix();
if (invert_x) draw.x *= -1.0f;
if (invert_y) draw.y *= -1.0f;
if (invert_z) draw.z *= -1.0f;
glTranslatef(
draw.x * xscale + xoff,
draw.z * yscale + yoff,