The unified diff between revisions [4cc7246c..] and [23a3e9a5..] 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 [4cc7246c1b6c809c9dc15997798f6deed15b3631]
# new_revision [23a3e9a50b4034343e3bd217d2c225dcaec064dd]
#
# patch "wmp.c"
#  from [76b67ab3d8729319797c8f0f7ca2839524a76a20]
#    to [1ab4017652548447277e83cf1697442cb7c6949b]
#
============================================================
--- wmp.c	76b67ab3d8729319797c8f0f7ca2839524a76a20
+++ wmp.c	1ab4017652548447277e83cf1697442cb7c6949b
@@ -3,11 +3,28 @@
 #include "i2c.h"
 #include "uart.h"
 #include "dcm.h"
+#include "fisqrt.h"
 
 #define WMP_ZERO_COUNT 100
 
-unsigned char wmp_init_command[2] = {0xfe, 0x04};
 
+#define ACCEL_ZERO_X 520
+#define ACCEL_ZERO_Y 516
+#define ACCEL_ZERO_Z 514
+
+/*
+516, 510, 710
+-4, -6, 196
+16, 36, 38416   = 38468
+sqrt(38468) = 196.1326...
+... somehow once we scale by gravity we get almost exactly 0.05.
+*/
+
+#define ACCEL_SCALE 0.05
+
+/* Nunchuck pass-through mode */
+unsigned char wmp_init_command[2] = {0xfe, 0x05};
+
 i2c_result wmp_result;
 unsigned int wmp_generation;
 
@@ -92,6 +109,10 @@ bool wmp_roll_fast;
 bool wmp_pitch_fast;
 bool wmp_roll_fast;
 
+int accel_x;
+int accel_y;
+int accel_z;
+
 bool wmp_update;
 bool wmp_zero;
 
@@ -139,15 +160,10 @@ bool wmp_start_sample(void)
 	return i2c_start_transaction(&wmp_sample_transaction);
 }
 
-void wmp_event_handler(void)
+void wmp_process_gyro_sample(void)
 {
 	float yaw, pitch, roll;
 
-	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];
@@ -185,8 +201,10 @@ void wmp_event_handler(void)
 
 		wmp_generation++;
 
+#if 1
 		if ((wmp_generation % 2) == 0)
 			dcm_send_packet();
+#endif
 
 	} else if (wmp_zero) {
 		wmp_yaw_zero += wmp_yaw;
@@ -205,6 +223,66 @@ void wmp_event_handler(void)
 	}
 }
 
+void wmp_process_accel_sample(void)
+{
+	float x, y, z;
+#if 0
+	float invmag;
+#endif
+
+	accel_x   = (wmp_sample_data[2]<<2) + ((wmp_sample_data[5]>>3) & 0x02);
+	accel_y   = (wmp_sample_data[3]<<2) + ((wmp_sample_data[5]>>4) & 0x02);
+	accel_z   = ((wmp_sample_data[4]<<2) & 0x3f8) +
+					      ((wmp_sample_data[5]>>5) & 0x06);
+
+	x = (accel_x - ACCEL_ZERO_X) * ACCEL_SCALE;
+	y = (accel_y - ACCEL_ZERO_Y) * ACCEL_SCALE;
+	z = (accel_z - ACCEL_ZERO_Z) * ACCEL_SCALE;
+
+#if 0
+	invmag = fisqrt(x*x + y*y + z*z);
+
+	x = x * invmag;
+	y = y * invmag;
+	z = z * invmag;
+#endif
+
+#if 0
+	accel_x = (x * 512.0 + 1000.0);
+	accel_y = (y * 512.0 + 1000.0);
+	accel_z = (z * 512.0 + 1000.0);
+#endif
+
+#if 0
+	putstr("(");
+	putint(accel_x);
+	putstr(", ");
+	putint(accel_y);
+	putstr(", ");
+	putint(accel_z);
+	putstr(")\r\n");
+#endif
+
+	/* The minus signs are needed because something is upside down.
+	 * It might actually be the WMP, but we're defining coordinates based
+	 * on that so we'll just fudge it here.
+	 */
+	dcm_drift_correction(x, -y, -z);
+}
+
+void wmp_event_handler(void)
+{
+	if (wmp_result != I2C_SUCCESS)
+		return;
+
+	wmp_result = I2C_IN_PROGRESS;
+
+	if (wmp_sample_data[5] & 0x02)
+		wmp_process_gyro_sample();
+	else
+		wmp_process_accel_sample();
+}
+
 void wmp_start_zero(void)
 {
 	wmp_zero = TRUE;