X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fkernel%2Fao_pyro.c;h=30d1518f051701146b8e96a95a74d7715a685ee5;hb=HEAD;hp=5a556d594c75b5b8947d49757d68deb3a58bab5c;hpb=b478d3c3569d2f9df50b0030197468d14af67688;p=fw%2Faltos diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c index 5a556d59..4c69b82a 100644 --- a/src/kernel/ao_pyro.c +++ b/src/kernel/ao_pyro.c @@ -38,21 +38,15 @@ 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); ); - 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; + return ao_igniter_check(value, AO_SENSE_PBATT(&packet)); } void @@ -69,6 +63,7 @@ ao_pyro_print_status(void) #endif uint16_t ao_pyro_fired; +uint16_t ao_pyro_inhibited; #ifndef PYRO_DBG #define PYRO_DBG 0 @@ -100,7 +95,7 @@ ao_sample_max_orient(void) * of the requirements */ static uint8_t -ao_pyro_ready(struct ao_pyro *pyro) +ao_pyro_ready(const struct ao_pyro *pyro) { enum ao_pyro_flag flag, flags; #if HAS_GYRO @@ -160,14 +155,14 @@ ao_pyro_ready(struct ao_pyro *pyro) #endif case ao_pyro_time_less: - if ((int16_t) (ao_time() - ao_boost_tick) <= pyro->time_less) + if ((AO_TICK_SIGNED) (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", (AO_TICK_SIGNED)(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 ((AO_TICK_SIGNED) (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", (AO_TICK_SIGNED)(ao_time() - ao_launch_tick), pyro->time_greater); break; case ao_pyro_ascending: @@ -182,7 +177,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; @@ -205,9 +200,9 @@ ao_pyro_ready(struct ao_pyro *pyro) default: continue; } - return FALSE; + return false; } - return TRUE; + return true; } #ifndef AO_FLIGHT_TEST @@ -216,28 +211,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; } @@ -263,10 +258,12 @@ ao_pyro_pins_fire(uint16_t fire) ao_delay(AO_MS_TO_TICKS(50)); } +static AO_TICK_TYPE pyro_delay_done[AO_PYRO_NUM]; + static uint8_t ao_pyro_check(void) { - struct ao_pyro *pyro; + const struct ao_pyro *pyro; uint8_t p, any_waiting; uint16_t fire = 0; @@ -274,9 +271,9 @@ ao_pyro_check(void) for (p = 0; p < AO_PYRO_NUM; p++) { pyro = &ao_config.pyro[p]; - /* Ignore igniters which have already fired + /* Ignore igniters which have already fired or inhibited */ - if (ao_pyro_fired & (1 << p)) + if ((ao_pyro_fired|ao_pyro_inhibited) & (1 << p)) continue; /* Ignore disabled igniters @@ -287,7 +284,7 @@ ao_pyro_check(void) any_waiting = 1; /* Check pyro state to see if it should fire */ - if (!pyro->delay_done) { + if (!pyro_delay_done[p]) { if (!ao_pyro_ready(pyro)) continue; @@ -295,31 +292,47 @@ ao_pyro_check(void) * it expires */ if (pyro->flags & ao_pyro_delay) { - pyro->delay_done = ao_time() + pyro->delay; - if (!pyro->delay_done) - pyro->delay_done = 1; + AO_TICK_TYPE delay_done; + AO_TICK_SIGNED delay = pyro->delay; + if (delay < 0) + delay = 0; + delay_done = ao_time() + (AO_TICK_TYPE) delay; + + /* + * delay_done is the time at which the + * delay ends, but it is also used as + * an indication that a delay is + * active -- non-zero values indicate + * an active delay. This code ensures + * the value is non-zero, which might + * introduce an additional tick + * of delay. + */ + if (delay_done == 0) + delay_done = 1; + pyro_delay_done[p] = delay_done; } } /* Check to see if we're just waiting for * the delay to expire */ - if (pyro->delay_done) { + if (pyro_delay_done[p] != 0) { /* Check to make sure the required conditions * remain valid. If not, inhibit the channel - * by setting the fired bit + * by setting the inhibited bit */ if (!ao_pyro_ready(pyro)) { - ao_pyro_fired |= (1 << p); + ao_pyro_inhibited |= (uint16_t) (1 << p); continue; } - if ((int16_t) (ao_time() - pyro->delay_done) < 0) + if ((AO_TICK_SIGNED) (ao_time() - pyro_delay_done[p]) < 0) continue; } - fire |= (1 << p); + fire |= (uint16_t) (1U << p); } if (fire) { @@ -404,7 +417,7 @@ ao_pyro(void) ao_exit(); } -__xdata struct ao_task ao_pyro_task; +struct ao_task ao_pyro_task; static void @@ -430,6 +443,55 @@ ao_pyro_help(void) } #endif +static int32_t +ao_pyro_get(void *base, uint8_t offset, uint8_t size) +{ + int32_t value; + switch (size) { + case 8: + value = *((uint8_t *) ((char *) base + offset)); + break; + case 16: + default: + value = *((int16_t *) (void *) ((char *) base + offset)); + break; + case 32: + value = *((int32_t *) (void *) ((char *) base + offset)); + break; + } + return value; +} + +static bool +ao_pyro_put(void *base, uint8_t offset, uint8_t size, int32_t value) +{ + switch (size) { + case 8: + if (value < 0) + return false; + *((uint8_t *) ((char *) base + offset)) = (uint8_t) value; + break; + case 16: + default: + *((int16_t *) (void *) ((char *) base + offset)) = (int16_t) value; + break; + case 32: + *((int32_t *) (void *) ((char *) base + offset)) = value; + break; + } + return true; +} + +static uint8_t +ao_pyro_size(enum ao_pyro_flag flag) +{ + if (flag & AO_PYRO_8_BIT_VALUE) + return 8; + if (flag & AO_PYRO_32_BIT_VALUE) + return 32; + return 16; +} + void ao_pyro_show(void) { @@ -450,13 +512,10 @@ ao_pyro_show(void) continue; ao_pyro_print_name(v); if (ao_pyro_values[v].offset != NO_VALUE) { - int16_t value; - - 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 *) (void *) ((char *) pyro + ao_pyro_values[v].offset)); - printf ("%6d ", value); + printf ("%6ld ", + (long) ao_pyro_get(pyro, + ao_pyro_values[v].offset, + ao_pyro_size(ao_pyro_values[v].flag))); } else { printf (" "); } @@ -468,7 +527,7 @@ ao_pyro_show(void) void ao_pyro_set(void) { - uint8_t p; + uint32_t p; struct ao_pyro pyro_tmp; char name[AO_PYRO_NAME_LEN]; uint8_t c; @@ -484,12 +543,11 @@ 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); + printf ("invalid pyro channel %lu\n", p); return; } memset(&pyro_tmp, '\0', sizeof (pyro_tmp)); @@ -516,25 +574,20 @@ ao_pyro_set(void) } pyro_tmp.flags |= ao_pyro_values[v].flag; if (ao_pyro_values[v].offset != NO_VALUE) { - uint8_t negative = 0; + int32_t r = 1; ao_cmd_white(); if (ao_cmd_lex_c == '-') { - negative = 1; + r = -1; ao_cmd_lex(); } - ao_cmd_decimal(); + r *= (int32_t) ao_cmd_decimal(); if (ao_cmd_status != ao_cmd_success) return; - 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 { - 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; + if (!ao_pyro_put(&pyro_tmp, ao_pyro_values[v].offset, + ao_pyro_size(ao_pyro_values[v].flag), r)) + { + ao_cmd_status = ao_cmd_syntax_error; + return; } } } @@ -554,32 +607,120 @@ ao_pyro_manual(uint8_t p) ao_pyro_pins_fire(1 << p); } +struct ao_pyro_old_values { + enum ao_pyro_flag flag; + uint8_t offset; + uint8_t size; +}; + +static const struct ao_pyro_old_values ao_pyro_1_24_values[] = { + { .flag = ao_pyro_accel_less, .offset = offsetof(struct ao_pyro_1_24, accel_less), 16 }, + { .flag = ao_pyro_accel_greater, .offset = offsetof(struct ao_pyro_1_24, accel_greater), 16 }, + { .flag = ao_pyro_speed_less, .offset = offsetof(struct ao_pyro_1_24, speed_less), 16 }, + { .flag = ao_pyro_speed_greater, .offset = offsetof(struct ao_pyro_1_24, speed_greater), 16 }, + { .flag = ao_pyro_height_less, .offset = offsetof(struct ao_pyro_1_24, height_less), 16 }, + { .flag = ao_pyro_height_greater, .offset = offsetof(struct ao_pyro_1_24, height_greater), 16 }, + { .flag = ao_pyro_orient_less, .offset = offsetof(struct ao_pyro_1_24, orient_less), 16 }, + { .flag = ao_pyro_orient_greater, .offset = offsetof(struct ao_pyro_1_24, orient_greater), 16 }, + { .flag = ao_pyro_time_less, .offset = offsetof(struct ao_pyro_1_24, time_less), 16 }, + { .flag = ao_pyro_time_greater, .offset = offsetof(struct ao_pyro_1_24, time_greater), 16 }, + { .flag = ao_pyro_delay, .offset = offsetof(struct ao_pyro_1_24, delay), 16 }, + { .flag = ao_pyro_state_less, .offset = offsetof(struct ao_pyro_1_24, state_less), 8 }, + { .flag = ao_pyro_state_greater_or_equal, .offset = offsetof(struct ao_pyro_1_24, state_greater_or_equal), 8 }, + { .flag = ao_pyro_after_motor, .offset = offsetof(struct ao_pyro_1_24, motor), 16 }, +}; + +#define NUM_PYRO_1_24_VALUES (sizeof ao_pyro_1_24_values / sizeof ao_pyro_1_24_values[0]) + +static int32_t +ao_pyro_get_1_24(void *base, enum ao_pyro_flag flag) +{ + unsigned v; + + for (v = 0; v < NUM_PYRO_1_24_VALUES; v++) { + if (ao_pyro_1_24_values[v].flag == flag) + return ao_pyro_get(base, ao_pyro_1_24_values[v].offset, ao_pyro_1_24_values[v].size); + } + return 0; +} + +void +ao_pyro_update_version(void) +{ + if (ao_config.minor <= 24) + { + + /* First, move all of the config bits that follow the pyro data */ + + struct ao_config_1_24 *ao_config_1_24 = (void *) &ao_config; + struct ao_pyro *pyro_1_25 = &ao_config.pyro[0]; + struct ao_pyro_1_24 *pyro_1_24 = &(ao_config_1_24->pyro)[0]; + + + char *pyro_base_1_25 = (void *) pyro_1_25; + char *pyro_base_1_24 = (void *) pyro_1_24; + char *after_pyro_1_25 = pyro_base_1_25 + AO_PYRO_NUM * sizeof (struct ao_pyro); + char *after_pyro_1_24 = pyro_base_1_24 + AO_PYRO_NUM * sizeof (struct ao_pyro_1_24); + + char *config_end_1_25 = (void *) (&ao_config + 1); + size_t to_move = (size_t) (config_end_1_25 - after_pyro_1_25); + + memmove(after_pyro_1_25, after_pyro_1_24, to_move); + + /* Now, adjust all of the pyro entries */ + + int p = AO_PYRO_NUM; + + /* New struct is larger than the old, so start at the + * last one and work towards the first + */ + while (p-- > 0) { + unsigned v; + int32_t value; + struct ao_pyro tmp; + + memset(&tmp, '\0', sizeof(tmp)); + tmp.flags = pyro_1_24[p].flags; + + for (v = 0; v < NUM_PYRO_VALUES; v++) + { + if (ao_pyro_values[v].offset != NO_VALUE) { + value = ao_pyro_get_1_24(&pyro_1_24[p], ao_pyro_values[v].flag); + ao_pyro_put(&tmp, ao_pyro_values[v].offset, + ao_pyro_size(ao_pyro_values[v].flag), value); + } + } + memcpy(&pyro_1_25[p], &tmp, sizeof(tmp)); + } + } +} + 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"); }