Merge branch 'master' of ssh://git.gag.com/scm/git/fw/altos

1 files changed, 140 insertions, 0 deletions

diff --git a/ao-tools/lib/cc-process.c b/ao-tools/lib/cc-process.c
new file mode 100644
index 00000000..e906b635
--- /dev/null
+++ b/ao-tools/lib/cc-process.c
@@ -0,0 +1,140 @@
+/*
+ * Copyright © 2009 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include "cc.h"
+#include <stdlib.h>
+#include <math.h>
+#include <string.h>
+
+static void
+cook_timed(struct cc_timedata *td, struct cc_perioddata *pd,
+	   double start_time, double stop_time,
+	   double omega_pass, double omega_stop, double error)
+{
+	struct cc_perioddata	*unfiltered, *filtered;
+
+	unfiltered = cc_period_make(td, start_time, stop_time);
+	filtered = cc_period_low_pass (unfiltered, omega_pass, omega_stop, error);
+	*pd = *filtered;
+	free (filtered);
+	free (unfiltered->data);
+	free (unfiltered);
+	free (td->data);
+	free (td);
+}
+
+static double
+barometer_to_altitude(double b, double pad_alt)
+{
+	return cc_barometer_to_altitude(b) - pad_alt;
+}
+
+struct cc_flightcooked *
+cc_flight_cook(struct cc_flightraw *raw)
+{
+	struct cc_flightcooked *cooked;
+	double			flight_start;
+	double			flight_stop;
+	int			start_set = 0;
+	int			stop_set = 0;
+	int			i;
+	struct cc_timedata	*accel;
+	struct cc_timedata	*accel_speed;
+	struct cc_timedata	*accel_pos;
+	struct cc_timedata	*pres;
+	struct cc_perioddata	*pres_speed;
+	struct cc_perioddata	*pres_accel;
+
+	if (raw->accel.num == 0)
+		return NULL;
+
+	cooked = calloc (1, sizeof (struct cc_flightcooked));
+
+	/*
+	 * Find flight start and stop times by looking at
+	 * state transitions. The stop time is set to the time
+	 * of landing, which may be long after it landed (due to radio
+	 * issues). Refine this value by looking through the sensor data
+	 */
+	for (i = 0; i < raw->state.num; i++) {
+		if (!start_set && raw->state.data[i].value > ao_flight_pad) {
+			flight_start = raw->state.data[i].time;
+			start_set = 1;
+		}
+		if (!stop_set && raw->state.data[i].value > ao_flight_main) {
+			flight_stop = raw->state.data[i].time;
+			stop_set = 1;
+		}
+	}
+
+	if (!start_set)
+		flight_start = raw->accel.data[0].time;
+	if (stop_set) {
+		for (i = 0; i < raw->accel.num - 1; i++) {
+			if (raw->accel.data[i+1].time >= flight_stop) {
+				flight_stop = raw->accel.data[i].time;
+				break;
+			}
+		}
+	} else {
+		flight_stop = raw->accel.data[raw->accel.num-1].time;
+	}
+
+	/* Integrate the accelerometer data to get speed and position */
+	accel = cc_timedata_convert(&raw->accel, cc_accelerometer_to_acceleration, raw->ground_accel);
+	accel_speed = cc_timedata_integrate(accel);
+	accel_pos = cc_timedata_integrate(accel_speed);
+
+#define ACCEL_OMEGA_PASS	(2 * M_PI * 5 / 100)
+#define ACCEL_OMEGA_STOP	(2 * M_PI * 8 / 100)
+#define BARO_OMEGA_PASS		(2 * M_PI * .5 / 100)
+#define BARO_OMEGA_STOP		(2 * M_PI * 1 / 100)
+#define FILTER_ERROR		(1e-8)
+
+	cook_timed(accel, &cooked->accel_accel,
+		   flight_start, flight_stop,
+		   ACCEL_OMEGA_PASS, ACCEL_OMEGA_STOP, FILTER_ERROR);
+	cook_timed(accel_speed, &cooked->accel_speed,
+		   flight_start, flight_stop,
+		   ACCEL_OMEGA_PASS, ACCEL_OMEGA_STOP, FILTER_ERROR);
+	cook_timed(accel_pos, &cooked->accel_pos,
+		   flight_start, flight_stop,
+		   ACCEL_OMEGA_PASS, ACCEL_OMEGA_STOP, FILTER_ERROR);
+
+	/* Filter the pressure data */
+	pres = cc_timedata_convert(&raw->pres, barometer_to_altitude,
+				   cc_barometer_to_altitude(raw->ground_pres));
+	cook_timed(pres, &cooked->pres_pos,
+		   flight_start, flight_stop,
+		   BARO_OMEGA_PASS, BARO_OMEGA_STOP, FILTER_ERROR);
+	/* differentiate twice to get to acceleration */
+	pres_speed = cc_perioddata_differentiate(&cooked->pres_pos);
+	pres_accel = cc_perioddata_differentiate(pres_speed);
+
+	cooked->pres_speed = *pres_speed;
+	free(pres_speed);
+	cooked->pres_accel = *pres_accel;
+	free(pres_accel);
+
+	/* copy state */
+	cooked->state.num = raw->state.num;
+	cooked->state.size = raw->state.num;
+	cooked->state.data = calloc(cooked->state.num, sizeof (struct cc_timedataelt));
+	memcpy(cooked->state.data, raw->state.data, cooked->state.num * sizeof (struct cc_timedataelt));
+	cooked->state.time_offset = raw->state.time_offset;
+	return cooked;
+}