X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=ao_flight.c;h=3aff866aa664c27a8383e5d8a6ed51f948aaa51b;hp=f31b4cd4e617af11e0bbe9237954fba8cf0c85f6;hb=c7555eb16876aa8ff9fe7f648d325a1b8fb54d23;hpb=acc4fc635edb70ec1ba2dff9f7ac0c8542c72c47 diff --git a/ao_flight.c b/ao_flight.c index f31b4cd4..3aff866a 100644 --- a/ao_flight.c +++ b/ao_flight.c @@ -3,8 +3,7 @@ * * 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; either version 2 of the License, or - * (at your option) any later version. + * 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 @@ -20,15 +19,32 @@ /* Main flight thread. */ -__xdata struct ao_adc ao_flight_data; -__data enum flight_state ao_flight_state; -__data uint16_t ao_flight_state_tick; -__data int16_t ao_flight_accel; -__data int16_t ao_flight_pres; -__data int16_t ao_ground_pres; -__data int16_t ao_ground_accel; -__data int16_t ao_min_pres; -__data uint16_t ao_launch_time; +__xdata struct ao_adc ao_flight_data; /* last acquired data */ +__pdata enum 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_ground_pres; /* startup pressure */ +__pdata int16_t ao_ground_accel; /* startup acceleration */ +__pdata int16_t ao_min_pres; /* minimum recorded pressure */ +__pdata uint16_t ao_launch_time; /* time of launch detect */ +__pdata int16_t ao_main_pres; /* pressure to eject main */ + +/* + * track min/max data over a long interval to detect + * resting + */ +__pdata uint16_t ao_interval_end; +__pdata int16_t ao_interval_cur_min_accel; +__pdata int16_t ao_interval_cur_max_accel; +__pdata int16_t ao_interval_cur_min_pres; +__pdata int16_t ao_interval_cur_max_pres; +__pdata int16_t ao_interval_min_accel; +__pdata int16_t ao_interval_max_accel; +__pdata int16_t ao_interval_min_pres; +__pdata int16_t ao_interval_max_pres; + +#define AO_INTERVAL_TICKS AO_SEC_TO_TICKS(5) /* Accelerometer calibration * @@ -73,6 +89,8 @@ __data uint16_t ao_launch_time; #define BARO_kPa 268 #define BARO_LAUNCH (BARO_kPa / 5) /* .2kPa */ #define BARO_APOGEE (BARO_kPa / 10) /* .1kPa */ +#define BARO_MAIN (BARO_kPa) /* 1kPa */ +#define BARO_LAND (BARO_kPa / 20) /* .05kPa */ /* We also have a clock, which can be used to sanity check things in * case of other failures @@ -83,7 +101,7 @@ __data uint16_t ao_launch_time; void ao_flight(void) { - __data static uint8_t nsamples = 0; + __pdata static uint8_t nsamples = 0; for (;;) { ao_sleep(&ao_adc_ring); @@ -92,7 +110,17 @@ ao_flight(void) ao_flight_accel += ao_flight_data.accel >> 4; ao_flight_pres -= ao_flight_pres >> 4; ao_flight_pres += ao_flight_data.pres >> 4; + ao_flight_tick = ao_time(); + ao_flight_tick = ao_time(); + 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_interval_max_accel = ao_interval_cur_max_accel; + ao_interval_min_accel = ao_interval_cur_min_accel; + ao_interval_end = ao_flight_tick + AO_INTERVAL_TICKS; + } + switch (ao_flight_state) { case ao_flight_startup: if (nsamples < 100) { @@ -102,24 +130,34 @@ ao_flight(void) ao_ground_accel = ao_flight_accel; ao_ground_pres = ao_flight_pres; ao_min_pres = ao_flight_pres; - if (ao_flight_accel < ACCEL_NOSE_UP) { + ao_main_pres = ao_ground_pres - BARO_MAIN; + + 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) { ao_flight_state = ao_flight_launchpad; - ao_flight_state_tick = ao_time(); - ao_report_notify(); + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); } else { ao_flight_state = ao_flight_idle; - ao_flight_state_tick = ao_time(); - ao_report_notify(); + + /* Turn on the Green LED in idle mode + * This also happens to bring the USB up for the TI board + */ + ao_led_on(AO_LED_GREEN); + ao_timer_set_adc_interval(100); + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); } + /* signal successful initialization by turning off the LED */ + ao_led_off(AO_LED_RED); break; case ao_flight_launchpad: if (ao_flight_accel < ACCEL_BOOST || ao_flight_pres + BARO_LAUNCH < ao_ground_pres) { ao_flight_state = ao_flight_boost; - ao_flight_state_tick = ao_time(); ao_log_start(); - ao_report_notify(); + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); break; } break; @@ -128,8 +166,7 @@ ao_flight(void) (int16_t) (ao_flight_data.tick - ao_launch_time) > BOOST_TICKS_MAX) { ao_flight_state = ao_flight_coast; - ao_flight_state_tick = ao_time(); - ao_report_notify(); + ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); break; } break; @@ -138,11 +175,33 @@ ao_flight(void) ao_min_pres = ao_flight_pres; if (ao_flight_pres - BARO_APOGEE > ao_min_pres) { ao_flight_state = ao_flight_apogee; - ao_flight_state_tick = ao_time(); - ao_report_notify(); + 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)); + break; + case ao_flight_drogue: + if (ao_flight_pres >= ao_main_pres) { +// ao_ignite(AO_IGNITE_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; + case ao_flight_main: + 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; + case ao_flight_landed: + ao_log_stop(); break; } } @@ -154,7 +213,12 @@ void ao_flight_init(void) { ao_flight_state = ao_flight_startup; + ao_interval_min_accel = 0; + ao_interval_max_accel = 0x7fff; + ao_interval_min_pres = 0; + ao_interval_max_pres = 0x7fff; + ao_interval_end = AO_INTERVAL_TICKS; - ao_add_task(&flight_task, ao_flight); + ao_add_task(&flight_task, ao_flight, "flight"); }