Browse Source
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.master
15 changed files with 336 additions and 22 deletions
@ -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(); |
||||
} |
||||
@ -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 |
||||
@ -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); |
||||
} |
||||
@ -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 |
||||
|
||||
@ -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; |
||||
} |
||||
} |
||||
@ -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 |
||||
Loading…
Reference in new issue