From: Keith Packard Date: Sun, 20 Sep 2009 20:33:26 +0000 (-0700) Subject: Add --gps option to ao-postflight X-Git-Tag: debian/0.5+77+gc57bd7f~6^2~1 X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=7ea371a09385e2a93199f78685e8cb86793ed104 Add --gps option to ao-postflight --- diff --git a/ao-tools/ao-postflight/ao-postflight.c b/ao-tools/ao-postflight/ao-postflight.c index 1e6ac744..6418521e 100644 --- a/ao-tools/ao-postflight/ao-postflight.c +++ b/ao-tools/ao-postflight/ao-postflight.c @@ -162,7 +162,7 @@ merge_data(struct cc_perioddata *first, struct cc_perioddata *last, double split } static void -analyse_flight(struct cc_flightraw *f, FILE *summary_file, FILE *detail_file, FILE *raw_file, char *plot_name) +analyse_flight(struct cc_flightraw *f, FILE *summary_file, FILE *detail_file, FILE *raw_file, char *plot_name, FILE *gps_file) { double height; double accel; @@ -312,6 +312,17 @@ analyse_flight(struct cc_flightraw *f, FILE *summary_file, FILE *detail_file, FI time, pres, accel); } } + if (gps_file) { + fprintf(gps_file, "%9s %12s %12s %12s\n", + "time", "lat", "lon", "alt"); + for (i = 0; i < f->gps.num; i++) { + fprintf(gps_file, "%12.7f %12.7f %12.7f %12.7f\n", + (f->gps.data[i].time - boost_start) / 100.0, + f->gps.data[i].lat, + f->gps.data[i].lon, + f->gps.data[i].alt); + } + } if (cooked && plot_name) { struct cc_perioddata *speed; plsdev("svgcairo"); @@ -350,6 +361,7 @@ static const struct option options[] = { { .name = "detail", .has_arg = 1, .val = 'd' }, { .name = "plot", .has_arg = 1, .val = 'p' }, { .name = "raw", .has_arg = 1, .val = 'r' }, + { .name = "gps", .has_arg = 1, .val = 'g' }, { 0, 0, 0, 0}, }; @@ -360,6 +372,7 @@ static void usage(char *program) "\t[--detail=]\n" "\t[--raw= -r -p ]\n" + "\t[--gps= -g ]\n" "\t{flight-log} ...\n", program); exit(1); } @@ -371,6 +384,7 @@ main (int argc, char **argv) FILE *summary_file = NULL; FILE *detail_file = NULL; FILE *raw_file = NULL; + FILE *gps_file = NULL; int i; int ret = 0; struct cc_flightraw *raw; @@ -381,8 +395,9 @@ main (int argc, char **argv) char *detail_name = NULL; char *raw_name = NULL; char *plot_name = NULL; + char *gps_name = NULL; - while ((c = getopt_long(argc, argv, "s:d:p:r:", options, NULL)) != -1) { + while ((c = getopt_long(argc, argv, "s:d:p:r:g:", options, NULL)) != -1) { switch (c) { case 's': summary_name = optarg; @@ -396,6 +411,9 @@ main (int argc, char **argv) case 'r': raw_name = optarg; break; + case 'g': + gps_name = optarg; + break; default: usage(argv[0]); break; @@ -427,6 +445,13 @@ main (int argc, char **argv) exit(1); } } + if (gps_name) { + gps_file = fopen(gps_name, "w"); + if (!gps_file) { + perror(gps_name); + exit(1); + } + } for (i = optind; i < argc; i++) { file = fopen(argv[i], "r"); if (!file) { @@ -447,7 +472,7 @@ main (int argc, char **argv) } if (!raw->serial) raw->serial = serial; - analyse_flight(raw, summary_file, detail_file, raw_file, plot_name); + analyse_flight(raw, summary_file, detail_file, raw_file, plot_name, gps_file); cc_flightraw_free(raw); } return ret; diff --git a/ao-tools/lib/cc-logfile.c b/ao-tools/lib/cc-logfile.c index 4abf7eb6..2136eec4 100644 --- a/ao-tools/lib/cc-logfile.c +++ b/ao-tools/lib/cc-logfile.c @@ -80,6 +80,44 @@ gpsdata_add(struct cc_gpsdata *data, struct cc_gpselt *elt) return 1; } +static int +gpssat_add(struct cc_gpsdata *data, struct cc_gpssat *sat) +{ + int i, j; + int reuse = 0; + int newsizesats; + struct cc_gpssats *newsats; + + for (i = data->numsats; --i >= 0;) { + if (data->sats[i].sat[0].time == sat->time) { + reuse = 1; + break; + } + if (data->sats[i].sat[0].time < sat->time) + break; + } + if (!reuse) { + if (data->numsats == data->sizesats) { + if (data->sizesats == 0) + newsats = malloc((newsizesats = 256) * sizeof (struct cc_gpssats)); + else + newsats = realloc (data->data, (newsizesats = data->sizesats * 2) + * sizeof (struct cc_gpssats)); + if (!newsats) + return 0; + data->sats = newsats; + } + i = data->numsats++; + data->sats[i].nsat = 0; + } + j = data->sats[i].nsat; + if (j < 12) { + data->sats[i].sat[j] = *sat; + data->sats[i].nsat = j + 1; + } + return 1; +} + static void gpsdata_free(struct cc_gpsdata *data) { @@ -100,13 +138,20 @@ gpsdata_free(struct cc_gpsdata *data) #define AO_LOG_POS_NONE (~0UL) +#define GPS_TIME 1 +#define GPS_LAT 2 +#define GPS_LON 4 +#define GPS_ALT 8 + static int read_eeprom(const char *line, struct cc_flightraw *f, double *ground_pres, int *ground_pres_count) { char type; int tick; int a, b; - struct cc_gpselt gps; + static struct cc_gpselt gps; + static int gps_valid; + struct cc_gpssat sat; int serial; if (sscanf(line, "serial-number %u", &serial) == 1) { @@ -145,23 +190,38 @@ read_eeprom(const char *line, struct cc_flightraw *f, double *ground_pres, int * timedata_add(&f->state, tick, a); break; case AO_LOG_GPS_TIME: + /* the flight computer writes TIME first, so reset + * any stale data before adding this record + */ gps.time = tick; + gps_valid = GPS_TIME; break; case AO_LOG_GPS_LAT: gps.lat = ((int32_t) (a + (b << 16))) / 10000000.0; + gps_valid |= GPS_LAT; break; case AO_LOG_GPS_LON: gps.lon = ((int32_t) (a + (b << 16))) / 10000000.0; + gps_valid |= GPS_LON; break; case AO_LOG_GPS_ALT: - gps.alt = ((int32_t) (a + (b << 16))); - gpsdata_add(&f->gps, &gps); + gps.alt = (int16_t) a; + gps_valid |= GPS_ALT; break; case AO_LOG_GPS_SAT: + sat.time = tick; + sat.svid = a; + sat.state = (b & 0xff); + sat.c_n = (b >> 8) & 0xff; + gpssat_add(&f->gps, &sat); break; default: return 0; } + if (gps_valid == 0xf) { + gps_valid = 0; + gpsdata_add(&f->gps, &gps); + } return 1; } diff --git a/ao-tools/lib/cc.h b/ao-tools/lib/cc.h index 01226958..b5f1132f 100644 --- a/ao-tools/lib/cc.h +++ b/ao-tools/lib/cc.h @@ -19,6 +19,7 @@ #define _CC_H_ #include +#include char * cc_fullname (char *dir, char *file); @@ -90,11 +91,35 @@ struct cc_gpselt { double alt; }; +#define SIRF_SAT_STATE_ACQUIRED (1 << 0) +#define SIRF_SAT_STATE_CARRIER_PHASE_VALID (1 << 1) +#define SIRF_SAT_BIT_SYNC_COMPLETE (1 << 2) +#define SIRF_SAT_SUBFRAME_SYNC_COMPLETE (1 << 3) +#define SIRF_SAT_CARRIER_PULLIN_COMPLETE (1 << 4) +#define SIRF_SAT_CODE_LOCKED (1 << 5) +#define SIRF_SAT_ACQUISITION_FAILED (1 << 6) +#define SIRF_SAT_EPHEMERIS_AVAILABLE (1 << 7) + +struct cc_gpssat { + double time; + uint16_t svid; + uint8_t state; + uint8_t c_n; +}; + +struct cc_gpssats { + int nsat; + struct cc_gpssat sat[12]; +}; + struct cc_gpsdata { int num; int size; struct cc_gpselt *data; double time_offset; + int numsats; + int sizesats; + struct cc_gpssats *sats; }; /*