X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=aoview%2Faoview_state.c;h=02ae0307c249ac9cb7c736de7ff649a1edf576e6;hp=046ccc92ea5d982d77de2472b01c6493f74e7508;hb=91b07410122d0eaaf292cdb31c200925d45eaf2c;hpb=be3f4fed7b863c8cdaabe32b61b65a8b3cd11355 diff --git a/aoview/aoview_state.c b/aoview/aoview_state.c index 046ccc92..02ae0307 100644 --- a/aoview/aoview_state.c +++ b/aoview/aoview_state.c @@ -18,25 +18,19 @@ #include "aoview.h" #include -static int pad_pres; -static int pad_accel; - -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 npad_gps; static int prev_tick; static double prev_accel; -static double velocity; static double pad_lat; static double pad_lon; static double pad_alt; +static double min_pres; +static double min_accel; -#define NUM_PAD_SAMPLES 50 - - +#define NUM_PAD_SAMPLES 10 static void aoview_great_circle (double start_lat, double start_lon, @@ -45,16 +39,33 @@ aoview_great_circle (double start_lat, double start_lon, { 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 lat1 = rad * start_lat; + double lon1 = -rad * start_lon; + double lat2 = rad * end_lat; + 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); + *dist = d * earth_radius; + *bearing = crs * 180/M_PI; +} + +static void +aoview_state_add_deg(char *label, double deg) +{ + double int_part; + double min; + + int_part = floor (deg); + min = (deg - int_part) * 60.0; + aoview_table_add_row(label, "%d°%lf'", + (int) int_part, min); + } void @@ -62,37 +73,39 @@ aoview_state_notify(struct aostate *state) { int altitude; double accel; - double velocity_change; 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 (npad < NUM_PAD_SAMPLES) { - pad_accel_total += state->accel; - pad_pres_total += state->pres; + if (state->locked && npad_gps < NUM_PAD_SAMPLES) { pad_lat_total += state->lat; pad_lon_total += state->lon; pad_alt_total += state->alt; - npad++; - velocity = 0; + npad_gps++; } - 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 && 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; } + min_pres = state->ground_pres; + min_accel = state->ground_accel; } - 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; + 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; - 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); @@ -101,19 +114,33 @@ aoview_state_notify(struct aostate *state) prev_accel = accel; prev_tick = state->tick; aoview_table_start(); - aoview_table_add_row("RSSI", "%ddB", state->rssi); + + if (npad_gps >= NUM_PAD_SAMPLES) + aoview_table_add_row("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(pad_pres)); + 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_table_add_row("Lat", "%g", state->lat); - aoview_table_add_row("Lon", "%g", state->lon); + aoview_state_add_deg("Latitude", state->lat); + aoview_state_add_deg("Longitude", state->lon); aoview_table_add_row("GPS alt", "%d", state->alt); aoview_table_add_row("GPS time", "%02d:%02d:%02d", state->gps_time.hour, @@ -121,14 +148,37 @@ aoview_state_notify(struct aostate *state) 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("Distance from pad", "%gm", dist * 1000); + aoview_table_add_row("Direction from pad", "%g°", bearing); } else { aoview_table_add_row("GPS", "unlocked"); } + if (npad_gps) { + aoview_state_add_deg("Pad latitude", pad_lat); + aoview_state_add_deg("Pad longitude", pad_lon); + aoview_table_add_row("Pad GPS alt", "%gm", pad_alt); + } aoview_table_finish(); } +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 aoview_state_init(GladeXML *xml) { + aoview_state_new(); }