X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fcore%2Fao_pyro.c;h=a260aa99f332cbaf7324e7e63f67956fb241719c;hp=a1377d21a2b8972b974624ce3a68629772b5ddb3;hb=58f08c4b3cb9049d0c9cb02cde0d8dbdc3d33920;hpb=3ce645a79b54e22d7835c6e390a22a5ad501a339 diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c index a1377d21..a260aa99 100644 --- a/src/core/ao_pyro.c +++ b/src/core/ao_pyro.c @@ -15,13 +15,60 @@ * 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)) +#ifndef AO_FLIGHT_TEST +enum ao_igniter_status +ao_pyro_status(uint8_t p) +{ + __xdata struct ao_data packet; + __pdata int16_t value; + + ao_arch_critical( + ao_data_get(&packet); + ); + + value = (AO_IGNITER_CLOSED>>1); + value = AO_SENSE_PYRO(&packet, p); + if (value < AO_IGNITER_OPEN) + return ao_igniter_open; + else if (value > AO_IGNITER_CLOSED) + return ao_igniter_ready; + else + return ao_igniter_unknown; +} + +void +ao_pyro_print_status(void) +{ + uint8_t p; + + for(p = 0; p < AO_PYRO_NUM; p++) { + enum ao_igniter_status status = ao_pyro_status(p); + printf("Igniter: %d Status: %s\n", + p, ao_igniter_status_names[status]); + } +} +#endif + +uint16_t ao_pyro_fired; + /* * Given a pyro structure, figure out * if the current flight state satisfies all @@ -66,13 +113,13 @@ ao_pyro_ready(struct ao_pyro *pyro) continue; break; -#if HAS_ORIENT +#if HAS_GYRO case ao_pyro_orient_less: - if (ao_orient <= pyro->orient_less) + if (ao_sample_orient <= pyro->orient_less) continue; break; case ao_pyro_orient_greater: - if (ao_orient >= pyro->orient_greater) + if (ao_sample_orient >= pyro->orient_greater) continue; break; #endif @@ -104,6 +151,15 @@ ao_pyro_ready(struct ao_pyro *pyro) /* 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,98 +168,116 @@ ao_pyro_ready(struct ao_pyro *pyro) return TRUE; } -#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) - - +#ifndef AO_FLIGHT_TEST static void -ao_pyro_fire(uint8_t p) +ao_pyro_pin_set(uint8_t p, uint8_t v) { switch (p) { #if AO_PYRO_NUM > 0 - case 0: ao_pyro_fire_port(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0); break; + case 0: ao_gpio_set(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0, v); break; #endif #if AO_PYRO_NUM > 1 - case 1: ao_pyro_fire_port(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1); break; + case 1: ao_gpio_set(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1, v); break; #endif #if AO_PYRO_NUM > 2 - case 2: ao_pyro_fire_port(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2); break; + case 2: ao_gpio_set(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2, v); break; #endif #if AO_PYRO_NUM > 3 - case 3: ao_pyro_fire_port(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3); break; + case 3: ao_gpio_set(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3, v); break; #endif #if AO_PYRO_NUM > 4 - case 4: ao_pyro_fire_port(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4); break; + case 4: ao_gpio_set(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4, v); break; #endif #if AO_PYRO_NUM > 5 - case 5: ao_pyro_fire_port(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5); break; + case 5: ao_gpio_set(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5, v); break; #endif #if AO_PYRO_NUM > 6 - case 6: ao_pyro_fire_port(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6); break; + case 6: ao_gpio_set(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6, v); break; #endif #if AO_PYRO_NUM > 7 - case 7: ao_pyro_fire_port(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7); break; + case 7: ao_gpio_set(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, v); break; #endif default: break; } - ao_delay(AO_MS_TO_TICKS(50)); } +#endif + +uint8_t ao_pyro_wakeup; static void -ao_pyro(void) +ao_pyro_pins_fire(uint16_t fire) { - uint8_t p; - struct ao_pyro *pyro; + uint8_t p; - ao_config_get(); - while (ao_flight_state < ao_flight_boost) - ao_sleep(&ao_flight_state); + for (p = 0; p < AO_PYRO_NUM; p++) { + if (fire & (1 << p)) + ao_pyro_pin_set(p, 1); + } + ao_delay(AO_MS_TO_TICKS(50)); + for (p = 0; p < AO_PYRO_NUM; p++) { + if (fire & (1 << p)) { + ao_pyro_pin_set(p, 0); + ao_config.pyro[p].fired = 1; + ao_pyro_fired |= (1 << p); + } + } + ao_delay(AO_MS_TO_TICKS(50)); +} - for (;;) { - ao_delay(AO_MS_TO_TICKS(100)); - for (p = 0; p < AO_PYRO_NUM; p++) { - pyro = &ao_config.pyro[p]; +static uint8_t +ao_pyro_check(void) +{ + struct ao_pyro *pyro; + uint8_t p, any_waiting; + uint16_t fire = 0; + + any_waiting = 0; + 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) + 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; - } + any_waiting = 1; + /* Check pyro state to see if it should fire + */ + if (!pyro->delay_done) { + if (!ao_pyro_ready(pyro)) + continue; - /* 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; } + + fire |= (1 << p); } -} -__xdata struct ao_task ao_pyro_task; + if (fire) + ao_pyro_pins_fire(fire); + + return any_waiting; +} #define NO_VALUE 0xff @@ -236,7 +310,7 @@ const struct { { "h<", ao_pyro_height_less, offsetof(struct ao_pyro, height_less), HELP("height less (m)") }, { "h>", ao_pyro_height_greater, offsetof(struct ao_pyro, height_greater), HELP("height greater (m)") }, -#if HAS_ORIENT +#if HAS_GYRO { "o<", ao_pyro_orient_less, offsetof(struct ao_pyro, orient_less), HELP("orient less (deg)") }, { "o>", ao_pyro_orient_greater, offsetof(struct ao_pyro, orient_greater), HELP("orient greater (deg)") }, #endif @@ -244,6 +318,9 @@ 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") }, @@ -253,6 +330,34 @@ const struct { { "", 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) { @@ -370,25 +475,17 @@ ao_pyro_set(void) _ao_config_edit_finish(); } -static void -ao_pyro_manual(void) +void +ao_pyro_manual(uint8_t p) { - 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) + printf ("ao_pyro_manual %d\n", p); + if (p >= AO_PYRO_NUM) { + ao_cmd_status = ao_cmd_syntax_error; return; - ao_pyro_fire(ao_cmd_lex_i); - + } + ao_pyro_pins_fire(1 << p); } -const struct ao_cmds ao_pyro_cmds[] = { - { ao_pyro_manual, "P DoIt \0Fire igniter" }, - { 0, NULL } -}; - void ao_pyro_init(void) { @@ -416,6 +513,6 @@ 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