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

Map sensor and screen axes better (but still not perfect...)

Browse source
This commit is contained in:
PaulStoffregen 2016-03-14 19:07:10 -07:00
commit 1a666bdd3b
2 changed files with 19 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

2
magcal.c
View file

@ -75,7 +75,7 @@ void MagCal_Run(void)
if (magcal.ValidMagCal) { if (magcal.ValidMagCal) {
// age the existing fit error to avoid one good calibration locking out future updates // age the existing fit error to avoid one good calibration locking out future updates
magcal.FitErrorAge *= 1.01f; magcal.FitErrorAge *= 1.02f;
} }
// is enough data collected // is enough data collected

20
visualize.c
View file

@ -100,11 +100,17 @@ void display_callback(void)
float longitude[MAGBUFFSIZE]; // -PI to +PI float longitude[MAGBUFFSIZE]; // -PI to +PI
float sumx=0.0, sumy=0.0, sumz=0.0, summag=0.0; float sumx=0.0, sumy=0.0, sumz=0.0, summag=0.0;
int spheredist[100]; int spheredist[100];
Quaternion_t orientation;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glColor3f(1, 0, 0); // set current color to red glColor3f(1, 0, 0); // set current color to red
memset(spheredist, 0, sizeof(spheredist)); memset(spheredist, 0, sizeof(spheredist));
glLoadIdentity();
#if 0
gluLookAt(0.0, 0.0, 0.8, // eye location
0.0, 0.0, 0.0, // center
0.0, 1.0, 0.0); // up direction
#endif
xscale = 0.05; xscale = 0.05;
yscale = 0.05; yscale = 0.05;
zscale = 0.05; zscale = 0.05;
@ -114,13 +120,23 @@ void display_callback(void)
//if (hard_iron.valid) { //if (hard_iron.valid) {
if (1) { if (1) {
quad_to_rotation(&current_orientation, rotation); 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;
quad_to_rotation(&orientation, rotation);
for (i=0; i < MAGBUFFSIZE; i++) { for (i=0; i < MAGBUFFSIZE; i++) {
//if (caldata[i].valid) { //if (caldata[i].valid) {
if (magcal.valid[i]) { if (magcal.valid[i]) {
//apply_calibration(&caldata[i], &point); //apply_calibration(&caldata[i], &point);
apply_calibration(magcal.BpFast[0][i], magcal.BpFast[1][i], apply_calibration(magcal.BpFast[0][i], magcal.BpFast[1][i],
magcal.BpFast[2][i], &point); magcal.BpFast[2][i], &point);
//point.x *= -1.0f;
//point.y *= -1.0f;
//point.z *= -1.0f;
sumx += point.x; sumx += point.x;
sumy += point.y; sumy += point.y;
sumz += point.z; sumz += point.z;