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         }
  89         return cur_state;
  90 }
  91
  92 static const char *state_names[] = {
  93         "startup",
  94         "idle",
  95         "pad",
  96         "boost",
  97         "fast",
  98         "coast",
  99         "drogue",
 100         "main",
 101         "landed",
 102         "invalid"
 103 };
 104
 105 static void
 106 send_telem(struct cc_usb *cc, union ao_telemetry_all *telem)
 107 {
 108         int     rssi = (int8_t) telem->generic.rssi / 2 - 74;
 109         int     i;
 110         uint8_t *b;
 111
 112         packet_state(telem);
 113         if (cur_state != last_state) {
 114                 if (0 <= cur_state && cur_state < sizeof(state_names) / sizeof (state_names[0]))
 115                         printf ("%s\n", state_names[cur_state]);
 116                 last_state = cur_state;
 117         }
 118         cc_usb_printf(cc, "S 20\n");
 119         b = (uint8_t *) telem;
 120         for (i = 0; i < 0x20; i++)
 121                 cc_usb_printf(cc, "%02x", b[i]);
 122         cc_usb_sync(cc);
 123 }
 124
 125 static void
 126 do_delay(uint16_t now, uint16_t then)
 127 {
 128         int16_t delay = (int16_t) (now - then);
 129
 130         if (delay > 0 && delay < 1000)
 131                 usleep(delay * 10 * 1000);
 132 }
 133
 134 static uint16_t
 135 send_queued(struct cc_usb *cc, int pause)
 136 {
 137         struct ao_telem_list    *next;
 138         uint16_t                tick = 0;
 139         int                     started = 0;
 140
 141         while (telem_list) {
 142                 if (started && pause)
 143                         do_delay(telem_list->telem.generic.tick, tick);
 144                 tick = telem_list->telem.generic.tick;
 145                 started = 1;
 146                 send_telem(cc, &telem_list->telem);
 147
 148                 next = telem_list->next;
 149                 free(telem_list);
 150                 telem_list = next;
 151         }
 152         return tick;
 153 }
 154
 155 int
 156 main (int argc, char **argv)
 157 {
 158         struct cc_usb   *cc;
 159         char            *tty = NULL;
 160         char            *device = NULL;
 161         char            line[80];
 162         int             c, i, ret = 0;
 163         int             freq = 434550;
 164         char            *s;
 165         FILE            *file;
 166         int             serial;
 167         uint16_t        last_tick;
 168         int             started;
 169         int             realtime = 0;
 170
 171
 172         while ((c = getopt_long(argc, argv, "RT:D:F:", options, NULL)) != -1) {
 173                 switch (c) {
 174                 case 'T':
 175                         tty = optarg;
 176                         break;
 177                 case 'D':
 178                         device = optarg;
 179                         break;
 180                 case 'F':
 181                         freq = atoi(optarg);
 182                         break;
 183                 case 'R':
 184                         realtime = 1;
 185                         break;
 186                 default:
 187                         usage(argv[0]);
 188                         break;
 189                 }
 190         }
 191         if (!tty)
 192                 tty = cc_usbdevs_find_by_arg(device, "TeleDongle");
 193         if (!tty)
 194                 tty = getenv("ALTOS_TTY");
 195         if (!tty)
 196                 tty="/dev/ttyACM0";
 197         cc = cc_usb_open(tty);
 198         if (!cc)
 199                 exit (1);
 200
 201         cc_usb_printf(cc, "m 0\n");
 202         cc_usb_printf(cc, "F %d\n", freq);
 203         for (i = optind; i < argc; i++) {
 204                 file = fopen(argv[i], "r");
 205                 if (!file) {
 206                         perror(argv[i]);
 207                         ret++;
 208                         continue;
 209                 }
 210                 started = 0;
 211                 last_tick = 0;
 212                 while (fgets(line, sizeof (line), file)) {
 213                         union ao_telemetry_all telem;
 214
 215                         if (cc_telemetry_parse(line, &telem)) {
 216                                 /*
 217                                  * Skip packets with CRC errors.
 218                                  */
 219                                 if ((telem.generic.status & (1 << 7)) == 0)
 220                                         continue;
 221
 222                                 if (started) {
 223                                         do_delay(telem.generic.tick, last_tick);
 224                                         last_tick = telem.generic.tick;
 225                                         send_telem(cc, &telem);
 226                                 } else {
 227                                         add_telem(&telem);
 228                                         if (packet_state(&telem) > ao_flight_pad) {
 229                                                 started = 1;
 230                                                 last_tick = send_queued(cc, realtime);
 231                                         }
 232                                 }
 233                         }
 234                 }
 235                 fclose (file);
 236
 237         }
 238         return ret;
 239 }