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);
41 barometer_to_altitude(double b, double pad_alt)
43 return cc_barometer_to_altitude(b) - pad_alt;
46 struct cc_flightcooked *
47 cc_flight_cook(struct cc_flightraw *raw)
49 struct cc_flightcooked *cooked;
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;
62 if (raw->accel.num == 0)
65 cooked = calloc (1, sizeof (struct cc_flightcooked));
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
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;
78 if (!stop_set && raw->state.data[i].value > ao_flight_main) {
79 flight_stop = raw->state.data[i].time;
85 flight_start = raw->accel.data[0].time;
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;
94 flight_stop = raw->accel.data[raw->accel.num-1].time;
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);
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)
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);
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);
128 cooked->pres_speed = *pres_speed;
130 cooked->pres_accel = *pres_accel;
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;