diff options
| -rw-r--r-- | Makefile | 6 | ||||
| -rw-r--r-- | ao.h | 31 | ||||
| -rw-r--r-- | ao_adc.c | 9 | ||||
| -rw-r--r-- | ao_flight.c | 121 | ||||
| -rw-r--r-- | ao_flight_test.c | 205 | ||||
| -rw-r--r-- | ao_ignite.c | 123 | ||||
| -rw-r--r-- | ao_log.c | 4 | ||||
| -rw-r--r-- | ao_monitor.c | 1 | ||||
| -rw-r--r-- | ao_telemetrum.c | 1 |
9 files changed, 462 insertions, 39 deletions
@@ -61,7 +61,8 @@ TELE_DRIVER_SRC = \ # TM_DRIVER_SRC = \ ao_adc.c \ - ao_ee.c + ao_ee.c \ + ao_ignite.c # # Tasks run on TeleMetrum @@ -193,3 +194,6 @@ clean: rm -f $(PROGS) $(PCDB) $(PLNK) $(PMAP) $(PMEM) $(PAOM) install: + +ao_flight_test: ao_flight.c ao_flight_test.c + cc -g -o $@ ao_flight_test.c @@ -29,7 +29,7 @@ /* Stack runs from above the allocated __data space to 0xfe, which avoids * writing to 0xff as that triggers the stack overflow indicator */ -#define AO_STACK_START 0x68 +#define AO_STACK_START 0x70 #define AO_STACK_END 0xfe #define AO_STACK_SIZE (AO_STACK_END - AO_STACK_START + 1) @@ -123,6 +123,8 @@ ao_timer_init(void); */ #define AO_ADC_RING 64 +#define ao_adc_ring_next(n) (((n) + 1) & (AO_ADC_RING - 1)) +#define ao_adc_ring_prev(n) (((n) - 1) & (AO_ADC_RING - 1)) /* * One set of samples read from the A/D converter @@ -544,7 +546,7 @@ enum ao_flight_state { }; extern __xdata struct ao_adc ao_flight_data; -extern __pdata enum flight_state ao_flight_state; +extern __pdata enum ao_flight_state ao_flight_state; extern __pdata uint16_t ao_flight_tick; extern __pdata int16_t ao_flight_accel; extern __pdata int16_t ao_flight_pres; @@ -734,5 +736,30 @@ ao_monitor_init(void); void flush(void); +/* + * ao_ignite.c + */ + +enum ao_igniter { + ao_igniter_drogue = 0, + ao_igniter_main = 1 +}; + +void +ao_ignite(enum ao_igniter igniter); + +enum ao_igniter_status { + ao_igniter_unknown, /* unknown status (ambiguous voltage) */ + ao_igniter_ready, /* continuity detected */ + ao_igniter_active, /* igniter firing */ + ao_igniter_open, /* open circuit detected */ +}; + +enum ao_igniter_status +ao_igniter_status(enum ao_igniter igniter); + +void +ao_igniter_init(void); + #endif /* _AO_H_ */ @@ -35,10 +35,7 @@ ao_adc_sleep(void) void ao_adc_get(__xdata struct ao_adc *packet) { - uint8_t i = ao_adc_head; - if (i == 0) - i = AO_ADC_RING; - i--; + uint8_t i = ao_adc_ring_prev(ao_adc_head); memcpy(packet, &ao_adc_ring[i], sizeof (struct ao_adc)); } @@ -58,9 +55,7 @@ ao_adc_isr(void) interrupt 1 } else { /* record this conversion series */ ao_adc_ring[ao_adc_head].tick = ao_time(); - ao_adc_head++; - if (ao_adc_head == AO_ADC_RING) - ao_adc_head = 0; + ao_adc_head = ao_adc_ring_next(ao_adc_head); ao_wakeup(ao_adc_ring); } } diff --git a/ao_flight.c b/ao_flight.c index 3aff866a..5998f291 100644 --- a/ao_flight.c +++ b/ao_flight.c @@ -15,12 +15,13 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */ +#ifndef AO_FLIGHT_TEST #include "ao.h" +#endif /* Main flight thread. */ -__xdata struct ao_adc ao_flight_data; /* last acquired data */ -__pdata enum flight_state ao_flight_state; /* current flight state */ +__pdata enum ao_flight_state ao_flight_state; /* current flight state */ __pdata uint16_t ao_flight_tick; /* time of last data */ __pdata int16_t ao_flight_accel; /* filtered acceleration */ __pdata int16_t ao_flight_pres; /* filtered pressure */ @@ -44,6 +45,9 @@ __pdata int16_t ao_interval_max_accel; __pdata int16_t ao_interval_min_pres; __pdata int16_t ao_interval_max_pres; +__data uint8_t ao_flight_adc; +__xdata int16_t ao_accel, ao_prev_accel, ao_pres; + #define AO_INTERVAL_TICKS AO_SEC_TO_TICKS(5) /* Accelerometer calibration @@ -66,6 +70,7 @@ __pdata int16_t ao_interval_max_pres; #define ACCEL_ZERO_G 16000 #define ACCEL_NOSE_UP (ACCEL_ZERO_G - ACCEL_G * 2 /3) #define ACCEL_BOOST (ACCEL_NOSE_UP - ACCEL_G * 2) +#define ACCEL_LAND (ACCEL_G / 10) /* * Barometer calibration @@ -96,23 +101,45 @@ __pdata int16_t ao_interval_max_pres; * case of other failures */ -#define BOOST_TICKS_MAX AO_SEC_TO_TICKS(10) +#define BOOST_TICKS_MAX AO_SEC_TO_TICKS(15) + +/* This value is scaled in a weird way. It's a running total of accelerometer + * readings minus the ground accelerometer reading. That means it measures + * velocity, and quite accurately too. As it gets updated 100 times a second, + * it's scaled by 100 + */ +__data int32_t ao_flight_vel; + +/* convert m/s to velocity count */ +#define VEL_MPS_TO_COUNT(mps) ((int32_t) ((int32_t) (mps) * (int32_t) 100 / (int32_t) ACCEL_G)) void ao_flight(void) { __pdata static uint8_t nsamples = 0; + ao_flight_adc = ao_adc_head; + ao_prev_accel = 0; + ao_accel = 0; + ao_pres = 0; for (;;) { ao_sleep(&ao_adc_ring); - ao_adc_get(&ao_flight_data); + while (ao_flight_adc != ao_adc_head) { + ao_accel = ao_adc_ring[ao_flight_adc].accel; + ao_pres = ao_adc_ring[ao_flight_adc].pres; + ao_flight_tick = ao_adc_ring[ao_flight_adc].tick; + ao_flight_vel += (int32_t) (((ao_accel + ao_prev_accel) >> 4) - (ao_ground_accel << 1)); + ao_prev_accel = ao_accel; + ao_flight_adc = ao_adc_ring_next(ao_flight_adc); + } ao_flight_accel -= ao_flight_accel >> 4; - ao_flight_accel += ao_flight_data.accel >> 4; + ao_flight_accel += ao_accel >> 4; ao_flight_pres -= ao_flight_pres >> 4; - ao_flight_pres += ao_flight_data.pres >> 4; - ao_flight_tick = ao_time(); + ao_flight_pres += ao_pres >> 4; - ao_flight_tick = ao_time(); + if (ao_flight_pres < ao_min_pres) + ao_min_pres = ao_flight_pres; + if ((int16_t) (ao_flight_tick - ao_interval_end) >= 0) { ao_interval_max_pres = ao_interval_cur_max_pres; ao_interval_min_pres = ao_interval_cur_min_pres; @@ -131,11 +158,12 @@ ao_flight(void) ao_ground_pres = ao_flight_pres; ao_min_pres = ao_flight_pres; ao_main_pres = ao_ground_pres - BARO_MAIN; + ao_flight_vel = 0; ao_interval_end = ao_flight_tick; - /* Go to launchpad state if the nose is pointing up and the battery is charged */ - if (ao_flight_accel < ACCEL_NOSE_UP && ao_flight_data.v_batt > 23000) { + /* Go to launchpad state if the nose is pointing up */ + if (ao_flight_accel < ACCEL_NOSE_UP) { ao_flight_state = ao_flight_launchpad; ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); } else { @@ -152,6 +180,12 @@ ao_flight(void) ao_led_off(AO_LED_RED); break; case ao_flight_launchpad: + + /* pad to boost: + * + * accelerometer: > 2g + * barometer: > 20m vertical motion + */ if (ao_flight_accel < ACCEL_BOOST || ao_flight_pres + BARO_LAUNCH < ao_ground_pres) { @@ -162,8 +196,14 @@ ao_flight(void) } break; case ao_flight_boost: - if (ao_flight_accel > ACCEL_ZERO_G || - (int16_t) (ao_flight_data.tick - ao_launch_time) > BOOST_TICKS_MAX) + + /* boost to coast: + * + * accelerometer: start to fall at > 1/4 G + * time: boost for more than 15 seconds + */ + if (ao_flight_accel > ao_ground_accel + (ACCEL_G >> 2) || + (int16_t) (ao_flight_tick - ao_launch_time) > BOOST_TICKS_MAX) { ao_flight_state = ao_flight_coast; ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); @@ -171,31 +211,60 @@ ao_flight(void) } break; case ao_flight_coast: - if (ao_flight_pres < ao_min_pres) - ao_min_pres = ao_flight_pres; - if (ao_flight_pres - BARO_APOGEE > ao_min_pres) { + + /* coast to apogee detect: + * + * accelerometer: integrated velocity < 200 m/s + * barometer: fall at least 500m from max altitude + */ + if (ao_flight_vel < VEL_MPS_TO_COUNT(200) || + ao_flight_pres - (5 * BARO_kPa) > ao_min_pres) + { ao_flight_state = ao_flight_apogee; ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); } break; case ao_flight_apogee: -// ao_ignite(AO_IGNITE_DROGUE); - ao_flight_state = ao_flight_drogue; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + + /* apogee to drogue deploy: + * + * accelerometer: integrated velocity < 10m/s + * barometer: fall at least 10m + */ + if (ao_flight_vel < VEL_MPS_TO_COUNT(-10) || + ao_flight_pres - BARO_APOGEE > ao_min_pres) + { + ao_ignite(ao_igniter_drogue); + ao_flight_state = ao_flight_drogue; + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); + } break; case ao_flight_drogue: - if (ao_flight_pres >= ao_main_pres) { -// ao_ignite(AO_IGNITE_MAIN); + + /* drogue to main deploy: + * + * accelerometer: abs(velocity) > 50m/s + * barometer: reach main deploy altitude + */ + if (ao_flight_vel < VEL_MPS_TO_COUNT(-50) || + ao_flight_vel > VEL_MPS_TO_COUNT(50) || + ao_flight_pres >= ao_main_pres) + { + ao_ignite(ao_igniter_main); ao_flight_state = ao_flight_main; ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); } - if ((ao_interval_max_pres - ao_interval_min_pres) < BARO_LAND) { - ao_flight_state = ao_flight_landed; - ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); - } - break; + /* fall through... */ case ao_flight_main: - if ((ao_interval_max_pres - ao_interval_min_pres) < BARO_LAND) { + + /* drogue/main to land: + * + * accelerometer: value stable + * barometer: altitude stable + */ + if ((ao_interval_max_accel - ao_interval_min_accel) < ACCEL_LAND || + (ao_interval_max_pres - ao_interval_min_pres) < BARO_LAND) + { ao_flight_state = ao_flight_landed; ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); } diff --git a/ao_flight_test.c b/ao_flight_test.c new file mode 100644 index 00000000..a88d2b14 --- /dev/null +++ b/ao_flight_test.c @@ -0,0 +1,205 @@ +/* + * Copyright © 2009 Keith Packard <keithp@keithp.com> + * + * 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 <stdint.h> +#include <stdio.h> +#include <stdlib.h> + +#define AO_ADC_RING 64 +#define ao_adc_ring_next(n) (((n) + 1) & (AO_ADC_RING - 1)) +#define ao_adc_ring_prev(n) (((n) - 1) & (AO_ADC_RING - 1)) + +/* + * One set of samples read from the A/D converter + */ +struct ao_adc { + uint16_t tick; /* tick when the sample was read */ + int16_t accel; /* accelerometer */ + int16_t pres; /* pressure sensor */ + int16_t temp; /* temperature sensor */ + int16_t v_batt; /* battery voltage */ + int16_t sense_d; /* drogue continuity sense */ + int16_t sense_m; /* main continuity sense */ +}; + +#define __pdata +#define __data +#define __xdata + +enum ao_flight_state { + ao_flight_startup = 0, + ao_flight_idle = 1, + ao_flight_launchpad = 2, + ao_flight_boost = 3, + ao_flight_coast = 4, + ao_flight_apogee = 5, + ao_flight_drogue = 6, + ao_flight_main = 7, + ao_flight_landed = 8, + ao_flight_invalid = 9 +}; + +struct ao_adc ao_adc_ring[AO_ADC_RING]; +uint8_t ao_adc_head; + +#define ao_led_on(l) +#define ao_led_off(l) +#define ao_timer_set_adc_interval(i) +#define ao_wakeup(wchan) ao_dump_state() + +enum ao_igniter { + ao_igniter_drogue = 0, + ao_igniter_main = 1 +}; + +void +ao_ignite(enum ao_igniter igniter) +{ + printf ("ignite %s\n", igniter == ao_igniter_drogue ? "drogue" : "main"); +} + +struct ao_task { + int dummy; +}; + +#define ao_add_task(t,f,n) + +#define ao_log_start() +#define ao_log_stop() + +#define AO_MS_TO_TICKS(ms) ((ms) / 10) +#define AO_SEC_TO_TICKS(s) ((s) * 100) + +#define AO_FLIGHT_TEST + +struct ao_adc ao_adc_static; + +FILE *emulator_in; + +void +ao_dump_state(void); + +void +ao_sleep(void *wchan); + +#include "ao_flight.c" + +void +ao_insert(void) +{ + ao_adc_ring[ao_adc_head] = ao_adc_static; + ao_adc_head = ao_adc_ring_next(ao_adc_head); + if (ao_flight_state != ao_flight_startup) { + printf("tick %04x accel %04x pres %04x\n", + ao_adc_static.tick, + ao_adc_static.accel, + ao_adc_static.pres); + } +} + +static int ao_records_read = 0; + +void +ao_sleep(void *wchan) +{ + ao_dump_state(); + if (wchan == &ao_adc_ring) { + char type; + uint16_t tick; + uint16_t a, b; + int ret; + + for (;;) { + if (ao_records_read > 20 && ao_flight_state == ao_flight_startup) + { + ao_insert(); + return; + } + + ret = fscanf(emulator_in, "%c %hx %hx %hx\n", &type, &tick, &a, &b); + if (ret == EOF) { + ao_insert(); + return; + } + if (ret != 4) + continue; + switch (type) { + case 'F': + break; + case 'S': + break; + case 'A': + ao_adc_static.tick = tick; + ao_adc_static.accel = a; + ao_adc_static.pres = b; + ao_records_read++; + ao_insert(); + return; + case 'T': + ao_adc_static.tick = tick; + ao_adc_static.temp = a; + ao_adc_static.v_batt = b; + break; + case 'D': + case 'G': + case 'N': + case 'W': + case 'H': + break; + } + } + + } +} + +const char const * const ao_state_names[] = { + "startup", "idle", "pad", "boost", "coast", + "apogee", "drogue", "main", "landed", "invalid" +}; + +static int16_t altitude[2048] = { +#include "altitude.h" +}; + +#define GRAVITY 9.80665 +#define COUNTS_PER_G 264.8 + +void +ao_dump_state(void) +{ + if (ao_flight_state == ao_flight_startup) + return; + printf ("\t%s accel %g vel %g alt %d\n", + ao_state_names[ao_flight_state], + (ao_flight_accel - ao_ground_accel) / COUNTS_PER_G * GRAVITY, + (double) ao_flight_vel, + altitude[ao_flight_pres >> 4]); + if (ao_flight_state == ao_flight_landed) + exit(0); +} + +int +main (int argc, char **argv) +{ + emulator_in = fopen (argv[1], "r"); + if (!emulator_in) { + perror(argv[1]); + exit(1); + } + ao_flight_init(); + ao_flight(); +} diff --git a/ao_ignite.c b/ao_ignite.c new file mode 100644 index 00000000..acb9399b --- /dev/null +++ b/ao_ignite.c @@ -0,0 +1,123 @@ +/* + * Copyright © 2009 Keith Packard <keithp@keithp.com> + * + * 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 "ao.h" + +#define AO_IGNITER_DROGUE P2_3 +#define AO_IGNITER_MAIN P2_4 + +/* XXX test these values with real igniters */ +#define AO_IGNITER_OPEN 100 +#define AO_IGNITER_CLOSED 20000 +#define AO_IGNITER_FIRE_TIME AO_MS_TO_TICKS(50) +#define AO_IGNITER_CHARGE_TIME AO_MS_TO_TICKS(200) + +struct ao_ignition { + uint8_t request; + uint8_t fired; + uint8_t firing; +}; + +__xdata struct ao_ignition ao_ignition[2]; + +void +ao_ignite(enum ao_igniter igniter) __critical +{ + ao_ignition[igniter].request = 1; + ao_wakeup(&ao_ignition[0]); +} + +enum ao_igniter_status +ao_igniter_status(enum ao_igniter igniter) +{ + __xdata struct ao_adc adc; + __xdata int16_t value; + __xdata uint8_t request, firing, fired; + + __critical { + ao_adc_sleep(); + ao_adc_get(&adc); + request = ao_ignition[igniter].request; + fired = ao_ignition[igniter].fired; + firing = ao_ignition[igniter].firing; + } + if (firing || (request && !fired)) + return ao_igniter_active; + + value = (AO_IGNITER_CLOSED>>1); + switch (igniter) { + case ao_igniter_drogue: + value = adc.sense_d; + break; + case ao_igniter_main: + value = adc.sense_m; + break; + } + 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_igniter_fire(enum ao_igniter igniter) __critical +{ + ao_ignition[igniter].firing = 1; + switch (igniter) { + case ao_igniter_drogue: + AO_IGNITER_DROGUE = 1; + ao_delay(AO_IGNITER_FIRE_TIME); + AO_IGNITER_DROGUE = 0; + break; + case ao_igniter_main: + AO_IGNITER_MAIN = 1; + ao_delay(AO_IGNITER_FIRE_TIME); + AO_IGNITER_MAIN = 0; + break; + } + ao_ignition[igniter].firing = 0; +} + +void +ao_igniter(void) +{ + __xdata enum ao_ignter igniter; + __xdata enum ao_igniter_status status; + + for (;;) { + ao_sleep(&ao_ignition); + for (igniter = ao_igniter_drogue; igniter <= ao_igniter_main; igniter++) { + if (ao_ignition[igniter].request && !ao_ignition[igniter].fired) { + ao_igniter_fire(igniter); + ao_delay(AO_IGNITER_CHARGE_TIME); + status = ao_igniter_status(igniter); + if (status == ao_igniter_open) + ao_ignition[igniter].fired = 1; + } + } + } +} + +__xdata struct ao_task ao_igniter_task; + +void +ao_igniter_init(void) +{ + ao_add_task(&ao_igniter_task, ao_igniter, "igniter"); +} @@ -153,9 +153,7 @@ ao_log(void) log.u.deploy.main = ao_adc_ring[ao_log_adc_pos].sense_m; ao_log_data(&log); } - ao_log_adc_pos++; - if (ao_log_adc_pos == AO_ADC_RING) - ao_log_adc_pos = 0; + ao_log_adc_pos = ao_adc_ring_next(ao_log_adc_pos); } /* Wait for a while */ diff --git a/ao_monitor.c b/ao_monitor.c index 3db5e42d..845b63bf 100644 --- a/ao_monitor.c +++ b/ao_monitor.c @@ -52,6 +52,7 @@ ao_monitor(void) recv.telemetry.adc.sense_m); ao_gps_print(&recv.telemetry.gps); ao_usb_flush(); + ao_led_for(AO_LED_GREEN, AO_MS_TO_TICKS(10)); } } diff --git a/ao_telemetrum.c b/ao_telemetrum.c index d2e05eb5..0a4b3a49 100644 --- a/ao_telemetrum.c +++ b/ao_telemetrum.c @@ -41,6 +41,7 @@ main(void) ao_gps_init(); ao_telemetry_init(); ao_radio_init(); + ao_igniter_init(); ao_dbg_init(); ao_start_scheduler(); } |