Send calibration data (on keystroke, Linux cmdline only)

imuread.c

@@ -21,6 +21,23 @@ static void glut_display_callback(void)
 	glutSwapBuffers();
 }
 
+static void glut_keystroke_callback(unsigned char ch, int x, int y)
+{
+	if (magcal.fFitErrorpc > 9.0) {
+		printf("Poor Calibration: ");
+		printf("soft iron fit error = %.1f%%\n", magcal.fFitErrorpc);
+		return;
+	}
+	printf("Magnetic Calibration:   (%.1f%% fit error)\n", magcal.fFitErrorpc);
+	printf("   %7.2f   %6.3f %6.3f %6.3f\n",
+		magcal.fV[0], magcal.finvW[0][0], magcal.finvW[0][1], magcal.finvW[0][2]);
+	printf("   %7.2f   %6.3f %6.3f %6.3f\n",
+		magcal.fV[1], magcal.finvW[1][0], magcal.finvW[1][1], magcal.finvW[1][2]);
+	printf("   %7.2f   %6.3f %6.3f %6.3f\n",
+		magcal.fV[2], magcal.finvW[2][0], magcal.finvW[2][1], magcal.finvW[2][2]);
+	send_calibration();
+}
+
 int main(int argc, char *argv[])
 {
 	raw_data_reset();
@@ -35,6 +52,7 @@ int main(int argc, char *argv[])
 	glutReshapeFunc(resize_callback);
 	glutDisplayFunc(glut_display_callback);
 	glutTimerFunc(TIMEOUT_MSEC, timer_callback, 0);
+	glutKeyboardFunc(glut_keystroke_callback);
 
 	if (!open_port(PORT)) die("Unable to open %s\n", PORT);
 	glutMainLoop();

imuread.h

@@ -62,9 +62,11 @@ extern Quaternion_t current_orientation;
 
 extern int open_port(const char *name);
 extern int read_serial_data(void);
+extern int write_serial_data(const void *ptr, int len);
 extern void close_port(void);
 void raw_data_reset(void);
 void raw_data(const int16_t *data);
+int send_calibration(void);
 void visualize_init(void);
 void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out);
 void display_callback(void);
@@ -79,6 +81,7 @@ typedef struct {
     float fB;                     // current geomagnetic field magnitude (uT)
     float fFourBsq;               // current 4*B*B (uT^2)
     float fFitErrorpc;            // current fit error %
+    float fFitErrorAge;           // current fit error % (grows automatically with age)
     float ftrV[3];                // trial value of hard iron offset z, y, z (uT)
     float ftrinvW[3][3];          // trial inverse soft iron matrix size
     float ftrB;                   // trial value of geomagnetic field magnitude in uT

magcal.c

@@ -75,7 +75,7 @@ void MagCal_Run(void)
 
 	if (magcal.iValidMagCal) {
 		// age the existing fit error to avoid one good calibration locking out future updates
-		magcal.fFitErrorpc *= 1.02f;
+		magcal.fFitErrorAge *= 1.01f;
 	}
 
 	// is enough data collected
@@ -97,12 +97,13 @@ void MagCal_Run(void)
 		//  2: the calibration fit is reduced or
 		//  3: an improved solver was used giving a good trial calibration (4% or under)
 		if ((magcal.iValidMagCal == 0) ||
-				(magcal.ftrFitErrorpc <= magcal.fFitErrorpc) ||
+				(magcal.ftrFitErrorpc <= magcal.fFitErrorAge) ||
 				((isolver > magcal.iValidMagCal) && (magcal.ftrFitErrorpc <= 4.0F))) {
 			// accept the new calibration solution
 			//printf("new magnetic cal, B=%.2f uT\n", magcal.ftrB);
 			magcal.iValidMagCal = isolver;
 			magcal.fFitErrorpc = magcal.ftrFitErrorpc;
+			magcal.fFitErrorAge = magcal.ftrFitErrorpc;
 			magcal.fB = magcal.ftrB;
 			magcal.fFourBsq = 4.0F * magcal.ftrB * magcal.ftrB;
 			for (i = X; i <= Z; i++) {

rawdata.c

@@ -12,11 +12,12 @@ void raw_data_reset(void)
 	rawcount = OVERSAMPLE_RATIO;
 	fInit_9DOF_GBY_KALMAN(&fusionstate, 100, OVERSAMPLE_RATIO);
 	memset(&magcal, 0, sizeof(magcal));
-	magcal.fV[1] = 10.0f;
 	magcal.fV[2] = 80.0f;  // initial guess
 	magcal.finvW[0][0] = 1.0f;
 	magcal.finvW[1][1] = 1.0f;
 	magcal.finvW[2][2] = 1.0f;
+	magcal.fFitErrorpc = 100.0f;
+	magcal.fFitErrorAge = 100.0f;
 }
 
 static void add_magcal_data(const int16_t *data)
@@ -118,5 +119,58 @@ void raw_data(const int16_t *data)
 	}
 }
 
