quadrotor/panic.c

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