The unified diff between revisions [d0420ebd..] and [be147b11..] is displayed below. It can also be downloaded as a raw diff.
This diff has been restricted to the following files: 'status.c'
# # old_revision [d0420ebd87c820e33a32b29727989516e15980a8] # new_revision [be147b11caac304fda1579ac71017eecc3bb79e0] # # add_file "status.c" # content [31db654aeccf16a2aa7a73255e38f2dbe05034aa] # ============================================================ --- /dev/null +++ status.c 31db654aeccf16a2aa7a73255e38f2dbe05034aa @@ -0,0 +1,91 @@ +/* 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); +}