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_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 __data int16_t                  ao_main_pres;           /* pressure to eject main */
  33
  34 /*
  35  * track min/max data over a long interval to detect
  36  * resting
  37  */
  38 __data uint16_t                 ao_interval_end;
  39 __data int16_t                  ao_interval_cur_min_accel;
  40 __data int16_t                  ao_interval_cur_max_accel;
  41 __data int16_t                  ao_interval_cur_min_pres;
  42 __data int16_t                  ao_interval_cur_max_pres;
  43 __data int16_t                  ao_interval_min_accel;
  44 __data int16_t                  ao_interval_max_accel;
  45 __data int16_t                  ao_interval_min_pres;
  46 __data int16_t                  ao_interval_max_pres;
  47
  48 #define AO_INTERVAL_TICKS       AO_SEC_TO_TICKS(5)
  49
  50 /* Accelerometer calibration
  51  *
  52  * We're sampling the accelerometer through a resistor divider which
  53  * consists of 5k and 10k resistors. This multiplies the values by 2/3.
  54  * That goes into the cc1111 A/D converter, which is running at 11 bits
  55  * of precision with the bits in the MSB of the 16 bit value. Only positive
  56  * values are used, so values should range from 0-32752 for 0-3.3V. The
  57  * specs say we should see 40mV/g (uncalibrated), multiply by 2/3 for what
  58  * the A/D converter sees (26.67 mV/g). We should see 32752/3300 counts/mV,
  59  * for a final computation of:
  60  *
  61  * 26.67 mV/g * 32767/3300 counts/mV = 264.8 counts/g
  62  *
  63  * Zero g was measured at 16000 (we would expect 16384)
  64  */
  65
  66 #define ACCEL_G         265
  67 #define ACCEL_ZERO_G    16000
  68 #define ACCEL_NOSE_UP   (ACCEL_ZERO_G - ACCEL_G * 2 /3)
  69 #define ACCEL_BOOST     (ACCEL_NOSE_UP - ACCEL_G * 2)
  70
  71 /*
  72  * Barometer calibration
  73  *
  74  * We directly sample the barometer. The specs say:
  75  *
  76  * Pressure range: 15-115 kPa
  77  * Voltage at 115kPa: 2.82
  78  * Output scale: 27mV/kPa
  79  *
  80  * If we want to detect launch with the barometer, we need
  81  * a large enough bump to not be fooled by noise. At typical
  82  * launch elevations (0-2000m), a 200Pa pressure change cooresponds
  83  * to about a 20m elevation change. This is 5.4mV, or about 3LSB.
  84  * As all of our calculations are done in 16 bits, we'll actually see a change
  85  * of 16 times this though
  86  *
  87  * 27 mV/kPa * 32767 / 3300 counts/mV = 268.1 counts/kPa
  88  */
  89
  90 #define BARO_kPa        268
  91 #define BARO_LAUNCH     (BARO_kPa / 5)  /* .2kPa */
  92 #define BARO_APOGEE     (BARO_kPa / 10) /* .1kPa */
  93 #define BARO_MAIN       (BARO_kPa)      /* 1kPa */
  94 #define BARO_LAND       (BARO_kPa / 20) /* .05kPa */
  95
  96 /* We also have a clock, which can be used to sanity check things in
  97  * case of other failures
  98  */
  99
 100 #define BOOST_TICKS_MAX AO_SEC_TO_TICKS(10)
 101
 102 void
 103 ao_flight(void)
 104 {
 105         __data static uint8_t   nsamples = 0;
 106
 107         for (;;) {
 108                 ao_sleep(&ao_adc_ring);
 109                 ao_adc_get(&ao_flight_data);
 110                 ao_flight_accel -= ao_flight_accel >> 4;
 111                 ao_flight_accel += ao_flight_data.accel >> 4;
 112                 ao_flight_pres -= ao_flight_pres >> 4;
 113                 ao_flight_pres += ao_flight_data.pres >> 4;
 114                 ao_flight_tick = ao_time();
 115
 116                 ao_flight_tick = ao_time();
 117                 if ((int16_t) (ao_flight_tick - ao_interval_end) >= 0) {
 118                         ao_interval_max_pres = ao_interval_cur_max_pres;
 119                         ao_interval_min_pres = ao_interval_cur_min_pres;
 120                         ao_interval_max_accel = ao_interval_cur_max_accel;
 121                         ao_interval_min_accel = ao_interval_cur_min_accel;
 122                         ao_interval_end = ao_flight_tick + AO_INTERVAL_TICKS;
 123                 }
 124
 125                 switch (ao_flight_state) {
 126                 case ao_flight_startup:
 127                         if (nsamples < 100) {
 128                                 ++nsamples;
 129                                 continue;
 130                         }
 131                         ao_ground_accel = ao_flight_accel;
 132                         ao_ground_pres = ao_flight_pres;
 133                         ao_min_pres = ao_flight_pres;
 134                         ao_main_pres = ao_ground_pres - BARO_MAIN;
 135
 136                         ao_interval_end = ao_flight_tick;
 137
 138                         if (ao_flight_accel < ACCEL_NOSE_UP) {
 139                                 ao_flight_state = ao_flight_launchpad;
 140                                 ao_report_notify();
 141                         } else {
 142                                 ao_flight_state = ao_flight_idle;
 143                                 ao_report_notify();
 144                         }
 145                         /* signal successful initialization by turning off the LED */
 146                         ao_led_off(AO_LED_RED);
 147                         break;
 148                 case ao_flight_launchpad:
 149                         if (ao_flight_accel < ACCEL_BOOST ||
 150                             ao_flight_pres + BARO_LAUNCH < ao_ground_pres)
 151                         {
 152                                 ao_flight_state = ao_flight_boost;
 153                                 ao_log_start();
 154                                 ao_report_notify();
 155                                 break;
 156                         }
 157                         break;
 158                 case ao_flight_boost:
 159                         if (ao_flight_accel > ACCEL_ZERO_G ||
 160                             (int16_t) (ao_flight_data.tick - ao_launch_time) > BOOST_TICKS_MAX)
 161                         {
 162                                 ao_flight_state = ao_flight_coast;
 163                                 ao_report_notify();
 164                                 break;
 165                         }
 166                         break;
 167                 case ao_flight_coast:
 168                         if (ao_flight_pres < ao_min_pres)
 169                                 ao_min_pres = ao_flight_pres;
 170                         if (ao_flight_pres - BARO_APOGEE > ao_min_pres) {
 171                                 ao_flight_state = ao_flight_apogee;
 172                                 ao_report_notify();
 173                         }
 174                         break;
 175                 case ao_flight_apogee:
 176 //                      ao_ignite(AO_IGNITE_DROGUE);
 177                         ao_flight_state = ao_flight_drogue;
 178                         ao_report_notify();
 179                         break;
 180                 case ao_flight_drogue:
 181                         if (ao_flight_pres >= ao_main_pres) {
 182 //                              ao_ignite(AO_IGNITE_MAIN);
 183                                 ao_flight_state = ao_flight_main;
 184                                 ao_report_notify();
 185                         }
 186                         if ((ao_interval_max_pres - ao_interval_min_pres) < BARO_LAND) {
 187                                 ao_flight_state = ao_flight_landed;
 188                                 ao_report_notify();
 189                         }
 190                         break;
 191                 case ao_flight_main:
 192                         if ((ao_interval_max_pres - ao_interval_min_pres) < BARO_LAND) {
 193                                 ao_flight_state = ao_flight_landed;
 194                                 ao_report_notify();
 195                         }
 196                         break;
 197                 case ao_flight_landed:
 198                         ao_log_stop();
 199                         break;
 200                 }
 201         }
 202 }
 203
 204 static __xdata struct ao_task   flight_task;
 205
 206 void
 207 ao_flight_init(void)
 208 {
 209         ao_flight_state = ao_flight_startup;
 210         ao_interval_min_accel = 0;
 211         ao_interval_max_accel = 0x7fff;
 212         ao_interval_min_pres = 0;
 213         ao_interval_max_pres = 0x7fff;
 214         ao_interval_end = AO_INTERVAL_TICKS;
 215
 216         ao_add_task(&flight_task, ao_flight);
 217 }
 218