X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fkernel%2Fao_pyro.c;h=30d1518f051701146b8e96a95a74d7715a685ee5;hp=3044d56517aa06fceaf198fb31a4165e5d6b9370;hb=0686a7b8aec524d81bda4c572549a3a068ce0eed;hpb=8c212cd5bfa03f71a31d84bd0051314e77d88461 diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c index 3044d565..30d1518f 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 @@ -37,8 +38,8 @@ enum ao_igniter_status ao_pyro_status(uint8_t p) { - __xdata struct ao_data packet; - __pdata int16_t value; + struct ao_data packet; + int16_t value; ao_arch_critical( ao_data_get(&packet); @@ -74,11 +75,25 @@ uint16_t ao_pyro_fired; #endif #if PYRO_DBG -#define DBG(...) do { printf("\t%d: ", (int) (pyro - ao_config.pyro)); printf(__VA_ARGS__); } while (0) +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 +static angle_t +ao_sample_max_orient(void) +{ + uint8_t i; + angle_t max = ao_sample_orients[0]; + + for (i = 1; i < AO_NUM_ORIENT; i++) { + angle_t a = ao_sample_orients[i]; + if (a > max) + max = a; + } + return max; +} /* * Given a pyro structure, figure out * if the current flight state satisfies all @@ -88,6 +103,9 @@ static uint8_t ao_pyro_ready(struct ao_pyro *pyro) { enum ao_pyro_flag flag, flags; +#if HAS_GYRO + angle_t max_orient; +#endif flags = pyro->flags; while (flags != ao_pyro_none) { @@ -128,26 +146,28 @@ ao_pyro_ready(struct ao_pyro *pyro) #if HAS_GYRO case ao_pyro_orient_less: - if (ao_sample_orient <= pyro->orient_less) + max_orient = ao_sample_max_orient(); + if (max_orient <= pyro->orient_less) continue; - DBG("orient %d > %d\n", ao_sample_orient, pyro->orient_less); + DBG("orient %d > %d\n", max_orient, pyro->orient_less); break; case ao_pyro_orient_greater: - if (ao_sample_orient >= pyro->orient_greater) + max_orient = ao_sample_max_orient(); + if (max_orient >= pyro->orient_greater) continue; - DBG("orient %d < %d\n", ao_sample_orient, pyro->orient_greater); + DBG("orient %d < %d\n", max_orient, pyro->orient_greater); break; #endif case ao_pyro_time_less: - if ((int16_t) (ao_time() - ao_boost_tick) <= pyro->time_less) + if ((int16_t) (ao_time() - ao_launch_tick) <= pyro->time_less) continue; - DBG("time %d > %d\n", (int16_t)(ao_time() - ao_boost_tick), pyro->time_less); + DBG("time %d > %d\n", (int16_t)(ao_time() - ao_launch_tick), pyro->time_less); break; case ao_pyro_time_greater: - if ((int16_t) (ao_time() - ao_boost_tick) >= pyro->time_greater) + if ((int16_t) (ao_time() - ao_launch_tick) >= pyro->time_greater) continue; - DBG("time %d < %d\n", (int16_t)(ao_time() - ao_boost_tick), pyro->time_greater); + DBG("time %d < %d\n", (int16_t)(ao_time() - ao_launch_tick), pyro->time_greater); break; case ao_pyro_ascending: @@ -162,7 +182,7 @@ ao_pyro_ready(struct ao_pyro *pyro) break; case ao_pyro_after_motor: - if (ao_motor_number == pyro->motor) + if (ao_motor_number >= pyro->motor) continue; DBG("motor %d != %d\n", ao_motor_number, pyro->motor); break; @@ -179,15 +199,15 @@ ao_pyro_ready(struct ao_pyro *pyro) 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_less); + DBG("state %d < %d\n", ao_flight_state, pyro->state_greater_or_equal); break; default: continue; } - return FALSE; + return false; } - return TRUE; + return true; } #ifndef AO_FLIGHT_TEST @@ -196,28 +216,28 @@ ao_pyro_pin_set(uint8_t p, uint8_t v) { switch (p) { #if AO_PYRO_NUM > 0 - case 0: ao_gpio_set(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0, v); break; + case 0: ao_gpio_set(AO_PYRO_PORT_0, AO_PYRO_PIN_0, v); break; #endif #if AO_PYRO_NUM > 1 - case 1: ao_gpio_set(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1, v); break; + case 1: ao_gpio_set(AO_PYRO_PORT_1, AO_PYRO_PIN_1, v); break; #endif #if AO_PYRO_NUM > 2 - case 2: ao_gpio_set(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2, v); break; + case 2: ao_gpio_set(AO_PYRO_PORT_2, AO_PYRO_PIN_2, v); break; #endif #if AO_PYRO_NUM > 3 - case 3: ao_gpio_set(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3, v); break; + case 3: ao_gpio_set(AO_PYRO_PORT_3, AO_PYRO_PIN_3, v); break; #endif #if AO_PYRO_NUM > 4 - case 4: ao_gpio_set(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4, v); break; + case 4: ao_gpio_set(AO_PYRO_PORT_4, AO_PYRO_PIN_4, v); break; #endif #if AO_PYRO_NUM > 5 - case 5: ao_gpio_set(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5, v); break; + case 5: ao_gpio_set(AO_PYRO_PORT_5, AO_PYRO_PIN_5, v); break; #endif #if AO_PYRO_NUM > 6 - case 6: ao_gpio_set(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6, v); break; + case 6: ao_gpio_set(AO_PYRO_PORT_6, AO_PYRO_PIN_6, v); break; #endif #if AO_PYRO_NUM > 7 - case 7: ao_gpio_set(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, v); break; + case 7: ao_gpio_set(AO_PYRO_PORT_7, AO_PYRO_PIN_7, v); break; #endif default: break; } @@ -237,11 +257,8 @@ ao_pyro_pins_fire(uint16_t fire) } ao_delay(ao_config.pyro_time); for (p = 0; p < AO_PYRO_NUM; p++) { - if (fire & (1 << 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)); } @@ -259,7 +276,7 @@ ao_pyro_check(void) /* Ignore igniters which have already fired */ - if (pyro->fired) + if (ao_pyro_fired & (1 << p)) continue; /* Ignore disabled igniters @@ -294,7 +311,7 @@ ao_pyro_check(void) * by setting the fired bit */ if (!ao_pyro_ready(pyro)) { - pyro->fired = 1; + ao_pyro_fired |= (1 << p); continue; } @@ -305,8 +322,10 @@ ao_pyro_check(void) fire |= (1 << p); } - if (fire) + if (fire) { + ao_pyro_fired |= fire; ao_pyro_pins_fire(fire); + } return any_waiting; } @@ -375,9 +394,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(); @@ -387,7 +404,7 @@ ao_pyro(void) ao_exit(); } -__xdata struct ao_task ao_pyro_task; +struct ao_task ao_pyro_task; static void @@ -438,7 +455,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 (" "); @@ -467,10 +484,9 @@ ao_pyro_set(void) } #endif - ao_cmd_decimal(); + p = ao_cmd_decimal(); if (ao_cmd_status != ao_cmd_success) return; - p = ao_cmd_lex_i; if (AO_PYRO_NUM <= p) { printf ("invalid pyro channel %d\n", p); return; @@ -482,7 +498,7 @@ ao_pyro_set(void) 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(); @@ -499,25 +515,23 @@ ao_pyro_set(void) } pyro_tmp.flags |= ao_pyro_values[v].flag; if (ao_pyro_values[v].offset != NO_VALUE) { - uint8_t negative = 0; + int16_t r = 1; ao_cmd_white(); if (ao_cmd_lex_c == '-') { - negative = 1; + r = -1; ao_cmd_lex(); } - ao_cmd_decimal(); + r *= ao_cmd_decimal(); if (ao_cmd_status != ao_cmd_success) return; if (ao_pyro_values[v].flag & AO_PYRO_8_BIT_VALUE) { - if (negative) { + if (r < 0) { ao_cmd_status = ao_cmd_syntax_error; return; } - *((uint8_t *) ((char *) &pyro_tmp + ao_pyro_values[v].offset)) = ao_cmd_lex_i; + *((uint8_t *) ((char *) &pyro_tmp + ao_pyro_values[v].offset)) = r; } else { - if (negative) - ao_cmd_lex_i = -ao_cmd_lex_i; - *((int16_t *) ((char *) &pyro_tmp + ao_pyro_values[v].offset)) = ao_cmd_lex_i; + *((int16_t *) (void *) ((char *) &pyro_tmp + ao_pyro_values[v].offset)) = r; } } } @@ -541,28 +555,28 @@ void ao_pyro_init(void) { #if AO_PYRO_NUM > 0 - ao_enable_output(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0, 0); + ao_enable_output(AO_PYRO_PORT_0, AO_PYRO_PIN_0, 0); #endif #if AO_PYRO_NUM > 1 - ao_enable_output(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1, 0); + ao_enable_output(AO_PYRO_PORT_1, AO_PYRO_PIN_1, 0); #endif #if AO_PYRO_NUM > 2 - ao_enable_output(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2, 0); + ao_enable_output(AO_PYRO_PORT_2, AO_PYRO_PIN_2, 0); #endif #if AO_PYRO_NUM > 3 - ao_enable_output(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3, 0); + ao_enable_output(AO_PYRO_PORT_3, AO_PYRO_PIN_3, 0); #endif #if AO_PYRO_NUM > 4 - ao_enable_output(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4, 0); + ao_enable_output(AO_PYRO_PORT_4, AO_PYRO_PIN_4, 0); #endif #if AO_PYRO_NUM > 5 - ao_enable_output(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5, 0); + ao_enable_output(AO_PYRO_PORT_5, AO_PYRO_PIN_5, 0); #endif #if AO_PYRO_NUM > 6 - ao_enable_output(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6, 0); + ao_enable_output(AO_PYRO_PORT_6, AO_PYRO_PIN_6, 0); #endif #if AO_PYRO_NUM > 7 - ao_enable_output(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, 0); + ao_enable_output(AO_PYRO_PORT_7, AO_PYRO_PIN_7, 0); #endif ao_add_task(&ao_pyro_task, ao_pyro, "pyro"); }