74b02a74b53af644707d816aca2c40c453456b37
[fw/altos] / 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;
50         double                  flight_stop;
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 }