git.gag.com Git - fw/altos/blob - ao-tools/lib/cc-telemetry.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 #include "cc.h"
  19 #include <string.h>
  20
  21 static int
  22 parse_byte(char *data, uint8_t *byte)
  23 {
  24         char    d[3];
  25         int x;
  26         d[0] = data[0];
  27         d[1] = data[1];
  28         d[2] = '\0';
  29
  30         if (sscanf(d, "%x", &x) != 1)
  31                 return FALSE;
  32         *byte = x;
  33         return TRUE;
  34 }
  35
  36 int
  37 cc_telemetry_parse(const char *input_line, union ao_telemetry_all *telemetry)
  38 {
  39         uint8_t *byte;
  40         char *data;
  41         uint8_t hex[35];
  42         int i;
  43
  44         if (strncmp(input_line, "TELEM", 5) != 0)
  45                 return FALSE;
  46
  47         data = strchr (input_line, ' ');
  48         if (!data)
  49                 return FALSE;
  50         data++;
  51         byte = hex;
  52         for (i = 0; i < 35; i++) {
  53                 if (!parse_byte(data, byte))
  54                         return FALSE;
  55                 data += 2;
  56                 byte++;
  57         }
  58         if (hex[0] != 34)
  59                 return FALSE;
  60         memcpy(telemetry, hex+1, 34);
  61         return TRUE;
  62 }
  63
  64 uint8_t
  65 cc_telemetry_cksum(const union ao_telemetry_all *telemetry)
  66 {
  67         const uint8_t   *x = (const uint8_t *) telemetry;
  68         int i;
  69         uint8_t sum = 0x5a;
  70         for (i = 0; i < 34; i++)
  71                 sum += x[i];
  72         return sum;
  73 }
  74
  75 void
  76 cc_telemetry_unparse(const union ao_telemetry_all *telemetry, char output_line[CC_TELEMETRY_BUFSIZE])
  77 {
  78         uint8_t hex[36];
  79         int i;
  80         int p;
  81
  82         hex[0] = 34;
  83         memcpy(hex+1, telemetry, 34);
  84         hex[35] = cc_telemetry_cksum(telemetry);
  85         strcpy(output_line, "TELEM ");
  86         p = strlen(output_line);
  87         for (i = 0; i < 36; i++) {
  88                 sprintf(output_line + p, "%02x", hex[i]);
  89                 p += 2;
  90         }
  91 }
  92