Bye bye Wii Motion Plus, hello MPU6050. Also, increase control loop

to 200Hz, add SD card logging, and a number of other changes.

dcm.c

@@ -11,8 +11,13 @@
 #include "motor.h"
 #include "status.h"
 #include "abs.h"
+#include "log.h"
 
+#if 0
 #define GRAVITY 9.80665f
+#endif
+
+#define GRAVITY 1.0f
 
 #define KP_ROLLPITCH 0.05967
 #define KI_ROLLPITCH 0.00001278
@@ -22,6 +27,9 @@
 /* Maximum allowed error for arming */
 #define ERROR_THRESHOLD 0.20f
 
+#define LOG_MAGIC_DCM_UPDATE 0x00DC111A
+#define LOG_MAGIC_DCM_DRIFT 0x00DC111B
+
 
 /* Implementation of the DCM IMU concept as described by Premerlani
  * and Bizard
@@ -36,7 +44,15 @@ float omega_i[3] = {0.0, 0.0, 0.0};
 
 float omega_x, omega_y, omega_z;
 
-float delta_t = 0.01;
+float delta_t = 0.005;
+
+void dcm_log(unsigned int magic)
+{
+	int i;
+	log_put_uint(magic);
+	for (i = 0; i < 9; i++)
+		log_put_float(dcm[i]);
+}
 
 void dcm_update(float x, float y, float z)
 {
@@ -57,6 +73,8 @@ void dcm_update(float x, float y, float z)
 	matrix_add(dcm, dcm, temp_matrix, 3, 3);
 
 	dcm_normalise();
+
+/*	dcm_log(LOG_MAGIC_DCM_UPDATE); */
 }
 
 void dcm_setvector(float x, float y, float z)
@@ -200,6 +218,8 @@ void dcm_drift_correction(float x, float y, float z)
 		omega_i[i] += error[i] * (KI_ROLLPITCH * weight);
 	}
 
+	dcm_log(LOG_MAGIC_DCM_DRIFT);
+
 #if 0
 	putstr("w: ");
 	putint_s((int)(weight * 100000.0f));