From 2a0251ff6a6d1e9b78d70a22e739894cfd7e83ad Mon Sep 17 00:00:00 2001 From: PaulStoffregen Date: Sun, 14 Feb 2016 09:22:11 -0800 Subject: [PATCH] Raw calibration data visualization --- imuread.c | 218 +++++++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 208 insertions(+), 10 deletions(-) diff --git a/imuread.c b/imuread.c index fdbf933..4003a88 100644 --- a/imuread.c +++ b/imuread.c @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -14,6 +15,28 @@ #define PORT "/dev/ttyACM0" #define TIMEOUT_MSEC 40 + +#define MAGBUFFSIZEX 14 +#define MAGBUFFSIZEY 28 +#define MAGBUFFSIZE (MAGBUFFSIZEX * MAGBUFFSIZEY) + +typedef struct { + float x; + float y; + float z; + int valid; +} magdata_t; +magdata_t caldata[MAGBUFFSIZE]; +magdata_t hard_iron; +magdata_t current_position; +typedef struct { + float q1; + float q2; + float q3; + float q4; +} quat_t; +quat_t current_orientation; + int fd=-1; void die(const char *format, ...) __attribute__ ((format (printf, 1, 2))); void *malloc_or_die(size_t size); @@ -30,10 +53,79 @@ void print_data(const char *name, const unsigned char *data, int len) } +void packet_primary_data(const unsigned char *data) +{ + int16_t x, y, z, w; + + x = (data[13] << 8) | data[12]; + y = (data[15] << 8) | data[14]; + z = (data[17] << 8) | data[16]; + current_position.x = (float)x / 10.0f; + current_position.y = (float)y / 10.0f; + current_position.z = (float)z / 10.0f; + current_orientation.q1 = (float)((int16_t)((data[25] << 8) | data[24])) / 30000.0f; + current_orientation.q2 = (float)((int16_t)((data[27] << 8) | data[26])) / 30000.0f; + current_orientation.q3 = (float)((int16_t)((data[29] << 8) | data[28])) / 30000.0f; + current_orientation.q4 = (float)((int16_t)((data[31] << 8) | data[30])) / 30000.0f; +#if 0 + printf("mag data, %5.2f %5.2f %5.2f\n", + current_position.x, + current_position.y, + current_position.z + ); +#endif +#if 0 + printf("orientation: %5.3f %5.3f %5.3f %5.3f\n", + current_orientation.q1, + current_orientation.q2, + current_orientation.q3, + current_orientation.q4 + ); +#endif +} + +void packet_magnetic_cal(const unsigned char *data) +{ + int16_t id, x, y, z; + magdata_t *cal; + float newx, newy, newz; + + id = (data[7] << 8) | data[6]; + x = (data[9] << 8) | data[8]; + y = (data[11] << 8) | data[10]; + z = (data[13] << 8) | data[12]; + + if (id == 1) { + cal = &hard_iron; + cal->x = (float)x / 10.0f; + cal->y = (float)y / 10.0f; + cal->z = (float)z / 10.0f; + cal->valid = 1; + } else if (id >= 10 && id < MAGBUFFSIZE+10) { + newx = (float)x / 10.0f; + newy = (float)y / 10.0f; + newz = (float)z / 10.0f; + cal = &caldata[id - 10]; + if (!cal->valid || cal->x != newx || cal->y != newy || cal->z != newz) { + cal->x = newx; + cal->y = newy; + cal->z = newz; + cal->valid = 1; + printf("mag cal, id=%3d: %5d %5d %5d\n", id, x, y, z); + } + } +} + void packet(const unsigned char *data, int len) { - print_data("packet", data, len); + if (len <= 0) return; + //print_data("packet", data, len); + if (data[0] == 1 && len == 34) { + packet_primary_data(data); + } else if (data[0] == 6 && len == 14) { + packet_magnetic_cal(data); + } // TODO: actually do something with the arriving data... } @@ -111,41 +203,126 @@ void newdata(const unsigned char *data, int len) } } +void resize_callback(int width, int height) +{ + const float ar = (float) width / (float) height; + + glViewport(0, 0, width, height); + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + glFrustum(-ar, ar, -1.0, 1.0, 2.0, 100.0); + + glMatrixMode(GL_MODELVIEW); + glLoadIdentity() ; +} + +/* +typedef struct { + float x; + float y; + float z; + int valid; +} magdata_t; +magdata_t caldata[MAGBUFFSIZE]; +*/ + void display_callback(void) { - glClear(GL_COLOR_BUFFER_BIT); // clear the display - glColor3f(1.0, 1.0, 1.0); // set current color to white + int i; + float x, y, z; + float xscale, yscale, zscale; + float xoff, yoff, zoff; + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glColor3f(1, 0, 0); // set current color to black +#if 0 glBegin(GL_POLYGON); // draw filled triangle glVertex2i(200, 125); // specify each vertex of triangle glVertex2i(100, 375); glVertex2i(300, 375); glEnd(); // OpenGL draws the filled triangle - glFlush(); // Complete any pending operations +#endif + + xscale = 0.05; + yscale = 0.05; + zscale = 0.05; + xoff = 0.0; + yoff = 0.0; + zoff = -7.0; + + if (hard_iron.valid) { + for (i=0; i < MAGBUFFSIZE; i++) { + if (caldata[i].valid) { + x = caldata[i].x - hard_iron.x; + y = caldata[i].y - hard_iron.y; + z = caldata[i].z - hard_iron.z; + glPushMatrix(); + glTranslatef( + x * xscale + xoff, + y * yscale + yoff, + z * zscale + zoff + ); + glutSolidSphere(0.08,16,16); // radius, slices, stacks + glPopMatrix(); + } + } + } + //glTranslated(0.0,1.2,-30); + //glutSolidSphere(1,20,20); // radius, slices, stacks + //glutWireSphere(3,20,20); // radius, slices, stacks + + + glutSwapBuffers(); } + + void timer_callback(int val) { unsigned char buf[256]; + static int nodata_count=0; int n; - printf("timer_callback\n"); + //printf("timer_callback\n"); glutTimerFunc(TIMEOUT_MSEC, timer_callback, 0); while (fd >= 0) { n = read(fd, buf, sizeof(buf)); if (n > 0 && n <= sizeof(buf)) { //printf("read %d bytes\n", r); newdata(buf, n); + nodata_count = 0; } else if (n == 0) { printf("read no data\n"); + if (++nodata_count > 6) { + close(fd); + fd = -1; + die("No data from %s\n", PORT); + } break; } else { - printf("read error\n"); - break; + n = errno; + if (n == EAGAIN) { + break; + } else if (n == EAGAIN) { + } else { + printf("read error, n=%d\n", n); + break; + } } } glutPostRedisplay(); } +const GLfloat light_ambient[] = { 0.0f, 0.0f, 0.0f, 1.0f }; +const GLfloat light_diffuse[] = { 1.0f, 1.0f, 1.0f, 1.0f }; +const GLfloat light_specular[] = { 1.0f, 1.0f, 1.0f, 1.0f }; +const GLfloat light_position[] = { 2.0f, 5.0f, 5.0f, 0.0f }; + +const GLfloat mat_ambient[] = { 0.7f, 0.7f, 0.7f, 1.0f }; +const GLfloat mat_diffuse[] = { 0.8f, 0.8f, 0.8f, 1.0f }; +const GLfloat mat_specular[] = { 1.0f, 1.0f, 1.0f, 1.0f }; +const GLfloat high_shininess[] = { 100.0f }; + int main(int argc, char *argv[]) { int r; @@ -155,11 +332,32 @@ int main(int argc, char *argv[]) //unsigned char buf[256]; glutInit(&argc, argv); - glutInitDisplayMode(GLUT_RGBA); + glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH); glutInitWindowSize(600, 500); glutCreateWindow("IMU Read"); - glClearColor(0.0, 0.0, 0.0, 0.0); - gluOrtho2D(0, 600, 0, 500); + glClearColor(1.0, 1.0, 1.0, 1.0); + glEnable(GL_CULL_FACE); + glCullFace(GL_BACK); + + //glShadeModel(GL_FLAT); + //gluOrtho2D(0, 600, 0, 500); + + glEnable(GL_DEPTH_TEST); + glDepthFunc(GL_LESS); + glEnable(GL_LIGHT0); + //glEnable(GL_NORMALIZE); + glEnable(GL_COLOR_MATERIAL); + glEnable(GL_LIGHTING); + glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient); + glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse); + glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular); + glLightfv(GL_LIGHT0, GL_POSITION, light_position); + glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient); + glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuse); + glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular); + glMaterialfv(GL_FRONT, GL_SHININESS, high_shininess); + + glutReshapeFunc(resize_callback); glutDisplayFunc(display_callback); glutTimerFunc(TIMEOUT_MSEC, timer_callback, 0);