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