X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=aoview%2Faoview_state.c;h=9852cabd257633687bbe691ce2db0a64fcca86ab;hp=046ccc92ea5d982d77de2472b01c6493f74e7508;hb=d7c2d358ed8a1afc9f0ba2bd830b10f6b56dc7b1;hpb=be3f4fed7b863c8cdaabe32b61b65a8b3cd11355

diff --git a/aoview/aoview_state.c b/aoview/aoview_state.c
index 046ccc92..9852cabd 100644
--- a/aoview/aoview_state.c
+++ b/aoview/aoview_state.c
@@ -18,117 +18,332 @@
 #include "aoview.h"
 #include <math.h>
 
-static int	pad_pres;
-static int	pad_accel;
+static inline double sqr(double a) { return a * a; };
 
-static int	pad_pres_total;
-static int	pad_accel_total;
-static double	pad_lat_total;
-static double	pad_lon_total;
-static int	pad_alt_total;
-static int	npad;
-static int	prev_tick;
-static double	prev_accel;
-static double	velocity;
-static double	pad_lat;
-static double	pad_lon;
-static double	pad_alt;
+static void
+aoview_great_circle (double start_lat, double start_lon,
+		     double end_lat, double end_lon,
+		     double *dist, double *bearing)
+{
+	const double rad = M_PI / 180;
+	const double earth_radius = 6371.2 * 1000;	/* in meters */
+	double lat1 = rad * start_lat;
+	double lon1 = rad * -start_lon;
+	double lat2 = rad * end_lat;
+	double lon2 = rad * -end_lon;
 
-#define NUM_PAD_SAMPLES	50
+	double d_lat = lat2 - lat1;
+	double d_lon = lon2 - lon1;
 
+	/* From http://en.wikipedia.org/wiki/Great-circle_distance */
+	double vdn = sqrt(sqr(cos(lat2) * sin(d_lon)) +
+			  sqr(cos(lat1) * sin(lat2) -
+			      sin(lat1) * cos(lat2) * cos(d_lon)));
+	double vdd = sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(d_lon);
+	double d = atan2(vdn,vdd);
+	double course;
 
+	if (cos(lat1) < 1e-20) {
+		if (lat1 > 0)
+			course = M_PI;
+		else
+			course = -M_PI;
+	} else {
+		if (d < 1e-10)
+			course = 0;
+		else
+			course = acos((sin(lat2)-sin(lat1)*cos(d)) /
+				      (sin(d)*cos(lat1)));
+		if (sin(lon2-lon1) > 0)
+			course = 2 * M_PI-course;
+	}
+	*dist = d * earth_radius;
+	*bearing = course * 180/M_PI;
+}
 
 static void
-aoview_great_circle (double start_lat, double start_lon,
-		     double end_lat, double end_lon,
-		     double *dist, double *bearing)
+aoview_state_add_deg(int column, char *label, double deg, char pos, char neg)
 {
-	double rad = M_PI / 180;
-	double earth_radius = 6371.2;
-	double a = (90 - start_lat) * rad;
-	double b = (90 - end_lat) * rad;
-	double phi = (end_lon - start_lon) * rad;
-	double cosr = cos(a) * cos(b) + sin(a) * sin(b) * cos(phi);
-	double r = acos(cosr);
-	double rdist = earth_radius * r;
-	double sinth = sin(phi) * sin(b) / sin(r);
-	double th = asin(sinth) / rad;
-	*dist = rdist;
-	*bearing = th;
+	double	int_part;
+	double	min;
+	char	sign = pos;
+
+	if (deg < 0) {
+		deg = -deg;
+		sign = neg;
+	}
+	int_part = floor (deg);
+	min = (deg - int_part) * 60.0;
+	aoview_table_add_row(column, label, "%d°%lf'%c",
+			     (int) int_part, min, sign);
+
 }
 
