Capture raw data into magnatic cal buffer

Makefile

@@ -35,10 +35,11 @@ VERSION = 0.01
 
 endif
 
+OBJS = visualize.o serialdata.o rawdata.o
 
 all: $(ALL)
 
-gui: gui.o visualize.o serialdata.o
+gui: gui.o $(OBJS)
 	$(CXX) -s $(CFLAGS) $(LDFLAGS) -o $@ $^ `$(WXCONFIG) --libs all,opengl`
 
 gui.exe: gui
@@ -53,7 +54,7 @@ gui.app: gui Info.plist
 	#cp icon.icns $@/Contents/Resources/
 	touch $@
 
-imuread: imuread.o visualize.o serialdata.o
+imuread: imuread.o $(OBJS)
 	$(CC) -s $(CFLAGS) $(LDFLAGS) -o $@ $^ $(CLILIBS)
 
 clean:
@@ -64,5 +65,6 @@ 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
+rawdata.o: rawdata.c imuread.h
 
 

imuread.h

@@ -52,6 +52,7 @@ typedef struct {
 	float y;
 	float z;
 	int valid;
+	float distsqsum;
 } magdata_t;
 extern magdata_t caldata[MAGBUFFSIZE];
 extern magdata_t hard_iron;
@@ -67,6 +68,7 @@ extern quat_t current_orientation;
 extern int open_port(const char *name);
 extern int read_serial_data(void);
 extern void close_port(void);
+void raw_data(const int *data);
 void visualize_init(void);
 void display_callback(void);
 void resize_callback(int width, int height);

rawdata.c

@@ -0,0 +1,82 @@
+#include "imuread.h"
+
+
+#define HIST_LEN 12
+uint32_t mag_histogram[3][65536];
+int raw_history[9][HIST_LEN];
+int raw_history_count=0;
+int raw_history_last=0;
+
+void raw_data_reset(void)
+{
+
+
+}
+
+//#undef MAGBUFFSIZE
+//#define MAGBUFFSIZE 6
+
+static void add_magcal_data(const int *data)
+{
+	float distsq, dx, dy, dz, magscale;
+	float minsum=1e30f;
+	int i, j, minindex=0;
+
+	// first look for an unused caldata slot
+	for (i=0; i < MAGBUFFSIZE; i++) {
+		if (!caldata[i].valid) break;
+	}
+	// if no unused, find the ones closest to each other
+	if (i >= MAGBUFFSIZE) {
+		for (i=0; i < MAGBUFFSIZE; i++) {
+			for (j=i+1; j < MAGBUFFSIZE; j++) {
+				dx = caldata[i].x - caldata[j].x;
+				dy = caldata[i].y - caldata[j].y;
+				dz = caldata[i].z - caldata[j].z;
+				distsq = dx * dx + dy * dy + dz * dz;
+				if (distsq < minsum) {
+					minsum = distsq;
+					minindex = (random() & 1) ? i : j;
+				}
+			}
+		}
+		i = minindex;
+	}
+	// add it to the cal buffer
+	magscale = 0.1f;
+	caldata[i].x = (float)data[6] * magscale;
+	caldata[i].y = (float)data[7] * magscale;
+	caldata[i].z = (float)data[8] * magscale;
+	caldata[i].valid = 1;
+}
+
+
+
+void raw_data(const int *data)
+{
+
+	//printf("raw_data: %d %d %d %d %d %d %d %d %d\n", data[0], data[1], data[2],
+		//data[3], data[4], data[5], data[6], data[7], data[8]);
+	mag_histogram[0][data[6]+32786]++;
+	mag_histogram[1][data[7]+32786]++;
+	mag_histogram[2][data[8]+32786]++;
+
+	// RdSensData_Run (tasks.c) - reads 8 samples, sums, scales to sci units
+	// Fusion_Run (tasks.c)
+	//   fInvertMagCal - applies hard & soft iron cal
+	//     fV[3]       = hard iron -- ftrV[3]
+        //     finvW[3][3] = soft iron
+	//   iUpdateMagnetometerBuffer - adds data to buffer
+	//   fRun_6DOF_GY_KALMAN
+	//   fRun_9DOF_GBY_KALMAN
+	// MagCal_Run (tasks.c)
+	//   fUpdateCalibration4INV
+	//   fUpdateCalibration7EIG
+	//   fUpdateCalibration10EIG
+
+	add_magcal_data(data);
+}
+
+
+
+

serialdata.c

@@ -179,11 +179,13 @@ static int packet_parse(const unsigned char *data, int len)
 	return ret;
 }
 
+/*
 void raw_data(const int *data)
 {
 	//printf("raw_data: %d %d %d %d %d %d %d %d %d\n", data[0], data[1], data[2],
 		//data[3], data[4], data[5], data[6], data[7], data[8]);
 }
+*/
 
 static int ascii_parse(const unsigned char *data, int len)
 {

visualize.c

@@ -1,7 +1,7 @@
 #include "imuread.h"
 
 magdata_t caldata[MAGBUFFSIZE];
-magdata_t hard_iron = {0.0, 0.0, 0.0, 1};
+magdata_t hard_iron = {0.0, 0.0, 80.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;
 
@@ -125,9 +125,9 @@ void display_callback(void)
 	glColor3f(1, 0, 0);	// set current color to red
 	memset(spheredist, 0, sizeof(spheredist));
 
-	xscale = 0.06;
-	yscale = 0.06;
-	zscale = 0.06;
+	xscale = 0.05;
+	yscale = 0.05;
+	zscale = 0.05;
 	xoff = 0.0;
 	yoff = 0.0;
 	zoff = -7.0;