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; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along
  15  * with this program; if not, write to the Free Software Foundation, Inc.,
  16  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  17  */
  18
  19 #include "cc.h"
  20 #include <stdlib.h>
  21 #include <math.h>
  22 #include <string.h>
  23
  24 static void
  25 cook_timed(struct cc_timedata *td, struct cc_perioddata *pd,
  26            double start_time, double stop_time,
  27            double omega_pass, double omega_stop, double error)
  28 {
  29         struct cc_perioddata    *unfiltered, *filtered;
  30
  31         unfiltered = cc_period_make(td, start_time, stop_time);
  32         filtered = cc_period_low_pass (unfiltered, omega_pass, omega_stop, error);
  33         *pd = *filtered;
  34         free (filtered);
  35         free (unfiltered->data);
  36         free (unfiltered);
  37 }
  38
  39 static double
  40 barometer_to_altitude(double b, double pad_alt)
  41 {
  42         return cc_barometer_to_altitude(b) - pad_alt;
  43 }
  44
  45 struct cc_flightcooked *
  46 cc_flight_cook(struct cc_flightraw *raw)
  47 {
  48         struct cc_flightcooked *cooked;
  49         double                  flight_start = 0;
  50         double                  flight_stop = 0;
  51         int                     start_set = 0;
  52         int                     stop_set = 0;
  53         int                     i;
  54         struct cc_timedata      *accel;
  55         struct cc_timedata      *accel_speed;
  56         struct cc_timedata      *accel_pos;
  57         struct cc_timedata      *pres;
  58         struct cc_perioddata    *pres_speed;
  59         struct cc_perioddata    *pres_accel;
  60
  61         if (raw->accel.num == 0)
  62                 return NULL;
  63
  64         cooked = calloc (1, sizeof (struct cc_flightcooked));
  65
  66         /*
  67          * Find flight start and stop times by looking at
  68          * state transitions. The stop time is set to the time
  69          * of landing, which may be long after it landed (due to radio
  70          * issues). Refine this value by looking through the sensor data
  71          */
  72         for (i = 0; i < raw->state.num; i++) {
  73                 if (!start_set && raw->state.data[i].value > ao_flight_pad) {
  74                         flight_start = raw->state.data[i].time - 10;
  75                         start_set = 1;
  76                 }
  77                 if (!stop_set && raw->state.data[i].value > ao_flight_main) {
  78                         flight_stop = raw->state.data[i].time;
  79                         stop_set = 1;
  80                 }
  81         }
  82
  83         if (!start_set || flight_start < raw->accel.data[0].time)
  84                 flight_start = raw->accel.data[0].time;
  85         if (stop_set) {
  86                 for (i = 0; i < raw->accel.num - 1; i++) {
  87                         if (raw->accel.data[i+1].time >= flight_stop) {
  88                                 flight_stop = raw->accel.data[i].time;
  89                                 break;
  90                         }
  91                 }
  92         } else {
  93                 flight_stop = raw->accel.data[raw->accel.num-1].time;
  94         }
  95         cooked->flight_start = flight_start;
  96         cooked->flight_stop = flight_stop;
  97
  98         /* Integrate the accelerometer data to get speed and position */
  99         accel = cc_timedata_convert(&raw->accel, cc_accelerometer_to_acceleration, raw->ground_accel);
 100         cooked->accel = *accel;
 101         free(accel);
 102         accel_speed = cc_timedata_integrate(&cooked->accel, flight_start - 10, flight_stop);
 103         accel_pos = cc_timedata_integrate(accel_speed, flight_start - 10, flight_stop);
 104
 105 #define ACCEL_OMEGA_PASS        (2 * M_PI * 20 / 100)
 106 #define ACCEL_OMEGA_STOP        (2 * M_PI * 30 / 100)
 107 #define BARO_OMEGA_PASS         (2 * M_PI * .5 / 100)
 108 #define BARO_OMEGA_STOP         (2 * M_PI * 1 / 100)
 109 #define FILTER_ERROR            (1e-8)
 110
 111         cook_timed(&cooked->accel, &cooked->accel_accel,
 112                    flight_start, flight_stop,
 113                    ACCEL_OMEGA_PASS, ACCEL_OMEGA_STOP, FILTER_ERROR);
 114         cook_timed(accel_speed, &cooked->accel_speed,
 115                    flight_start, flight_stop,
 116                    ACCEL_OMEGA_PASS, ACCEL_OMEGA_STOP, FILTER_ERROR);
 117         free(accel_speed->data); free(accel_speed);
 118         cook_timed(accel_pos, &cooked->accel_pos,
 119                    flight_start, flight_stop,
 120                    ACCEL_OMEGA_PASS, ACCEL_OMEGA_STOP, FILTER_ERROR);
 121         free(accel_pos->data); free(accel_pos);
 122
 123         /* Filter the pressure data */
 124         pres = cc_timedata_convert(&raw->pres, barometer_to_altitude,
 125                                    cc_barometer_to_altitude(raw->ground_pres));
 126         cooked->pres = *pres;
 127         free(pres);
 128         cook_timed(&cooked->pres, &cooked->pres_pos,
 129                    flight_start, flight_stop,
 130                    BARO_OMEGA_PASS, BARO_OMEGA_STOP, FILTER_ERROR);
 131         /* differentiate twice to get to acceleration */
 132         pres_speed = cc_perioddata_differentiate(&cooked->pres_pos);
 133         pres_accel = cc_perioddata_differentiate(pres_speed);
 134
 135         cooked->pres_speed = *pres_speed;
 136         free(pres_speed);
 137         cooked->pres_accel = *pres_accel;
 138         free(pres_accel);
 139
 140         /* copy state */
 141         cooked->state.num = raw->state.num;
 142         cooked->state.size = raw->state.num;
 143         cooked->state.data = calloc(cooked->state.num, sizeof (struct cc_timedataelt));
 144         memcpy(cooked->state.data, raw->state.data, cooked->state.num * sizeof (struct cc_timedataelt));
 145         cooked->state.time_offset = raw->state.time_offset;
 146         return cooked;
 147 }
 148
 149 #define if_free(x)      ((x) ? free(x) : (void) 0)
 150
 151 void
 152 cc_flightcooked_free(struct cc_flightcooked *cooked)
 153 {
 154         if_free(cooked->accel_accel.data);
 155         if_free(cooked->accel_speed.data);
 156         if_free(cooked->accel_pos.data);
 157         if_free(cooked->pres_pos.data);
 158         if_free(cooked->pres_speed.data);
 159         if_free(cooked->pres_accel.data);
 160         if_free(cooked->gps_lat.data);
 161         if_free(cooked->gps_lon.data);
 162         if_free(cooked->gps_alt.data);
 163         if_free(cooked->state.data);
 164         if_free(cooked->accel.data);
 165         if_free(cooked->pres.data);
 166         free(cooked);
 167 }