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
  41 __data uint8_t          ao_sample_data;
  42
  43 /*
  44  * Sensor calibration values
  45  */
  46
  47 __pdata pres_t          ao_ground_pres;         /* startup pressure */
  48 __pdata alt_t           ao_ground_height;       /* MSL of ao_ground_pres */
  49
  50 #if HAS_ACCEL
  51 __pdata accel_t         ao_ground_accel;        /* startup acceleration */
  52 __pdata accel_t         ao_accel_2g;            /* factory accel calibration */
  53 __pdata int32_t         ao_accel_scale;         /* sensor to m/s² conversion */
  54 #endif
  55
  56 static __pdata uint8_t  ao_preflight;           /* in preflight mode */
  57
  58 static __pdata uint16_t nsamples;
  59 __pdata int32_t ao_sample_pres_sum;
  60 #if HAS_ACCEL
  61 __pdata int32_t ao_sample_accel_sum;
  62 #endif
  63
  64 static void
  65 ao_sample_preflight_add(void)
  66 {
  67 #if HAS_ACCEL
  68         ao_sample_accel_sum += ao_sample_accel;
  69 #endif
  70         ao_sample_pres_sum += ao_sample_pres;
  71         ++nsamples;
  72 }
  73
  74 static void
  75 ao_sample_preflight_set(void)
  76 {
  77 #if HAS_ACCEL
  78         ao_ground_accel = ao_sample_accel_sum >> 9;
  79         ao_sample_accel_sum = 0;
  80 #endif
  81         ao_ground_pres = ao_sample_pres_sum >> 9;
  82         ao_ground_height = pres_to_altitude(ao_ground_pres);
  83         nsamples = 0;
  84         ao_sample_pres_sum = 0;
  85 }
  86
  87 static void
  88 ao_sample_preflight(void)
  89 {
  90         /* startup state:
  91          *
  92          * Collect 512 samples of acceleration and pressure
  93          * data and average them to find the resting values
  94          */
  95         if (nsamples < 512) {
  96                 ao_sample_preflight_add();
  97         } else {
  98 #if HAS_ACCEL
  99                 ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g;
 100                 ao_accel_scale = to_fix32(GRAVITY * 2 * 16) / ao_accel_2g;
 101 #endif
 102                 ao_sample_preflight_set();
 103                 ao_preflight = FALSE;
 104         }
 105 }
 106
 107 /*
 108  * While in pad mode, constantly update the ground state by
 109  * re-averaging the data.  This tracks changes in orientation, which
 110  * might be caused by adjustments to the rocket on the pad and
 111  * pressure, which might be caused by changes in the weather.
 112  */
 113
 114 static void
 115 ao_sample_preflight_update(void)
 116 {
 117         if (nsamples < 512)
 118                 ao_sample_preflight_add();
 119         else if (nsamples < 1024)
 120                 ++nsamples;
 121         else
 122                 ao_sample_preflight_set();
 123 }
 124
 125 uint8_t
 126 ao_sample(void)
 127 {
 128         ao_wakeup(DATA_TO_XDATA(&ao_sample_data));
 129         ao_sleep((void *) DATA_TO_XDATA(&ao_data_head));
 130         while (ao_sample_data != ao_data_head) {
 131                 __xdata struct ao_data *ao_data;
 132
 133                 /* Capture a sample */
 134                 ao_data = (struct ao_data *) &ao_data_ring[ao_sample_data];
 135                 ao_sample_tick = ao_data->tick;
 136
 137 #if HAS_BARO
 138                 ao_data_pres_cook(ao_data);
 139                 ao_sample_pres = ao_data_pres(ao_data);
 140                 ao_sample_alt = pres_to_altitude(ao_sample_pres);
 141                 ao_sample_height = ao_sample_alt - ao_ground_height;
 142 #endif
 143
 144 #if HAS_ACCEL
 145                 ao_sample_accel = ao_data_accel_cook(ao_data);
 146                 if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP)
 147                         ao_sample_accel = ao_data_accel_invert(ao_sample_accel);
 148                 ao_data_set_accel(ao_data, ao_sample_accel);
 149 #endif
 150
 151                 if (ao_preflight)
 152                         ao_sample_preflight();
 153                 else {
 154                         if (ao_flight_state < ao_flight_boost)
 155                                 ao_sample_preflight_update();
 156                         ao_kalman();
 157                 }
 158                 ao_sample_data = ao_data_ring_next(ao_sample_data);
 159         }
 160         return !ao_preflight;
 161 }
 162
 163 void
 164 ao_sample_init(void)
 165 {
 166         ao_config_get();
 167         nsamples = 0;
 168         ao_sample_pres_sum = 0;
 169         ao_sample_pres = 0;
 170 #if HAS_ACCEL
 171         ao_sample_accel_sum = 0;
 172         ao_sample_accel = 0;
 173 #endif
 174         ao_sample_data = ao_data_head;
 175         ao_preflight = TRUE;
 176 }