The unified diff between revisions [d8ed90db..] and [65df00aa..] 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 [65df00aa2705ce33fd74f4dd706d2879fe99b2b0]
#
# add_file "wmp.c"
#  content [9acbe9ae30b2b7c95b8a700f4f7bb97c38097e21]
#
============================================================
--- /dev/null	
+++ wmp.c	9acbe9ae30b2b7c95b8a700f4f7bb97c38097e21
@@ -0,0 +1,156 @@
+
+#include "wmp.h"
+#include "i2c.h"
+#include "uart.h"
+
+unsigned char wmp_init_command[2] = {0xfe, 0x04};
+
+i2c_result wmp_result;
+unsigned int wmp_generation;
+
+struct i2c_transaction wmp_init_transaction = {
+	(0x53 << 1) + 0, /* write */
+	2,
+	wmp_init_command,
+	&wmp_result,
+	NULL
+};
+
+unsigned char wmp_read_cal_command[1] = {0x20};
+
+struct i2c_transaction wmp_read_cal_transaction2;
+
+struct i2c_transaction wmp_read_cal_transaction = {
+	(0x53 << 1) + 0, /* write */
+	1,
+	wmp_read_cal_command,
+	&wmp_result,
+	&wmp_read_cal_transaction2
+};
+
+struct i2c_transaction wmp_read_cal_transaction2 = {
+	(0x53 << 1) + 1, /* read */
+	0x20,
+	wmp_calibration_data,
+	&wmp_result,
+	NULL
+};
+
+unsigned char wmp_sample_command[1] = {0x00};
+
+unsigned char wmp_sample_data[6];
+
+struct i2c_transaction wmp_sample_transaction2;
+
+struct i2c_transaction wmp_sample_transaction = {
+	(0x52 << 1) + 0, /* write */
+	1,
+	wmp_sample_command,
+	&wmp_result,
+	&wmp_sample_transaction2
+};
+
+struct i2c_transaction wmp_sample_transaction2 = {
+	(0x52 << 1) + 1, /* read */
+	6,
+	wmp_sample_data,
+	&wmp_result,
+	NULL
+};
+
+
+bool wmp_init(void)
+{
+	if (!i2c_start_transaction(&wmp_init_transaction))
+		return FALSE;
+	while (i2c_busy()) ;
+	return (wmp_result == I2C_SUCCESS);
+}
+
+unsigned char wmp_calibration_data[0x20];
+
+bool wmp_read_calibration_data(void)
+{
+	if (!i2c_start_transaction(&wmp_read_cal_transaction))
+		return FALSE;
+	while (i2c_busy());
+	return (wmp_result == I2C_SUCCESS);
+}
+
+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)
+{
+	if (!i2c_start_transaction(&wmp_sample_transaction))
+		return FALSE;
+
+	while (i2c_busy());
+
+	if (wmp_result != I2C_SUCCESS)
+		return FALSE;
+
+	wmp_result = I2C_IN_PROGRESS;
+
+	wmp_yaw   = ((wmp_sample_data[3]>>2)<<8) + wmp_sample_data[0];
+	wmp_pitch = ((wmp_sample_data[4]>>2)<<8) + wmp_sample_data[1];
+	wmp_roll  = ((wmp_sample_data[5]>>2)<<8) + wmp_sample_data[2];
+
+	/* XXX We don't take into account the fast/slow mode flag here */
+	wmp_yaw_fast = !(wmp_sample_data[3] & 0x2);
+	wmp_pitch_fast = !(wmp_sample_data[3] & 0x1);
+	wmp_roll_fast = !(wmp_sample_data[4] & 0x2);
+
+	return TRUE;
+}
+
+bool wmp_start_sample(void)
+{
+	return i2c_start_transaction(&wmp_sample_transaction);
+}
+
+void wmp_event_handler(void)
+{
+	if (wmp_result != I2C_SUCCESS)
+		return;
+
+	wmp_result = I2C_IN_PROGRESS;
+
+	wmp_yaw   = ((wmp_sample_data[3]>>2)<<8) + wmp_sample_data[0];
+	wmp_pitch = ((wmp_sample_data[4]>>2)<<8) + wmp_sample_data[1];
+	wmp_roll  = ((wmp_sample_data[5]>>2)<<8) + wmp_sample_data[2];
+
+	/* XXX We don't take into account the fast/slow mode flag here */
+	wmp_yaw_fast = !(wmp_sample_data[3] & 0x2);
+	wmp_pitch_fast = !(wmp_sample_data[3] & 0x1);
+	wmp_roll_fast = !(wmp_sample_data[4] & 0x2);
+
+	wmp_generation++;
+	if ((wmp_generation % 100) == 0) {
+		putstr("(");
+		puthex(wmp_roll);
+		putstr(", ");
+		puthex(wmp_pitch);
+		putstr(", ");
+		puthex(wmp_yaw);
+		putstr(")\r\n");
+
+	}
+}