From d90587535676f9492f0fde6b974353158104ef88 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 14 Jul 2012 01:26:38 -0700 Subject: [PATCH] altos: Add arbitrary pyro channel support Programmed by specifying a conjunction of flight conditions that trigger the igniter to fire. Signed-off-by: Keith Packard --- src/core/ao.h | 15 +- src/core/ao_config.c | 12 +- src/core/ao_flight.c | 6 + src/core/ao_flight.h | 4 + src/core/ao_pyro.c | 378 +++++++++++++++++++++++++++++++++++++++++++ src/core/ao_pyro.h | 69 ++++++++ 6 files changed, 481 insertions(+), 3 deletions(-) create mode 100644 src/core/ao_pyro.c create mode 100644 src/core/ao_pyro.h diff --git a/src/core/ao.h b/src/core/ao.h index eb2d47cf..d4be3be4 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -688,6 +688,10 @@ ao_igniter_init(void); * ao_config.c */ +#if AO_PYRO_NUM +#include +#endif + #if HAS_FORCE_FREQ /* * Set this to force the frequency to 434.550MHz @@ -696,7 +700,7 @@ extern __xdata uint8_t ao_force_freq; #endif #define AO_CONFIG_MAJOR 1 -#define AO_CONFIG_MINOR 11 +#define AO_CONFIG_MINOR 12 #define AO_AES_LEN 16 @@ -718,6 +722,9 @@ struct ao_config { uint8_t aes_key[AO_AES_LEN]; /* minor version 9 */ uint32_t frequency; /* minor version 10 */ uint16_t apogee_lockout; /* minor version 11 */ +#if AO_PYRO_NUM + struct ao_pyro pyro[AO_PYRO_NUM]; /* minor version 12 */ +#endif }; #define AO_IGNITE_MODE_DUAL 0 @@ -731,6 +738,12 @@ extern __xdata struct ao_config ao_config; #define AO_CONFIG_MAX_SIZE 128 +void +_ao_config_edit_start(void); + +void +_ao_config_edit_finish(void); + void ao_config_get(void); diff --git a/src/core/ao_config.c b/src/core/ao_config.c index ced8b1f2..88fcc12d 100644 --- a/src/core/ao_config.c +++ b/src/core/ao_config.c @@ -131,6 +131,10 @@ _ao_config_get(void) ao_config.frequency = 434550; if (minor < 11) ao_config.apogee_lockout = 0; +#if AO_PYRO_NUM + if (minor < 12) + memset(&ao_config.pyro, '\0', sizeof (ao_config.pyro)); +#endif ao_config.minor = AO_CONFIG_MINOR; ao_config_dirty = 1; } @@ -144,14 +148,14 @@ _ao_config_get(void) ao_config_loaded = 1; } -static void +void _ao_config_edit_start(void) { ao_mutex_get(&ao_config_mutex); _ao_config_get(); } -static void +void _ao_config_edit_finish(void) { ao_config_dirty = 1; @@ -537,6 +541,10 @@ __code struct ao_config_var ao_config_vars[] = { #if HAS_AES { "k <32 hex digits>\0Set AES encryption key", ao_config_key_set, ao_config_key_show }, +#endif +#if AO_PYRO_NUM + { "P \0Configure pyro channels", + ao_pyro_set, ao_pyro_show }, #endif { "s\0Show", ao_config_show, 0 }, diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index c6786455..fc018f00 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -40,6 +40,9 @@ __pdata enum ao_flight_state ao_flight_state; /* current flight state */ __pdata uint16_t ao_boost_tick; /* time of launch detect */ +#if AO_PYRO_NUM +__pdata uint16_t ao_motor_number; /* number of motors burned so far */ +#endif /* * track min/max data over a long interval to detect @@ -214,6 +217,9 @@ ao_flight(void) ao_coast_avg_accel = ao_accel; #else ao_flight_state = ao_flight_coast; +#endif +#if AO_PYRO_NUM + ++ao_motor_number; #endif ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); } diff --git a/src/core/ao_flight.h b/src/core/ao_flight.h index aa5ab60d..c5c8af46 100644 --- a/src/core/ao_flight.h +++ b/src/core/ao_flight.h @@ -37,6 +37,10 @@ enum ao_flight_state { }; extern __pdata enum ao_flight_state ao_flight_state; +extern __pdata uint16_t ao_boost_tick; +#if AO_PYRO_NUM +extern __pdata uint16_t ao_motor_number; +#endif extern __pdata uint16_t ao_launch_time; extern __pdata uint8_t ao_flight_force_idle; diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c new file mode 100644 index 00000000..c060a866 --- /dev/null +++ b/src/core/ao_pyro.c @@ -0,0 +1,378 @@ +/* + * Copyright © 2012 Keith Packard + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include +#include +#include + +#define ao_lowbit(x) ((x) & (-x)) + +/* + * Given a pyro structure, figure out + * if the current flight state satisfies all + * of the requirements + */ +static uint8_t +ao_pyro_ready(struct ao_pyro *pyro) +{ + enum ao_pyro_flag flag, flags; + + flags = pyro->flags; + while (flags != ao_pyro_none) { + flag = ao_lowbit(flags); + flags &= ~flag; + switch (flag) { + + case ao_pyro_accel_less: + if (ao_accel <= pyro->accel_less) + continue; + break; + case ao_pyro_accel_greater: + if (ao_accel >= pyro->accel_greater) + continue; + break; + + + case ao_pyro_speed_less: + if (ao_speed <= pyro->speed_less) + continue; + break; + case ao_pyro_speed_greater: + if (ao_speed >= pyro->speed_greater) + continue; + break; + + case ao_pyro_height_less: + if (ao_height <= pyro->height_less) + continue; + break; + case ao_pyro_height_greater: + if (ao_height >= pyro->height_greater) + continue; + break; + + case ao_pyro_orient_less: +// if (ao_orient <= pyro->orient_less) + continue; + break; + case ao_pyro_orient_greater: +// if (ao_orient >= pyro->orient_greater) + continue; + break; + + case ao_pyro_time_less: + if ((int16_t) (ao_time() - ao_boost_tick) <= pyro->time_less) + continue; + break; + case ao_pyro_time_greater: + if ((int16_t) (ao_time() - ao_boost_tick) >= pyro->time_greater) + continue; + break; + + case ao_pyro_ascending: + if (ao_speed > 0) + continue; + break; + case ao_pyro_descending: + if (ao_speed < 0) + continue; + break; + + case ao_pyro_after_motor: + if (ao_motor_number == pyro->motor) + continue; + break; + + case ao_pyro_delay: + /* handled separately */ + continue; + + } + return FALSE; + } + return TRUE; +} + +#define ao_pyro_fire_port(port, pin) do { \ + ao_gpio_set(port, pin, 1); \ + ao_delay(AO_MS_TO_TICKS(50)); \ + ao_gpio_set(port, pin, 0); \ + } while (0) + + +static void +ao_pyro_fire(uint8_t p) +{ + switch (p) { +#if AO_PYRO_NUM > 0 + case 0: ao_pyro_fire_port(AO_PYRO_PORT_0, AO_PYRO_PIN_0); break; +#endif +#if AO_PYRO_NUM > 1 + case 1: ao_pyro_fire_port(AO_PYRO_PORT_1, AO_PYRO_PIN_1); break; +#endif +#if AO_PYRO_NUM > 2 + case 2: ao_pyro_fire_port(AO_PYRO_PORT_2, AO_PYRO_PIN_2); break; +#endif +#if AO_PYRO_NUM > 3 + case 3: ao_pyro_fire_port(AO_PYRO_PORT_3, AO_PYRO_PIN_3); break; +#endif +#if AO_PYRO_NUM > 4 + case 4: ao_pyro_fire_port(AO_PYRO_PORT_4, AO_PYRO_PIN_4); break; +#endif +#if AO_PYRO_NUM > 5 + case 5: ao_pyro_fire_port(AO_PYRO_PORT_5, AO_PYRO_PIN_5); break; +#endif +#if AO_PYRO_NUM > 6 + case 6: ao_pyro_fire_port(AO_PYRO_PORT_6, AO_PYRO_PIN_6); break; +#endif +#if AO_PYRO_NUM > 7 + case 7: ao_pyro_fire_port(AO_PYRO_PORT_7, AO_PYRO_PIN_7); break; +#endif + default: break; + } + ao_delay(AO_MS_TO_TICKS(50)); +} + +static void +ao_pyro(void) +{ + uint8_t p; + struct ao_pyro *pyro; + + 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 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; + } + + /* 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); + } + } +} + +__xdata struct ao_task ao_pyro_task; + +#define NO_VALUE 0xff + +#define AO_PYRO_NAME_LEN 3 + +const struct { + char name[AO_PYRO_NAME_LEN]; + enum ao_pyro_flag flag; + uint8_t offset; + char *help; +} ao_pyro_values[] = { + { "a<", ao_pyro_accel_less, offsetof(struct ao_pyro, accel_less), "accel less (m/ss * 16)" }, + { "a>", ao_pyro_accel_greater, offsetof(struct ao_pyro, accel_greater), "accel greater (m/ss * 16)" }, + + { "s<", ao_pyro_speed_less, offsetof(struct ao_pyro, speed_less), "speed less (m/s * 16)" }, + { "s>", ao_pyro_speed_greater, offsetof(struct ao_pyro, speed_greater), "speed greater (m/s * 16)" }, + + { "h<", ao_pyro_height_less, offsetof(struct ao_pyro, height_less), "height less (m)" }, + { "h>", ao_pyro_height_greater, offsetof(struct ao_pyro, height_greater), "height greater (m)" }, + + { "o<", ao_pyro_orient_less, offsetof(struct ao_pyro, orient_less), "orient less (deg)" }, + { "o>", ao_pyro_orient_greater, offsetof(struct ao_pyro, orient_greater), "orient greater (deg)" }, + + { "t<", ao_pyro_time_less, offsetof(struct ao_pyro, time_less), "time less (s * 100)" }, + { "t>", ao_pyro_time_greater, offsetof(struct ao_pyro, time_greater), "time greater (s * 100)" }, + + { "A", ao_pyro_ascending, NO_VALUE, "ascending" }, + { "D", ao_pyro_descending, NO_VALUE, "descending" }, + + { "m", ao_pyro_after_motor, offsetof(struct ao_pyro, motor), "after motor" }, + + { "d", ao_pyro_delay, offsetof(struct ao_pyro, delay), "delay before firing (s * 100)" }, + { "", ao_pyro_none, NO_VALUE, NULL }, +}; + +static void +ao_pyro_print_name(uint8_t v) +{ + const char *s = ao_pyro_values[v].name; + printf ("%s%s", s, " " + strlen(s)); +} + +static void +ao_pyro_help(void) +{ + uint8_t v; + for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) { + ao_pyro_print_name(v); + if (ao_pyro_values[v].offset != NO_VALUE) + printf (" "); + else + printf (" "); + printf ("%s\n", ao_pyro_values[v].help); + } +} + +void +ao_pyro_show(void) +{ + uint8_t p; + uint8_t v; + struct ao_pyro *pyro; + + for (p = 0; p < AO_PYRO_NUM; p++) { + printf ("Pyro %2d: ", p); + pyro = &ao_config.pyro[p]; + if (!pyro->flags) { + printf ("\n"); + continue; + } + for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) { + if (!(pyro->flags & ao_pyro_values[v].flag)) + continue; + ao_pyro_print_name(v); + if (ao_pyro_values[v].offset != NO_VALUE) { + int16_t value; + + value = *((int16_t *) ((char *) pyro + ao_pyro_values[v].offset)); + printf ("%6d ", value); + } else { + printf (" "); + } + } + printf ("\n"); + } +} + +void +ao_pyro_set(void) +{ + uint8_t p; + struct ao_pyro *pyro; + struct ao_pyro pyro_tmp; + char name[AO_PYRO_NAME_LEN]; + uint8_t c; + uint8_t v; + + ao_cmd_white(); + switch (ao_cmd_lex_c) { + case '?': + ao_pyro_help(); + return; + } + + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + p = ao_cmd_lex_i; + if (p < 0 || AO_PYRO_NUM <= p) { + printf ("invalid pyro channel %d\n", p); + return; + } + pyro_tmp.flags = 0; + 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()) + break; + name[c] = ao_cmd_lex_c; + ao_cmd_lex(); + } + name[c] = '\0'; + for (v = 0; ao_pyro_values[v].flag != ao_pyro_none; v++) { + if (!strcmp (ao_pyro_values[v].name, name)) + break; + } + if (ao_pyro_values[v].flag == ao_pyro_none) { + printf ("invalid pyro field %s\n", name); + ao_cmd_status = ao_cmd_syntax_error; + return; + } + pyro_tmp.flags |= ao_pyro_values[v].flag; + if (ao_pyro_values[v].offset != NO_VALUE) { + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + *((int16_t *) ((char *) &pyro_tmp + ao_pyro_values[v].offset)) = ao_cmd_lex_i; + } + } + _ao_config_edit_start(); + ao_config.pyro[p] = pyro_tmp; + _ao_config_edit_finish(); +} + +void +ao_pyro_init(void) +{ +#if AO_PYRO_NUM > 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, 0); +#endif +#if AO_PYRO_NUM > 2 + 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, 0); +#endif +#if AO_PYRO_NUM > 4 + 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, 0); +#endif +#if AO_PYRO_NUM > 6 + 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, 0); +#endif + ao_add_task(&ao_pyro_task, ao_pyro, "pyro"); +} diff --git a/src/core/ao_pyro.h b/src/core/ao_pyro.h new file mode 100644 index 00000000..1989a9e5 --- /dev/null +++ b/src/core/ao_pyro.h @@ -0,0 +1,69 @@ +/* + * Copyright © 2012 Keith Packard + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PYRO_H_ +#define _AO_PYRO_H_ + +enum ao_pyro_flag { + ao_pyro_none = 0x00000000, + + ao_pyro_accel_less = 0x00000001, + ao_pyro_accel_greater = 0x00000002, + + ao_pyro_speed_less = 0x00000004, + ao_pyro_speed_greater = 0x00000008, + + ao_pyro_height_less = 0x00000010, + ao_pyro_height_greater = 0x00000020, + + ao_pyro_orient_less = 0x00000040, + ao_pyro_orient_greater = 0x00000080, + + ao_pyro_time_less = 0x00000100, + ao_pyro_time_greater = 0x00000200, + + ao_pyro_ascending = 0x00000400, + ao_pyro_descending = 0x00000800, + + ao_pyro_after_motor = 0x00001000, + + ao_pyro_delay = 0x00002000, +}; + +struct ao_pyro { + enum ao_pyro_flag flags; + int16_t accel_less, accel_greater; + int16_t speed_less, speed_greater; + int16_t height_less, height_greater; + int16_t orient_less, orient_greater; + int16_t time_less, time_greater; + int16_t delay; + int16_t motor; + uint16_t delay_done; + uint8_t fired; +}; + +void +ao_pyro_set(void); + +void +ao_pyro_show(void); + +void +ao_pyro_init(void); + +#endif -- 2.30.2