Raw calibration data visualization
This commit is contained in:
PaulStoffregen
parent
81a42de4eb
commit
2a0251ff6a
1 changed files with 208 additions and 10 deletions
218
imuread.c
218
imuread.c
|
|
@ -5,6 +5,7 @@
|
|||
#include <string.h>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
#include <errno.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue