git.gag.com Git - fw/altos/blob - ao_flight.c

   1 /*
   2  * Copyright © 2009 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along
  15  * with this program; if not, write to the Free Software Foundation, Inc.,
  16  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  17  */
  18
  19 #include "ao.h"
  20
  21 /* Main flight thread. */
  22
  23 __xdata struct ao_adc           ao_flight_data;         /* last acquired data */
  24 __data enum flight_state        ao_flight_state;        /* current flight state */
  25 __data uint16_t                 ao_flight_state_tick;   /* time of last data */
  26 __data int16_t                  ao_flight_accel;        /* filtered acceleration */
  27 __data int16_t                  ao_flight_pres;         /* filtered pressure */
  28 __data int16_t                  ao_ground_pres;         /* startup pressure */
  29 __data int16_t                  ao_ground_accel;        /* startup acceleration */
  30 __data int16_t                  ao_min_pres;            /* minimum recorded pressure */
  31 __data uint16_t                 ao_launch_time;         /* time of launch detect */
  32
  33 /* Accelerometer calibration
  34  *
  35  * We're sampling the accelerometer through a resistor divider which
  36  * consists of 5k and 10k resistors. This multiplies the values by 2/3.
  37  * That goes into the cc1111 A/D converter, which is running at 11 bits
  38  * of precision with the bits in the MSB of the 16 bit value. Only positive
  39  * values are used, so values should range from 0-32752 for 0-3.3V. The
  40  * specs say we should see 40mV/g (uncalibrated), multiply by 2/3 for what
  41  * the A/D converter sees (26.67 mV/g). We should see 32752/3300 counts/mV,
  42  * for a final computation of:
  43  *
  44  * 26.67 mV/g * 32767/3300 counts/mV = 264.8 counts/g
  45  *
  46  * Zero g was measured at 16000 (we would expect 16384)
  47  */
  48
  49 #define ACCEL_G         265
  50 #define ACCEL_ZERO_G    16000
  51 #define ACCEL_NOSE_UP   (ACCEL_ZERO_G - ACCEL_G * 2 /3)
  52 #define ACCEL_BOOST     (ACCEL_NOSE_UP - ACCEL_G * 2)
  53
  54 /*
  55  * Barometer calibration
  56  *
  57  * We directly sample the barometer. The specs say:
  58  *
  59  * Pressure range: 15-115 kPa
  60  * Voltage at 115kPa: 2.82
  61  * Output scale: 27mV/kPa
  62  *
  63  * If we want to detect launch with the barometer, we need
  64  * a large enough bump to not be fooled by noise. At typical
  65  * launch elevations (0-2000m), a 200Pa pressure change cooresponds
  66  * to about a 20m elevation change. This is 5.4mV, or about 3LSB.
  67  * As all of our calculations are done in 16 bits, we'll actually see a change
  68  * of 16 times this though
  69  *
  70  * 27 mV/kPa * 32767 / 3300 counts/mV = 268.1 counts/kPa
  71  */
  72
  73 #define BARO_kPa        268
  74 #define BARO_LAUNCH     (BARO_kPa / 5)  /* .2kPa */
  75 #define BARO_APOGEE     (BARO_kPa / 10) /* .1kPa */
  76
  77 /* We also have a clock, which can be used to sanity check things in
  78  * case of other failures
  79  */
  80
  81 #define BOOST_TICKS_MAX AO_SEC_TO_TICKS(10)
  82
  83 void
  84 ao_flight(void)
  85 {
  86         __data static uint8_t   nsamples = 0;
  87
  88         for (;;) {
  89                 ao_sleep(&ao_adc_ring);
  90                 ao_adc_get(&ao_flight_data);
  91                 ao_flight_accel -= ao_flight_accel >> 4;
  92                 ao_flight_accel += ao_flight_data.accel >> 4;
  93                 ao_flight_pres -= ao_flight_pres >> 4;
  94                 ao_flight_pres += ao_flight_data.pres >> 4;
  95
  96                 switch (ao_flight_state) {
  97                 case ao_flight_startup:
  98                         if (nsamples < 100) {
  99                                 ++nsamples;
 100                                 continue;
 101                         }
 102                         ao_ground_accel = ao_flight_accel;
 103                         ao_ground_pres = ao_flight_pres;
 104                         ao_min_pres = ao_flight_pres;
 105                         if (ao_flight_accel < ACCEL_NOSE_UP) {
 106                                 ao_flight_state = ao_flight_launchpad;
 107                                 ao_flight_state_tick = ao_time();
 108                                 ao_report_notify();
 109                         } else {
 110                                 ao_flight_state = ao_flight_idle;
 111                                 ao_flight_state_tick = ao_time();
 112                                 ao_report_notify();
 113                         }
 114                         /* signal successful initialization by turning off the LED */
 115                         ao_led_off(AO_LED_RED);
 116                         break;
 117                 case ao_flight_launchpad:
 118                         if (ao_flight_accel < ACCEL_BOOST ||
 119                             ao_flight_pres + BARO_LAUNCH < ao_ground_pres)
 120                         {
 121                                 ao_flight_state = ao_flight_boost;
 122                                 ao_flight_state_tick = ao_time();
 123                                 ao_log_start();
 124                                 ao_report_notify();
 125                                 break;
 126                         }
 127                         break;
 128                 case ao_flight_boost:
 129                         if (ao_flight_accel > ACCEL_ZERO_G ||
 130                             (int16_t) (ao_flight_data.tick - ao_launch_time) > BOOST_TICKS_MAX)
 131                         {
 132                                 ao_flight_state = ao_flight_coast;
 133                                 ao_flight_state_tick = ao_time();
 134                                 ao_report_notify();
 135                                 break;
 136                         }
 137                         break;
 138                 case ao_flight_coast:
 139                         if (ao_flight_pres < ao_min_pres)
 140                                 ao_min_pres = ao_flight_pres;
 141                         if (ao_flight_pres - BARO_APOGEE > ao_min_pres) {
 142                                 ao_flight_state = ao_flight_apogee;
 143                                 ao_flight_state_tick = ao_time();
 144                                 ao_report_notify();
 145                         }
 146                         break;
 147                 case ao_flight_apogee:
 148                         break;
 149                 }
 150         }
 151 }
 152
 153 static __xdata struct ao_task   flight_task;
 154
 155 void
 156 ao_flight_init(void)
 157 {
 158         ao_flight_state = ao_flight_startup;
 159
 160         ao_add_task(&flight_task, ao_flight);
 161 }
 162