X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fkernel%2Fao_pyro.c;h=0aed50d5434f65b8b0c7c03190bf47cc5b89160c;hb=770998be2c15dd41a63520d0c8747d7cf32ec447;hp=0ee7fbee711feb45edde9a681897acf44271a4ad;hpb=d20c608ce833fb8949dce527f92887775d216823;p=fw%2Faltos diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c index 0ee7fbee..0aed50d5 100644 --- a/src/kernel/ao_pyro.c +++ b/src/kernel/ao_pyro.c @@ -3,7 +3,8 @@ * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of @@ -69,6 +70,17 @@ ao_pyro_print_status(void) uint16_t ao_pyro_fired; +#ifndef PYRO_DBG +#define PYRO_DBG 0 +#endif + +#if PYRO_DBG +int pyro_dbg; +#define DBG(...) do { if (pyro_dbg) printf("\t%d: ", (int) (pyro - ao_config.pyro)); printf(__VA_ARGS__); } while (0) +#else +#define DBG(...) +#endif + /* * Given a pyro structure, figure out * if the current flight state satisfies all @@ -88,63 +100,73 @@ ao_pyro_ready(struct ao_pyro *pyro) case ao_pyro_accel_less: if (ao_accel <= pyro->accel_less) continue; + DBG("accel %d > %d\n", ao_accel, pyro->accel_less); break; case ao_pyro_accel_greater: if (ao_accel >= pyro->accel_greater) continue; + DBG("accel %d < %d\n", ao_accel, pyro->accel_greater); break; - - case ao_pyro_speed_less: if (ao_speed <= pyro->speed_less) continue; + DBG("speed %d > %d\n", ao_speed, pyro->speed_less); break; case ao_pyro_speed_greater: if (ao_speed >= pyro->speed_greater) continue; + DBG("speed %d < %d\n", ao_speed, pyro->speed_greater); break; - case ao_pyro_height_less: if (ao_height <= pyro->height_less) continue; + DBG("height %d > %d\n", ao_height, pyro->height_less); break; case ao_pyro_height_greater: if (ao_height >= pyro->height_greater) continue; + DBG("height %d < %d\n", ao_height, pyro->height_greater); break; #if HAS_GYRO case ao_pyro_orient_less: if (ao_sample_orient <= pyro->orient_less) continue; + DBG("orient %d > %d\n", ao_sample_orient, pyro->orient_less); break; case ao_pyro_orient_greater: if (ao_sample_orient >= pyro->orient_greater) continue; + DBG("orient %d < %d\n", ao_sample_orient, pyro->orient_greater); break; #endif case ao_pyro_time_less: if ((int16_t) (ao_time() - ao_boost_tick) <= pyro->time_less) continue; + DBG("time %d > %d\n", (int16_t)(ao_time() - ao_boost_tick), pyro->time_less); break; case ao_pyro_time_greater: if ((int16_t) (ao_time() - ao_boost_tick) >= pyro->time_greater) continue; + DBG("time %d < %d\n", (int16_t)(ao_time() - ao_boost_tick), pyro->time_greater); break; case ao_pyro_ascending: if (ao_speed > 0) continue; + DBG("not ascending speed %d\n", ao_speed); break; case ao_pyro_descending: if (ao_speed < 0) continue; + DBG("not descending speed %d\n", ao_speed); break; case ao_pyro_after_motor: if (ao_motor_number == pyro->motor) continue; + DBG("motor %d != %d\n", ao_motor_number, pyro->motor); break; case ao_pyro_delay: @@ -154,10 +176,12 @@ ao_pyro_ready(struct ao_pyro *pyro) case ao_pyro_state_less: if (ao_flight_state < pyro->state_less) continue; + DBG("state %d >= %d\n", ao_flight_state, pyro->state_less); break; case ao_pyro_state_greater_or_equal: if (ao_flight_state >= pyro->state_greater_or_equal) continue; + DBG("state %d < %d\n", ao_flight_state, pyro->state_greater_or_equal); break; default: @@ -213,7 +237,7 @@ ao_pyro_pins_fire(uint16_t fire) if (fire & (1 << p)) ao_pyro_pin_set(p, 1); } - ao_delay(AO_MS_TO_TICKS(50)); + ao_delay(ao_config.pyro_time); for (p = 0; p < AO_PYRO_NUM; p++) { if (fire & (1 << p)) { ao_pyro_pin_set(p, 0); @@ -230,7 +254,7 @@ 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]; @@ -266,6 +290,16 @@ ao_pyro_check(void) * the delay to expire */ if (pyro->delay_done) { + + /* Check to make sure the required conditions + * remain valid. If not, inhibit the channel + * by setting the fired bit + */ + if (!ao_pyro_ready(pyro)) { + pyro->fired = 1; + continue; + } + if ((int16_t) (ao_time() - pyro->delay_done) < 0) continue; } @@ -343,9 +377,7 @@ ao_pyro(void) ao_sleep(&ao_flight_state); for (;;) { - ao_alarm(AO_MS_TO_TICKS(100)); - ao_sleep(&ao_pyro_wakeup); - ao_clear_alarm(); + ao_sleep_for(&ao_pyro_wakeup, AO_MS_TO_TICKS(100)); if (ao_flight_state >= ao_flight_landed) break; any_waiting = ao_pyro_check(); @@ -406,7 +438,7 @@ ao_pyro_show(void) if (ao_pyro_values[v].flag & AO_PYRO_8_BIT_VALUE) value = *((uint8_t *) ((char *) pyro + ao_pyro_values[v].offset)); else - value = *((int16_t *) ((char *) pyro + ao_pyro_values[v].offset)); + value = *((int16_t *) (void *) ((char *) pyro + ao_pyro_values[v].offset)); printf ("%6d ", value); } else { printf (" "); @@ -443,14 +475,14 @@ ao_pyro_set(void) printf ("invalid pyro channel %d\n", p); return; } - pyro_tmp.flags = 0; + memset(&pyro_tmp, '\0', sizeof (pyro_tmp)); for (;;) { 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()) + if (ao_cmd_is_white() || ao_cmd_lex_c == '\n') break; name[c] = ao_cmd_lex_c; ao_cmd_lex(); @@ -467,13 +499,26 @@ ao_pyro_set(void) } pyro_tmp.flags |= ao_pyro_values[v].flag; if (ao_pyro_values[v].offset != NO_VALUE) { + uint8_t negative = 0; + ao_cmd_white(); + if (ao_cmd_lex_c == '-') { + negative = 1; + ao_cmd_lex(); + } ao_cmd_decimal(); if (ao_cmd_status != ao_cmd_success) return; - if (ao_pyro_values[v].flag & AO_PYRO_8_BIT_VALUE) + if (ao_pyro_values[v].flag & AO_PYRO_8_BIT_VALUE) { + if (negative) { + ao_cmd_status = ao_cmd_syntax_error; + return; + } *((uint8_t *) ((char *) &pyro_tmp + ao_pyro_values[v].offset)) = ao_cmd_lex_i; - else - *((int16_t *) ((char *) &pyro_tmp + ao_pyro_values[v].offset)) = ao_cmd_lex_i; + } else { + if (negative) + ao_cmd_lex_i = -ao_cmd_lex_i; + *((int16_t *) (void *) ((char *) &pyro_tmp + ao_pyro_values[v].offset)) = ao_cmd_lex_i; + } } } _ao_config_edit_start();