From 999e129e2c3a2303a3d3ecf233b70c828eff2fca Mon Sep 17 00:00:00 2001 From: Gavan Fantom Date: Sat, 22 Sep 2012 12:08:07 +0000 Subject: [PATCH] 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 | 2 +- abs.h | 3 ++ dcm.c | 26 ++++++++++++++++ led.c | 14 +++++++-- led.h | 2 ++ main.c | 15 +++++---- motor.c | 5 +-- panic.c | 58 ++++++++++++++++++++++++++++++++++ panic.h | 8 +++++ status.c | 91 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ status.h | 36 +++++++++++++++++++++ stick.c | 18 ++++++----- watchdog.c | 50 ++++++++++++++++++++++++++++++ watchdog.h | 11 +++++++ wmp.c | 19 ++++++++++++ 15 files changed, 336 insertions(+), 22 deletions(-) create mode 100644 abs.h create mode 100644 panic.c create mode 100644 panic.h create mode 100644 status.c create mode 100644 status.h create mode 100644 watchdog.c create mode 100644 watchdog.h diff --git a/Makefile b/Makefile index 223e2e6..093d4d9 100644 --- a/Makefile +++ b/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 diff --git a/abs.h b/abs.h new file mode 100644 index 0000000..1a6cfb0 --- /dev/null +++ b/abs.h @@ -0,0 +1,3 @@ +/* abs.h */ + +#define abs(x) (((x) < 0)? (-(x)) : (x)) diff --git a/dcm.c b/dcm.c index d830612..30f4148 100644 --- a/dcm.c +++ b/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); } diff --git a/led.c b/led.c index 5a15fe7..11ded2a 100644 --- a/led.c +++ b/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(); diff --git a/led.h b/led.h index d42505c..e8f83bc 100644 --- a/led.h +++ b/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); diff --git a/main.c b/main.c index aaf9b04..1b75786 100644 --- a/main.c +++ b/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; diff --git a/motor.c b/motor.c index 61e1601..dc76a9d 100644 --- a/motor.c +++ b/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; } diff --git a/panic.c b/panic.c new file mode 100644 index 0000000..ecbe4a2 --- /dev/null +++ b/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(); +} diff --git a/panic.h b/panic.h new file mode 100644 index 0000000..62cf4ee --- /dev/null +++ b/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 diff --git a/status.c b/status.c new file mode 100644 index 0000000..e443199 --- /dev/null +++ b/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); +} diff --git a/status.h b/status.h new file mode 100644 index 0000000..5b6ffa6 --- /dev/null +++ b/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 + diff --git a/stick.c b/stick.c index 2e35bed..fd0d800 100644 --- a/stick.c +++ b/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)) && - (wmp_zero == FALSE)) { - putstr("ARMED!!!\r\n"); - armed = TRUE; - } + (z < (CENTRE_Z + CENTRE_ZONE))) + status_set_ready(STATUS_MODULE_STICK, TRUE); + else + 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; diff --git a/watchdog.c b/watchdog.c new file mode 100644 index 0000000..c2d3cc8 --- /dev/null +++ b/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; + } +} diff --git a/watchdog.h b/watchdog.h new file mode 100644 index 0000000..cb20148 --- /dev/null +++ b/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 diff --git a/wmp.c b/wmp.c index 2e9b1dd..5a2a750 100644 --- a/wmp.c +++ b/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(); }