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