Capture raw data into magnatic cal buffer

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);
+}
+
+
+
+