#
TM_DRIVER_SRC = \
ao_adc.c \
- ao_ee.c
+ ao_ee.c \
+ ao_ignite.c
#
# Tasks run on TeleMetrum
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
/* 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)
*/
#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
};
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;
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_ */
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));
}
} 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);
}
}
* 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 */
__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
#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
* 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;
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 {
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)
{
}
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));
}
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));
}
--- /dev/null
+/*
+ * 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();
+}
--- /dev/null
+/*
+ * 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");
+}
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 */
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));
}
}
ao_gps_init();
ao_telemetry_init();
ao_radio_init();
+ ao_igniter_init();
ao_dbg_init();
ao_start_scheduler();
}