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