git.gag.com Git - fw/altos/blob - src/core/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 #include <ao_data.h>
  21 #endif
  22
  23 /*
  24  * Current sensor values
  25  */
  26
  27 #ifndef PRES_TYPE
  28 #define PRES_TYPE int32_t
  29 #define ALT_TYPE int32_t
  30 #define ACCEL_TYPE int16_t
  31 #endif
  32
  33 __pdata uint16_t        ao_sample_tick;         /* time of last data */
  34 __pdata pres_t          ao_sample_pres;
  35 __pdata alt_t           ao_sample_alt;
  36 __pdata alt_t           ao_sample_height;
  37 #if HAS_ACCEL
  38 __pdata accel_t         ao_sample_accel;
  39 #endif
  40 #if HAS_GYRO
  41 __pdata accel_t         ao_sample_accel_along;
  42 __pdata accel_t         ao_sample_accel_across;
  43 __pdata accel_t         ao_sample_accel_through;
  44 __pdata gyro_t          ao_sample_roll;
  45 __pdata gyro_t          ao_sample_pitch;
  46 __pdata gyro_t          ao_sample_yaw;
  47 __pdata angle_t         ao_sample_angle;
  48 __pdata angle_t         ao_sample_roll_angle;
  49 #endif
  50
  51 __data uint8_t          ao_sample_data;
  52
  53 /*
  54  * Sensor calibration values
  55  */
  56
  57 __pdata pres_t          ao_ground_pres;         /* startup pressure */
  58 __pdata alt_t           ao_ground_height;       /* MSL of ao_ground_pres */
  59
  60 #if HAS_ACCEL
  61 __pdata accel_t         ao_ground_accel;        /* startup acceleration */
  62 __pdata accel_t         ao_accel_2g;            /* factory accel calibration */
  63 __pdata int32_t         ao_accel_scale;         /* sensor to m/s² conversion */
  64 #endif
  65
  66 #if HAS_GYRO
  67 __pdata accel_t         ao_ground_accel_along;
  68 __pdata accel_t         ao_ground_accel_across;
  69 __pdata accel_t         ao_ground_accel_through;
  70 __pdata gyro_t          ao_ground_pitch;
  71 __pdata gyro_t          ao_ground_yaw;
  72 __pdata gyro_t          ao_ground_roll;
  73 #endif
  74
  75 static __pdata uint8_t  ao_preflight;           /* in preflight mode */
  76
  77 static __pdata uint16_t nsamples;
  78 __pdata int32_t ao_sample_pres_sum;
  79 #if HAS_ACCEL
  80 __pdata int32_t ao_sample_accel_sum;
  81 #endif
  82 #if HAS_GYRO
  83 __pdata int32_t ao_sample_accel_along_sum;
  84 __pdata int32_t ao_sample_accel_across_sum;
  85 __pdata int32_t ao_sample_accel_through_sum;
  86 __pdata int32_t ao_sample_pitch_sum;
  87 __pdata int32_t ao_sample_yaw_sum;
  88 __pdata int32_t ao_sample_roll_sum;
  89 #endif
  90
  91 static void
  92 ao_sample_preflight_add(void)
  93 {
  94 #if HAS_ACCEL
  95         ao_sample_accel_sum += ao_sample_accel;
  96 #endif
  97         ao_sample_pres_sum += ao_sample_pres;
  98 #if HAS_GYRO
  99         ao_sample_accel_along_sum += ao_sample_accel_along;
 100         ao_sample_accel_across_sum += ao_sample_accel_across;
 101         ao_sample_accel_through_sum += ao_sample_accel_through;
 102         ao_sample_pitch_sum += ao_sample_pitch;
 103         ao_sample_yaw_sum += ao_sample_yaw;
 104         ao_sample_roll_sum += ao_sample_roll;
 105 #endif
 106         ++nsamples;
 107 }
 108
 109 static void
 110 ao_sample_preflight_set(void)
 111 {
 112 #if HAS_ACCEL
 113         ao_ground_accel = ao_sample_accel_sum >> 9;
 114         ao_sample_accel_sum = 0;
 115 #endif
 116         ao_ground_pres = ao_sample_pres_sum >> 9;
 117         ao_ground_height = pres_to_altitude(ao_ground_pres);
 118         ao_sample_pres_sum = 0;
 119 #if HAS_GYRO
 120         ao_ground_accel_along = ao_sample_accel_along_sum >> 9;
 121         ao_ground_accel_across = ao_sample_accel_across_sum >> 9;
 122         ao_ground_accel_through = ao_sample_accel_through_sum >> 9;
 123         ao_ground_pitch = ao_sample_pitch_sum >> 9;
 124         ao_ground_yaw = ao_sample_yaw_sum >> 9;
 125         ao_ground_roll = ao_sample_roll_sum >> 9;
 126         ao_sample_accel_along_sum = 0;
 127         ao_sample_accel_across_sum = 0;
 128         ao_sample_accel_through_sum = 0;
 129         ao_sample_pitch_sum = 0;
 130         ao_sample_yaw_sum = 0;
 131         ao_sample_roll_sum = 0;
 132         ao_sample_angle = 0;
 133 #endif
 134         nsamples = 0;
 135 }
 136
 137 static void
 138 ao_sample_preflight(void)
 139 {
 140         /* startup state:
 141          *
 142          * Collect 512 samples of acceleration and pressure
 143          * data and average them to find the resting values
 144          */
 145         if (nsamples < 512) {
 146                 ao_sample_preflight_add();
 147         } else {
 148 #if HAS_ACCEL
 149                 ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g;
 150                 ao_accel_scale = to_fix32(GRAVITY * 2 * 16) / ao_accel_2g;
 151 #endif
 152                 ao_sample_preflight_set();
 153                 ao_preflight = FALSE;
 154         }
 155 }
 156
 157 /*
 158  * While in pad mode, constantly update the ground state by
 159  * re-averaging the data.  This tracks changes in orientation, which
 160  * might be caused by adjustments to the rocket on the pad and
 161  * pressure, which might be caused by changes in the weather.
 162  */
 163
 164 static void
 165 ao_sample_preflight_update(void)
 166 {
 167         if (nsamples < 512)
 168                 ao_sample_preflight_add();
 169         else if (nsamples < 1024)
 170                 ++nsamples;
 171         else
 172                 ao_sample_preflight_set();
 173 }
 174
 175 #if HAS_GYRO
 176
 177 static int32_t  p_filt;
 178 static int32_t  y_filt;
 179
 180 static gyro_t inline ao_gyro(void) {
 181         gyro_t  p = ao_sample_pitch - ao_ground_pitch;
 182         gyro_t  y = ao_sample_yaw - ao_ground_yaw;
 183
 184         p_filt = p_filt - (p_filt >> 6) + p;
 185         y_filt = y_filt - (y_filt >> 6) + y;
 186
 187         p = p_filt >> 6;
 188         y = y_filt >> 6;
 189         return ao_sqrt(p*p + y*y);
 190 }
 191 #endif
 192
 193 uint8_t
 194 ao_sample(void)
 195 {
 196         ao_wakeup(DATA_TO_XDATA(&ao_sample_data));
 197         ao_sleep((void *) DATA_TO_XDATA(&ao_data_head));
 198         while (ao_sample_data != ao_data_head) {
 199                 __xdata struct ao_data *ao_data;
 200
 201                 /* Capture a sample */
 202                 ao_data = (struct ao_data *) &ao_data_ring[ao_sample_data];
 203                 ao_sample_tick = ao_data->tick;
 204
 205 #if HAS_BARO
 206                 ao_data_pres_cook(ao_data);
 207                 ao_sample_pres = ao_data_pres(ao_data);
 208                 ao_sample_alt = pres_to_altitude(ao_sample_pres);
 209                 ao_sample_height = ao_sample_alt - ao_ground_height;
 210 #endif
 211
 212 #if HAS_ACCEL
 213                 ao_sample_accel = ao_data_accel_cook(ao_data);
 214                 if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP)
 215                         ao_sample_accel = ao_data_accel_invert(ao_sample_accel);
 216                 ao_data_set_accel(ao_data, ao_sample_accel);
 217 #endif
 218 #if HAS_GYRO
 219                 ao_sample_accel_along = ao_data_along(ao_data);
 220                 ao_sample_pitch = ao_data_pitch(ao_data);
 221                 ao_sample_yaw = ao_data_yaw(ao_data);
 222                 ao_sample_roll = ao_data_roll(ao_data);
 223 #endif
 224
 225                 if (ao_preflight)
 226                         ao_sample_preflight();
 227                 else {
 228                         if (ao_flight_state < ao_flight_boost)
 229                                 ao_sample_preflight_update();
 230                         ao_kalman();
 231 #if HAS_GYRO
 232                         ao_sample_angle += ao_gyro();
 233                         ao_sample_roll_angle += (ao_sample_roll - ao_ground_roll);
 234 #endif
 235                 }
 236                 ao_sample_data = ao_data_ring_next(ao_sample_data);
 237         }
 238         return !ao_preflight;
 239 }
 240
 241 void
 242 ao_sample_init(void)
 243 {
 244         ao_config_get();
 245         nsamples = 0;
 246         ao_sample_pres_sum = 0;
 247         ao_sample_pres = 0;
 248 #if HAS_ACCEL
 249         ao_sample_accel_sum = 0;
 250         ao_sample_accel = 0;
 251 #endif
 252 #if HAS_GYRO
 253         ao_sample_accel_along_sum = 0;
 254         ao_sample_accel_across_sum = 0;
 255         ao_sample_accel_through_sum = 0;
 256         ao_sample_accel_along = 0;
 257         ao_sample_accel_across = 0;
 258         ao_sample_accel_through = 0;
 259         ao_sample_pitch_sum = 0;
 260         ao_sample_yaw_sum = 0;
 261         ao_sample_roll_sum = 0;
 262         ao_sample_pitch = 0;
 263         ao_sample_yaw = 0;
 264         ao_sample_roll = 0;
 265         ao_sample_angle = 0;
 266 #endif
 267         ao_sample_data = ao_data_head;
 268         ao_preflight = TRUE;
 269 }