git.gag.com Git - fw/altos/blob - ao-tools/ao-edit-telem/ao-edit-telem.c

   1 /*
   2  * Copyright © 2013 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #define _GNU_SOURCE
  19 #include <string.h>
  20 #include <stdio.h>
  21 #include <stdlib.h>
  22 #include <unistd.h>
  23 #include <getopt.h>
  24 #include "cc.h"
  25
  26 static const struct option options[] = {
  27         { .name = "lat", .has_arg = 1, .val = 'L' },
  28         { .name = "lon", .has_arg = 1, .val = 'l' },
  29         { 0, 0, 0, 0},
  30 };
  31
  32 static void usage(char *program)
  33 {
  34         fprintf(stderr, "usage: %s [--lat <pad-lat>] [--lon <pad-lon>]\n"
  35                 "\t{flight-log} ...\n", program);
  36         exit(1);
  37 }
  38
  39 #define bool(b) ((b) ? "true" : "false")
  40
  41 struct telem_ent {
  42         struct telem_ent        *next;
  43         union ao_telemetry_all  telem;
  44 };
  45
  46 static struct telem_ent *pad, **last = &pad;
  47
  48 static void
  49 save_telem(union ao_telemetry_all *telem)
  50 {
  51         struct telem_ent *t = malloc (sizeof *t);
  52         t->telem = *telem;
  53         t->next = NULL;
  54         *last = t;
  55         last = &t->next;
  56 }
  57
  58 static void
  59 dump_telem(union ao_telemetry_all *telem)
  60 {
  61         char s[CC_TELEMETRY_BUFSIZE];
  62
  63         cc_telemetry_unparse(telem, s);
  64         printf("%s\n", s);
  65 }
  66
  67 double  pad_lat = 0, pad_lon = 0;
  68 double  target_pad_lat = 0, target_pad_lon = 0;
  69 double  lat_off = 0, lon_off = 0;
  70 int pending = 1;
  71
  72 static void
  73 dump_saved(void);
  74
  75 void
  76 doit(union ao_telemetry_all *telem)
  77 {
  78         double lat, lon;
  79
  80         switch (telem->generic.type) {
  81         case AO_TELEMETRY_SENSOR_TELEMETRUM:
  82         case AO_TELEMETRY_SENSOR_TELEMINI:
  83         case AO_TELEMETRY_SENSOR_TELENANO:
  84                 if (telem->sensor.state > ao_flight_pad && pad) {
  85                         pending = 0;
  86                         if (target_pad_lat)
  87                                 lat_off = target_pad_lat - pad_lat;
  88                         if (target_pad_lon)
  89                                 lon_off = target_pad_lon - pad_lon;
  90                         dump_saved();
  91                 }
  92                 break;
  93         case AO_TELEMETRY_LOCATION: {
  94                 lat = telem->location.latitude / 1.0e7;
  95                 lon = telem->location.longitude / 1.0e7;
  96                 if (pending) {
  97                         if (telem->location.flags & (1 << 4)) {
  98                                 if (pad_lat) {
  99                                         pad_lat = pad_lat - pad_lat / 32 + lat / 32.0;
 100                                         pad_lon = pad_lon - pad_lon / 32 + lon / 32.0;
 101                                 } else {
 102                                         pad_lat = lat;
 103                                         pad_lon = lon;
 104                                 }
 105                         }
 106                 } else {
 107                         lat += lat_off;
 108                         lon += lon_off;
 109                         if (lat > 90)
 110                                 lat = 90;
 111                         if (lat < -90)
 112                                 lat = -90;
 113                         while (lon > 180)
 114                                 lon -= 360;
 115                         while (lon < -180)
 116                                 lon += 360;
 117                         telem->location.latitude = lat * 1.0e7;
 118                         telem->location.longitude = lon * 1.0e7;
 119                 }
 120                 break;
 121         }
 122         }
 123 }
 124
 125 static void
 126 dump_saved(void)
 127 {
 128         struct telem_ent        *t, *n;
 129
 130         for (t = pad; t; t = n) {
 131                 n = t->next;
 132                 doit(&t->telem);
 133                 dump_telem(&t->telem);
 134                 free(t);
 135         }
 136         pad = NULL;
 137         last = &pad;
 138 }
 139
 140 int
 141 main (int argc, char **argv)
 142 {
 143         char    line[80];
 144         int c, i, ret;
 145         char *s;
 146         FILE *file;
 147         int serial;
 148         while ((c = getopt_long(argc, argv, "l:L:", options, NULL)) != -1) {
 149                 switch (c) {
 150                 case 'L':
 151                         target_pad_lat = strtod(optarg, NULL);
 152                         break;
 153                 case 'l':
 154                         target_pad_lon = strtod(optarg, NULL);
 155                         break;
 156                 default:
 157                         usage(argv[0]);
 158                         break;
 159                 }
 160         }
 161         for (i = optind; i < argc; i++) {
 162                 file = fopen(argv[i], "r");
 163                 if (!file) {
 164                         perror(argv[i]);
 165                         ret++;
 166                         continue;
 167                 }
 168                 s = strstr(argv[i], "-serial-");
 169                 if (s)
 170                         serial = atoi(s + 8);
 171                 else
 172                         serial = 0;
 173                 while (fgets(line, sizeof (line), file)) {
 174                         union ao_telemetry_all telem;
 175
 176                         if (cc_telemetry_parse(line, &telem)) {
 177                                 if ((telem.generic.status & (1 << 7)) == 0) {
 178                                         dump_telem(&telem);
 179                                         continue;
 180                                 }
 181                                 doit (&telem);
 182                                 if (pending)
 183                                         save_telem(&telem);
 184                                 else
 185                                         dump_telem(&telem);
 186                         }
 187                 }
 188                 fclose (file);
 189
 190         }
 191         return ret;
 192 }