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.

Makefile

@@ -4,7 +4,7 @@ NAME=quad
 
 SSRCS=crt0.s
 CSRCS=main.c i2c.c wmp.c timer.c interrupt.c uart.c event.c matrix.c dcm.c
-CSRCS+=fisqrt.c stick.c trig.c motor.c led.c
+CSRCS+=fisqrt.c stick.c trig.c motor.c led.c watchdog.c panic.c status.c
 
 #PROJOPTS=-DUSE_UART -DSEND_DCM
 

abs.h

@@ -0,0 +1,3 @@
+/* abs.h */
+
+#define abs(x) (((x) < 0)? (-(x)) : (x))

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);
 }
 

led.c

@@ -11,13 +11,14 @@ unsigned int led_next_time;
 bool led_next_state;
 
 led_pattern led_pattern_active[] = {250, 250, 0};
+led_pattern led_pattern_unknown[] = {100, 100, 0};
 
 void led_set(bool on)
 {
 	if (on)
-		FP0XVAL |= 0x04000000;
-	else
 		FP0XVAL &= ~0x04000000;
+	else
+		FP0XVAL |= 0x04000000;
 }
 
 void led_update(void)
@@ -43,8 +44,15 @@ void led_update(void)
 
 void led_set_pattern(led_pattern *pattern)
 {
+	if (led_current_pattern == pattern)
+		return;
 	led_current_pattern = pattern;
-	led_current_pointer = pattern;
+}
+
+void led_init(void)
+{
+	led_current_pattern = led_pattern_unknown;
+	led_current_pointer = led_pattern_unknown;
 	led_next_state = TRUE;
 	led_next_time = timer_read();
 	led_update();

led.h

@@ -5,7 +5,9 @@
 typedef unsigned int led_pattern;
 
 extern led_pattern led_pattern_active[];
+extern led_pattern led_pattern_unknown[];
 
+void led_init(void);
 void led_set(bool on);
 void led_update(void);
 void led_set_pattern(led_pattern *pattern);

main.c

@@ -6,8 +6,9 @@
 #include "uart.h"
 #include "interrupt.h"
 #include "event.h"
-#include "stick.h"
 #include "led.h"
+#include "status.h"
+#include "watchdog.h"
 
 #define PINSEL0 (*((volatile unsigned int *) 0xE002C000))
 #define PINSEL1 (*((volatile unsigned int *) 0xE002C004))
@@ -179,13 +180,12 @@ void timer_event_handler(void)
 void menu_handler(void);
 
 int main(void) {
-	armed = FALSE;
-
 	init_interrupt();
 	init_uart();
 	init_i2c();
 	init_pins();
 	init_timer();
+	init_status();
 
 	event_register(EVENT_UART_INPUT, menu_handler);
 
@@ -197,11 +197,7 @@ int main(void) {
 
 	putstr("prompt> ");
 
-	led_set(FALSE);
 	timer_delay_ms(1000);
-	led_set(TRUE);
-	timer_delay_ms(1000);
-	led_set(FALSE);
 	if (!wmp_init())
 		putstr("WMP initialisation failed\r\n");
 
@@ -209,12 +205,15 @@ int main(void) {
 	timer_set_period(5*TIMER_MS);
 	wmp_start_zero();
 
-	led_set_pattern(led_pattern_active);
+	led_init();
+
+	init_watchdog();
 
 	/* Good luck! */
 	while (1) {
 		led_update();
 		event_dispatch();
+		watchdog_check();
 	}
 
 	return 0;

motor.c

@@ -4,6 +4,7 @@
 #include "timer.h"
 #include "dcm.h"
 #include "uart.h"
+#include "status.h"
 
 float integral[3] = {0.0f, 0.0f, 0.0f};
 float last[3];
@@ -69,7 +70,7 @@ void motor_pid_update(float troll, float mroll,
 	out[1] = pitch * Kp   + integral[1] * Ki   + derivative[1] * Kd;
 	out[2] = yaw   * Kp_y + integral[2] * Ki_y + derivative[2] * Kd_y;
 
-	if (armed) {
+	if (status_armed()) {
 		/* Front right */
 		motor[0] = throttle + out[0] + out[1] + out[2];
 		/* Front left */
@@ -147,7 +148,7 @@ void motor_kill(void) {
 }
 
 void motor_set_throttle(float t) {
-	if (armed)
+	if (status_armed())
 		throttle = t;
 }
 

panic.c

@@ -0,0 +1,58 @@
+/* panic.c */
+
+/*
+ * Something has gone horribly, horribly wrong.
+ *
+ * If we are in the air, we are going to crash. This could be nasty.
+ * Try to limit the damage by turning off all of the motors.
+ * There's not much else we can do at this point.
+ *
+ */
+
+#include "panic.h"
+#include "motor.h"
+#include "led.h"
+
+led_pattern led_pattern_panic[] = {100, 100, 100, 100, 100, 100, 100, 100,
+				   100, 100, 100, 100, 100, 100, 100, 100,
+				   100, 100, 100, 100, 100, 100, 100, 100,
+				   100, 100, 100, 100, 100, 100, 100, 3000, 0};
+
+/* Take the lower 16 bits and make a pattern of them, MSB first */
+static void panic_create_pattern(led_pattern *pattern, unsigned int reason)
+{
+	int i;
+	for (i = 0; i < 16; i++) {
+		if (reason & (1<<(15-i))) {
+			pattern[2*i] = 400;
+			pattern[2*i+1] = 100;
+		} else {
+			pattern[2*i] = 100;
+			pattern[2*i+1] = 400;
+		}
+		if ((i % 4) == 3)
+			pattern[2*i+1] += 500;
+		if (i == 15)
+			pattern[2*i+1] += 2500;
+	}
+}
+
+void panic(unsigned int reason)
+{
+	/*
+	 * We may one day be able to do something with the reason, like emit
+	 * a final deathbed confession. So we'll provide the reasons in the
+	 * caller and just ignore them for now.
+	 */
+	(void)reason;
+
+	motor_kill();
+
+	panic_create_pattern(led_pattern_panic, reason);
+
+	led_set_pattern(led_pattern_panic);
+
+	/* Wait for the inevitable plunge to the death */
+	for (;;)
+		led_update();
+}

panic.h

@@ -0,0 +1,8 @@
+/* panic.h */
+
+/* OMG we're all going to die!!!! */
+
+void panic(unsigned int reason);
+
+/* Panic code goes in the low 8 bits */
+#define PANIC_WATCHDOG_TIMEOUT 0x100

status.c

@@ -0,0 +1,91 @@
+/* status.c */
+
+#include "status.h"
+#include "led.h"
+
+static bool armed = FALSE;
+
+static unsigned int module_ready[STATUS_MODULES];
+static const unsigned int module_count[STATUS_MODULES] = STATUS_COUNT;
+
+led_pattern led_pattern_stick[] = {100, 1000, 0};
+led_pattern led_pattern_gyro_zero[]  = {100, 200, 100, 1000, 0};
+led_pattern led_pattern_gyro_rate[] = {100, 200, 100, 200, 100, 1000, 0};
+led_pattern led_pattern_attitude[] = {100, 200, 100, 200, 100, 200, 100,
+							1000, 0};
+led_pattern led_pattern_dcm_error[] = {100, 200, 100, 200, 100, 200,
+						100, 200, 100, 1000, 0};
+
+bool status_armed(void)
+{
+	return armed;
+}
+
+void status_set_ready(unsigned int module, bool ready)
+{
+	int i;
+	int all_ready;
+
+	if (module >= STATUS_MODULES)
+		return;
+
+	if (ready) {
+		if (module_ready[module] < module_count[module])
+			module_ready[module]++;
+	} else {
+		module_ready[module] = 0;
+	}
+
+	/* We can't un-arm. */
+	if (armed)
+		return;
+
+	all_ready = TRUE;
+
+	for (i = 0; i < STATUS_MODULES; i++)
+		if (module_ready[i] < module_count[i]) {
+			if (all_ready) {
+				status_set_led_pattern(i);
+				all_ready = FALSE;
+			}
+		}
+
+	if (all_ready) {
+		armed = TRUE;
+		led_set_pattern(led_pattern_active);
+	}
+}
+
+void status_set_led_pattern(unsigned int module)
+{
+	switch (module) {
+	case STATUS_MODULE_STICK:
+		led_set_pattern(led_pattern_stick);
+		break;
+	case STATUS_MODULE_GYRO_ZERO:
+		led_set_pattern(led_pattern_gyro_zero);
+		break;
+	case STATUS_MODULE_GYRO_RATE:
+		led_set_pattern(led_pattern_gyro_rate);
+		break;
+	case STATUS_MODULE_ATTITUDE:
+		led_set_pattern(led_pattern_attitude);
+		break;
+	case STATUS_MODULE_DCM_ERROR:
+		led_set_pattern(led_pattern_dcm_error);
+		break;
+	default:
+		led_set_pattern(led_pattern_unknown);
+		break;
+	}
+}
+
+void init_status(void)
+{
+	int i;
+
+	armed = FALSE;
+	for (i = 0; i < STATUS_MODULES; i++)
+		module_ready[i] = 0;
+	led_set_pattern(led_pattern_unknown);
+}

status.h

@@ -0,0 +1,36 @@
+/* status.h */
+
+#include "types.h"
+
+bool status_armed(void);
+void status_set_ready(unsigned int module, bool ready);
+void status_set_led_pattern(unsigned int module);
+void init_status(void);
+
+
+#define STATUS_MODULE_GYRO_ZERO 0
+#define STATUS_MODULE_GYRO_RATE 1
+#define STATUS_MODULE_ATTITUDE  2
+#define STATUS_MODULE_DCM_ERROR 3
+#define STATUS_MODULE_STICK     4
+
+#define STATUS_MODULES		5
+
+#define STATUS_COUNT {				\
+		STATUS_COUNT_GYRO_ZERO,		\
+		STATUS_COUNT_GYRO_RATE,		\
+		STATUS_COUNT_ATTITUDE,		\
+		STATUS_COUNT_DCM_ERROR,		\
+		STATUS_COUNT_STICK		\
+	}
+
+/*
+ * Each condition must be valid for so many samples, typically once
+ * per 100Hz loop
+ */
+#define STATUS_COUNT_STICK     100
+#define STATUS_COUNT_GYRO_ZERO 1
+#define STATUS_COUNT_GYRO_RATE 100
+#define STATUS_COUNT_ATTITUDE  100
+#define STATUS_COUNT_DCM_ERROR 100
+

stick.c

@@ -13,6 +13,8 @@
 #include "trig.h"
 #include "motor.h"
 #include "wmp.h"
+#include "status.h"
+#include "watchdog.h"
 
 #define TWO_PI 6.28318531f
 #define PI 3.14159265f
@@ -50,8 +52,6 @@ float stick_yaw = 0.0f;
 
 unsigned int stick_counter;
 
-bool armed = FALSE;
-
 void stick_update(float x, float y, float z)
 {
 	float tz = delta_t * z;
@@ -85,18 +85,17 @@ void stick_input(void) {
 		throttle = timer_input(2);
 		z = timer_input(3);
 
-		if (!armed) {
+		if (!status_armed()) {
 			if ((throttle < MIN_THR) &&
 			    (x > (CENTRE_X - CENTRE_ZONE)) &&
 			    (x < (CENTRE_X + CENTRE_ZONE)) &&
 			    (y > (CENTRE_Y - CENTRE_ZONE)) &&
 			    (y < (CENTRE_Y + CENTRE_ZONE)) &&
 			    (z > (CENTRE_Z - CENTRE_ZONE)) &&
-			    (z < (CENTRE_Z + CENTRE_ZONE)) &&
+			    (z < (CENTRE_Z + CENTRE_ZONE)))
-			    (wmp_zero == FALSE)) {
+				    status_set_ready(STATUS_MODULE_STICK, TRUE);
-				putstr("ARMED!!!\r\n");
+			else
-				armed = TRUE;
+				    status_set_ready(STATUS_MODULE_STICK,FALSE);
-			}
 
 		}
 
@@ -114,10 +113,13 @@ void stick_input(void) {
 		y = 0.0f;
 		z = 0.0f;
 		throttle = 0.0f;
+		status_set_ready(STATUS_MODULE_STICK,FALSE);
 	}
 
 	motor_set_throttle(throttle);
 
+	watchdog_kick(WATCHDOG_STICK);
+
 	/* So the controls are back to front. Let's fix that. */
 	x = -x;
 	y = -y;

watchdog.c

@@ -0,0 +1,50 @@
+/* watchdog.c */
+
+#include "watchdog.h"
+#include "panic.h"
+#include "timer.h"
+
+/* There are two watchdogs to worry about. The hardware one, and watchdogs
+   to make sure that critical parts of the software are running.
+ */
+
+/*
+ * This is about 10 times round the main loop. If we haven't had a kick by
+ * now, something's gone horribly wrong.
+ */
+
+#define WATCHDOG_TIMEOUT (100 * TIMER_MS)
+
+static unsigned int watchdog_last_seen[WATCHDOG_MODULES];
+
+void watchdog_kick(unsigned int module)
+{
+	if (module > WATCHDOG_MODULES)
+		return;
+	watchdog_last_seen[module] = timer_read();
+}
+
+void watchdog_check(void)
+{
+	unsigned int time = timer_read();
+	int i;
+
+	/* XXX not yet */
+/*	return; */
+	for (i = 0; i < WATCHDOG_MODULES; i++) {
+		if ((signed int)(watchdog_last_seen[i] + WATCHDOG_TIMEOUT
+					- time) < 0) {
+			panic(PANIC_WATCHDOG_TIMEOUT + i);
+		}
+	}
+}
+
+void init_watchdog(void)
+{
+	unsigned int time = timer_read();
+	int i;
+
+	for (i = 0; i < WATCHDOG_MODULES; i++) {
+		watchdog_last_seen[i] = time;
+	}
+}

watchdog.h

@@ -0,0 +1,11 @@
+/* watchdog.h */
+
+void watchdog_kick(unsigned int module);
+void watchdog_check(void);
+void init_watchdog(void);
+
+#define WATCHDOG_STICK 0
+#define WATCHDOG_GYRO  1
+#define WATCHDOG_ACCEL 2
+
+#define WATCHDOG_MODULES 3

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();
 }