add motionplus calibration utility

3 files changed, 135 insertions, 3 deletions

diff --git a/.gitignore b/.gitignore
index f3ab4a5..070bede 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,4 @@
 /bal
+/mpcal
 /mplog
 /wibble
diff --git a/Makefile b/Makefile
index 1f10491..015ee27 100644
--- a/Makefile
+++ b/Makefile
@@ -1,16 +1,19 @@
 CFLAGS = -Wall -Wextra -pedantic
 LDFLAGS = -lbluetooth -lcwiid
 
-all: bal mplog wibble
+all: bal mpcal mplog wibble
 
 bal: bal.c
 	${CC} ${CFLAGS} ${LDFLAGS} -o $@ $<
 
+mpcal: mpcal.c
+	${CC} ${CFLAGS} ${LDFLAGS} -o $@ $<
+
 mplog: mplog.c
-	$(CC) ${CFLAGS} ${LDFLAGS} -lm -o $@ $<
+	${CC} ${CFLAGS} ${LDFLAGS} -lm -o $@ $<
 
 wibble: wibble.c
-	$(CC) ${CFLAGS} ${LDFLAGS} -lm -o $@ $<
+	${CC} ${CFLAGS} ${LDFLAGS} -lm -o $@ $<
 
 clean:
 	rm -f bal mplog wibble
diff --git a/mpcal.c b/mpcal.c
new file mode 100644
index 0000000..0cbc954
--- /dev/null
+++ b/mpcal.c
@@ -0,0 +1,128 @@
+/*
+ * Copyright (C) 2011 by Daniel Friesel <derf@finalrewind.org>
+ * License: WTFPL <http://sam.zoy.org/wtfpl>
+ *   0. You just DO WHAT THE FUCK YOU WANT TO.
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <time.h>
+#include <math.h>
+#include <bluetooth/bluetooth.h>
+#include <cwiid.h>
+
+#define NUM_VALUES 1000
+
+cwiid_mesg_callback_t cwiid_callback;
+
+volatile unsigned short values[3][NUM_VALUES];
+
+int main()
+{
+	cwiid_wiimote_t *wiimote = NULL;
+
+	fputs("Press 1+2 to connect wiimote\n", stdout);
+	fputs("Put it onto a flat surface and do not move it\n", stdout);
+
+	if ((wiimote = cwiid_open(BDADDR_ANY, 0)) == NULL) {
+		fputs("Unable to connect\n", stderr);
+		return EXIT_FAILURE;
+	}
+
+	sleep(2);
+
+	cwiid_set_led(wiimote, 1);
+
+	if (cwiid_set_mesg_callback(wiimote, cwiid_callback))
+		fputs("cannot set callback\n", stderr);
+	
+	cwiid_enable(wiimote, CWIID_FLAG_MOTIONPLUS);
+
+	if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC))
+		fputs("cannot enable callback\n", stderr);
+
+	if (cwiid_set_rpt_mode(wiimote,
+			CWIID_RPT_EXT))
+		fputs("cannot set report mode\n", stderr);
+
+	while (1) {
+		/* nothing to do here */
+		sleep(1);
+	}
+
+	return EXIT_SUCCESS;
+}
+
+void show_calibration()
+{
+	long unsigned int total[3] = {0, 0, 0};
+	unsigned short min[3], max[3];
+
+	min[0] = max[0] = values[0][0];
+	min[1] = max[1] = values[1][0];
+	min[2] = max[2] = values[2][0];
+
+	for (int step = 0; step < NUM_VALUES; step++) {
+		for (int i = 0; i < 3; i++) {
+
+			total[i] += values[i][step];
+
+			if (values[i][step] < min[i])
+				min[i] = values[i][step];
+
+			if (values[i][step] > max[i])
+				max[i] = values[i][step];
+		}
+	}
+
+	printf("\r\033[2K        avg  ( min  ..  max )\n");
+	printf("Pitch: %5lu (%5d .. %5d)\n",
+		total[0] / NUM_VALUES, min[0], max[0]);
+	printf("Roll : %5lu (%5d .. %5d)\n",
+		total[1] / NUM_VALUES, min[1], max[1]);
+	printf("Yaw  : %5lu (%5d .. %5d)\n",
+		total[2] / NUM_VALUES, min[2], max[2]);
+
+	exit(EXIT_SUCCESS);
+}
+
+
+void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count,
+	union cwiid_mesg mesg[], struct timespec *ts)
+{
+	static int cnt = 0;
+	static int skip = 100;
+
+	if (skip > 0) {
+		skip--;
+		return;
+	}
+
+	if (!(cnt % 100)) {
+		printf("\r\033[2K%4d / %d", cnt, NUM_VALUES);
+		fflush(stdout);
+	}
+
+	if (cnt == (NUM_VALUES/4))
+		cwiid_set_led(wiimote, 0x3);
+
+	else if (cnt == (NUM_VALUES/2))
+		cwiid_set_led(wiimote, 0x7);
+
+	else if (cnt == (NUM_VALUES*3/4))
+		cwiid_set_led(wiimote, 0xf);
+
+	for (int i = 0; i < mesg_count; i++) {
+		if (mesg[i].type == CWIID_MESG_MOTIONPLUS) {
+			values[0][cnt] = mesg[i].motionplus_mesg.angle_rate[0];
+			values[1][cnt] = mesg[i].motionplus_mesg.angle_rate[1];
+			values[2][cnt] = mesg[i].motionplus_mesg.angle_rate[2];
+			cnt++;
+
+			if (cnt >= NUM_VALUES)
+				show_calibration();
+		}
+	}
+}
+