Improve handling of arming, providing LED blink codes if the arming fails.

Also, implement a software watchdog to make sure that the main real-time
modules are actually being run. Provide a panic facility, also giving blink
codes to indicate the panic reason.

dcm.c

@@ -9,6 +9,8 @@
 #include "dcm.h"
 #include "uart.h"
 #include "motor.h"
+#include "status.h"
+#include "abs.h"
 
 #define GRAVITY 9.80665f
 
@@ -17,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
  */
@@ -173,6 +179,15 @@ void dcm_drift_correction(float x, float y, float z)
 	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;
@@ -208,6 +223,9 @@ void dcm_drift_correction(float x, float y, float z)
 #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 */
@@ -221,6 +239,14 @@ void dcm_attitude_error(float roll, float pitch, float yaw)
 
 	/* 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);
 }