quadrotor/status.c

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