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.

92 lines
1.8 KiB

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