The unified diff between revisions [23a3e9a5..] and [64de686d..] is displayed below. It can also be downloaded as a raw diff.

This diff has been restricted to the following files: 'dcm.c'

#
# old_revision [23a3e9a50b4034343e3bd217d2c225dcaec064dd]
# new_revision [64de686d701acb9539dc52fe0bff299405612ab0]
#
# patch "dcm.c"
#  from [35a44d5b49a5c9bf56678ae7af0b611fde405621]
#    to [90d94d47a262c72d472ee7aca36bb55e2b328e66]
#
============================================================
--- dcm.c	35a44d5b49a5c9bf56678ae7af0b611fde405621
+++ dcm.c	90d94d47a262c72d472ee7aca36bb55e2b328e66
@@ -8,6 +8,9 @@
 #include "matrix.h"
 #include "dcm.h"
 #include "uart.h"
+#include "motor.h"
+#include "status.h"
+#include "abs.h"
 
 #define GRAVITY 9.80665f
 
@@ -16,6 +19,10 @@
 
 #define ERROR_LIMIT 1.17f
 
+/* Maximum allowed error for arming */
+#define ERROR_THRESHOLD 0.20f
+
+
 /* Implementation of the DCM IMU concept as described by Premerlani
  * and Bizard
  */
@@ -27,13 +34,15 @@ float omega_i[3] = {0.0, 0.0, 0.0};
 float omega_p[3] = {0.0, 0.0, 0.0};
 float omega_i[3] = {0.0, 0.0, 0.0};
 
+float omega_x, omega_y, omega_z;
+
 float delta_t = 0.01;
 
 void dcm_update(float x, float y, float z)
 {
-	float omega_x = x + omega_i[0] + omega_p[0];
-	float omega_y = y + omega_i[1] + omega_p[1];
-	float omega_z = z + omega_i[2] + omega_p[2];
+	omega_x = x + omega_i[0] + omega_p[0];
+	omega_y = y + omega_i[1] + omega_p[1];
+	omega_z = z + omega_i[2] + omega_p[2];
 
 	float tx = delta_t * omega_x;
 	float ty = delta_t * omega_y;
@@ -170,6 +179,15 @@ void dcm_drift_correction(float x, float
 	error[1] = dcm[8]*x - dcm[6]*z;
 	error[2] = dcm[6]*y - dcm[7]*x;
 
+	if (!status_armed()) {
+		if ((abs(error[0]) < ERROR_THRESHOLD) &&
+		    (abs(error[1]) < ERROR_THRESHOLD) &&
+		    (abs(error[2]) < ERROR_THRESHOLD))
+			status_set_ready(STATUS_MODULE_DCM_ERROR, TRUE);
+		else
+			status_set_ready(STATUS_MODULE_DCM_ERROR, FALSE);
+	}
+
 	for (i = 0; i < 3; i++) {
 		if (error[i] > ERROR_LIMIT)
 			error[i] = ERROR_LIMIT;
@@ -182,9 +200,12 @@ void dcm_drift_correction(float x, float
 		omega_i[i] += error[i] * (KI_ROLLPITCH * weight);
 	}
 
+#if 0
 	putstr("w: ");
 	putint_s((int)(weight * 100000.0f));
 	putstr("\r\n");
+#endif
+
 #if 0
 	putstr("p: ");
 	putint_s((int)(omega_p[0] * 100000.0f));
@@ -202,6 +223,33 @@ void dcm_drift_correction(float x, float
 #endif
 }
 
+/* Maximum angle to the horizontal for arming: 30 degrees */
+#define ATTITUDE_THRESHOLD (0.5)
+
+void dcm_attitude_error(float roll, float pitch, float yaw)
+{
+	/* dcm[6] = sine of pitch */
+	/* dcm[7] = sine of roll */
+
+	/* pitch error = pitch - dcm[6] */
+	/* roll error = roll - dcm[7] */
+
+	/* That was the theory. In practice, there appears to be some
+	   confusion over axes. Pitch and roll seem.. reversed. */
+
+	/* TODO: What if we are upside down? */
+
+	if (!status_armed()) {
+		if ((abs(dcm[6]) < ATTITUDE_THRESHOLD) &&
+		    (abs(dcm[7]) < ATTITUDE_THRESHOLD))
+			status_set_ready(STATUS_MODULE_ATTITUDE, TRUE);
+		else
+			status_set_ready(STATUS_MODULE_ATTITUDE, FALSE);
+	}
+
+	motor_pid_update(roll, dcm[6], pitch, -dcm[7], yaw, -omega_z);
+}
+
 void dcm_dump(void)
 {
 	putstr("dcm: ");