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