X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=aoview%2Faoview_state.c;h=cf1594cd8c143297fe8ab587d8f8d8313fc880e2;hp=356828c65a3b0bc1e58e063974e11bbe0e6a10a8;hb=31d5670a9144b943ce9c8cb00deb5fb659af0b1c;hpb=778cae8fc5a4b30e5045e4703316fc61ae18562a

diff --git a/aoview/aoview_state.c b/aoview/aoview_state.c
index 356828c6..cf1594cd 100644
--- a/aoview/aoview_state.c
+++ b/aoview/aoview_state.c
@@ -18,45 +18,51 @@
 #include "aoview.h"
 #include <math.h>
 
-static double	pad_lat_total;
-static double	pad_lon_total;
-static int	pad_alt_total;
-static int	npad_gps;
-static int	prev_tick;
-static double	prev_accel;
-static double	pad_lat;
-static double	pad_lon;
-static double	pad_alt;
-static double	min_pres;
-static double	min_accel;
-
-#define NUM_PAD_SAMPLES	10
+static inline double sqr(double a) { return a * a; };
 
 static void
 aoview_great_circle (double start_lat, double start_lon,
 		     double end_lat, double end_lon,
 		     double *dist, double *bearing)
 {
-	double rad = M_PI / 180;
-	double earth_radius = 6371.2;
+	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 lon1 = rad * -start_lon;
 	double lat2 = rad * end_lat;
-	double lon2 = -rad * end_lon;
+	double lon2 = rad * -end_lon;
 
-	double d = acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2));
-	double argacos = (sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1));
-	double crs;
-	if (sin(lon2-lon1) < 0)
-		crs = acos(argacos);
-	else
-		crs = 2 * M_PI - acos(argacos);
+	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 = crs * 180/M_PI;
+	*bearing = course * 180/M_PI;
 }
 
 static void
-aoview_state_add_deg(char *label, double deg, char pos, char neg)
+aoview_state_add_deg(int column, char *label, double deg, char pos, char neg)
 {
 	double	int_part;
 	double	min;
@@ -68,118 +74,263 @@ aoview_state_add_deg(char *label, double deg, char pos, char neg)
 	}
 	int_part = floor (deg);
 	min = (deg - int_part) * 60.0;
-	aoview_table_add_row(label, "%d°%lf'%c",
+	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)
+{
+	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	altitude;
-	double	accel;
-	int	ticks;
-	double	dist;
-	double	bearing;
-	double	temp;
-	double	velocity;
-	double	battery;
-	double	drogue_sense, main_sense;
-	double	max_accel;
-
-	if (!strcmp(state->state, "pad")) {
-		if (state->locked && npad_gps < NUM_PAD_SAMPLES) {
-			pad_lat_total += state->lat;
-			pad_lon_total += state->lon;
-			pad_alt_total += state->alt;
-			npad_gps++;
+	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->locked && data->nsat >= 4) {
+			state->npad++;
+			state->pad_lat_total += data->lat;
+			state->pad_lon_total += data->lon;
+			state->pad_alt_total += data->alt;
+			if (state->npad > 1) {
+				state->pad_lat = (state->pad_lat * 31 + data->lat) / 32.0;
+				state->pad_lon = (state->pad_lon * 31 + data->lon) / 32.0;
+				state->pad_alt = (state->pad_alt * 31 + data->alt) / 32.0;
+			} else {
+				state->pad_lat = data->lat;
+				state->pad_lon = data->lon;
+				state->pad_alt = data->alt;
+			}
 		}
-		if (state->locked && npad_gps <= NUM_PAD_SAMPLES) {
-			pad_lat = pad_lat_total / npad_gps;
-			pad_lon = pad_lon_total / npad_gps;
-			pad_alt = pad_alt_total / npad_gps;
+	}
+	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;
+	if (data->locked) {
+		state->lat = data->lat;
+		state->lon = data->lon;
+		aoview_great_circle(state->pad_lat, state->pad_lon, data->lat, data->lon,
+				    &state->distance, &state->bearing);
+		state->gps_valid = 1;
+	}
+	if (state->npad) {
+		state->gps_height = data->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_valid) {
+				aoview_voice_speak("rocket reported %s of pad distance %d meters\n",
+						   aoview_compass_point(aostate.bearing),
+						   (int) aostate.distance);
+			}
 		}
-		min_pres = state->ground_pres;
-		min_accel = state->ground_accel;
+		aostate_timeout = 0;
+		return FALSE;
 	}
