git.gag.com Git - fw/altos/blob - src/ao_sample.c

   1 /*
   2  * Copyright © 2011 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; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #ifndef AO_FLIGHT_TEST
  19 #include "ao.h"
  20 #endif
  21
  22 /*
  23  * Current sensor values
  24  */
  25
  26 __pdata uint16_t        ao_sample_tick;         /* time of last data */
  27 __pdata int16_t         ao_sample_pres;
  28 __pdata int16_t         ao_sample_alt;
  29 __pdata int16_t         ao_sample_height;
  30 #if HAS_ACCEL
  31 __pdata int16_t         ao_sample_accel;
  32 #endif
  33
  34 __data uint8_t          ao_sample_adc;
  35
  36 /*
  37  * Sensor calibration values
  38  */
  39
  40 __pdata int16_t         ao_ground_pres;         /* startup pressure */
  41 __pdata int16_t         ao_ground_height;       /* MSL of ao_ground_pres */
  42
  43 #if HAS_ACCEL
  44 __pdata int16_t         ao_ground_accel;        /* startup acceleration */
  45 __pdata int16_t         ao_accel_2g;            /* factory accel calibration */
  46 __pdata int32_t         ao_accel_scale;         /* sensor to m/s² conversion */
  47 #endif
  48
  49 static __pdata uint8_t  ao_preflight;           /* in preflight mode */
  50
  51 static __pdata uint16_t nsamples;
  52 __pdata int32_t ao_sample_pres_sum;
  53 #if HAS_ACCEL
  54 __pdata int32_t ao_sample_accel_sum;
  55 #endif
  56
  57 static void
  58 ao_sample_preflight(void)
  59 {
  60         /* startup state:
  61          *
  62          * Collect 512 samples of acceleration and pressure
  63          * data and average them to find the resting values
  64          */
  65         if (nsamples < 512) {
  66 #if HAS_ACCEL
  67                 ao_sample_accel_sum += ao_sample_accel;
  68 #endif
  69                 ao_sample_pres_sum += ao_sample_pres;
  70                 ++nsamples;
  71         } else {
  72                 ao_config_get();
  73 #if HAS_ACCEL
  74                 ao_ground_accel = ao_sample_accel_sum >> 9;
  75                 ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g;
  76                 ao_accel_scale = to_fix32(GRAVITY * 2 * 16) / ao_accel_2g;
  77 #endif
  78                 ao_ground_pres = ao_sample_pres_sum >> 9;
  79                 ao_ground_height = ao_pres_to_altitude(ao_ground_pres);
  80                 ao_preflight = FALSE;
  81         }
  82 }
  83
  84 uint8_t
  85 ao_sample(void)
  86 {
  87         ao_wakeup(DATA_TO_XDATA(&ao_sample_adc));
  88         ao_sleep(DATA_TO_XDATA(&ao_adc_head));
  89         while (ao_sample_adc != ao_adc_head) {
  90                 __xdata struct ao_adc *ao_adc;
  91
  92                 /* Capture a sample */
  93                 ao_adc = &ao_adc_ring[ao_sample_adc];
  94                 ao_sample_tick = ao_adc->tick;
  95                 ao_sample_pres = ao_adc->pres;
  96                 ao_sample_alt = ao_pres_to_altitude(ao_sample_pres);
  97                 ao_sample_height = ao_sample_alt - ao_ground_height;
  98 #if HAS_ACCEL
  99                 ao_sample_accel = ao_adc->accel;
 100 #if HAS_ACCEL_REF
 101                 /*
 102                  * Ok, the math here is a bit tricky.
 103                  *
 104                  * ao_sample_accel:  ADC output for acceleration
 105                  * ao_accel_ref:  ADC output for the 5V reference.
 106                  * ao_cook_accel: Corrected acceleration value
 107                  * Vcc:           3.3V supply to the CC1111
 108                  * Vac:           5V supply to the accelerometer
 109                  * accel:         input voltage to accelerometer ADC pin
 110                  * ref:           input voltage to 5V reference ADC pin
 111                  *
 112                  *
 113                  * Measured acceleration is ratiometric to Vcc:
 114                  *
 115                  *     ao_sample_accel   accel
 116                  *     ------------ = -----
 117                  *        32767        Vcc
 118                  *
 119                  * Measured 5v reference is also ratiometric to Vcc:
 120                  *
 121                  *     ao_accel_ref    ref
 122                  *     ------------ = -----
 123                  *        32767        Vcc
 124                  *
 125                  *
 126                  *      ao_accel_ref = 32767 * (ref / Vcc)
 127                  *
 128                  * Acceleration is measured ratiometric to the 5V supply,
 129                  * so what we want is:
 130                  *
 131                  *      ao_cook_accel    accel
 132                  *      ------------- =  -----
 133                  *          32767         ref
 134                  *
 135                  *
 136                  *                      accel    Vcc
 137                  *                    = ----- *  ---
 138                  *                       Vcc     ref
 139                  *
 140                  *                      ao_sample_accel       32767
 141                  *                    = ------------ *  ------------
 142                  *                         32737        ao_accel_ref
 143                  *
 144                  * Multiply through by 32767:
 145                  *
 146                  *                      ao_sample_accel * 32767
 147                  *      ao_cook_accel = --------------------
 148                  *                          ao_accel_ref
 149                  *
 150                  * Now, the tricky part. Getting this to compile efficiently
 151                  * and keeping all of the values in-range.
 152                  *
 153                  * First off, we need to use a shift of 16 instead of * 32767 as SDCC
 154                  * does the obvious optimizations for byte-granularity shifts:
 155                  *
 156                  *      ao_cook_accel = (ao_sample_accel << 16) / ao_accel_ref
 157                  *
 158                  * Next, lets check our input ranges:
 159                  *
 160                  *      0 <= ao_sample_accel <= 0x7fff          (singled ended ADC conversion)
 161                  *      0x7000 <= ao_accel_ref <= 0x7fff        (the 5V ref value is close to 0x7fff)
 162                  *
 163                  * Plugging in our input ranges, we get an output range of 0 - 0x12490,
 164                  * which is 17 bits. That won't work. If we take the accel ref and shift
 165                  * by a bit, we'll change its range:
 166                  *
 167                  *      0xe000 <= ao_accel_ref<<1 <= 0xfffe
 168                  *
 169                  *      ao_cook_accel = (ao_sample_accel << 16) / (ao_accel_ref << 1)
 170                  *
 171                  * Now the output range is 0 - 0x9248, which nicely fits in 16 bits. It
 172                  * is, however, one bit too large for our signed computations. So, we
 173                  * take the result and shift that by a bit:
 174                  *
 175                  *      ao_cook_accel = ((ao_sample_accel << 16) / (ao_accel_ref << 1)) >> 1
 176                  *
 177                  * This finally creates an output range of 0 - 0x4924. As the ADC only
 178                  * provides 11 bits of data, we haven't actually lost any precision,
 179                  * just dropped a bit of noise off the low end.
 180                  */
 181                 ao_sample_accel = (uint16_t) ((((uint32_t) ao_sample_accel << 16) / (ao_accel_ref[ao_sample_adc] << 1))) >> 1;
 182                 if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP)
 183                         ao_sample_accel = 0x7fff - ao_sample_accel;
 184                 ao_adc->accel = ao_sample_accel;
 185 #endif
 186 #endif
 187
 188                 if (ao_preflight)
 189                         ao_sample_preflight();
 190                 else
 191                         ao_kalman();
 192                 ao_sample_adc = ao_adc_ring_next(ao_sample_adc);
 193         }
 194         return !ao_preflight;
 195 }
 196
 197 void
 198 ao_sample_init(void)
 199 {
 200         nsamples = 0;
 201         ao_sample_pres_sum = 0;
 202         ao_sample_pres = 0;
 203 #if HAS_ACCEL
 204         ao_sample_accel_sum = 0;
 205         ao_sample_accel = 0;
 206 #endif
 207         ao_sample_adc = ao_adc_head;
 208         ao_preflight = TRUE;
 209 }