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     rssi = (int8_t) telem->generic.rssi / 2 - 74;
 118         int     i;
 119         uint8_t *b;
 120
 121         packet_state(telem);
 122         if (cur_state != last_state) {
 123                 if (0 <= cur_state && cur_state < sizeof(state_names) / sizeof (state_names[0]))
 124                         printf ("%s\n", state_names[cur_state]);
 125                 last_state = cur_state;
 126         }
 127         cc_usb_printf(cc, "S 20\n");
 128         b = (uint8_t *) telem;
 129         for (i = 0; i < 0x20; i++)
 130                 cc_usb_printf(cc, "%02x", b[i]);
 131         cc_usb_sync(cc);
 132 }
 133
 134 static void
 135 do_delay(uint16_t now, uint16_t then)
 136 {
 137         int16_t delay = (int16_t) (now - then);
 138
 139         if (delay > 0 && delay < 1000)
 140                 usleep(delay * 10 * 1000);
 141 }
 142
 143 static uint16_t
 144 send_queued(struct cc_usb *cc, int pause)
 145 {
 146         struct ao_telem_list    *next;
 147         uint16_t                tick = 0;
 148         int                     started = 0;
 149
 150         while (telem_list) {
 151                 if (started && pause)
 152                         do_delay(telem_list->telem.generic.tick, tick);
 153                 tick = telem_list->telem.generic.tick;
 154                 started = 1;
 155                 send_telem(cc, &telem_list->telem);
 156
 157                 next = telem_list->next;
 158                 free(telem_list);
 159                 telem_list = next;
 160         }
 161         return tick;
 162 }
 163
 164 int
 165 main (int argc, char **argv)
 166 {
 167         struct cc_usb   *cc;
 168         char            *tty = NULL;
 169         char            *device = NULL;
 170         char            line[80];
 171         int             c, i, ret = 0;
 172         int             freq = 434550;
 173         char            *s;
 174         FILE            *file;
 175         int             serial;
 176         uint16_t        last_tick;
 177         int             started;
 178         int             realtime = 0;
 179         int             verbose = 0;
 180         int             fake = 0;
 181         int             rate = 0;
 182
 183         while ((c = getopt_long(argc, argv, "vRfT:D:F:r:", options, NULL)) != -1) {
 184                 switch (c) {
 185                 case 'T':
 186                         tty = optarg;
 187                         break;
 188                 case 'D':
 189                         device = optarg;
 190                         break;
 191                 case 'F':
 192                         freq = atoi(optarg);
 193                         break;
 194                 case 'R':
 195                         realtime = 1;
 196                         break;
 197                 case 'v':
 198                         verbose++;
 199                         break;
 200                 case 'f':
 201                         fake++;
 202                         break;
 203                 case 'r':
 204                         rate = atoi(optarg);
 205                         switch (rate) {
 206                         case 38400:
 207                                 rate = 0;
 208                                 break;
 209                         case 9600:
 210                                 rate = 1;
 211                                 break;
 212                         case 2400:
 213                                 rate = 2;
 214                                 break;
 215                         default:
 216                                 fprintf(stderr, "Rate %d isn't 38400, 9600 or 2400\n", rate);
 217                                 usage(argv[0]);
 218                                 break;
 219                         }
 220                         break;
 221                 default:
 222                         usage(argv[0]);
 223                         break;
 224                 }
 225         }
 226         if (!tty)
 227                 tty = cc_usbdevs_find_by_arg(device, "TeleDongle");
 228         if (!tty)
 229                 tty = getenv("ALTOS_TTY");
 230         if (!tty)
 231                 tty="/dev/ttyACM0";
 232         cc = cc_usb_open(tty);
 233         if (!cc)
 234                 exit (1);
 235
 236         cc_usb_printf(cc, "m 0\n");
 237         cc_usb_printf(cc, "c F %d\n", freq);
 238         cc_usb_printf(cc, "c T %d\n", rate);
 239
 240         if (fake) {
 241                 union ao_telemetry_all  telem;
 242                 uint16_t                tick;
 243                 int                     i;
 244
 245                 memset(&telem, '\0', sizeof (telem));
 246                 telem.generic.serial = 1;
 247                 telem.generic.type = 0;
 248                 for (i = 0; i < sizeof (telem.generic.payload); i++)
 249                         telem.generic.payload[i] = i & 7;
 250                 for (;;) {
 251                         telem.generic.tick += 50;
 252                         send_telem(cc, &telem);
 253                         do_delay(50, 0);
 254                 }
 255         } else {
 256                 for (i = optind; i < argc; i++) {
 257                         file = fopen(argv[i], "r");
 258                         if (!file) {
 259                                 perror(argv[i]);
 260                                 ret++;
 261                                 continue;
 262                         }
 263                         started = 0;
 264                         last_tick = 0;
 265                         while (fgets(line, sizeof (line), file)) {
 266                                 union ao_telemetry_all telem;
 267
 268                                 if (cc_telemetry_parse(line, &telem)) {
 269                                         /*
 270                                          * Skip packets with CRC errors.
 271                                          */
 272                                         if ((telem.generic.status & (1 << 7)) == 0)
 273                                                 continue;
 274
 275                                         if (verbose)
 276                                                 printf ("type %4d\n", telem.generic.type);
 277
 278                                         if (started || realtime) {
 279                                                 do_delay(telem.generic.tick, last_tick);
 280                                                 last_tick = telem.generic.tick;
 281                                                 send_telem(cc, &telem);
 282                                         } else {
 283                                                 enum ao_flight_state state = packet_state(&telem);
 284                                                 printf ("\tstate %4d\n", state);
 285                                                 add_telem(&telem);
 286                                                 if (ao_flight_pad < state && state < ao_flight_landed) {
 287                                                         printf ("started\n");
 288                                                         started = 1;
 289                                                         last_tick = send_queued(cc, realtime);
 290                                                 }
 291                                         }
 292                                 }
 293                         }
 294                         fclose (file);
 295
 296                 }
 297         }
 298         return ret;
 299 }