git.gag.com Git - fw/altos/blob - ao-tools/lib/cc-process.c

   1 /*
   2  * Copyright © 2009 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 #include "cc.h"
  19 #include <stdlib.h>
  20 #include <math.h>
  21 #include <string.h>
  22
  23 static void
  24 cook_timed(struct cc_timedata *td, struct cc_perioddata *pd,
  25            double start_time, double stop_time,
  26            double omega_pass, double omega_stop, double error)
  27 {
  28         struct cc_perioddata    *unfiltered, *filtered;
  29
  30         unfiltered = cc_period_make(td, start_time, stop_time);
  31         filtered = cc_period_low_pass (unfiltered, omega_pass, omega_stop, error);
  32         *pd = *filtered;
  33         free (filtered);
  34         free (unfiltered->data);
  35         free (unfiltered);
  36         free (td->data);
  37         free (td);
  38 }
  39
  40 static double
  41 barometer_to_altitude(double b, double pad_alt)
  42 {
  43         return cc_barometer_to_altitude(b) - pad_alt;
  44 }
  45
  46 struct cc_flightcooked *
  47 cc_flight_cook(struct cc_flightraw *raw)
  48 {
  49         struct cc_flightcooked *cooked;
  50         double                  flight_start;
  51         double                  flight_stop;
  52         int                     start_set = 0;
  53         int                     stop_set = 0;
  54         int                     i;
  55         struct cc_timedata      *accel;
  56         struct cc_timedata      *accel_speed;
  57         struct cc_timedata      *accel_pos;
  58         struct cc_timedata      *pres;
  59         struct cc_perioddata    *pres_speed;
  60         struct cc_perioddata    *pres_accel;
  61
  62         if (raw->accel.num == 0)
  63                 return NULL;
  64
  65         cooked = calloc (1, sizeof (struct cc_flightcooked));
  66
  67         /*
  68          * Find flight start and stop times by looking at
  69          * state transitions. The stop time is set to the time
  70          * of landing, which may be long after it landed (due to radio
  71          * issues). Refine this value by looking through the sensor data
  72          */
  73         for (i = 0; i < raw->state.num; i++) {
  74                 if (!start_set && raw->state.data[i].value > ao_flight_pad) {
  75                         flight_start = raw->state.data[i].time;
  76                         start_set = 1;
  77                 }
  78                 if (!stop_set && raw->state.data[i].value > ao_flight_main) {
  79                         flight_stop = raw->state.data[i].time;
  80                         stop_set = 1;
  81                 }
  82         }
  83
  84         if (!start_set)
  85                 flight_start = raw->accel.data[0].time;
  86         if (stop_set) {
  87                 for (i = 0; i < raw->accel.num - 1; i++) {
  88                         if (raw->accel.data[i+1].time >= flight_stop) {
  89                                 flight_stop = raw->accel.data[i].time;
  90                                 break;
  91                         }
  92                 }
  93         } else {
  94                 flight_stop = raw->accel.data[raw->accel.num-1].time;
  95         }
  96
  97         /* Integrate the accelerometer data to get speed and position */
  98         accel = cc_timedata_convert(&raw->accel, cc_accelerometer_to_acceleration, raw->ground_accel);
  99         accel_speed = cc_timedata_integrate(accel);
 100         accel_pos = cc_timedata_integrate(accel_speed);
 101
 102 #define ACCEL_OMEGA_PASS        (2 * M_PI * 5 / 100)
 103 #define ACCEL_OMEGA_STOP        (2 * M_PI * 8 / 100)
 104 #define BARO_OMEGA_PASS         (2 * M_PI * .5 / 100)
 105 #define BARO_OMEGA_STOP         (2 * M_PI * 1 / 100)
 106 #define FILTER_ERROR            (1e-8)
 107
 108         cook_timed(accel, &cooked->accel_accel,
 109                    flight_start, flight_stop,
 110                    ACCEL_OMEGA_PASS, ACCEL_OMEGA_STOP, FILTER_ERROR);
 111         cook_timed(accel_speed, &cooked->accel_speed,
 112                    flight_start, flight_stop,
 113                    ACCEL_OMEGA_PASS, ACCEL_OMEGA_STOP, FILTER_ERROR);
 114         cook_timed(accel_pos, &cooked->accel_pos,
 115                    flight_start, flight_stop,
 116                    ACCEL_OMEGA_PASS, ACCEL_OMEGA_STOP, FILTER_ERROR);
 117
 118         /* Filter the pressure data */
 119         pres = cc_timedata_convert(&raw->pres, barometer_to_altitude,
 120                                    cc_barometer_to_altitude(raw->ground_pres));
 121         cook_timed(pres, &cooked->pres_pos,
 122                    flight_start, flight_stop,
 123                    BARO_OMEGA_PASS, BARO_OMEGA_STOP, FILTER_ERROR);
 124         /* differentiate twice to get to acceleration */
 125         pres_speed = cc_perioddata_differentiate(&cooked->pres_pos);
 126         pres_accel = cc_perioddata_differentiate(pres_speed);
 127
 128         cooked->pres_speed = *pres_speed;
 129         free(pres_speed);
 130         cooked->pres_accel = *pres_accel;
 131         free(pres_accel);
 132
 133         /* copy state */
 134         cooked->state.num = raw->state.num;
 135         cooked->state.size = raw->state.num;
 136         cooked->state.data = calloc(cooked->state.num, sizeof (struct cc_timedataelt));
 137         memcpy(cooked->state.data, raw->state.data, cooked->state.num * sizeof (struct cc_timedataelt));
 138         cooked->state.time_offset = raw->state.time_offset;
 139         return cooked;
 140 }
 141
 142 #define if_free(x)      ((x) ? free(x) : (void) 0)
 143
 144 void
 145 cc_flightcooked_free(struct cc_flightcooked *cooked)
 146 {
 147         if_free(cooked->accel_accel.data);
 148         if_free(cooked->accel_speed.data);
 149         if_free(cooked->accel_pos.data);
 150         if_free(cooked->pres_pos.data);
 151         if_free(cooked->pres_speed.data);
 152         if_free(cooked->pres_accel.data);
 153         if_free(cooked->gps_lat.data);
 154         if_free(cooked->gps_lon.data);
 155         if_free(cooked->gps_alt.data);
 156         if_free(cooked->state.data);
 157         free(cooked);
 158 }