Begin GUI, apply soft iron cal, compute stats
This commit is contained in:
PaulStoffregen
parent
2312575ace
commit
8139b49c8a
6 changed files with 96 additions and 38 deletions
18
Makefile
18
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
|
||||
|
||||
|
||||
|
|
|
|||
25
gui.cpp
25
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;
|
||||
|
|
|
|||
7
gui.h
7
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();
|
||||
};
|
||||
|
|
|
|||
|
|
@ -6,6 +6,7 @@
|
|||
#include <stdint.h>
|
||||
#include <stdarg.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include <errno.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
14
serialdata.c
14
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;
|
||||
|
|
|
|||
67
visualize.c
67
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)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue