ViewMTN: Diff from [a39fe798..] to [24d5b9f4..]

The unified diff between revisions [a39fe798..] and [24d5b9f4..] 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 [24d5b9f4dff9135787b198fe1127d9c1e3326b9c]
#
# patch "wmp.c"
#  from [bba0c9cfc92f2ce2c56d9a49d01b47f374a40a29]
#    to [e6627850f4ae970c9aee0dbb4c6f1933f0cb31c7]
#
============================================================
--- wmp.c	bba0c9cfc92f2ce2c56d9a49d01b47f374a40a29
+++ wmp.c	e6627850f4ae970c9aee0dbb4c6f1933f0cb31c7
@@ -1,10 +1,37 @@
+/* wmp.c */
 
 #include "wmp.h"
 #include "i2c.h"
+#include "uart.h"
+#include "dcm.h"
+#include "fisqrt.h"
+#include "stick.h"
+#include "watchdog.h"
+#include "status.h"
+#include "abs.h"
 
-unsigned char wmp_init_command[2] = {0xfe, 0x04};
+#define WMP_ZERO_COUNT 100
 
+
+#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;
 
 struct i2c_transaction wmp_init_transaction = {
 	(0x53 << 1) + 0, /* write */
@@ -79,22 +106,40 @@ 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;
 
+int accel_x;
+int accel_y;
+int accel_z;
+
+bool wmp_update;
+bool wmp_zero;
+unsigned int wmp_discard;
+
+#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)
 
+/* The gyro has to stay within this limit in each axis in order to arm */
+#define GYRO_RATE_THRESHOLD (0.01 / DEG_TO_RAD)
+
 bool wmp_sample(void)
 {
 	if (!i2c_start_transaction(&wmp_sample_transaction))
@@ -105,6 +150,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 +164,157 @@ bool wmp_sample(void)
 	return TRUE;
 }
 
+bool wmp_start_sample(void)
+{
+	return i2c_start_transaction(&wmp_sample_transaction);
+}
+
+void wmp_process_gyro_sample(void)
+{
+	float yaw, pitch, roll;
+
+	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);
+
+	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);
+
+		if (!status_armed()) {
+			if (  (abs(roll)  < GYRO_RATE_THRESHOLD) &&
+			      (abs(pitch) < GYRO_RATE_THRESHOLD) &&
+			      (abs(yaw)   < GYRO_RATE_THRESHOLD)  ) {
+			    status_set_ready(STATUS_MODULE_GYRO_RATE, TRUE);
+			} else {
+			    status_set_ready(STATUS_MODULE_GYRO_RATE, FALSE);
+			}
+		}
+
+		wmp_generation++;
+
+#if SEND_DCM
+		if ((wmp_generation % 20) == 0)
+			dcm_send_packet();
+#endif
+
+	} else if (wmp_zero) {
+		if (wmp_discard) {
+			wmp_discard--;
+		} else {
+			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");
+				status_set_ready(STATUS_MODULE_GYRO_ZERO, TRUE);
+			}
+		}
+	}
+	watchdog_kick(WATCHDOG_GYRO);
+}
+
+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);
+	watchdog_kick(WATCHDOG_ACCEL);
+	stick_input();
+}
+
+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;
+	wmp_update = FALSE;
+	wmp_discard = 100;
+	wmp_generation = 0;
+	putstr("Starting zero\r\n");
+}