diff --git a/Makefile b/Makefile index d8a6369..78ed4f4 100644 --- a/Makefile +++ b/Makefile @@ -1,8 +1,16 @@ CC = gcc CFLAGS = -O2 -Wall +CPPFLAGS = `$(WXCONFIG) --cppflags` +WXCONFIG = ~/wxwidgets/3.0.2.gtk2-opengl/bin/wx-config -imuread: imuread.o serialdata.o + +all: gui imuread + +gui: gui.o visualize.o serialdata.o + g++ $(CFLAGS) -o $@ $^ `$(WXCONFIG) --libs` -lglut -lGLU -lGL + +imuread: imuread.o visualize.o serialdata.o $(CC) $(CFLAGS) -o $@ $^ -lglut -lGLU -lGL clean: - rm -f imuread *.o + rm -f gui imuread *.o diff --git a/gui.cpp b/gui.cpp new file mode 100644 index 0000000..ecbcbd7 --- /dev/null +++ b/gui.cpp @@ -0,0 +1,59 @@ +#include "gui.h" +#include "imuread.h" + +BEGIN_EVENT_TABLE(MyFrame,wxFrame) + EVT_MENU(wxID_ABOUT, MyFrame::OnAbout) + EVT_MENU(wxID_EXIT, MyFrame::OnQuit) +END_EVENT_TABLE() + + +MyFrame::MyFrame(wxWindow *parent, wxWindowID id, const wxString &title, + const wxPoint &position, const wxSize& size, long style) : + wxFrame( parent, id, title, position, size, style ) +{ + +} + +void MyFrame::OnAbout(wxCommandEvent &event) +{ + wxMessageDialog dialog(this, + "IMU Read\n\n\nCopyright 2016, PJRC.COM, LLC.", + "About IMUREAD", wxOK|wxICON_INFORMATION); + dialog.ShowModal(); +} + +void MyFrame::OnQuit( wxCommandEvent &event ) +{ + Close(true); +} + +MyFrame::~MyFrame(void) +{ +} + + + + +IMPLEMENT_APP(MyApp) + +MyApp::MyApp() +{ +} + +bool MyApp::OnInit() +{ + // make sure we exit properly on macosx + SetExitOnFrameDelete(true); + + wxPoint pos(100, 100); + + MyFrame *frame = new MyFrame(NULL, -1, wxT("Teensy"), pos, wxDefaultSize, + wxDEFAULT_FRAME_STYLE & ~(wxMAXIMIZE_BOX | wxRESIZE_BORDER)); + frame->Show( true ); + return true; +} + +int MyApp::OnExit() +{ + return 0; +} diff --git a/gui.h b/gui.h new file mode 100644 index 0000000..71b942d --- /dev/null +++ b/gui.h @@ -0,0 +1,33 @@ +#ifndef gui__h_ +#define gui__h_ + +#include + + +class MyFrame: public wxFrame +{ +public: + MyFrame(wxWindow *parent, wxWindowID id, + const wxString &title, + const wxPoint &pos = wxDefaultPosition, + const wxSize &size = wxDefaultSize, + long style = wxDEFAULT_FRAME_STYLE); + ~MyFrame(void); +private: + void OnAbout(wxCommandEvent &event); + void OnQuit(wxCommandEvent &event); + DECLARE_EVENT_TABLE() +}; + + +class MyApp: public wxApp +{ +public: + MyApp(); + virtual bool OnInit(); + virtual int OnExit(); +private: + //wxSingleInstanceChecker *m_instance; +}; + +#endif diff --git a/imuread.c b/imuread.c index ad155d1..8fa1df6 100644 --- a/imuread.c +++ b/imuread.c @@ -1,100 +1,5 @@ #include "imuread.h" -magdata_t caldata[MAGBUFFSIZE]; -magdata_t hard_iron; -magdata_t current_position; -quat_t current_orientation; - - -void quad_to_rotation(const quat_t *quat, float *rmatrix) -{ - float qx = quat->x; - float qy = quat->y; - float qz = quat->z; - float qw = quat->w; - - rmatrix[0] = 1.0f - 2.0f * qy * qy - 2.0f * qz * qz; - rmatrix[1] = 2.0f * qx * qy - 2.0f * qz * qw; - rmatrix[2] = 2.0f * qx * qz + 2.0f * qy * qw; - rmatrix[3] = 2.0f * qx * qy + 2.0f * qz * qw; - rmatrix[4] = 1.0f - 2.0f * qx * qx - 2.0f * qz * qz; - rmatrix[5] = 2.0f * qy * qz - 2.0f * qx * qw; - rmatrix[6] = 2.0f * qx * qz - 2.0f * qy * qw; - rmatrix[7] = 2.0f * qy * qz + 2.0f * qx * qw; - rmatrix[8] = 1.0f - 2.0f * qx * qx - 2.0f * qy * qy; -} - -void rotate(const magdata_t *in, magdata_t *out, const float *rmatrix) -{ - if (out == NULL) return; - if (in == NULL || in->valid == 0) { - out->valid = 0; - out->x = out->y = out->z = 0; - return; - } - out->x = in->x * rmatrix[0] + in->y * rmatrix[1] + in->z * rmatrix[2]; - out->y = in->x * rmatrix[3] + in->y * rmatrix[4] + in->z * rmatrix[5]; - out->z = in->x * rmatrix[6] + in->y * rmatrix[7] + in->z * rmatrix[8]; - out->valid = 1; -} - - -/* -typedef struct { - float x; - float y; - float z; - int valid; -} magdata_t; -magdata_t caldata[MAGBUFFSIZE]; -*/ - -void display_callback(void) -{ - int i; - //float x, y, z; - float xscale, yscale, zscale; - float xoff, yoff, zoff; - float rotation[9]; - magdata_t point, draw; - - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - glColor3f(1, 0, 0); // set current color to red - - xscale = 0.05; - yscale = 0.05; - zscale = 0.05; - xoff = 0.0; - yoff = 0.0; - zoff = -7.0; - - if (hard_iron.valid) { - quad_to_rotation(¤t_orientation, rotation); - for (i=0; i < MAGBUFFSIZE; i++) { - if (caldata[i].valid) { - point.x = caldata[i].x - hard_iron.x; - point.y = caldata[i].y - hard_iron.y; - point.z = caldata[i].z - hard_iron.z; - point.valid = 1; - // TODO: apply soft iron cal - - memcpy(&draw, &point, sizeof(draw)); - rotate(&point, &draw, rotation); - - glPushMatrix(); - glTranslatef( - draw.x * xscale + xoff, - draw.z * yscale + yoff, - draw.y * zscale + zoff - ); - glutSolidSphere(0.08,16,16); // radius, slices, stacks - glPopMatrix(); - } - } - } - glutSwapBuffers(); -} - void timer_callback(int val) { glutTimerFunc(TIMEOUT_MSEC, timer_callback, 0); @@ -115,41 +20,14 @@ void resize_callback(int width, int height) glLoadIdentity() ; } -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[]) { glutInit(&argc, argv); glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH); glutInitWindowSize(600, 500); glutCreateWindow("IMU Read"); - 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); + + visualize_init(); glutReshapeFunc(resize_callback); glutDisplayFunc(display_callback); diff --git a/imuread.h b/imuread.h index 3422cd9..c06c744 100644 --- a/imuread.h +++ b/imuread.h @@ -22,6 +22,10 @@ #define MAGBUFFSIZEY 28 #define MAGBUFFSIZE (MAGBUFFSIZEX * MAGBUFFSIZEY) +#ifdef __cplusplus +extern "C"{ +#endif + typedef struct { float x; float y; @@ -46,6 +50,13 @@ extern void rotate(const magdata_t *in, magdata_t *out, const float *rmatrix); extern int open_port(const char *name); extern void read_serial_data(void); extern void close_port(void); +void visualize_init(void); +void display_callback(void); +void resize_callback(int width, int height); +void timer_callback(int val); +#ifdef __cplusplus +} // extern "C" +#endif #endif diff --git a/visualize.c b/visualize.c new file mode 100644 index 0000000..da8db92 --- /dev/null +++ b/visualize.c @@ -0,0 +1,133 @@ +#include "imuread.h" + +magdata_t caldata[MAGBUFFSIZE]; +magdata_t hard_iron; +magdata_t current_position; +quat_t current_orientation; + + +void quad_to_rotation(const quat_t *quat, float *rmatrix) +{ + float qx = quat->x; + float qy = quat->y; + float qz = quat->z; + float qw = quat->w; + + rmatrix[0] = 1.0f - 2.0f * qy * qy - 2.0f * qz * qz; + rmatrix[1] = 2.0f * qx * qy - 2.0f * qz * qw; + rmatrix[2] = 2.0f * qx * qz + 2.0f * qy * qw; + rmatrix[3] = 2.0f * qx * qy + 2.0f * qz * qw; + rmatrix[4] = 1.0f - 2.0f * qx * qx - 2.0f * qz * qz; + rmatrix[5] = 2.0f * qy * qz - 2.0f * qx * qw; + rmatrix[6] = 2.0f * qx * qz - 2.0f * qy * qw; + rmatrix[7] = 2.0f * qy * qz + 2.0f * qx * qw; + rmatrix[8] = 1.0f - 2.0f * qx * qx - 2.0f * qy * qy; +} + +void rotate(const magdata_t *in, magdata_t *out, const float *rmatrix) +{ + if (out == NULL) return; + if (in == NULL || in->valid == 0) { + out->valid = 0; + out->x = out->y = out->z = 0; + return; + } + out->x = in->x * rmatrix[0] + in->y * rmatrix[1] + in->z * rmatrix[2]; + out->y = in->x * rmatrix[3] + in->y * rmatrix[4] + in->z * rmatrix[5]; + out->z = in->x * rmatrix[6] + in->y * rmatrix[7] + in->z * rmatrix[8]; + out->valid = 1; +} + + +/* +typedef struct { + float x; + float y; + float z; + int valid; +} magdata_t; +magdata_t caldata[MAGBUFFSIZE]; +*/ + +void display_callback(void) +{ + int i; + //float x, y, z; + float xscale, yscale, zscale; + float xoff, yoff, zoff; + float rotation[9]; + magdata_t point, draw; + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glColor3f(1, 0, 0); // set current color to red + + xscale = 0.05; + yscale = 0.05; + zscale = 0.05; + xoff = 0.0; + yoff = 0.0; + zoff = -7.0; + + if (hard_iron.valid) { + quad_to_rotation(¤t_orientation, rotation); + for (i=0; i < MAGBUFFSIZE; i++) { + if (caldata[i].valid) { + point.x = caldata[i].x - hard_iron.x; + point.y = caldata[i].y - hard_iron.y; + point.z = caldata[i].z - hard_iron.z; + point.valid = 1; + // TODO: apply soft iron cal + + memcpy(&draw, &point, sizeof(draw)); + rotate(&point, &draw, rotation); + + glPushMatrix(); + glTranslatef( + draw.x * xscale + xoff, + draw.z * yscale + yoff, + draw.y * zscale + zoff + ); + glutSolidSphere(0.08,16,16); // radius, slices, stacks + glPopMatrix(); + } + } + } + glutSwapBuffers(); +} + + +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 }; + +void visualize_init(void) +{ + 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); +} + + +