|
|
|
/* 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};
|
|
|
|
led_pattern led_pattern_config[] = {100, 200, 100, 200, 100, 200,
|
|
|
|
1000, 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;
|
|
|
|
case STATUS_MODULE_CONFIG:
|
|
|
|
led_set_pattern(led_pattern_config);
|
|
|
|
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);
|
|
|
|
}
|