src/main/tracker/pitlab.c

   1 #include "config.h"
   2 #include "telemetry.h"
   3
   4 int32_t Restore_long(int idx);
   5 int16_t Restore_short(int idx);
   6 uint8_t Restore_byte(int idx);
   7 void pitlab_encodeTargetData(uint8_t c);
   8 void preProcessHexString(void);
   9 void processPitlabFrame(void);
  10 uint8_t hex2int(uint8_t *a, uint8_t len);
  11 /*
  12 Telemetry packet on USART port is always 10 ASCII characters as follow:
  13 '$' //sync
  14 ('A'+ N) //packet numer 'N': 'A'=0, 'B'=1 'C'=2 etc
  15 32 bit data in HEX format, MSB first
  16
  17 Example:
  18 $CA9FGDB15
  19 $-start of packet
  20 C- packet nr 2
  21 A9 - byte3
  22 FG - byte2
  23 DB - byte1
  24 15 - byte0
  25 */
  26
  27 int32_t gps_lat;
  28 int32_t gps_lon;
  29
  30
  31 //uint8_t type is single, unsigned byte (unsigned char)
  32 uint8_t lsRxData[5]; //bufor na kolejne bajty odczytane z komunikatu (dan Hex zamienione na bajty)
  33 uint8_t hexString[8];
  34
  35 enum PitlabDataState {
  36     IDLE,
  37     STATE_START1,
  38     STATE_START2,
  39     STATE_DATA
  40   };
  41
  42 static uint8_t dataState = IDLE;
  43 uint8_t dataIdx=0;
  44
  45 int32_t Restore_long(int idx)
  46 {
  47   return (int32_t)lsRxData[idx] + ((int32_t)lsRxData[idx+1] << 8) + ((int32_t)lsRxData[idx+2] << 16) + ((int32_t)lsRxData[idx+3] << 24);
  48 }
  49
  50 int16_t Restore_short(int idx)
  51 {
  52   return (int16_t)lsRxData[idx] + ((int16_t)lsRxData[idx+1]  << 8);
  53 }
  54
  55 uint8_t Restore_byte(int idx)
  56 {
  57   return lsRxData[idx];
  58 }
  59
  60
  61 void pitlab_encodeTargetData(uint8_t c) {
  62
  63         if (dataState == IDLE && c == '$') {
  64                 dataIdx = 0;
  65                 dataState = STATE_START1;
  66                 return;
  67         } else if (dataState == STATE_START1) {
  68                 lsRxData[0] = c - 'A';
  69                 dataState=STATE_START2;
  70                 return;
  71         } else if (dataState == STATE_START2) {
  72                 hexString[dataIdx++] = c;
  73                 if(dataIdx == 8)
  74                         dataState = STATE_DATA;
  75         }
  76         if (dataState == STATE_DATA){
  77                 preProcessHexString();
  78                 processPitlabFrame();
  79                 dataState = IDLE;
  80         }
  81 }
  82
  83 void preProcessHexString(void){
  84         uint8_t str_buffer[2];
  85         uint8_t sIdx = 0;
  86         for(uint8_t i = 1; i < 5; i++){
  87                 for(uint8_t j = 0; j < 2; ++j){
  88                         str_buffer[j] = hexString[sIdx++];
  89                 }
  90                 lsRxData[5-i] = hex2int(str_buffer,2);
  91         }
  92 }
  93
  94 void processPitlabFrame(void){
  95         switch(lsRxData[0])
  96         {
  97         case 0: // A
  98                 telemetry_sats = (uint16_t)Restore_byte(4);
  99                 break;
 100         case 10: // 10 = K, pos 3 : Absolute altitude, 1 = B, pos 2 : Relative altitude
 101                 telemetry_alt = (int16_t)Restore_short(3);
 102                 gotAlt = true;
 103                 break;
 104         case 2: // C
 105                 gps_lon = Restore_long(1);
 106                 telemetry_lon = (int32_t)(round(((double)gps_lon * 100.0)/60.0));
 107                 break;
 108         case 3: // D
 109                 gps_lat = Restore_long(1);
 110                 telemetry_lat = (int32_t)(round(((double)gps_lat * 100.0)/60.0));
 111                 if(telemetry_sats >= 5) gotFix = true;
 112                 break;
 113         }
 114 }
 115
 116 uint8_t hex2int(uint8_t *a, uint8_t len)
 117 {
 118    uint8_t i;
 119    int val = 0;
 120
 121    for(i=0;i<len;i++) {
 122       if(a[i] <= 57)
 123        val += (a[i]-48)*(1<<(4*(len-1-i)));
 124       else
 125        val += (a[i]-55)*(1<<(4*(len-1-i)));
 126    }
 127    return val;
 128 }