The unified diff between revisions [65df00aa..] and [4cc7246c..] 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 [65df00aa2705ce33fd74f4dd706d2879fe99b2b0]
# new_revision [4cc7246c1b6c809c9dc15997798f6deed15b3631]
#
# patch "wmp.c"
#  from [9acbe9ae30b2b7c95b8a700f4f7bb97c38097e21]
#    to [76b67ab3d8729319797c8f0f7ca2839524a76a20]
#
============================================================
--- wmp.c	9acbe9ae30b2b7c95b8a700f4f7bb97c38097e21
+++ wmp.c	76b67ab3d8729319797c8f0f7ca2839524a76a20
@@ -2,7 +2,10 @@
 #include "wmp.h"
 #include "i2c.h"
 #include "uart.h"
+#include "dcm.h"
 
+#define WMP_ZERO_COUNT 100
+
 unsigned char wmp_init_command[2] = {0xfe, 0x04};
 
 i2c_result wmp_result;
@@ -81,21 +84,31 @@ unsigned int wmp_roll;
 unsigned int wmp_pitch;
 unsigned int wmp_roll;
 
+unsigned int wmp_yaw_zero;
+unsigned int wmp_pitch_zero;
+unsigned int wmp_roll_zero;
+
 bool wmp_yaw_fast;
 bool wmp_pitch_fast;
 bool wmp_roll_fast;
 
+bool wmp_update;
+bool wmp_zero;
+
+#define TWO_PI 6.28318531f
+#define DEG_TO_RAD (TWO_PI/360.0f)
+
 /* 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)
+#define SLOW_YAW_STEP	(20 / DEG_TO_RAD)
+#define SLOW_PITCH_STEP (20 / DEG_TO_RAD)
+#define SLOW_ROLL_STEP  (20 / DEG_TO_RAD)
+#define FAST_YAW_STEP	(4 / DEG_TO_RAD)
+#define FAST_PITCH_STEP (4 / DEG_TO_RAD)
+#define FAST_ROLL_STEP  (4 / DEG_TO_RAD)
 
 bool wmp_sample(void)
 {
@@ -128,6 +141,8 @@ void wmp_event_handler(void)
 
 void wmp_event_handler(void)
 {
+	float yaw, pitch, roll;
+
 	if (wmp_result != I2C_SUCCESS)
 		return;
 
@@ -142,15 +157,58 @@ void wmp_event_handler(void)
 	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");
+	if (wmp_update) {
+		int tmp_yaw = wmp_yaw;
+		int tmp_pitch = wmp_pitch;
+		int tmp_roll = wmp_roll;
 
+		tmp_yaw -= wmp_yaw_zero;
+		tmp_pitch -= wmp_pitch_zero;
+		tmp_roll -= wmp_roll_zero;
+
+		if (wmp_yaw_fast)
+			yaw = ((float)tmp_yaw) / FAST_YAW_STEP;
+		else
+			yaw = ((float)tmp_yaw) / SLOW_YAW_STEP;
+
+		if (wmp_pitch_fast)
+			pitch = ((float)tmp_pitch) / FAST_PITCH_STEP;
+		else
+			pitch = ((float)tmp_pitch) / SLOW_PITCH_STEP;
+
+		if (wmp_roll_fast)
+			roll = ((float)tmp_roll) / FAST_ROLL_STEP;
+		else
+			roll = ((float)tmp_roll) / SLOW_ROLL_STEP;
+
+		dcm_update(roll, pitch, yaw);
+
+		wmp_generation++;
+
+		if ((wmp_generation % 2) == 0)
+			dcm_send_packet();
+
+	} else if (wmp_zero) {
+		wmp_yaw_zero += wmp_yaw;
+		wmp_pitch_zero += wmp_pitch;
+		wmp_roll_zero += wmp_roll;
+		wmp_generation++;
+		if (wmp_generation >= WMP_ZERO_COUNT) {
+			wmp_zero = FALSE;
+			wmp_update = TRUE;
+			wmp_generation = 0;
+			wmp_yaw_zero /= WMP_ZERO_COUNT;
+			wmp_pitch_zero /= WMP_ZERO_COUNT;
+			wmp_roll_zero /= WMP_ZERO_COUNT;
+			putstr("Zero finished\r\n");
+		}
 	}
 }
+
+void wmp_start_zero(void)
+{
+	wmp_zero = TRUE;
+	wmp_update = FALSE;
+	wmp_generation = 0;
+	putstr("Starting zero\r\n");
+}