// switch relay to selected output, turn correct LED on as a side effect
static void
ao_relay_control(uint8_t output)
{
switch (output) {
case 1:
lpc_gpio.pin[RELAY_PORT] |= RELAY_BIT;
ao_led_on(AO_LED_RED);
ao_led_off(AO_LED_GREEN);
break;
default:
lpc_gpio.pin[RELAY_PORT] &= ~RELAY_BIT;
ao_led_off(AO_LED_RED);
ao_led_on(AO_LED_GREEN);
}
}
void
ao_panic(uint8_t reason)
{
uint8_t n;
#if LOW_LEVEL_DEBUG
ao_cur_task = NULL;
printf ("panic %d\n", reason);
#endif
ao_arch_block_interrupts();
for (;;) {
ao_panic_delay(20);
for (n = 0; n < 5; n++) {
ao_led_on(AO_LED_PANIC);
ao_beep(AO_BEEP_HIGH);
ao_panic_delay(1);
ao_led_off(AO_LED_PANIC);
ao_beep(AO_BEEP_LOW);
ao_panic_delay(1);
}
ao_beep(AO_BEEP_OFF);
ao_panic_delay(2);
#ifdef SDCC
#pragma disable_warning 126
#endif
if (reason & 0x40) {
ao_led_on(AO_LED_PANIC);
ao_beep(AO_BEEP_HIGH);
ao_panic_delay(40);
ao_led_off(AO_LED_PANIC);
ao_beep(AO_BEEP_OFF);
ao_panic_delay(10);
}
for (n = 0; n < (reason & 0x3f); n++) {
ao_led_on(AO_LED_PANIC);
ao_beep(AO_BEEP_MID);
ao_panic_delay(10);
ao_led_off(AO_LED_PANIC);
ao_beep(AO_BEEP_OFF);
ao_panic_delay(10);
}
}
}
void
blink_0(void)
{
uint8_t b = 0;
for (;;) {
b = 1 - b;
if (b)
ao_led_on(AO_LED_GREEN);
else
ao_led_off(AO_LED_GREEN);
ao_sleep(&blink_chan);
}
}
void
blink_1(void)
{
static __xdata struct ao_adc adc;
for (;;) {
ao_sleep(&ao_adc_head);
ao_adc_get(&adc);
if (adc.accel < 15900)
ao_led_on(AO_LED_RED);
else
ao_led_off(AO_LED_RED);
}
}
void
ao_flight(void)
{
ao_sample_init();
ao_flight_state = ao_flight_startup;
for (;;) {
/*
* Process ADC samples, just looping
* until the sensors are calibrated.
*/
if (!ao_sample())
continue;
switch (ao_flight_state) {
case ao_flight_startup:
/* Check to see what mode we should go to.
* - Invalid mode if accel cal appears to be out
* - pad mode if we're upright,
* - idle mode otherwise
*/
#if HAS_ACCEL
if (ao_config.accel_plus_g == 0 ||
ao_config.accel_minus_g == 0 ||
ao_ground_accel < ao_config.accel_plus_g - ACCEL_NOSE_UP ||
ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP)
{
/* Detected an accel value outside -1.5g to 1.5g
* (or uncalibrated values), so we go into invalid mode
*/
ao_flight_state = ao_flight_invalid;
/* Turn on packet system in invalid mode on TeleMetrum */
ao_packet_slave_start();
} else
#endif
if (!ao_flight_force_idle
#if HAS_ACCEL
&& ao_ground_accel < ao_config.accel_plus_g + ACCEL_NOSE_UP
#endif
)
{
/* Set pad mode - we can fly! */
ao_flight_state = ao_flight_pad;
#if HAS_USB
/* Disable the USB controller in flight mode
* to save power
*/
ao_usb_disable();
#endif
#if !HAS_ACCEL
/* Disable packet mode in pad state on TeleMini */
ao_packet_slave_stop();
#endif
/* Turn on telemetry system */
ao_rdf_set(1);
ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD);
/* signal successful initialization by turning off the LED */
ao_led_off(AO_LED_RED);
} else {
/* Set idle mode */
ao_flight_state = ao_flight_idle;
#if HAS_ACCEL
/* Turn on packet system in idle mode on TeleMetrum */
ao_packet_slave_start();
#endif
/* signal successful initialization by turning off the LED */
ao_led_off(AO_LED_RED);
}
/* wakeup threads due to state change */
ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
break;
case ao_flight_pad:
/* pad to boost:
*
* barometer: > 20m vertical motion
* OR
* accelerometer: > 2g AND velocity > 5m/s
*
* The accelerometer should always detect motion before
* the barometer, but we use both to make sure this
* transition is detected. If the device
* doesn't have an accelerometer, then ignore the
* speed and acceleration as they are quite noisy
* on the pad.
*/
if (ao_height > AO_M_TO_HEIGHT(20)
#if HAS_ACCEL
|| (ao_accel > AO_MSS_TO_ACCEL(20) &&
ao_speed > AO_MS_TO_SPEED(5))
#endif
)
{
ao_flight_state = ao_flight_boost;
ao_launch_tick = ao_sample_tick;
/* start logging data */
ao_log_start();
/* Increase telemetry rate */
ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_FLIGHT);
/* disable RDF beacon */
ao_rdf_set(0);
#if HAS_GPS
/* Record current GPS position by waking up GPS log tasks */
ao_wakeup(&ao_gps_data);
ao_wakeup(&ao_gps_tracking_data);
#endif
ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
}
break;
case ao_flight_boost:
/* boost to fast:
*
* accelerometer: start to fall at > 1/4 G
* OR
* time: boost for more than 15 seconds
*
* Detects motor burn out by the switch from acceleration to
* deceleration, or by waiting until the maximum burn duration
* (15 seconds) has past.
*/
if ((ao_accel < AO_MSS_TO_ACCEL(-2.5) && ao_height > AO_M_TO_HEIGHT(100)) ||
(int16_t) (ao_sample_tick - ao_launch_tick) > BOOST_TICKS_MAX)
{
#if HAS_ACCEL
ao_flight_state = ao_flight_fast;
#else
ao_flight_state = ao_flight_coast;
#endif
ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
}
break;
#if HAS_ACCEL
case ao_flight_fast:
/*
* This is essentially the same as coast,
* but the barometer is being ignored as
* it may be unreliable.
*/
if (ao_speed < AO_MS_TO_SPEED(AO_MAX_BARO_SPEED))
{
ao_flight_state = ao_flight_coast;
ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
}
break;
#endif
case ao_flight_coast:
if ((int16_t) (ao_sample_tick - ao_launch_tick) > DESIRED_AUX_TIME)
{
ao_ignite(ao_igniter_main);
}
/* apogee detect: coast to drogue deploy:
*
* speed: < 0
*
* Also make sure the model altitude is tracking
* the measured altitude reasonably closely; otherwise
* we're probably transsonic.
*/
if (ao_speed < 0
#if !HAS_ACCEL
&& (ao_sample_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < 100)
#endif
)
{
/* ignite the drogue charge */
/*ao_ignite(ao_igniter_drogue); */
/* slow down the telemetry system */
ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_RECOVER);
/* Turn the RDF beacon back on */
ao_rdf_set(1);
/* and enter drogue state */
ao_flight_state = ao_flight_drogue;
ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
}
break;
case ao_flight_drogue:
/* drogue to main deploy:
*
* barometer: reach main deploy altitude
*
* Would like to use the accelerometer for this test, but
* the orientation of the flight computer is unknown after
* drogue deploy, so we ignore it. Could also detect
* high descent rate using the pressure sensor to
* recognize drogue deploy failure and eject the main
* at that point. Perhaps also use the drogue sense lines
* to notice continutity?
*/
if (ao_height <= ao_config.main_deploy)
{
/*ao_ignite(ao_igniter_main);*/
/*
* Start recording min/max height
* to figure out when the rocket has landed
*/
/* initialize interval values */
ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
ao_interval_min_height = ao_interval_max_height = ao_avg_height;
ao_flight_state = ao_flight_main;
ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
}
break;
/* fall through... */
case ao_flight_main:
/* main to land:
*
* barometer: altitude stable
*/
if (ao_avg_height < ao_interval_min_height)
ao_interval_min_height = ao_avg_height;
if (ao_avg_height > ao_interval_max_height)
ao_interval_max_height = ao_avg_height;
if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) {
if (ao_interval_max_height - ao_interval_min_height <= AO_M_TO_HEIGHT(4))
{
ao_flight_state = ao_flight_landed;
/* turn off the ADC capture */
ao_timer_set_adc_interval(0);
ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
}
ao_interval_min_height = ao_interval_max_height = ao_avg_height;
ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
}
break;
case ao_flight_landed:
break;
}
}
}
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;
}
}
}
