The unified diff between revisions [d8ed90db..] and [961b04dd..] is displayed below. It can also be downloaded as a raw diff.

This diff has been restricted to the following files: 'wmp.c'

#
# old_revision [d8ed90db2d4284a290224447c40a0d9cef3fbc31]
# new_revision [961b04ddb07ba2b5dd6bccfa66a03e442e40d8f0]
#
# add_file "wmp.c"
#  content [5f733d08f12cc7ab58dbe720d7ac20c713bf5687]
#
============================================================
--- /dev/null	
+++ wmp.c	5f733d08f12cc7ab58dbe720d7ac20c713bf5687
@@ -0,0 +1,101 @@
+
+#include "wmp.h"
+#include "i2c.h"
+
+bool wmp_init(void)
+{
+	if (!i2c_send_start())
+		return FALSE;
+	if (!i2c_send_address(0x53, TRUE))
+		return FALSE;
+	if (!i2c_send_data(0xfe))
+		return FALSE;
+	if (!i2c_send_data(0x04))
+		return FALSE;
+	i2c_send_stop();
+	return TRUE;
+}
+
+unsigned char wmp_calibration_data[0x20];
+
+bool wmp_read_calibration_data(void)
+{
+	int i;
+
+	if (!i2c_send_start())
+		return FALSE;
+	if (!i2c_send_address(0x53, TRUE))
+		return FALSE;
+	if (!i2c_send_data(0x20))
+		return FALSE;
+	i2c_send_stop();
+
+	if (!i2c_send_start())
+		return FALSE;
+	if (!i2c_send_address(0x53, FALSE))
+		return FALSE;
+	for (i = 0; i < 0x20; i++) {
+		unsigned int data;
+		if (!i2c_receive_data(&data, (i == 0x1f)))
+			return FALSE;
+		wmp_calibration_data[i] = data;
+	}
+	i2c_send_stop();
+	return TRUE;
+}
+
+unsigned int wmp_yaw;
+unsigned int wmp_pitch;
+unsigned int wmp_roll;
+
+bool wmp_yaw_fast;
+bool wmp_pitch_fast;
+bool wmp_roll_fast;
+
+/* There's considerable debate about these values, and they may vary
+ * between different models of the Wii Motion Plus. It would be nice
+ * to be able to use the calibration data stored on the device itself
+ * but we don't know the format yet.
+ */
+#define SLOW_YAW_STEP	(1000/20)
+#define SLOW_PITCH_STEP (1000/20)
+#define SLOW_ROLL_STEP  (1000/20)
+#define FAST_YAW_STEP	(1000/4)
+#define FAST_PITCH_STEP (1000/4)
+#define FAST_ROLL_STEP  (1000/4)
+
+bool wmp_sample(void)
+{
+	int i;
+	unsigned int b[6];
+
+	if (!i2c_send_start())
+		return FALSE;
+	if (!i2c_send_address(0x52, TRUE))
+		return FALSE;
+	if (!i2c_send_data(0x00))
+		return FALSE;
+	i2c_send_stop();
+
+	if (!i2c_send_start())
+		return FALSE;
+	if (!i2c_send_address(0x52, FALSE))
+		return FALSE;
+	for (i = 0; i < 6; i++) {
+		if (!i2c_receive_data(&(b[i]), (i == 5)))
+			return FALSE;
+	}
+	i2c_send_stop();
+
+	wmp_yaw   = ((b[3]>>2)<<8) + b[0];
+	wmp_pitch = ((b[4]>>2)<<8) + b[1];
+	wmp_roll  = ((b[5]>>2)<<8) + b[2];
+
+	/* XXX We don't take into account the fast/slow mode flag here */
+	wmp_yaw_fast = !(b[3] & 0x2);
+	wmp_pitch_fast = !(b[3] & 0x1);
+	wmp_roll_fast = !(b[4] & 0x2);
+
+	return TRUE;
+}
+