void
ao_report(void)
{
ao_report_state = ao_flight_state;
for(;;) {
#if HAS_BATTERY_REPORT
if (ao_flight_state == ao_flight_startup)
ao_report_battery();
else
#endif
ao_report_beep();
if (ao_flight_state == ao_flight_landed) {
ao_report_altitude();
#if HAS_FLIGHT
ao_delay(AO_SEC_TO_TICKS(5));
continue;
#endif
}
#if HAS_IGNITE_REPORT
if (ao_flight_state == ao_flight_idle)
ao_report_continuity();
while (ao_flight_state == ao_flight_pad) {
uint8_t c;
ao_report_continuity();
c = 50;
while (c-- && ao_flight_state == ao_flight_pad)
pause(AO_MS_TO_TICKS(100));
}
#endif
while (ao_report_state == ao_flight_state)
ao_sleep(DATA_TO_XDATA(&ao_flight_state));
ao_report_state = ao_flight_state;
}
}
void
ao_hmc5883_sample(struct ao_hmc5883_sample *sample)
{
uint16_t *d = (uint16_t *) sample;
int i = sizeof (*sample) / 2;
ao_hmc5883_done = 0;
ao_exti_enable(AO_HMC5883_INT_PORT, AO_HMC5883_INT_PIN);
ao_hmc5883_reg_write(HMC5883_MODE, HMC5883_MODE_SINGLE);
ao_alarm(AO_MS_TO_TICKS(10));
ao_arch_block_interrupts();
while (!ao_hmc5883_done)
if (ao_sleep(&ao_hmc5883_done))
++ao_hmc5883_missed_irq;
ao_arch_release_interrupts();
ao_clear_alarm();
ao_hmc5883_read(HMC5883_X_MSB, (uint8_t *) sample, sizeof (struct ao_hmc5883_sample));
#if __BYTE_ORDER == __LITTLE_ENDIAN
/* byte swap */
while (i--) {
uint16_t t = *d;
*d++ = (t >> 8) | (t << 8);
}
#endif
}
void
wakeup(void)
{
for (;;) {
ao_delay(AO_MS_TO_TICKS(100));
ao_wakeup(&blink_chan);
}
}
void
ao_rssi_set(int rssi_value)
{
if (rssi_value > 0)
rssi_value = 0;
ao_rssi_delay = AO_MS_TO_TICKS((-rssi_value) * 5);
ao_rssi_time = ao_time();
ao_wakeup(&ao_rssi_time);
}
static void
ao_launch_status(void)
{
uint8_t i;
for (;;) {
ao_delay(AO_SEC_TO_TICKS(1));
if (ao_igniter_status(ao_igniter_drogue) == ao_igniter_ready) {
if (ao_igniter_status(ao_igniter_main) == ao_igniter_ready) {
for (i = 0; i < 5; i++) {
ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(50));
ao_delay(AO_MS_TO_TICKS(100));
}
} else {
ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200));
}
}
}
}
void
ao_rssi(void)
{
for (;;) {
while ((int16_t) (ao_time() - ao_rssi_time) > AO_SEC_TO_TICKS(3))
ao_sleep(&ao_rssi_time);
ao_led_for(ao_rssi_led, AO_MS_TO_TICKS(100));
ao_delay(ao_rssi_delay);
}
}
void
beep(void)
{
static __xdata struct ao_adc adc;
for (;;) {
ao_delay(AO_SEC_TO_TICKS(1));
ao_adc_get(&adc);
if (adc.temp > 7400)
ao_beep_for(AO_BEEP_LOW, AO_MS_TO_TICKS(50));
}
}
/*
* A thread to initialize the bluetooth device and
* hang around to blink the LED when connected
*/
void
ao_btm(void)
{
/*
* Wait for the bluetooth device to boot
*/
ao_delay(AO_SEC_TO_TICKS(3));
/*
* The first time we connect, the BTM-180 comes up at 19200 baud.
* After that, it will remember and come up at 57600 baud. So, see
* if it is already running at 57600 baud, and if that doesn't work
* then tell it to switch to 57600 from 19200 baud.
*/
while (!ao_btm_try_speed(AO_SERIAL_SPEED_57600)) {
ao_delay(AO_SEC_TO_TICKS(1));
if (ao_btm_try_speed(AO_SERIAL_SPEED_19200))
ao_btm_cmd("ATL4\r");
ao_delay(AO_SEC_TO_TICKS(1));
}
/* Disable echo */
ao_btm_cmd("ATE0\r");
/* Enable flow control */
ao_btm_cmd("ATC1\r");
/* Set the reported name to something we can find on the host */
ao_btm_set_name();
/* Turn off status reporting */
ao_btm_cmd("ATQ1\r");
ao_btm_stdio = ao_add_stdio(_ao_serial_btm_pollchar,
ao_serial_btm_putchar,
NULL);
ao_btm_echo(0);
for (;;) {
while (!ao_btm_connected)
ao_sleep(&ao_btm_connected);
while (ao_btm_connected) {
ao_led_for(AO_BT_LED, AO_MS_TO_TICKS(20));
ao_delay(AO_SEC_TO_TICKS(3));
}
}
}
static int
ao_btm_getchar(void)
{
int c;
ao_arch_block_interrupts();
while ((c = _ao_serial_btm_pollchar()) == AO_READ_AGAIN) {
ao_alarm(AO_MS_TO_TICKS(10));
c = ao_sleep(&ao_serial_btm_rx_fifo);
ao_clear_alarm();
if (c) {
c = AO_READ_AGAIN;
break;
}
}
ao_arch_release_interrupts();
return c;
}
int8_t
ao_lco_query(uint16_t box, struct ao_pad_query *query, uint16_t *tick_offset)
{
int8_t r;
uint16_t sent_time;
ao_mutex_get(&ao_lco_mutex);
command.tick = ao_time() - *tick_offset;
command.box = box;
command.cmd = AO_LAUNCH_QUERY;
command.channels = 0;
ao_radio_cmac_send(&command, sizeof (command));
sent_time = ao_time();
r = ao_radio_cmac_recv(query, sizeof (*query), AO_MS_TO_TICKS(10));
if (r == AO_RADIO_CMAC_OK)
*tick_offset = sent_time - query->tick;
ao_mutex_put(&ao_lco_mutex);
return r;
}
static void
ao_launch_run(void)
{
for (;;) {
while (!ao_launch_ignite)
ao_sleep(&ao_launch_ignite);
ao_ignition[ao_igniter_drogue].firing = 1;
ao_ignition[ao_igniter_main].firing = 1;
AO_IGNITER_DIR |= AO_IGNITER_DROGUE_BIT | AO_IGNITER_MAIN_BIT;
AO_IGNITER_DROGUE = 1;
while (ao_launch_ignite) {
ao_launch_ignite = 0;
ao_delay(AO_MS_TO_TICKS(500));
}
AO_IGNITER_DROGUE = 0;
ao_ignition[ao_igniter_drogue].firing = 0;
ao_ignition[ao_igniter_main].firing = 0;
}
}
void
ao_log(void)
{
__pdata uint16_t next_sensor, next_other;
ao_storage_setup();
ao_log_scan();
while (!ao_log_running)
ao_sleep(&ao_log_running);
#if HAS_FLIGHT
log.type = AO_LOG_FLIGHT;
log.tick = ao_sample_tick;
#if HAS_ACCEL
log.u.flight.ground_accel = ao_ground_accel;
#endif
log.u.flight.ground_pres = ao_ground_pres;
log.u.flight.flight = ao_flight_number;
ao_log_metrum(&log);
#endif
/* Write the whole contents of the ring to the log
* when starting up.
*/
ao_log_data_pos = ao_data_ring_next(ao_data_head);
next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick;
ao_log_state = ao_flight_startup;
for (;;) {
/* Write samples to EEPROM */
while (ao_log_data_pos != ao_data_head) {
log.tick = ao_data_ring[ao_log_data_pos].tick;
if ((int16_t) (log.tick - next_sensor) >= 0) {
log.type = AO_LOG_SENSOR;
#if HAS_MS5607
log.u.sensor.pres = ao_data_ring[ao_log_data_pos].ms5607_raw.pres;
log.u.sensor.temp = ao_data_ring[ao_log_data_pos].ms5607_raw.temp;
#endif
#if HAS_ACCEL
log.u.sensor.accel = ao_data_accel(&ao_data_ring[ao_log_data_pos]);
#endif
ao_log_metrum(&log);
if (ao_log_state <= ao_flight_coast)
next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT;
else
next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT;
}
if ((int16_t) (log.tick - next_other) >= 0) {
log.type = AO_LOG_TEMP_VOLT;
log.u.volt.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt;
log.u.volt.sense_a = ao_data_ring[ao_log_data_pos].adc.sense_a;
log.u.volt.sense_m = ao_data_ring[ao_log_data_pos].adc.sense_m;
ao_log_metrum(&log);
next_other = log.tick + AO_OTHER_INTERVAL;
}
ao_log_data_pos = ao_data_ring_next(ao_log_data_pos);
}
#if HAS_FLIGHT
/* Write state change to EEPROM */
if (ao_flight_state != ao_log_state) {
ao_log_state = ao_flight_state;
log.type = AO_LOG_STATE;
log.tick = ao_time();
log.u.state.state = ao_log_state;
log.u.state.reason = 0;
ao_log_metrum(&log);
if (ao_log_state == ao_flight_landed)
ao_log_stop();
}
#endif
ao_log_flush();
/* Wait for a while */
ao_delay(AO_MS_TO_TICKS(100));
/* Stop logging when told to */
while (!ao_log_running)
ao_sleep(&ao_log_running);
}
}
static void
ao_fake_flight(void)
{
int16_t calib_size, data_size;
struct ao_fake_calib save_calib;
uint16_t my_pyro_fired = 0;
enum ao_flight_state my_state = ao_flight_invalid;
int i;
ao_cmd_hex();
if (ao_cmd_status != ao_cmd_success)
return;
calib_size = ao_cmd_lex_i;
ao_cmd_hex();
if (ao_cmd_status != ao_cmd_success)
return;
data_size = ao_cmd_lex_i;
if ((unsigned) calib_size != sizeof (struct ao_fake_calib)) {
printf ("calib size %d larger than actual size %d\n",
calib_size, sizeof (struct ao_fake_calib));
ao_cmd_status = ao_cmd_syntax_error;
return;
}
if (data_size != sizeof (struct ao_data)) {
printf ("data size %d doesn't match actual size %d\n",
data_size, sizeof (struct ao_data));
ao_cmd_status = ao_cmd_syntax_error;
return;
}
ao_fake_calib_get(&save_calib);
if (!ao_fake_calib_read())
return;
ao_fake_has_next = 0;
ao_fake_has_cur = 0;
ao_fake_flight_active = 1;
ao_sample_init();
#if PACKET_HAS_SLAVE
ao_packet_slave_stop();
#endif
#if AO_LED_RED
/* Turn on the LED to indicate startup */
ao_led_on(AO_LED_RED);
#endif
ao_flight_state = ao_flight_startup;
for (;;) {
if (my_state != ao_flight_state) {
printf("state %d\n", ao_flight_state);
my_state = ao_flight_state;
flush();
}
if (my_pyro_fired != ao_pyro_fired) {
int pyro;
for (pyro = 0; pyro < AO_PYRO_NUM; pyro++) {
uint16_t bit = (1 << pyro);
if (!(my_pyro_fired & bit) && (ao_pyro_fired & bit))
printf ("fire %d\n", pyro);
}
my_pyro_fired = ao_pyro_fired;
}
while (ao_fake_has_next)
ao_sleep((void *) &ao_fake_has_next);
if (!ao_fake_data_read())
break;
}
/* Wait 20 seconds to see if we enter landed state */
for (i = 0; i < 200; i++)
{
if (ao_flight_state == ao_flight_landed)
break;
ao_delay(AO_MS_TO_TICKS(100));
}
#if AO_LED_RED
/* Turn on the LED to indicate startup */
ao_led_on(AO_LED_RED);
#endif
ao_fake_flight_active = 0;
ao_flight_state = ao_flight_startup;
ao_sample_init();
ao_fake_calib_set(&save_calib);
}
void
ao_log(void)
{
__pdata uint16_t next_sensor, next_other;
uint8_t i;
ao_storage_setup();
ao_log_scan();
while (!ao_log_running)
ao_sleep(&ao_log_running);
#if HAS_FLIGHT
log.type = AO_LOG_FLIGHT;
log.tick = ao_sample_tick;
#if HAS_ACCEL
log.u.flight.ground_accel = ao_ground_accel;
#endif
#if HAS_GYRO
log.u.flight.ground_accel_along = ao_ground_accel_along;
log.u.flight.ground_accel_across = ao_ground_accel_across;
log.u.flight.ground_accel_through = ao_ground_accel_through;
log.u.flight.ground_roll = ao_ground_roll;
log.u.flight.ground_pitch = ao_ground_pitch;
log.u.flight.ground_yaw = ao_ground_yaw;
#endif
log.u.flight.ground_pres = ao_ground_pres;
log.u.flight.flight = ao_flight_number;
ao_log_mega(&log);
#endif
/* Write the whole contents of the ring to the log
* when starting up.
*/
ao_log_data_pos = ao_data_ring_next(ao_data_head);
next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick;
ao_log_state = ao_flight_startup;
for (;;) {
/* Write samples to EEPROM */
while (ao_log_data_pos != ao_data_head) {
log.tick = ao_data_ring[ao_log_data_pos].tick;
if ((int16_t) (log.tick - next_sensor) >= 0) {
log.type = AO_LOG_SENSOR;
#if HAS_MS5607
log.u.sensor.pres = ao_data_ring[ao_log_data_pos].ms5607_raw.pres;
log.u.sensor.temp = ao_data_ring[ao_log_data_pos].ms5607_raw.temp;
#endif
#if HAS_MPU6000
log.u.sensor.accel_x = ao_data_ring[ao_log_data_pos].mpu6000.accel_x;
log.u.sensor.accel_y = ao_data_ring[ao_log_data_pos].mpu6000.accel_y;
log.u.sensor.accel_z = ao_data_ring[ao_log_data_pos].mpu6000.accel_z;
log.u.sensor.gyro_x = ao_data_ring[ao_log_data_pos].mpu6000.gyro_x;
log.u.sensor.gyro_y = ao_data_ring[ao_log_data_pos].mpu6000.gyro_y;
log.u.sensor.gyro_z = ao_data_ring[ao_log_data_pos].mpu6000.gyro_z;
#endif
#if HAS_HMC5883
log.u.sensor.mag_x = ao_data_ring[ao_log_data_pos].hmc5883.x;
log.u.sensor.mag_y = ao_data_ring[ao_log_data_pos].hmc5883.y;
log.u.sensor.mag_z = ao_data_ring[ao_log_data_pos].hmc5883.z;
#endif
log.u.sensor.accel = ao_data_accel(&ao_data_ring[ao_log_data_pos]);
ao_log_mega(&log);
if (ao_log_state <= ao_flight_coast)
next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT;
else
next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT;
}
if ((int16_t) (log.tick - next_other) >= 0) {
log.type = AO_LOG_TEMP_VOLT;
log.u.volt.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt;
log.u.volt.v_pbatt = ao_data_ring[ao_log_data_pos].adc.v_pbatt;
log.u.volt.n_sense = AO_ADC_NUM_SENSE;
for (i = 0; i < AO_ADC_NUM_SENSE; i++)
log.u.volt.sense[i] = ao_data_ring[ao_log_data_pos].adc.sense[i];
log.u.volt.pyro = ao_pyro_fired;
ao_log_mega(&log);
next_other = log.tick + AO_OTHER_INTERVAL;
}
ao_log_data_pos = ao_data_ring_next(ao_log_data_pos);
}
#if HAS_FLIGHT
/* Write state change to EEPROM */
if (ao_flight_state != ao_log_state) {
ao_log_state = ao_flight_state;
log.type = AO_LOG_STATE;
log.tick = ao_time();
log.u.state.state = ao_log_state;
log.u.state.reason = 0;
ao_log_mega(&log);
if (ao_log_state == ao_flight_landed)
ao_log_stop();
}
#endif
ao_log_flush();
/* Wait for a while */
ao_delay(AO_MS_TO_TICKS(100));
/* Stop logging when told to */
while (!ao_log_running)
ao_sleep(&ao_log_running);
}
}
static void
ao_launch(void)
{
static __xdata struct ao_launch_command command;
static __xdata struct ao_launch_query query;
int16_t time_difference;
ao_led_off(AO_LED_RED);
ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200));
for (;;) {
flush();
if (ao_radio_cmac_recv(&command, sizeof (command), 0) != AO_RADIO_CMAC_OK)
continue;
PRINTD ("tick %d serial %d cmd %d channel %d\n",
command.tick, command.serial, command.cmd, command.channel);
switch (command.cmd) {
case AO_LAUNCH_QUERY:
if (command.serial != ao_serial_number) {
PRINTD ("serial number mismatch\n");
break;
}
if (command.channel == 0) {
query.valid = 1;
query.arm_status = ao_igniter_status(ao_igniter_drogue);
query.igniter_status = ao_igniter_status(ao_igniter_main);
} else {
query.valid = 0;
}
query.tick = ao_time();
query.serial = ao_serial_number;
query.channel = command.channel;
PRINTD ("query tick %d serial %d channel %d valid %d arm %d igniter %d\n",
query.tick, query.serial, query.channel, query.valid, query.arm_status,
query.igniter_status);
ao_radio_cmac_send(&query, sizeof (query));
break;
case AO_LAUNCH_ARM:
if (command.serial != ao_serial_number) {
PRINTD ("serial number mismatch\n");
break;
}
if (command.channel != 0)
break;
time_difference = command.tick - ao_time();
PRINTD ("arm tick %d local tick %d\n", command.tick, ao_time());
if (time_difference < 0)
time_difference = -time_difference;
if (time_difference > 10) {
PRINTD ("time difference too large %d\n", time_difference);
break;
}
PRINTD ("armed\n");
ao_launch_armed = 1;
ao_launch_arm_time = ao_time();
break;
case AO_LAUNCH_FIRE:
if (!ao_launch_armed) {
PRINTD ("not armed\n");
break;
}
if ((uint16_t) (ao_time() - ao_launch_arm_time) > AO_SEC_TO_TICKS(20)) {
PRINTD ("late launch arm_time %d time %d\n",
ao_launch_arm_time, ao_time());
break;
}
time_difference = command.tick - ao_time();
if (time_difference < 0)
time_difference = -time_difference;
if (time_difference > 10) {
PRINTD ("time different too large %d\n", time_difference);
break;
}
PRINTD ("ignite\n");
ao_launch_ignite = 1;
ao_wakeup(&ao_launch_ignite);
break;
}
}
}