+static uint16_t crc16(uint16_t crc, uint8_t data)
+{
+        unsigned int i;
 
+        crc ^= data;
+        for (i = 0; i < 8; ++i) {
+                if (crc & 1) {
+                        crc = (crc >> 1) ^ 0xA001;
+                } else {
+                        crc = (crc >> 1);
+                }
+        }
+        return crc;
+}
 
+static uint8_t * copy_lsb_first(uint8_t *dst, float f)
+{
+	union {
+		float f;
+		uint32_t n;
+	} data;
+
+	data.f = f;
+	*dst++ = data.n;
+	*dst++ = data.n >> 8;
+	*dst++ = data.n >> 16;
+	*dst++ = data.n >> 24;
+	return dst;
+}
+
+int send_calibration(void)
+{
+	uint8_t *p, buf[52];
+	uint16_t crc;
+	int i, j;
+
+	p = buf;
+	*p++ = 117; // 2 byte signature
+	*p++ = 84;
+	for (i=0; i < 3; i++) {
+		p = copy_lsb_first(p, magcal.fV[i]); // 12 bytes offset/hardiron
+	}
+	for (i=0; i < 3; i++) {
+		for (j=0; j < 3; j++) {
+			p = copy_lsb_first(p, magcal.finvW[i][j]); // 36 bytes softiron
+		}
+	}
+	crc = 0xFFFF;
+	for (i=0; i < 50; i++) {
+		crc = crc16(crc, buf[i]);
+	}
+	*p++ = crc;   // 2 byte crc check
+	*p++ = crc >> 8;
+	return write_serial_data(buf, 52);
+}

serialdata.c

@@ -240,6 +240,7 @@ static void newdata(const unsigned char *data, int len)
 }
 
 
+
 #if defined(LINUX) || defined(MACOSX)
 
 static int portfd=-1;
@@ -300,6 +301,34 @@ int read_serial_data(void)
 	}
 }
 
+int write_serial_data(const void *ptr, int len)
+{
+	int n, written=0;
+	fd_set wfds;
+	struct timeval tv;
+
+	//printf("Write %d\n", len);
+	if (portfd < 0) return -1;
+	while (written < len) {
+		n = write(portfd, (const char *)ptr + written, len - written);
+		if (n < 0 && (errno == EAGAIN || errno == EINTR)) n = 0;
+		//printf("Write, n = %d\n", n);
+		if (n < 0) return -1;
+		if (n > 0) {
+			written += n;
+		} else {
+			tv.tv_sec = 0;
+			tv.tv_usec = 5000;
+			FD_ZERO(&wfds);
+			FD_SET(portfd, &wfds);
+			n = select(portfd+1, NULL, &wfds, NULL, &tv);
+			if (n < 0 && errno == EINTR) n = 1;
+			if (n <= 0) return -1;
+		}
+	}
+	return written;
+}
+
 void close_port(void)
 {
 	if (portfd >= 0) {
@@ -425,6 +454,37 @@ int read_serial_data(void)
         return r;
 }
 
+int write_serial_data(const void *ptr, int len)
+{
+	DWORD num_written;
+	OVERLAPPED ov;
+	int r;
+
+	ov.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
+	if (ov.hEvent == NULL) return -1;
+	ov.Internal = ov.InternalHigh = 0;
+	ov.Offset = ov.OffsetHigh = 0;
+	if (WriteFile(port_handle, ptr, len, &num_written, &ov)) {
+		//printf("Write, immediate complete, num_written=%lu\n", num_written);
+		r = num_written;
+	} else {
+		if (GetLastError() == ERROR_IO_PENDING) {
+			if (GetOverlappedResult(port_handle, &ov, &num_written, TRUE)) {
+			//printf("Write, delayed, num_written=%lu\n", num_written);
+			r = num_written;
+			} else {
+				//printf("Write, delayed error\n");
+				r = -1;
+			}
+		} else {
+			//printf("Write, error\n");
+			r = -1;
+		}
+	};
+	CloseHandle(ov.hEvent);
+	return r;
+}
+
 void close_port(void)
 {
 	CloseHandle(port_handle);