X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=aoview%2Faoview_state.c;h=d5e978b68833a856651fd0a999f9c7c9506d4997;hp=efd49042e840b559b9e99dd7082340a044c02b5e;hb=80cadf44f5f1accd6ddfca25c2af8d4d424f26d9;hpb=09771c644de54ae354e8f98af7ba74289b3c0fcc

diff --git a/aoview/aoview_state.c b/aoview/aoview_state.c
index efd49042..d5e978b6 100644
--- a/aoview/aoview_state.c
+++ b/aoview/aoview_state.c
@@ -18,94 +18,314 @@
 #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(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(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;
-	double	velocity_change;
-	int	ticks;
-	double	dist;
-	double	bearing;
-
-	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;
+	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->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;
+			state->pad_lat = state->pad_lat_total / state->npad;
+			state->pad_lon = state->pad_lon_total / state->npad;
+			state->pad_alt = state->pad_alt_total / state->npad;
 		}
-		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;
+	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);
+	}
+}
+
+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);
+			}
 		}
+		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);
-
-	prev_accel = accel;
-	prev_tick = state->tick;
-	printf ("Pad altitude: %dm\n", aoview_pres_to_altitude(pad_pres));
-	printf ("AGL: %dm\n", altitude);
-	printf ("Acceleration: %gm/s²\n", accel);
-	printf ("Velocity: %gm/s\n", velocity);
-	printf ("Lat: %g\n", state->lat);
-	printf ("Lon: %g\n", state->lon);
-	printf ("GPS alt: %d\n", state->alt);
-	aoview_great_circle(pad_lat, pad_lon, state->lat, state->lon,
-			    &dist, &bearing);
-	printf ("Course: %gkm %g°\n", dist, bearing);
+	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 (state->npad >= MIN_PAD_SAMPLES)
+		aoview_table_add_row("Ground state", "ready");
+	else
+		aoview_table_add_row("Ground state", "waiting for gps (%d)",
+				     MIN_PAD_SAMPLES - state->npad);
+	aoview_table_add_row("Rocket state", "%s", state->data.state);
+	aoview_table_add_row("Callsign", "%s", state->data.callsign);
+	aoview_table_add_row("Rocket serial", "%d", state->data.serial);
+
+	aoview_table_add_row("RSSI", "%6ddBm", state->data.rssi);
+	aoview_table_add_row("Height", "%6dm", state->height);
+	aoview_table_add_row("Max height", "%6dm", state->max_height);
+	aoview_table_add_row("Acceleration", "%7.1fm/s²", state->acceleration);
+	aoview_table_add_row("Max acceleration", "%7.1fm/s²", state->max_acceleration);
+	aoview_table_add_row("Speed", "%7.1fm/s", state->ascent ? state->speed : state->baro_speed);
+	aoview_table_add_row("Max Speed", "%7.1fm/s", state->max_speed);
+	aoview_table_add_row("Temperature", "%6.2f°C", state->temperature);
+	aoview_table_add_row("Battery", "%5.2fV", state->battery);
+	aoview_table_add_row("Drogue", "%5.2fV", state->drogue_sense);
+	aoview_table_add_row("Main", "%5.2fV", state->main_sense);
+	aoview_table_add_row("Pad altitude", "%dm", state->ground_altitude);
+	aoview_table_add_row("Satellites", "%d", state->data.nsat);
+	if (state->data.locked) {
+		aoview_state_add_deg("Latitude", state->data.lat, 'N', 'S');
+		aoview_state_add_deg("Longitude", state->data.lon, 'E', 'W');
+		aoview_table_add_row("GPS height", "%d", state->gps_height);
+		aoview_table_add_row("GPS time", "%02d:%02d:%02d",
+				     state->data.gps_time.hour,
+				     state->data.gps_time.minute,
+				     state->data.gps_time.second);
+		aoview_table_add_row("GPS ground speed", "%7.1fm/s %d°",
+				     state->data.ground_speed,
+				     state->data.course);
+		aoview_table_add_row("GPS climb rate", "%7.1fm/s",
+				     state->data.climb_rate);
+		aoview_table_add_row("GPS precision", "%f(hdop) %dm(h) %dm(v)\n",
+				     state->data.hdop, state->data.h_error, state->data.v_error);
+		aoview_table_add_row("Distance from pad", "%5.0fm", state->distance);
+		aoview_table_add_row("Direction from pad", "%4.0f°", state->bearing);
+	} else {
+		aoview_table_add_row("GPS", "unlocked");
+	}
+	if (state->npad) {
+		aoview_state_add_deg("Pad latitude", state->pad_lat, 'N', 'S');
+		aoview_state_add_deg("Pad longitude", state->pad_lon, 'E', 'W');
+		aoview_table_add_row("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();
 }