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.

wmp.c

@@ -6,6 +6,9 @@
 #include "dcm.h"
 #include "fisqrt.h"
 #include "stick.h"
+#include "watchdog.h"
+#include "status.h"
+#include "abs.h"
 
 #define WMP_ZERO_COUNT 100
 
@@ -134,6 +137,9 @@ unsigned int wmp_discard;
 #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))
@@ -202,6 +208,16 @@ void wmp_process_gyro_sample(void)
 
 		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
@@ -225,9 +241,11 @@ void wmp_process_gyro_sample(void)
 				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)
@@ -275,6 +293,7 @@ void wmp_process_accel_sample(void)
 	 * on that so we'll just fudge it here.
 	 */
 	dcm_drift_correction(x, -y, -z);
+	watchdog_kick(WATCHDOG_ACCEL);
 	stick_input();
 }