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