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