X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fcore%2Fao_pyro.c;h=b3cda656a7e7b8299a7cfce7c0082d83f0f35461;hb=62547a042d042fadec652c5081f96816a8e66970;hp=162f5e1ea978a8fcc8430f8d801fdaf57059c180;hpb=6581eefbdbd8d3e94f615bdf11652a000d131c8e;p=fw%2Faltos diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c index 162f5e1e..b3cda656 100644 --- a/src/core/ao_pyro.c +++ b/src/core/ao_pyro.c @@ -15,13 +15,26 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */ +#ifndef AO_FLIGHT_TEST #include -#include #include #include +#endif +#include + +#if IS_COMPANION +#include +#define ao_accel ao_companion_command.accel +#define ao_speed ao_companion_command.speed +#define ao_height ao_companion_command.height +#define ao_flight_state ao_companion_command.flight_state +#define ao_motor_number ao_companion_command.motor_number +#endif #define ao_lowbit(x) ((x) & (-x)) +uint16_t ao_pyro_fired; + /* * Given a pyro structure, figure out * if the current flight state satisfies all @@ -103,7 +116,16 @@ ao_pyro_ready(struct ao_pyro *pyro) case ao_pyro_delay: /* handled separately */ continue; - + + case ao_pyro_state_less: + if (ao_flight_state < pyro->state_less) + continue; + break; + case ao_pyro_state_greater_or_equal: + if (ao_flight_state >= pyro->state_greater_or_equal) + continue; + break; + default: continue; } @@ -112,12 +134,13 @@ ao_pyro_ready(struct ao_pyro *pyro) return TRUE; } +#ifndef AO_FLIGHT_TEST #define ao_pyro_fire_port(port, bit, pin) do { \ ao_gpio_set(port, bit, pin, 1); \ ao_delay(AO_MS_TO_TICKS(50)); \ ao_gpio_set(port, bit, pin, 0); \ } while (0) - +#endif static void ao_pyro_fire(uint8_t p) @@ -152,59 +175,60 @@ ao_pyro_fire(uint8_t p) ao_delay(AO_MS_TO_TICKS(50)); } -static void -ao_pyro(void) +uint8_t ao_pyro_wakeup; + +static uint8_t +ao_pyro_check(void) { - uint8_t p; struct ao_pyro *pyro; + uint8_t p, any_waiting; + + any_waiting = 0; + for (p = 0; p < AO_PYRO_NUM; p++) { + pyro = &ao_config.pyro[p]; - ao_config_get(); - while (ao_flight_state < ao_flight_boost) - ao_sleep(&ao_flight_state); - - for (;;) { - ao_delay(AO_MS_TO_TICKS(100)); - for (p = 0; p < AO_PYRO_NUM; p++) { - pyro = &ao_config.pyro[p]; + /* Ignore igniters which have already fired + */ + if (pyro->fired) + continue; - /* Ignore igniters which have already fired - */ - if (pyro->fired) - continue; + /* Ignore disabled igniters + */ + if (!pyro->flags) + continue; - /* Ignore disabled igniters - */ - if (!pyro->flags) + any_waiting = 1; + /* Check pyro state to see if it should fire + */ + if (!pyro->delay_done) { + if (!ao_pyro_ready(pyro)) continue; - /* Check pyro state to see if it shoule fire - */ - if (!pyro->delay_done) { - if (!ao_pyro_ready(pyro)) - continue; - - /* If there's a delay set, then remember when - * it expires - */ - if (pyro->flags & ao_pyro_delay) - pyro->delay_done = ao_time() + pyro->delay; - } - - /* Check to see if we're just waiting for - * the delay to expire + /* If there's a delay set, then remember when + * it expires */ - if (pyro->delay_done) { - if ((int16_t) (ao_time() - pyro->delay_done) < 0) - continue; + if (pyro->flags & ao_pyro_delay) { + pyro->delay_done = ao_time() + pyro->delay; + if (!pyro->delay_done) + pyro->delay_done = 1; } + } - ao_pyro_fire(p); + /* Check to see if we're just waiting for + * the delay to expire + */ + if (pyro->delay_done) { + if ((int16_t) (ao_time() - pyro->delay_done) < 0) + continue; } + + ao_pyro_fire(p); + pyro->fired = 1; + ao_pyro_fired |= (1 << p); } + return any_waiting; } -__xdata struct ao_task ao_pyro_task; - #define NO_VALUE 0xff #define AO_PYRO_NAME_LEN 3 @@ -244,15 +268,46 @@ const struct { { "t<", ao_pyro_time_less, offsetof(struct ao_pyro, time_less), HELP("time less (s * 100)") }, { "t>", ao_pyro_time_greater, offsetof(struct ao_pyro, time_greater), HELP("time greater (s * 100)") }, + { "f<", ao_pyro_state_less, offsetof(struct ao_pyro, state_less), HELP("state less") }, + { "f>=",ao_pyro_state_greater_or_equal, offsetof(struct ao_pyro, state_greater_or_equal), HELP("state greater or equal") }, + { "A", ao_pyro_ascending, NO_VALUE, HELP("ascending") }, { "D", ao_pyro_descending, NO_VALUE, HELP("descending") }, - + { "m", ao_pyro_after_motor, offsetof(struct ao_pyro, motor), HELP("after motor") }, { "d", ao_pyro_delay, offsetof(struct ao_pyro, delay), HELP("delay before firing (s * 100)") }, { "", ao_pyro_none, NO_VALUE, HELP(NULL) }, }; +#define NUM_PYRO_VALUES (sizeof ao_pyro_values / sizeof ao_pyro_values[0]) + +#ifndef AO_FLIGHT_TEST +static void +ao_pyro(void) +{ + uint8_t any_waiting; + + ao_config_get(); + while (ao_flight_state < ao_flight_boost) + ao_sleep(&ao_flight_state); + + for (;;) { + ao_alarm(AO_MS_TO_TICKS(100)); + ao_sleep(&ao_pyro_wakeup); + ao_clear_alarm(); + if (ao_flight_state >= ao_flight_landed) + break; + any_waiting = ao_pyro_check(); + if (!any_waiting) + break; + } + ao_exit(); +} + +__xdata struct ao_task ao_pyro_task; + + static void ao_pyro_print_name(uint8_t v) { @@ -275,7 +330,7 @@ ao_pyro_help(void) } } #endif - + void ao_pyro_show(void) { @@ -340,7 +395,7 @@ ao_pyro_set(void) ao_cmd_white(); if (ao_cmd_lex_c == '\n') break; - + for (c = 0; c < AO_PYRO_NAME_LEN - 1; c++) { if (ao_cmd_is_white()) break; @@ -370,6 +425,25 @@ ao_pyro_set(void) _ao_config_edit_finish(); } +static void +ao_pyro_manual(void) +{ + ao_cmd_white(); + if (!ao_match_word("DoIt")) + return; + ao_cmd_white(); + ao_cmd_decimal(); + if (ao_cmd_lex_i < 0 || AO_PYRO_NUM <= ao_cmd_lex_i) + return; + ao_pyro_fire(ao_cmd_lex_i); + +} + +const struct ao_cmds ao_pyro_cmds[] = { + { ao_pyro_manual, "P DoIt \0Fire igniter" }, + { 0, NULL } +}; + void ao_pyro_init(void) { @@ -397,5 +471,7 @@ ao_pyro_init(void) #if AO_PYRO_NUM > 7 ao_enable_output(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, 0); #endif + ao_cmd_register(&ao_pyro_cmds[0]); ao_add_task(&ao_pyro_task, ao_pyro, "pyro"); } +#endif