diff --git a/Makefile b/Makefile index 81e6700..f85be6e 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,6 @@ -#OS = LINUX +OS = LINUX #OS = MACOSX -OS = WINDOWS +#OS = WINDOWS ifeq ($(OS), LINUX) ALL = gui imuread @@ -10,7 +10,8 @@ CFLAGS = -O2 -Wall -D$(OS) CXXFLAGS = $(CFLAGS) `$(WXCONFIG) --cppflags` LDFLAGS = WXCONFIG = ~/wxwidgets/3.0.2.gtk2-opengl/bin/wx-config -CLILIBS = -lglut -lGLU -lGL +CLILIBS = -lglut -lGLU -lGL -lm +MAKEFLAGS = --jobs=12 else ifeq ($(OS), MACOSX) ALL = gui.app @@ -19,7 +20,7 @@ CXX = g++-4.2 CFLAGS = -O2 -Wall -D$(OS) CXXFLAGS = $(CFLAGS) `$(WXCONFIG) --cppflags` WXCONFIG = ~/wxwidgets/3.0.2.mac-opengl/bin/wx-config -CLILIBS = -lglut -lGLU -lGL +CLILIBS = -lglut -lGLU -lGL -lm else ifeq ($(OS), WINDOWS) ALL = gui.exe @@ -29,7 +30,7 @@ CFLAGS = -O2 -Wall -D$(OS) CXXFLAGS = $(CFLAGS) `$(WXCONFIG) --cppflags` LDFLAGS = -static -static-libgcc WXCONFIG = ~/wxwidgets/3.0.2.mingw-opengl/bin/wx-config -CLILIBS = -lglut32 -lglu32 -lopengl32 +CLILIBS = -lglut32 -lglu32 -lopengl32 -lm VERSION = 0.01 endif @@ -58,3 +59,10 @@ imuread: imuread.o visualize.o serialdata.o clean: rm -f gui imuread *.o *.exe rm -rf gui.app .DS_Store + +gui.o: gui.cpp gui.h imuread.h +imuread.o: imuread.c imuread.h +visualize.o: visualize.c imuread.h +serialdata.o: serialdata.c imuread.h + + diff --git a/gui.cpp b/gui.cpp index 4a754b6..9dbd24f 100644 --- a/gui.cpp +++ b/gui.cpp @@ -54,7 +54,7 @@ void MyCanvas::InitGL() - +/*****************************************************************************/ BEGIN_EVENT_TABLE(MyFrame,wxFrame) EVT_MENU(wxID_ABOUT, MyFrame::OnAbout) @@ -75,26 +75,36 @@ MyFrame::MyFrame(wxWindow *parent, wxWindowID id, const wxString &title, menu = new wxMenu; menu->Append(wxID_EXIT, wxT("Quit")); menuBar->Append(menu, wxT("&File")); - menu = new wxMenu; //item = new wxMenuItem(menu, ID_ABOUT, "About"); menu->Append(wxID_ABOUT, wxT("About")); menuBar->Append(menu, wxT("&Help")); - SetMenuBar(menuBar); + wxBoxSizer *topsizer = new wxBoxSizer(wxHORIZONTAL); + wxBoxSizer *leftsizer = new wxStaticBoxSizer(wxVERTICAL, this, "Communication"); + wxBoxSizer *middlesizer = new wxStaticBoxSizer(wxVERTICAL, this, "Magnetometer"); + wxBoxSizer *rightsizer = new wxStaticBoxSizer(wxVERTICAL, this, "Gyroscope"); + + topsizer->Add(leftsizer, 0, wxALL, 5); + topsizer->Add(middlesizer, 1, wxALL | wxEXPAND, 5); + topsizer->Add(rightsizer, 0, wxALL, 5); + int gl_attrib[20] = { WX_GL_RGBA, WX_GL_MIN_RED, 1, WX_GL_MIN_GREEN, 1, WX_GL_MIN_BLUE, 1, WX_GL_DEPTH_SIZE, 1, WX_GL_DOUBLEBUFFER, 0}; - - m_canvas = new MyCanvas(this, wxID_ANY, gl_attrib); + m_canvas->SetMinSize(wxSize(340,340)); + + middlesizer->Add(m_canvas, 1, wxEXPAND | wxALL, 10); + topsizer->SetSizeHints(this); + SetSizerAndFit(topsizer); Show(true); Raise(); - m_canvas->InitGL(); + m_canvas->InitGL(); open_port(PORT); m_timer = new wxTimer(this, ID_TIMER); m_timer->Start(40, wxTIMER_CONTINUOUS); @@ -126,6 +136,7 @@ MyFrame::~MyFrame(void) } +/*****************************************************************************/ IMPLEMENT_APP(MyApp) @@ -141,7 +152,7 @@ bool MyApp::OnInit() wxPoint pos(100, 100); - MyFrame *frame = new MyFrame(NULL, -1, wxT("Teensy"), pos, wxDefaultSize, + MyFrame *frame = new MyFrame(NULL, -1, wxT("IMU Read"), pos, wxSize(1160,660), wxDEFAULT_FRAME_STYLE & ~(wxMAXIMIZE_BOX | wxRESIZE_BORDER)); frame->Show( true ); return true; diff --git a/gui.h b/gui.h index 3326aaf..3c05665 100644 --- a/gui.h +++ b/gui.h @@ -46,13 +46,6 @@ public: private: wxGLContext *m_glRC; - - //GLfloat m_verts[MAXVERTS][3]; - //GLfloat m_norms[MAXVERTS][3]; - //GLint m_numverts; - //GLfloat m_xrot; - //GLfloat m_yrot; - wxDECLARE_NO_COPY_CLASS(MyCanvas); wxDECLARE_EVENT_TABLE(); }; diff --git a/imuread.h b/imuread.h index 71f0703..09ddfc6 100644 --- a/imuread.h +++ b/imuread.h @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -53,7 +54,7 @@ typedef struct { } magdata_t; extern magdata_t caldata[MAGBUFFSIZE]; extern magdata_t hard_iron; -extern magdata_t current_position; +extern float soft_iron[9]; typedef struct { float w; float x; diff --git a/serialdata.c b/serialdata.c index 56e2423..50c45a1 100644 --- a/serialdata.c +++ b/serialdata.c @@ -14,9 +14,9 @@ void print_data(const char *name, const unsigned char *data, int len) static void packet_primary_data(const unsigned char *data) { - current_position.x = (float)((int16_t)((data[13] << 8) | data[12])) / 10.0f; - current_position.y = (float)((int16_t)((data[15] << 8) | data[14])) / 10.0f; - current_position.z = (float)((int16_t)((data[17] << 8) | data[16])) / 10.0f; + //current_position.x = (float)((int16_t)((data[13] << 8) | data[12])) / 10.0f; + //current_position.y = (float)((int16_t)((data[15] << 8) | data[14])) / 10.0f; + //current_position.z = (float)((int16_t)((data[17] << 8) | data[16])) / 10.0f; current_orientation.w = (float)((int16_t)((data[25] << 8) | data[24])) / 30000.0f; current_orientation.x = (float)((int16_t)((data[27] << 8) | data[26])) / 30000.0f; current_orientation.y = (float)((int16_t)((data[29] << 8) | data[28])) / 30000.0f; @@ -54,6 +54,14 @@ static void packet_magnetic_cal(const unsigned char *data) cal->x = (float)x / 10.0f; cal->y = (float)y / 10.0f; cal->z = (float)z / 10.0f; + } else if (id == 2) { + soft_iron[0] = (float)x / 1000.0f; + soft_iron[4] = (float)y / 1000.0f; + soft_iron[8] = (float)z / 1000.0f; + } else if (id == 3) { + soft_iron[1] = soft_iron[3] = (float)x / 1000.0f; // finvW[X][Y] + soft_iron[2] = soft_iron[6] = (float)y / 1000.0f; // finvW[X][Z] + soft_iron[5] = soft_iron[7] = (float)z / 1000.0f; // finvW[Y][Z] cal->valid = 1; } else if (id >= 10 && id < MAGBUFFSIZE+10) { newx = (float)x / 10.0f; diff --git a/visualize.c b/visualize.c index 6078058..72fac33 100644 --- a/visualize.c +++ b/visualize.c @@ -1,11 +1,25 @@ #include "imuread.h" magdata_t caldata[MAGBUFFSIZE]; -magdata_t hard_iron; -magdata_t current_position; +magdata_t hard_iron = {0.0, 0.0, 0.0, 1}; +float soft_iron[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; quat_t current_orientation; + +static void apply_calibration(const magdata_t *in, magdata_t *out) +{ + float x, y, z; + + x = in->x - hard_iron.x; + y = in->y - hard_iron.y; + z = in->z - hard_iron.z; + out->x = x * soft_iron[0] + y * soft_iron[1] + z * soft_iron[2]; + out->y = x * soft_iron[3] + y * soft_iron[4] + z * soft_iron[5]; + out->z = x * soft_iron[6] + y * soft_iron[7] + z * soft_iron[8]; + out->valid = 1; +} + static void quad_to_rotation(const quat_t *quat, float *rmatrix) { float qx = quat->x; @@ -54,18 +68,23 @@ static GLuint spherelowreslist; void display_callback(void) { - int i; + static int updatenum=0; + int i, count=0; float xscale, yscale, zscale; float xoff, yoff, zoff; float rotation[9]; magdata_t point, draw; + float magnitude[MAGBUFFSIZE]; + float latitude[MAGBUFFSIZE]; // 0 to PI + float longitude[MAGBUFFSIZE]; // -PI to +PI + float sumx=0.0, sumy=0.0, sumz=0.0, summag=0.0; 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; + xscale = 0.06; + yscale = 0.06; + zscale = 0.06; xoff = 0.0; yoff = 0.0; zoff = -7.0; @@ -74,15 +93,18 @@ void display_callback(void) 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)); + apply_calibration(&caldata[i], &point); + sumx += point.x; + sumy += point.y; + sumz += point.z; + magnitude[i] = sqrtf(point.x * point.x + + point.y * point.y + point.z * point.z); + summag += magnitude[i]; + longitude[i] = atan2f(point.y, point.x); + latitude[i] = atan2f(sqrtf(point.x * point.x + + point.y * point.y), point.z); + count++; rotate(&point, &draw, rotation); - glPushMatrix(); glTranslatef( draw.x * xscale + xoff, @@ -94,11 +116,26 @@ void display_callback(void) } else { glCallList(spherelowreslist); } - glPopMatrix(); } } } +#if 0 + if (updatenum++ == 500) { + for (i=0; i < MAGBUFFSIZE; i++) { + if (caldata[i].valid) { + printf("long: %6.2f lat: %5.2f\n", + longitude[i] * 180.0 / M_PI, + latitude[i] * 180.0 / M_PI); + } + } + printf("count = %d\n", count); + printf("sum = %.2f %.2f %.2f\n", sumx, sumy, sumz); + printf("summag = %.2f avg: %.2f\n", summag, summag / (float)count); + printf("exit here\n"); + exit(1); + } +#endif } void resize_callback(int width, int height)