X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=aoview%2Faoview_state.c;h=4ba1854e0237fc3584786aa2793f6901b957c6a3;hp=046ccc92ea5d982d77de2472b01c6493f74e7508;hb=2e06772c8b6fd74f86e640ed97f0d5bc8c095c2f;hpb=be3f4fed7b863c8cdaabe32b61b65a8b3cd11355 diff --git a/aoview/aoview_state.c b/aoview/aoview_state.c index 046ccc92..4ba1854e 100644 --- a/aoview/aoview_state.c +++ b/aoview/aoview_state.c @@ -18,117 +18,226 @@ #include "aoview.h" #include -static int pad_pres; -static int pad_accel; +static inline double sqr(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, +}; + +/* + * Fill out the derived data fields + */ +static void +aoview_state_derive(struct aostate *state) { - int altitude; - double accel; - double velocity_change; - int ticks; - double dist; - double bearing; - double temp; - double battery; - double drogue_sense, main_sense; + int i; + state->ground_altitude = aoview_pres_to_altitude(state->ground_pres); + state->height = aoview_pres_to_altitude(state->flight_pres) - state->ground_altitude; + state->acceleration = (state->ground_accel - state->flight_accel) / 27.0; + state->speed = state->flight_vel / 2700.0; + state->temperature = ((state->temp / 32767.0 * 3.3) - 0.5) / 0.01; + state->drogue_sense = state->drogue / 32767.0 * 15.0; + state->main_sense = state->main / 32767.0 * 15.0; + state->battery = state->batt / 32767.0 * 5.0; 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; - } - 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; + if (state->locked) { + state->npad++; + state->pad_lat_total += state->lat; + state->pad_lon_total += state->lon; + state->pad_alt_total += state->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; } } - 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; + state->ascent = FALSE; + for (i = 0; ascent_states[i]; i++) + if (!strcmp(state->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; + aoview_great_circle(state->pad_lat, state->pad_lon, state->lat, state->lon, + &state->distance, &state->bearing); +} + +void +aoview_state_speak(struct aostate *state) +{ + static char last_state[32]; + int i; + gboolean report = FALSE; + int this_tick; + static int last_tick; + static int last_altitude; + int this_altitude; + + if (strcmp(state->state, last_state)) { + aoview_voice_speak("%s\n", state->state); + if (!strcmp(state->state, "drogue")) + aoview_voice_speak("apogee %d meters\n", + (int) state->max_height); + report = TRUE; + strcpy(last_state, state->state); + } + this_altitude = aoview_pres_to_altitude(state->flight_pres) - aoview_pres_to_altitude(state->ground_pres); + this_tick = state->tick; + while (this_tick < last_tick) + this_tick += 65536; + if (strcmp(state->state, "pad") != 0) { + if (this_altitude / 1000 != last_altitude / 1000) + report = TRUE; + if (this_tick - last_tick >= 10 * 100) + report = TRUE; + } + if (report) { + aoview_voice_speak("%d meters\n", + this_altitude); + if (state->ascent) + aoview_voice_speak("%d meters per second\n", + state->flight_vel / 2700); + last_tick = state->tick; + last_altitude = this_altitude; + printf ("report at tick %d height %d\n", + state->tick, this_altitude); + } +} + +void +aoview_state_notify(struct aostate *state) +{ + aoview_state_derive(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)); + + 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->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", state->height); + aoview_table_add_row("Max height", "%dm", state->max_height); + aoview_table_add_row("Acceleration", "%gm/s²", state->acceleration); + aoview_table_add_row("Max acceleration", "%gm/s²", state->max_acceleration); + aoview_table_add_row("Speed", "%gm/s", state->speed); + aoview_table_add_row("Max Speed", "%gm/s", state->max_speed); + aoview_table_add_row("Temperature", "%g°C", state->temperature); + aoview_table_add_row("Battery", "%gV", state->battery); + aoview_table_add_row("Drogue", "%gV", state->drogue_sense); + aoview_table_add_row("Main", "%gV", state->main_sense); + aoview_table_add_row("Pad altitude", "%dm", state->ground_altitude); 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_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("Course", "%gkm %g°", dist, bearing); + aoview_table_add_row("GPS ground speed", "%fm/s %d°", + state->ground_speed, + state->course); + aoview_table_add_row("GPS climb rate", "%fm/s", + state->climb_rate); + aoview_table_add_row("GPS precision", "%f(hdop) %dm(h) %dm(v)\n", + state->hdop, state->h_error, state->v_error); + aoview_table_add_row("Distance from pad", "%gm", state->distance); + aoview_table_add_row("Direction from pad", "%g°", 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_state_speak(state); +} + +void +aoview_state_new(void) +{ } void aoview_state_init(GladeXML *xml) { + aoview_state_new(); + aoview_voice_speak("initializing rocket flight monitoring system\n"); }