The unified diff between revisions [a39fe798..] and [8f4e93ab..] 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 [a39fe7980c8f14b70401f4c97f3e10232dce016a]
# new_revision [8f4e93ab4d89edfdbd524b06b83511c6867a9150]
#
# patch "wmp.c"
#  from [bba0c9cfc92f2ce2c56d9a49d01b47f374a40a29]
#    to [9acbe9ae30b2b7c95b8a700f4f7bb97c38097e21]
#
============================================================
--- wmp.c	bba0c9cfc92f2ce2c56d9a49d01b47f374a40a29
+++ wmp.c	9acbe9ae30b2b7c95b8a700f4f7bb97c38097e21
@@ -1,10 +1,12 @@
 
 #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 */
@@ -105,6 +107,8 @@ bool wmp_sample(void)
 	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];
@@ -117,3 +121,36 @@ bool wmp_sample(void)
 	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");
+
+	}
+}