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

   1 /*
   2  * Copyright © 2011 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 #include "cc-usb.h"
  27
  28 static const struct option options[] = {
  29         { .name = "tty", .has_arg = 1, .val = 'T' },
  30         { .name = "device", .has_arg = 1, .val = 'D' },
  31         { .name = "frequency", .has_arg = 1, .val = 'F' },
  32         { .name = "realtime", .has_arg = 0, .val = 'R' },
  33         { .name = "verbose", .has_arg = 0, .val = 'v' },
  34         { .name = "fake", .has_arg = 0, .val = 'f' },
  35         { 0, 0, 0, 0},
  36 };
  37
  38 static void usage(char *program)
  39 {
  40         fprintf(stderr, "usage: %s [--tty <tty-name>] [--device <device-name>] [--frequency <kHz>] [--realtime] [--verbose] [--fake] file.telem ...\n", program);
  41         exit(1);
  42 }
  43
  44 #define bool(b) ((b) ? "true" : "false")
  45
  46 struct ao_telem_list {
  47         struct ao_telem_list    *next;
  48         union ao_telemetry_all  telem;
  49 };
  50
  51 static struct ao_telem_list     *telem_list, **telem_last;
  52
  53 static void
  54 trim_telem(uint16_t time)
  55 {
  56         while (telem_list && (int16_t) (time - telem_list->telem.generic.tick) > 0) {
  57                 struct ao_telem_list    *next = telem_list->next;
  58                 free(telem_list);
  59                 telem_list = next;
  60         }
  61         if (!telem_list)
  62                 telem_last = &telem_list;
  63 }
  64
  65 static void
  66 add_telem(union ao_telemetry_all *telem)
  67 {
  68         struct ao_telem_list    *new = malloc (sizeof (struct ao_telem_list));
  69         trim_telem((uint16_t) (telem->generic.tick - 20 * 100));
  70         new->telem = *telem;
  71         new->next = 0;
  72         *telem_last = new;
  73         telem_last = &new->next;
  74 }
  75
  76 static enum ao_flight_state     cur_state = ao_flight_invalid;
  77 static enum ao_flight_state     last_state = ao_flight_invalid;
  78
  79 static enum ao_flight_state
  80 packet_state(union ao_telemetry_all *telem)
  81 {
  82         switch (telem->generic.type) {
  83         case AO_TELEMETRY_SENSOR_TELEMETRUM:
  84         case AO_TELEMETRY_SENSOR_TELEMINI:
  85         case AO_TELEMETRY_SENSOR_TELENANO:
  86                 cur_state = telem->sensor.state;
  87                 break;
  88         case AO_TELEMETRY_MEGA_DATA:
  89                 cur_state = telem->mega_data.state;
  90                 break;
  91         case AO_TELEMETRY_METRUM_SENSOR:
  92                 cur_state = telem->metrum_sensor.state;
  93                 break;
  94         case AO_TELEMETRY_MINI:
  95                 cur_state = telem->mini.state;
  96                 break;
  97         }
  98         return cur_state;
  99 }
 100
 101 static const char *state_names[] = {
 102         "startup",
 103         "idle",
 104         "pad",
 105         "boost",
 106         "fast",
 107         "coast",
 108         "drogue",
 109         "main",
 110         "landed",
 111         "invalid"
 112 };
 113
 114 static void
 115 send_telem(struct cc_usb *cc, union ao_telemetry_all *telem)
 116 {
 117         int     i;
 118         uint8_t *b;
 119
 120         packet_state(telem);
 121         if (cur_state != last_state) {
 122                 if (0 <= cur_state && cur_state < sizeof(state_names) / sizeof (state_names[0]))
 123                         printf ("%s\n", state_names[cur_state]);
 124                 last_state = cur_state;
 125         }
 126         cc_usb_printf(cc, "S 20\n");
 127         b = (uint8_t *) telem;
 128         for (i = 0; i < 0x20; i++)
 129                 cc_usb_printf(cc, "%02x", b[i]);
 130         cc_usb_sync(cc);
 131 }
 132
 133 static void
 134 do_delay(uint16_t now, uint16_t then)
 135 {
 136         int16_t delay = (int16_t) (now - then);
 137
 138         if (delay > 0 && delay < 1000)
 139                 usleep(delay * 10 * 1000);
 140 }
 141
 142 static uint16_t
 143 send_queued(struct cc_usb *cc, int pause)
 144 {
 145         struct ao_telem_list    *next;
 146         uint16_t                tick = 0;
 147         int                     started = 0;
 148
 149         while (telem_list) {
 150                 if (started && pause)
 151                         do_delay(telem_list->telem.generic.tick, tick);
 152                 tick = telem_list->telem.generic.tick;
 153                 started = 1;
 154                 send_telem(cc, &telem_list->telem);
 155
 156                 next = telem_list->next;
 157                 free(telem_list);
 158                 telem_list = next;
 159         }
 160         return tick;
 161 }
 162
 163 int
 164 main (int argc, char **argv)
 165 {
 166         struct cc_usb   *cc;
 167         char            *tty = NULL;
 168         char            *device = NULL;
 169         char            line[80];
 170         int             c, i, ret = 0;
 171         int             freq = 434550;
 172         FILE            *file;
 173         uint16_t        last_tick;
 174         int             started;
 175         int             realtime = 0;
 176         int             verbose = 0;
 177         int             fake = 0;
 178         int             rate = 0;
 179
 180         while ((c = getopt_long(argc, argv, "vRfT:D:F:r:", options, NULL)) != -1) {
 181                 switch (c) {
 182                 case 'T':
 183                         tty = optarg;
 184                         break;
 185                 case 'D':
 186                         device = optarg;
 187                         break;
 188                 case 'F':
 189                         freq = atoi(optarg);
 190                         break;
 191                 case 'R':
 192                         realtime = 1;
 193                         break;
 194                 case 'v':
 195                         verbose++;
 196                         break;
 197                 case 'f':
 198                         fake++;
 199                         break;
 200                 case 'r':
 201                         rate = atoi(optarg);
 202                         switch (rate) {
 203                         case 38400:
 204                                 rate = 0;
 205                                 break;
 206                         case 9600:
 207                                 rate = 1;
 208                                 break;
 209                         case 2400:
 210                                 rate = 2;
 211                                 break;
 212                         default:
 213                                 fprintf(stderr, "Rate %d isn't 38400, 9600 or 2400\n", rate);
 214                                 usage(argv[0]);
 215                                 break;
 216                         }
 217                         break;
 218                 default:
 219                         usage(argv[0]);
 220                         break;
 221                 }
 222         }
 223         if (!tty)
 224                 tty = cc_usbdevs_find_by_arg(device, "TeleDongle");
 225         if (!tty)
 226                 tty = getenv("ALTOS_TTY");
 227         if (!tty)
 228                 tty="/dev/ttyACM0";
 229         cc = cc_usb_open(tty);
 230         if (!cc)
 231                 exit (1);
 232
 233         cc_usb_printf(cc, "m 0\n");
 234         cc_usb_printf(cc, "c F %d\n", freq);
 235         cc_usb_printf(cc, "c T %d\n", rate);
 236
 237         if (fake) {
 238                 union ao_telemetry_all  telem;
 239                 int                     i;
 240
 241                 memset(&telem, '\0', sizeof (telem));
 242                 telem.generic.serial = 1;
 243                 telem.generic.type = 0;
 244                 for (i = 0; i < sizeof (telem.generic.payload); i++)
 245                         telem.generic.payload[i] = i & 7;
 246                 for (;;) {
 247                         telem.generic.tick += 50;
 248                         send_telem(cc, &telem);
 249                         do_delay(50, 0);
 250                 }
 251         } else {
 252                 for (i = optind; i < argc; i++) {
 253                         file = fopen(argv[i], "r");
 254                         if (!file) {
 255                                 perror(argv[i]);
 256                                 ret++;
 257                                 continue;
 258                         }
 259                         started = 0;
 260                         last_tick = 0;
 261                         while (fgets(line, sizeof (line), file)) {
 262                                 union ao_telemetry_all telem;
 263
 264                                 if (cc_telemetry_parse(line, &telem)) {
 265                                         /*
 266                                          * Skip packets with CRC errors.
 267                                          */
 268                                         if ((telem.generic.status & (1 << 7)) == 0)
 269                                                 continue;
 270
 271                                         if (verbose)
 272                                                 printf ("type %4d\n", telem.generic.type);
 273
 274                                         if (started || realtime) {
 275                                                 do_delay(telem.generic.tick, last_tick);
 276                                                 last_tick = telem.generic.tick;
 277                                                 send_telem(cc, &telem);
 278                                         } else {
 279                                                 enum ao_flight_state state = packet_state(&telem);
 280                                                 printf ("\tstate %4d\n", state);
 281                                                 add_telem(&telem);
 282                                                 if (ao_flight_pad < state && state < ao_flight_landed) {
 283                                                         printf ("started\n");
 284                                                         started = 1;
 285                                                         last_tick = send_queued(cc, realtime);
 286                                                 }
 287                                         }
 288                                 }
 289                         }
 290                         fclose (file);
 291
 292                 }
 293         }
 294         return ret;
 295 }