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.
96 lines
2.0 KiB
96 lines
2.0 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}; |
|
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); |
|
}
|
|
|