-void
-aoview_state_notify(struct aostate *state)
+static char *ascent_states[] = {
+	"boost",
+	"fast",
+	"coast",
+	0,
+};
+
+static double
+aoview_time(void)
 {
-	int	altitude;
-	double	accel;
-	double	velocity_change;
-	int	ticks;
-	double	dist;
-	double	bearing;
-	double	temp;
-	double	battery;
-	double	drogue_sense, main_sense;
-
-	if (!strcmp(state->state, "pad")) {
-		if (npad < NUM_PAD_SAMPLES) {
-			pad_accel_total += state->accel;
-			pad_pres_total += state->pres;
-			pad_lat_total += state->lat;
-			pad_lon_total += state->lon;
-			pad_alt_total += state->alt;
-			npad++;
-			velocity = 0;
+	struct timespec	now;
+
+	clock_gettime(CLOCK_MONOTONIC, &now);
+	return (double) now.tv_sec + (double) now.tv_nsec / 1.0e9;
+}
+
+/*
+ * Fill out the derived data fields
+ */
+static void
+aoview_state_derive(struct aodata *data, struct aostate *state)
+{
+	int	i;
+	double	new_height;
+	double	height_change;
+	double	time_change;
+	int	tick_count;
+
+	state->report_time = aoview_time();
+
+	state->prev_data = state->data;
+	state->prev_npad = state->npad;
+	state->data = *data;
+	tick_count = data->tick;
+	if (tick_count < state->prev_data.tick)
+		tick_count += 65536;
+	time_change = (tick_count - state->prev_data.tick) / 100.0;
+
+	state->ground_altitude = aoview_pres_to_altitude(data->ground_pres);
+	new_height = aoview_pres_to_altitude(data->flight_pres) - state->ground_altitude;
+	height_change = new_height - state->height;
+	state->height = new_height;
+	if (time_change)
+		state->baro_speed = (state->baro_speed * 3 + (height_change / time_change)) / 4.0;
+	state->acceleration = (data->ground_accel - data->flight_accel) / 27.0;
+	state->speed = data->flight_vel / 2700.0;
+	state->temperature = ((data->temp / 32767.0 * 3.3) - 0.5) / 0.01;
+	state->drogue_sense = data->drogue / 32767.0 * 15.0;
+	state->main_sense = data->main / 32767.0 * 15.0;
+	state->battery = data->batt / 32767.0 * 5.0;
+	if (!strcmp(data->state, "pad")) {
+		if (data->gps.gps_locked && data->gps.nsat >= 4) {
+			state->npad++;
+			state->pad_lat_total += data->gps.lat;
+			state->pad_lon_total += data->gps.lon;
+			state->pad_alt_total += data->gps.alt;
+			if (state->npad > 1) {
+				state->pad_lat = (state->pad_lat * 31 + data->gps.lat) / 32.0;
+				state->pad_lon = (state->pad_lon * 31 + data->gps.lon) / 32.0;
+				state->pad_alt = (state->pad_alt * 31 + data->gps.alt) / 32.0;
+			} else {
+				state->pad_lat = data->gps.lat;
+				state->pad_lon = data->gps.lon;
+				state->pad_alt = data->gps.alt;
+			}
 		}
-		if (npad <= NUM_PAD_SAMPLES) {
-			pad_pres = pad_pres_total / npad;
-			pad_accel = pad_accel_total / npad;
-			pad_lat = pad_lat_total / npad;
-			pad_lon = pad_lon_total / npad;
-			pad_alt = pad_alt_total / npad;
+	}
+	state->ascent = FALSE;
+	for (i = 0; ascent_states[i]; i++)
+		if (!strcmp(data->state, ascent_states[i]))
+			state->ascent = TRUE;
+
+	/* Only look at accelerometer data on the way up */
+	if (state->ascent && state->acceleration > state->max_acceleration)
+		state->max_acceleration = state->acceleration;
+	if (state->ascent && state->speed > state->max_speed)
+		state->max_speed = state->speed;
+
+	if (state->height > state->max_height)
+		state->max_height = state->height;
+	state->gps.gps_locked = data->gps.gps_locked;
+	state->gps.gps_connected = data->gps.gps_connected;
+	if (data->gps.gps_locked) {
+		state->gps = data->gps;
+		state->gps_valid = 1;
+		if (state->npad)
+			aoview_great_circle(state->pad_lat, state->pad_lon, state->gps.lat, state->gps.lon,
+					    &state->distance, &state->bearing);
+	}
+	if (state->npad) {
+		state->gps_height = state->gps.alt - state->pad_alt;
+	} else {
+		state->gps_height = 0;
+	}
+}
+
+void
+aoview_speak_state(struct aostate *state)
+{
+	if (strcmp(state->data.state, state->prev_data.state)) {
+		aoview_voice_speak("%s\n", state->data.state);
+		if (!strcmp(state->data.state, "drogue"))
+			aoview_voice_speak("apogee %d meters\n",
+					   (int) state->max_height);
+		if (!strcmp(state->prev_data.state, "boost"))
+			aoview_voice_speak("max speed %d meters per second\n",
+					   (int) state->max_speed);
+	}
+	if (state->prev_npad < MIN_PAD_SAMPLES && state->npad >= MIN_PAD_SAMPLES)
+		aoview_voice_speak("g p s ready\n");
+}
+
+void
+aoview_speak_height(struct aostate *state)
+{
+	aoview_voice_speak("%d meters\n", state->height);
+}
+
+struct aostate aostate;
+
+static guint aostate_timeout;
+
+#define COMPASS_LIMIT(n)	((n * 22.5) + 22.5/2)
+
+static char *compass_points[] = {
+	"north",
+	"north north east",
+	"north east",
+	"east north east",
+	"east",
+	"east south east",
+	"south east",
+	"south south east",
+	"south",
+	"south south west",
+	"south west",
+	"west south west",
+	"west",
+	"west north west",
+	"north west",
+	"north north west",
+};
+
+static char *
+aoview_compass_point(double bearing)
+{
+	int	i;
+	while (bearing < 0)
+		bearing += 360.0;
+	while (bearing >= 360.0)
+		bearing -= 360.0;
+
+	i = floor ((bearing - 22.5/2) / 22.5 + 0.5);
+	if (i < 0) i = 0;
+	if (i >= sizeof (compass_points) / sizeof (compass_points[0]))
+		i = 0;
+	return compass_points[i];
+}
+
+static gboolean
+aoview_state_timeout(gpointer data)
+{
+	double	now = aoview_time();
+
+	if (strlen(aostate.data.state) > 0 && strcmp(aostate.data.state, "pad") != 0)
+		aoview_speak_height(&aostate);
+	if (now - aostate.report_time >= 20 || !strcmp(aostate.data.state, "landed")) {
+		if (!aostate.ascent) {
+			if (fabs(aostate.baro_speed) < 20 && aostate.height < 100)
+				aoview_voice_speak("rocket landed safely\n");
+			else
+				aoview_voice_speak("rocket may have crashed\n");
+			if (aostate.gps.gps_connected) {
+				aoview_voice_speak("rocket reported %s of pad distance %d meters\n",
+						   aoview_compass_point(aostate.bearing),
+						   (int) aostate.distance);
+			}
 		}
+		aostate_timeout = 0;
+		return FALSE;
 	}
-	altitude = aoview_pres_to_altitude(state->pres) - aoview_pres_to_altitude(pad_pres);
-	accel = (pad_accel - state->accel) / 264.8 *  9.80665;
-	velocity_change = (accel + prev_accel) / 2.0;
-	ticks = state->tick - prev_tick;
-	velocity -= velocity_change * (ticks / 100.0);
-	temp = ((state->temp / 32767.0 * 3.3) - 0.5) / 0.01;
-	battery = (state->batt / 32767.0 * 5.0);
-	drogue_sense = (state->drogue / 32767.0 * 15.0);
-	main_sense = (state->main / 32767.0 * 15.0);
-
-	prev_accel = accel;
-	prev_tick = state->tick;
+	return TRUE;
+}
+
+void
+aoview_state_reset(void)
+{
+	memset(&aostate, '\0', sizeof (aostate));
+}
+
+void
+aoview_state_notify(struct aodata *data)
+{
+	struct aostate *state = &aostate;
+	aoview_state_derive(data, state);
 	aoview_table_start();
-	aoview_table_add_row("RSSI", "%ddB", state->rssi);
-	aoview_table_add_row("Height", "%dm", altitude);
-	aoview_table_add_row("Acceleration", "%gm/s²", accel);
-	aoview_table_add_row("Velocity", "%gm/s", velocity);
-	aoview_table_add_row("Temperature", "%g°C", temp);
-	aoview_table_add_row("Battery", "%gV", battery);
-	aoview_table_add_row("Drogue", "%gV", drogue_sense);
-	aoview_table_add_row("Main", "%gV", main_sense);
-	aoview_table_add_row("Pad altitude", "%dm", aoview_pres_to_altitude(pad_pres));
-	aoview_table_add_row("Satellites", "%d", state->nsat);
-	if (state->locked) {
-		aoview_table_add_row("Lat", "%g", state->lat);
-		aoview_table_add_row("Lon", "%g", state->lon);
-		aoview_table_add_row("GPS alt", "%d", state->alt);
-		aoview_table_add_row("GPS time", "%02d:%02d:%02d",
-				     state->gps_time.hour,
-				     state->gps_time.minute,
-				     state->gps_time.second);
-		aoview_great_circle(pad_lat, pad_lon, state->lat, state->lon,
-				    &dist, &bearing);
-		aoview_table_add_row("Course", "%gkm %g°", dist, bearing);
+
+	if (state->npad >= MIN_PAD_SAMPLES)
+		aoview_table_add_row(0, "Ground state", "ready");
+	else
+		aoview_table_add_row(0, "Ground state", "waiting for gps (%d)",
+				     MIN_PAD_SAMPLES - state->npad);
+	aoview_table_add_row(0, "Rocket state", "%s", state->data.state);
+	aoview_table_add_row(0, "Callsign", "%s", state->data.callsign);
+	aoview_table_add_row(0, "Rocket serial", "%d", state->data.serial);
+
+	aoview_table_add_row(0, "RSSI", "%6ddBm", state->data.rssi);
+	aoview_table_add_row(0, "Height", "%6dm", state->height);
+	aoview_table_add_row(0, "Max height", "%6dm", state->max_height);
+	aoview_table_add_row(0, "Acceleration", "%7.1fm/s²", state->acceleration);
+	aoview_table_add_row(0, "Max acceleration", "%7.1fm/s²", state->max_acceleration);
+	aoview_table_add_row(0, "Speed", "%7.1fm/s", state->ascent ? state->speed : state->baro_speed);
+	aoview_table_add_row(0, "Max Speed", "%7.1fm/s", state->max_speed);
+	aoview_table_add_row(0, "Temperature", "%6.2f°C", state->temperature);
+	aoview_table_add_row(0, "Battery", "%5.2fV", state->battery);
+	aoview_table_add_row(0, "Drogue", "%5.2fV", state->drogue_sense);
+	aoview_table_add_row(0, "Main", "%5.2fV", state->main_sense);
+	aoview_table_add_row(0, "Pad altitude", "%dm", state->ground_altitude);
+	aoview_table_add_row(1, "Satellites", "%d", state->gps.nsat);
+	if (state->gps.gps_locked) {
+		aoview_table_add_row(1, "GPS", "locked");
+	} else if (state->gps.gps_connected) {
+		aoview_table_add_row(1, "GPS", "unlocked");
 	} else {
-		aoview_table_add_row("GPS", "unlocked");
+		aoview_table_add_row(1, "GPS", "not available");
+	}
+	if (state->gps_valid) {
+		aoview_state_add_deg(1, "Latitude", state->gps.lat, 'N', 'S');
+		aoview_state_add_deg(1, "Longitude", state->gps.lon, 'E', 'W');
+		aoview_table_add_row(1, "GPS height", "%d", state->gps_height);
+		aoview_table_add_row(1, "GPS time", "%02d:%02d:%02d",
+				     state->gps.gps_time.hour,
+				     state->gps.gps_time.minute,
+				     state->gps.gps_time.second);
+	}
+	if (state->gps.gps_extended) {
+		aoview_table_add_row(1, "GPS ground speed", "%7.1fm/s %d°",
+				     state->gps.ground_speed,
+				     state->gps.course);
+		aoview_table_add_row(1, "GPS climb rate", "%7.1fm/s",
+				     state->gps.climb_rate);
+		aoview_table_add_row(1, "GPS precision", "%4.1f(hdop) %3dm(h) %3dm(v)",
+				     state->gps.hdop, state->gps.h_error, state->gps.v_error);
+	}
+	if (state->npad) {
+		aoview_table_add_row(1, "Distance from pad", "%5.0fm", state->distance);
+		aoview_table_add_row(1, "Direction from pad", "%4.0f°", state->bearing);
+		aoview_state_add_deg(1, "Pad latitude", state->pad_lat, 'N', 'S');
+		aoview_state_add_deg(1, "Pad longitude", state->pad_lon, 'E', 'W');
+		aoview_table_add_row(1, "Pad GPS alt", "%gm", state->pad_alt);
 	}
 	aoview_table_finish();
+	aoview_label_show(state);
+	aoview_speak_state(state);
+	if (!aostate_timeout && strcmp(state->data.state, "pad") != 0)
+		aostate_timeout = g_timeout_add_seconds(10, aoview_state_timeout, NULL);
+}
+
+void
+aoview_state_new(void)
+{
 }
 
 void
 aoview_state_init(GladeXML *xml)
 {
+	aoview_state_new();
 }