From 24167015705ae831692b95735968b04a876f935e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 4 Apr 2014 23:34:48 -0700 Subject: altos: Rename 'core' to 'kernel' core remains a bad name to use -- dirvish skips files (and directories, it seems) with that name. Signed-off-by: Keith Packard --- src/kernel/ao_pyro.c | 518 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 518 insertions(+) create mode 100644 src/kernel/ao_pyro.c (limited to 'src/kernel/ao_pyro.c') diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c new file mode 100644 index 00000000..e59f5bc4 --- /dev/null +++ b/src/kernel/ao_pyro.c @@ -0,0 +1,518 @@ +/* + * 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_FLIGHT_TEST +#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 + * 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; + +#if HAS_GYRO + case ao_pyro_orient_less: + if (ao_sample_orient <= pyro->orient_less) + continue; + break; + case ao_pyro_orient_greater: + if (ao_sample_orient >= pyro->orient_greater) + continue; + break; +#endif + + 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; + + 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; + } + return FALSE; + } + return TRUE; +} + +#ifndef AO_FLIGHT_TEST +static void +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; +#endif +#if AO_PYRO_NUM > 1 + 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_gpio_set(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_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; +#endif +#if AO_PYRO_NUM > 4 + 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_gpio_set(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_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; +#endif +#if AO_PYRO_NUM > 7 + case 7: ao_gpio_set(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, v); break; +#endif + default: break; + } +} +#endif + +uint8_t ao_pyro_wakeup; + +static void +ao_pyro_pins_fire(uint16_t fire) +{ + uint8_t p; + + 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)); +} + +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 disabled igniters + */ + if (!pyro->flags) + continue; + + any_waiting = 1; + /* Check pyro state to see if it should 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; + if (!pyro->delay_done) + pyro->delay_done = 1; + } + } + + /* 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); + } + + if (fire) + ao_pyro_pins_fire(fire); + + return any_waiting; +} + +#define NO_VALUE 0xff + +#define AO_PYRO_NAME_LEN 3 + +#if !DISABLE_HELP +#define ENABLE_HELP 1 +#endif + +#if ENABLE_HELP +#define HELP(s) (s) +#else +#define HELP(s) +#endif + +const struct { + char name[AO_PYRO_NAME_LEN]; + enum ao_pyro_flag flag; + uint8_t offset; +#if ENABLE_HELP + char *help; +#endif +} ao_pyro_values[] = { + { "a<", ao_pyro_accel_less, offsetof(struct ao_pyro, accel_less), HELP("accel less (m/ss * 16)") }, + { "a>", ao_pyro_accel_greater, offsetof(struct ao_pyro, accel_greater), HELP("accel greater (m/ss * 16)") }, + + { "s<", ao_pyro_speed_less, offsetof(struct ao_pyro, speed_less), HELP("speed less (m/s * 16)") }, + { "s>", ao_pyro_speed_greater, offsetof(struct ao_pyro, speed_greater), HELP("speed greater (m/s * 16)") }, + + { "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_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 + + { "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") }, + + { "m", ao_pyro_after_motor, offsetof(struct ao_pyro, motor), HELP("after motor") }, + + { "d", ao_pyro_delay, offsetof(struct ao_pyro, delay), HELP("delay before firing (s * 100)") }, + { "", 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) +{ + const char *s = ao_pyro_values[v].name; + printf ("%s%s", s, " " + strlen(s)); +} + +#if ENABLE_HELP +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); + } +} +#endif + +void +ao_pyro_show(void) +{ + uint8_t p; + uint8_t v; + struct ao_pyro *pyro; + + printf ("Pyro-count: %d\n", AO_PYRO_NUM); + 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_tmp; + char name[AO_PYRO_NAME_LEN]; + uint8_t c; + uint8_t v; + + ao_cmd_white(); + +#if ENABLE_HELP + switch (ao_cmd_lex_c) { + case '?': + ao_pyro_help(); + return; + } +#endif + + 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; + } + 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_manual(uint8_t p) +{ + printf ("ao_pyro_manual %d\n", p); + if (p >= AO_PYRO_NUM) { + ao_cmd_status = ao_cmd_syntax_error; + return; + } + ao_pyro_pins_fire(1 << p); +} + +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); +#endif +#if AO_PYRO_NUM > 1 + ao_enable_output(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1, 0); +#endif +#if AO_PYRO_NUM > 2 + ao_enable_output(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2, 0); +#endif +#if AO_PYRO_NUM > 3 + ao_enable_output(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3, 0); +#endif +#if AO_PYRO_NUM > 4 + ao_enable_output(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4, 0); +#endif +#if AO_PYRO_NUM > 5 + ao_enable_output(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5, 0); +#endif +#if AO_PYRO_NUM > 6 + ao_enable_output(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6, 0); +#endif +#if AO_PYRO_NUM > 7 + ao_enable_output(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, 0); +#endif + ao_add_task(&ao_pyro_task, ao_pyro, "pyro"); +} +#endif -- cgit v1.2.3 From b7abc063fb27da29cd7a717bbea15f92882bd205 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 2 Jun 2014 22:04:11 -0700 Subject: altos: Maximum pyro configuration parameter has 4 bytes in the name "f>=" needs four bytes, not just three to store the whole string. If we only store three, then we never manage to compare correctly as the null terminating byte is missing. Signed-off-by: Keith Packard --- src/kernel/ao_pyro.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/kernel/ao_pyro.c') diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c index e59f5bc4..56758fa4 100644 --- a/src/kernel/ao_pyro.c +++ b/src/kernel/ao_pyro.c @@ -281,7 +281,7 @@ ao_pyro_check(void) #define NO_VALUE 0xff -#define AO_PYRO_NAME_LEN 3 +#define AO_PYRO_NAME_LEN 4 #if !DISABLE_HELP #define ENABLE_HELP 1 -- cgit v1.2.3 From d20c608ce833fb8949dce527f92887775d216823 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 2 Jun 2014 22:05:11 -0700 Subject: altos: Fetch/store only 8 bits for pyro state values These fields are uint8_t, not int16_t. Fetching and storing 16 bits is a bad idea. Signed-off-by: Keith Packard --- src/kernel/ao_pyro.c | 10 ++++++++-- src/kernel/ao_pyro.h | 2 ++ 2 files changed, 10 insertions(+), 2 deletions(-) (limited to 'src/kernel/ao_pyro.c') diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c index 56758fa4..0ee7fbee 100644 --- a/src/kernel/ao_pyro.c +++ b/src/kernel/ao_pyro.c @@ -403,7 +403,10 @@ ao_pyro_show(void) if (ao_pyro_values[v].offset != NO_VALUE) { int16_t value; - value = *((int16_t *) ((char *) pyro + ao_pyro_values[v].offset)); + 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)); printf ("%6d ", value); } else { printf (" "); @@ -467,7 +470,10 @@ ao_pyro_set(void) 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; + if (ao_pyro_values[v].flag & AO_PYRO_8_BIT_VALUE) + *((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; } } _ao_config_edit_start(); diff --git a/src/kernel/ao_pyro.h b/src/kernel/ao_pyro.h index 34c99078..b37aaeb1 100644 --- a/src/kernel/ao_pyro.h +++ b/src/kernel/ao_pyro.h @@ -65,6 +65,8 @@ struct ao_pyro { uint8_t fired; }; +#define AO_PYRO_8_BIT_VALUE (ao_pyro_state_less|ao_pyro_state_greater_or_equal) + extern uint8_t ao_pyro_wakeup; extern uint16_t ao_pyro_fired; -- cgit v1.2.3 From da9575fce5ff4dfe83522e290973a01c43e4661f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 10 Jun 2014 09:42:43 -0700 Subject: altos: Make extra pyro channel firing time configurable This adds a 'I' parameter to set the extra pyro channel firing time (in ticks). This has no effect on the main/drogue channels. Signed-off-by: Keith Packard --- src/kernel/ao_config.c | 26 ++++++++++++++++++++++++++ src/kernel/ao_config.h | 5 ++++- src/kernel/ao_pyro.c | 2 +- 3 files changed, 31 insertions(+), 2 deletions(-) (limited to 'src/kernel/ao_pyro.c') diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c index b4847cdd..72170555 100644 --- a/src/kernel/ao_config.c +++ b/src/kernel/ao_config.c @@ -44,6 +44,7 @@ __xdata uint8_t ao_config_mutex; #define AO_CONFIG_DEFAULT_APOGEE_DELAY 0 #define AO_CONFIG_DEFAULT_IGNITE_MODE AO_IGNITE_MODE_DUAL #define AO_CONFIG_DEFAULT_PAD_ORIENTATION AO_PAD_ORIENTATION_ANTENNA_UP +#define AO_CONFIG_DEFAULT_PYRO_TIME AO_MS_TO_TICKS(50) #if HAS_EEPROM #ifndef USE_INTERNAL_FLASH #error Please define USE_INTERNAL_FLASH @@ -186,6 +187,10 @@ _ao_config_get(void) ao_config.tracker_start_horiz = AO_CONFIG_DEFAULT_TRACKER_START_HORIZ; ao_config.tracker_start_vert = AO_CONFIG_DEFAULT_TRACKER_START_VERT; } +#endif +#if AO_PYRO_NUM + if (minor < 18) + ao_config.pyro_time = AO_CONFIG_DEFAULT_PYRO_TIME; #endif ao_config.minor = AO_CONFIG_MINOR; ao_config_dirty = 1; @@ -713,6 +718,25 @@ ao_config_tracker_set(void) } #endif /* HAS_TRACKER */ +#if AO_PYRO_NUM +void +ao_config_pyro_time_show(void) +{ + printf ("Pyro time: %d\n", ao_config.pyro_time); +} + +void +ao_config_pyro_time_set(void) +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.pyro_time = ao_cmd_lex_i; + _ao_config_edit_finish(); +} +#endif + struct ao_config_var { __code char *str; void (*set)(void) __reentrant; @@ -778,6 +802,8 @@ __code struct ao_config_var ao_config_vars[] = { #if AO_PYRO_NUM { "P \0Pyro channels", ao_pyro_set, ao_pyro_show }, + { "I \0Pyro firing time", + ao_config_pyro_time_set, ao_config_pyro_time_show }, #endif #if HAS_APRS { "A \0APRS packet interval (0 disable)", diff --git a/src/kernel/ao_config.h b/src/kernel/ao_config.h index 7ad221c6..77f73fbe 100644 --- a/src/kernel/ao_config.h +++ b/src/kernel/ao_config.h @@ -53,7 +53,7 @@ #endif #define AO_CONFIG_MAJOR 1 -#define AO_CONFIG_MINOR 17 +#define AO_CONFIG_MINOR 18 #define AO_AES_LEN 16 @@ -99,6 +99,9 @@ struct ao_config { uint16_t tracker_start_horiz; /* minor version 17 */ uint16_t tracker_start_vert; /* minor version 17 */ #endif +#if AO_PYRO_NUM + uint16_t pyro_time; /* minor version 18 */ +#endif }; #define AO_IGNITE_MODE_DUAL 0 diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c index 0ee7fbee..85d88d98 100644 --- a/src/kernel/ao_pyro.c +++ b/src/kernel/ao_pyro.c @@ -213,7 +213,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); -- cgit v1.2.3