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(void)
  66 {
  67         /* startup state:
  68          *
  69          * Collect 512 samples of acceleration and pressure
  70          * data and average them to find the resting values
  71          */
  72         if (nsamples < 512) {
  73 #if HAS_ACCEL
  74                 ao_sample_accel_sum += ao_sample_accel;
  75 #endif
  76                 ao_sample_pres_sum += ao_sample_pres;
  77                 ++nsamples;
  78         } else {
  79                 ao_config_get();
  80 #if HAS_ACCEL
  81                 ao_ground_accel = ao_sample_accel_sum >> 9;
  82                 ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g;
  83                 ao_accel_scale = to_fix32(GRAVITY * 2 * 16) / ao_accel_2g;
  84 #endif
  85                 ao_ground_pres = ao_sample_pres_sum >> 9;
  86                 ao_ground_height = pres_to_altitude(ao_ground_pres);
  87                 ao_preflight = FALSE;
  88         }
  89 }
  90
  91
  92 uint8_t
  93 ao_sample(void)
  94 {
  95         ao_config_get();
  96         ao_wakeup(DATA_TO_XDATA(&ao_sample_data));
  97         ao_sleep((void *) DATA_TO_XDATA(&ao_data_head));
  98         while (ao_sample_data != ao_data_head) {
  99                 __xdata struct ao_data *ao_data;
 100
 101                 /* Capture a sample */
 102                 ao_data = (struct ao_data *) &ao_data_ring[ao_sample_data];
 103                 ao_sample_tick = ao_data->tick;
 104
 105                 ao_data_pres_cook(ao_data);
 106                 ao_sample_pres = ao_data_pres(ao_data);
 107                 ao_sample_alt = pres_to_altitude(ao_sample_pres);
 108                 ao_sample_height = ao_sample_alt - ao_ground_height;
 109
 110 #if HAS_ACCEL
 111                 ao_sample_accel = ao_data_accel_cook(ao_data);
 112                 if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP)
 113                         ao_sample_accel = ao_data_accel_invert(ao_sample_accel);
 114                 ao_data_set_accel(ao_data, ao_sample_accel);
 115 #endif
 116
 117                 if (ao_preflight)
 118                         ao_sample_preflight();
 119                 else
 120                         ao_kalman();
 121                 ao_sample_data = ao_data_ring_next(ao_sample_data);
 122         }
 123         return !ao_preflight;
 124 }
 125
 126 void
 127 ao_sample_init(void)
 128 {
 129         nsamples = 0;
 130         ao_sample_pres_sum = 0;
 131         ao_sample_pres = 0;
 132 #if HAS_ACCEL
 133         ao_sample_accel_sum = 0;
 134         ao_sample_accel = 0;
 135 #endif
 136         ao_sample_data = ao_data_head;
 137         ao_preflight = TRUE;
 138 }