Quadrotor from scratch
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

77 lines
1.8 KiB

/* 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"
#ifdef PANIC_CHECKPOINT
unsigned int checkpoint;
#endif
#ifdef PANIC_32BIT
#define PANIC_BITS 32
#else
#define PANIC_BITS 16
#endif
#ifdef PANIC_32BIT
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, 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, 100, 3000, 0};
#else
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};
#endif
/* 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 < PANIC_BITS; i++) {
if (reason & (1<<((PANIC_BITS-1)-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 == (PANIC_BITS-1))
pattern[2*i+1] += 2500;
}
}
void panic(unsigned int reason)
{
#if PANIC_CHECKPOINT
reason = checkpoint;
#endif
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();
}