2 * Copyright © 2009 Keith Packard <keithp@keithp.com>
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.
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.
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.
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)
28 struct cc_perioddata *unfiltered, *filtered;
30 unfiltered = cc_period_make(td, start_time, stop_time);
31 filtered = cc_period_low_pass (unfiltered, omega_pass, omega_stop, error);
34 free (unfiltered->data);
39 barometer_to_altitude(double b, double pad_alt)
41 return cc_barometer_to_altitude(b) - pad_alt;
44 struct cc_flightcooked *
45 cc_flight_cook(struct cc_flightraw *raw)
47 struct cc_flightcooked *cooked;
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;
60 if (raw->accel.num == 0)
63 cooked = calloc (1, sizeof (struct cc_flightcooked));
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
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 - 10;
76 if (!stop_set && raw->state.data[i].value > ao_flight_main) {
77 flight_stop = raw->state.data[i].time;
82 if (!start_set || flight_start < raw->accel.data[0].time)
83 flight_start = raw->accel.data[0].time;
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;
92 flight_stop = raw->accel.data[raw->accel.num-1].time;
94 cooked->flight_start = flight_start;
95 cooked->flight_stop = flight_stop;
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;
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);
104 #define ACCEL_OMEGA_PASS (2 * M_PI * 20 / 100)
105 #define ACCEL_OMEGA_STOP (2 * M_PI * 30 / 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)
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);
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;
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);
134 cooked->pres_speed = *pres_speed;
136 cooked->pres_accel = *pres_accel;
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;
148 #define if_free(x) ((x) ? free(x) : (void) 0)
151 cc_flightcooked_free(struct cc_flightcooked *cooked)
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);