-	if (state->flight_pres < min_pres)
-		min_pres = state->flight_pres;
-	if (state->flight_accel < min_accel)
-		min_accel = state->flight_accel;
-	altitude = aoview_pres_to_altitude(state->flight_pres) - aoview_pres_to_altitude(state->ground_pres);
-	accel = (state->ground_accel - state->flight_accel) / 27.0;
-	velocity = state->flight_vel / 2700.0;
-	max_accel = (state->ground_accel - min_accel) / 27.0;
-	ticks = state->tick - prev_tick;
-	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();
 
-	if (npad_gps >= NUM_PAD_SAMPLES)
-		aoview_table_add_row("Ground state", "ready");
+	if (state->npad >= MIN_PAD_SAMPLES)
+		aoview_table_add_row(0, "Ground state", "ready");
 	else
-		aoview_table_add_row("Ground state", "waiting for gps (%d)",
-				     NUM_PAD_SAMPLES - npad_gps);
-	aoview_table_add_row("Rocket state", "%s", state->state);
-	aoview_table_add_row("Callsign", "%s", state->callsign);
-	aoview_table_add_row("Rocket serial", "%d", state->serial);
-
-	aoview_table_add_row("RSSI", "%ddBm", state->rssi);
-	aoview_table_add_row("Height", "%dm", altitude);
-	aoview_table_add_row("Max height", "%dm",
-			     aoview_pres_to_altitude(min_pres) -
-			     aoview_pres_to_altitude(state->ground_pres));
-	aoview_table_add_row("Acceleration", "%gm/s²", accel);
-	aoview_table_add_row("Max acceleration", "%gm/s²", max_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(state->ground_pres));
-	aoview_table_add_row("Satellites", "%d", state->nsat);
-	if (state->locked) {
-		aoview_state_add_deg("Latitude", state->lat, 'N', 'S');
-		aoview_state_add_deg("Longitude", state->lon, 'E', 'W');
-		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("Distance from pad", "%gm", dist * 1000);
-		aoview_table_add_row("Direction from pad", "%g°", bearing);
+		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->data.nsat);
+	if (state->data.locked) {
+		aoview_state_add_deg(1, "Latitude", state->data.lat, 'N', 'S');
+		aoview_state_add_deg(1, "Longitude", state->data.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->data.gps_time.hour,
+				     state->data.gps_time.minute,
+				     state->data.gps_time.second);
+		aoview_table_add_row(1, "GPS ground speed", "%7.1fm/s %d°",
+				     state->data.ground_speed,
+				     state->data.course);
+		aoview_table_add_row(1, "GPS climb rate", "%7.1fm/s",
+				     state->data.climb_rate);
+		aoview_table_add_row(1, "GPS precision", "%f(hdop) %dm(h) %dm(v)\n",
+				     state->data.hdop, state->data.h_error, state->data.v_error);
+		aoview_table_add_row(1, "Distance from pad", "%5.0fm", state->distance);
+		aoview_table_add_row(1, "Direction from pad", "%4.0f°", state->bearing);
 	} else {
-		aoview_table_add_row("GPS", "unlocked");
+		aoview_table_add_row(1, "GPS", "unlocked");
 	}
-	if (npad_gps) {
-		aoview_state_add_deg("Pad latitude", pad_lat, 'N', 'S');
-		aoview_state_add_deg("Pad longitude", pad_lon, 'E', 'W');
-		aoview_table_add_row("Pad GPS alt", "%gm", pad_alt);
+	if (state->npad) {
+		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)
 {
-	pad_lat_total = 0;
-	pad_lon_total = 0;
-	pad_alt_total = 0;
-	npad_gps = 0;
-	prev_tick = 0;
-	prev_accel = 0;
-	pad_lat = 0;
-	pad_lon = 0;
-	pad_alt = 0;
-	min_pres = 32767;
-	min_accel = 32767;
 }
 
 void