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

   1 /*
   2  * Copyright © 2012 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_SAMPLE_H_
  19 #define _AO_SAMPLE_H_
  20
  21 #include <ao_data.h>
  22
  23 /*
  24  * ao_sample.c
  25  */
  26
  27 #ifndef AO_VALUE_32
  28 #define AO_VALUE_32     1
  29 #endif
  30
  31 #if AO_VALUE_32
  32 /*
  33  * For 32-bit computed values, use 64-bit intermediates.
  34  */
  35 typedef int64_t                 ao_k_t;
  36 typedef int32_t                 ao_v_t;
  37 #else
  38 /*
  39  * For 16-bit computed values, use 32-bit intermediates.
  40  */
  41 typedef int32_t                 ao_k_t;
  42 typedef int16_t                 ao_v_t;
  43 #endif
  44
  45 /*
  46  * Barometer calibration
  47  *
  48  * We directly sample the barometer. The specs say:
  49  *
  50  * Pressure range: 15-115 kPa
  51  * Voltage at 115kPa: 2.82
  52  * Output scale: 27mV/kPa
  53  *
  54  * If we want to detect launch with the barometer, we need
  55  * a large enough bump to not be fooled by noise. At typical
  56  * launch elevations (0-2000m), a 200Pa pressure change cooresponds
  57  * to about a 20m elevation change. This is 5.4mV, or about 3LSB.
  58  * As all of our calculations are done in 16 bits, we'll actually see a change
  59  * of 16 times this though
  60  *
  61  * 27 mV/kPa * 32767 / 3300 counts/mV = 268.1 counts/kPa
  62  */
  63
  64 /* Accelerometer calibration
  65  *
  66  * We're sampling the accelerometer through a resistor divider which
  67  * consists of 5k and 10k resistors. This multiplies the values by 2/3.
  68  * That goes into the cc1111 A/D converter, which is running at 11 bits
  69  * of precision with the bits in the MSB of the 16 bit value. Only positive
  70  * values are used, so values should range from 0-32752 for 0-3.3V. The
  71  * specs say we should see 40mV/g (uncalibrated), multiply by 2/3 for what
  72  * the A/D converter sees (26.67 mV/g). We should see 32752/3300 counts/mV,
  73  * for a final computation of:
  74  *
  75  * 26.67 mV/g * 32767/3300 counts/mV = 264.8 counts/g
  76  *
  77  * Zero g was measured at 16000 (we would expect 16384).
  78  * Note that this value is only require to tell if the
  79  * rocket is standing upright. Once that is determined,
  80  * the value of the accelerometer is averaged for 100 samples
  81  * to find the resting accelerometer value, which is used
  82  * for all further flight computations
  83  */
  84
  85 /*
  86  * Above this height, the baro sensor doesn't work
  87  */
  88 #if HAS_MS5607
  89 #define AO_MAX_BARO_HEIGHT      30000
  90 #else
  91 #define AO_MAX_BARO_HEIGHT      12000
  92 #endif
  93
  94 /*
  95  * Above this speed, baro measurements are unreliable
  96  */
  97 #define AO_MAX_BARO_SPEED       200
  98
  99 #define ACCEL_NOSE_UP   (ao_accel_2g >> 2)
 100
 101 /*
 102  * Speed and acceleration are scaled by 16 to provide a bit more
 103  * resolution while still having reasonable range. Note that this
 104  * limits speed to 2047m/s (around mach 6) and acceleration to
 105  * 2047m/s² (over 200g)
 106  */
 107
 108 #define AO_M_TO_HEIGHT(m)       ((ao_v_t) (m))
 109 #define AO_MS_TO_SPEED(ms)      ((ao_v_t) ((ms) * 16))
 110 #define AO_MSS_TO_ACCEL(mss)    ((ao_v_t) ((mss) * 16))
 111
 112 extern __pdata uint16_t ao_sample_tick;         /* time of last data */
 113 extern __data uint8_t   ao_sample_adc;          /* Ring position of last processed sample */
 114 extern __data uint8_t   ao_sample_data;         /* Ring position of last processed sample */
 115
 116 #if HAS_BARO
 117 extern __pdata pres_t   ao_sample_pres;         /* most recent pressure sensor reading */
 118 extern __pdata alt_t    ao_sample_alt;          /* MSL of ao_sample_pres */
 119 extern __pdata alt_t    ao_sample_height;       /* AGL of ao_sample_pres */
 120 extern __pdata pres_t   ao_ground_pres;         /* startup pressure */
 121 extern __pdata alt_t    ao_ground_height;       /* MSL of ao_ground_pres */
 122 #endif
 123
 124 #if HAS_ACCEL
 125 extern __pdata accel_t  ao_sample_accel;        /* most recent accel sensor reading */
 126 extern __pdata accel_t  ao_ground_accel;        /* startup acceleration */
 127 extern __pdata accel_t  ao_accel_2g;            /* factory accel calibration */
 128 extern __pdata int32_t  ao_accel_scale;         /* sensor to m/s² conversion */
 129 #endif
 130 #if HAS_GYRO
 131 extern __pdata accel_t  ao_ground_accel_along;
 132 extern __pdata accel_t  ao_ground_accel_across;
 133 extern __pdata accel_t  ao_ground_accel_through;
 134 extern __pdata int32_t  ao_ground_pitch;        /* * 512 */
 135 extern __pdata int32_t  ao_ground_yaw;          /* * 512 */
 136 extern __pdata int32_t  ao_ground_roll;         /* * 512 */
 137 extern __pdata accel_t  ao_sample_accel_along;
 138 extern __pdata accel_t  ao_sample_accel_across;
 139 extern __pdata accel_t  ao_sample_accel_through;
 140 extern __pdata gyro_t   ao_sample_roll;
 141 extern __pdata gyro_t   ao_sample_pitch;
 142 extern __pdata gyro_t   ao_sample_yaw;
 143 extern __pdata angle_t  ao_sample_orient;
 144 #endif
 145
 146 void ao_sample_init(void);
 147
 148 /* returns FALSE in preflight mode, TRUE in flight mode */
 149 uint8_t ao_sample(void);
 150
 151 /*
 152  * ao_kalman.c
 153  */
 154
 155 #define to_fix_16(x) ((int16_t) ((x) * 65536.0 + 0.5))
 156 #define to_fix_32(x) ((int32_t) ((x) * 65536.0 + 0.5))
 157 #define to_fix_64(x) ((int64_t) ((x) * 65536.0 + 0.5))
 158
 159 #ifdef AO_VALUE_32
 160 #if AO_VALUE_32
 161 #define to_fix_v(x)     to_fix_32(x)
 162 #define to_fix_k(x)     to_fix_64(x)
 163 #else
 164 #define to_fix_v(x)     to_fix_16(x)
 165 #define to_fix_k(x)     to_fix_32(x)
 166 #endif
 167
 168 #define from_fix(x)     ((x) >> 16)
 169
 170 extern __pdata ao_v_t                   ao_height;      /* meters */
 171 extern __pdata ao_v_t                   ao_speed;       /* m/s * 16 */
 172 extern __pdata ao_v_t                   ao_accel;       /* m/s² * 16 */
 173 extern __xdata ao_v_t                   ao_max_height;  /* max of ao_height */
 174 extern __xdata ao_v_t                   ao_avg_height;  /* running average of height */
 175
 176 extern __pdata ao_v_t                   ao_error_h;
 177 extern __pdata ao_v_t                   ao_error_h_sq_avg;
 178
 179 #if HAS_ACCEL
 180 extern __pdata ao_v_t                   ao_error_a;
 181 #endif
 182 #endif
 183
 184 void ao_kalman(void);
 185
 186 #endif /* _AO_SAMPLE_H_ */