src/main/tracker/frskyd.c

   1 /*
   2  * This file is part of u360gts, aka amv-open360tracker 32bits:
   3  * https://github.com/raul-ortega/amv-open360tracker-32bits
   4  *
   5  * The code below is an adaptation by Raúl Ortega of the original code written by Samuel Brucksch:
   6  * https://github.com/SamuelBrucksch/open360tracker
   7  *
   8  * u360gts is free software: you can redistribute it and/or modify
   9  * it under the terms of the GNU General Public License as published by
  10  * the Free Software Foundation, either version 3 of the License, or
  11  * (at your option) any later version.
  12  *
  13  * u360gts is distributed in the hope that it will be useful,
  14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16  * GNU General Public License for more details.
  17  *
  18  * You should have received a copy of the GNU General Public License
  19  * along with u360gts.  If not, see <http://www.gnu.org/licenses/>.
  20  *
  21  */
  22
  23 #include "config.h"
  24 #include "telemetry.h"
  25
  26 void processFrskyPacket(uint8_t *packet);
  27 void parseTelemHubByte(uint8_t c);
  28 int32_t gpsToLong(int8_t neg, uint16_t bp, uint16_t ap);
  29
  30 // Enumerate FrSky packet codes
  31 #define LINKPKT         0xfe
  32 #define USRPKT          0xfd
  33 #define A11PKT          0xfc
  34 #define A12PKT          0xfb
  35 #define A21PKT          0xfa
  36 #define A22PKT          0xf9
  37 #define RSSI1PKT        0xf7
  38 #define RSSI2PKT        0xf6
  39
  40 #define START_STOP      0x7E
  41 #define BYTESTUFF       0x7d
  42 #define STUFF_MASK      0x20
  43
  44 #define GPS_ALT_BP_ID     0x01
  45 #define GPS_ALT_AP_ID     0x09
  46 #define BARO_ALT_BP_ID    0x10
  47 #define BARO_ALT_AP_ID    0x21
  48 #define GPS_LON_BP_ID     0x12
  49 #define GPS_LAT_BP_ID     0x13
  50 #define GPS_LON_AP_ID     0x1A
  51 #define GPS_LAT_AP_ID     0x1B
  52 #define GPS_LON_EW_ID     0x22
  53 #define GPS_LAT_NS_ID     0x23
  54 #define TEMP2             0x05
  55
  56 #define FRSKY_RX_PACKET_SIZE 11
  57 uint8_t frskyRxBuffer[FRSKY_RX_PACKET_SIZE];
  58
  59 #define FRSKYD_LATLON_DIVIDER 100000
  60
  61 enum FrSkyDataState {
  62   STATE_DATA_IDLE, STATE_DATA_START, STATE_DATA_IN_FRAME, STATE_DATA_XOR,
  63 };
  64
  65 static uint8_t dataState = STATE_DATA_IDLE;
  66 static uint8_t numPktBytes = 0;
  67
  68 int16_t alt;
  69 uint16_t NS;
  70 uint16_t EW;
  71 uint8_t sats;
  72 uint8_t fix;
  73
  74 uint16_t lat_bp;
  75 uint16_t lon_bp;
  76 uint16_t lat_ap;
  77 uint16_t lon_ap;
  78
  79 int32_t prevTargetLat = 0;
  80 int32_t prevTargetLon = 0;
  81 uint16_t prevLatBp=0;
  82 uint16_t LatBp=0;
  83 uint16_t prevLonBp=0;
  84 uint16_t LonBp=0;
  85 int32_t prevAlt = 0;
  86
  87 uint8_t numBytes;
  88
  89
  90 void frskyd_setLat(void) {
  91   int32_t value = gpsToLong(NS == 'N' ? 1 : -1, lat_bp, lat_ap);
  92   if(abs(value)>180*FRSKYD_LATLON_DIVIDER) {
  93     value=prevTargetLat;
  94   }
  95   else {
  96     prevLatBp = (int16_t)(prevTargetLat/FRSKYD_LATLON_DIVIDER);
  97     LatBp = (int16_t)(value/FRSKYD_LATLON_DIVIDER);
  98     if(prevTargetLat > value && (prevLatBp-LatBp)>2 && prevTargetLat>FRSKYD_LATLON_DIVIDER) value=prevTargetLat;
  99     if(prevTargetLat < value && (LatBp-prevLatBp)>2 && prevTargetLat>FRSKYD_LATLON_DIVIDER) value=prevTargetLat;
 100     prevTargetLat=value;
 101   }
 102
 103   NS = 0;
 104   telemetry_lat = value*10;
 105 }
 106
 107 void frskyd_setLon(void) {
 108
 109   int32_t value = gpsToLong(EW == 'E' ? 1 : -1, lon_bp, lon_ap);
 110   if(abs(value)>180*FRSKYD_LATLON_DIVIDER) {
 111       value=prevTargetLon;
 112     }
 113   else {
 114     prevLonBp = (int16_t)(prevTargetLon/FRSKYD_LATLON_DIVIDER);
 115     LonBp=(int16_t)(value/FRSKYD_LATLON_DIVIDER);
 116     if(prevTargetLon > value && (prevLonBp-LonBp)>2 && prevTargetLon>FRSKYD_LATLON_DIVIDER) value=prevTargetLon;
 117     if(prevTargetLon < value && (LonBp-prevLonBp)>2 && prevTargetLon>FRSKYD_LATLON_DIVIDER) value=prevTargetLon;
 118     prevTargetLon=value;
 119   }
 120   EW = 0;
 121   telemetry_lon=value*10;
 122 }
 123
 124 void frskyd_setAlt() {
 125   if(((prevAlt-alt)>500 || (alt-prevAlt)>500)  && prevAlt > 0 && alt > 0 ) alt=prevAlt;
 126   prevAlt=alt;
 127   telemetry_alt = alt;
 128 }
 129
 130 void frskyd_setSats() {
 131   telemetry_sats = sats;
 132 }
 133
 134 void frskyd_encodeTargetData(uint8_t c) {
 135   switch (dataState) {
 136   case STATE_DATA_START:
 137     if (c == START_STOP)
 138       break; // Remain in userDataStart if possible 0x7e,0x7e doublet found.
 139
 140     frskyRxBuffer[numPktBytes++] = c;
 141     dataState = STATE_DATA_IN_FRAME;
 142     break;
 143   case STATE_DATA_IN_FRAME:
 144     if (c == BYTESTUFF) {
 145       dataState = STATE_DATA_XOR; // XOR next byte
 146       break;
 147     }
 148     if (c == START_STOP) // end of frame detected
 149     {
 150       processFrskyPacket(frskyRxBuffer); // FrskyRxBufferReady = 1;
 151       dataState = STATE_DATA_IDLE;
 152       break;
 153     }
 154     if (numPktBytes < FRSKY_RX_PACKET_SIZE)
 155       frskyRxBuffer[numPktBytes++] = c;
 156     else{
 157       dataState = STATE_DATA_IDLE;
 158       break;
 159     }
 160     break;
 161   case STATE_DATA_XOR:
 162     if (numPktBytes < FRSKY_RX_PACKET_SIZE)
 163       frskyRxBuffer[numPktBytes++] = c ^ STUFF_MASK;
 164     else{
 165       dataState = STATE_DATA_IDLE;
 166       break;
 167     }
 168     dataState = STATE_DATA_IN_FRAME;
 169     break;
 170   case STATE_DATA_IDLE:
 171     if (c == START_STOP) {
 172       numPktBytes = 0;
 173       dataState = STATE_DATA_START;
 174     }
 175     break;
 176   }
 177 }
 178
 179 void processFrskyPacket(uint8_t *packet) {
 180         //get package ID
 181         switch (packet[0]) {
 182                 case LINKPKT: // A1/A2/RSSI values
 183                         //maybe we can use RSSI here to start an alarm when RSSI level gets low
 184                         break;
 185                 case USRPKT:
 186                         numBytes = packet[1];
 187                         if (numBytes > 6)
 188                           return;
 189                         for (uint8_t i = 3; i < numBytes+3; i++) {
 190                                 /*if(packet[3]=='$')
 191                                         ltm_encodeTargetData(packet[i]);
 192                                 else*/
 193                                         parseTelemHubByte(packet[i]);
 194                         }
 195                 break;
 196         }
 197 }
 198
 199 typedef enum {
 200   IDLE = 0, DATA_ID, DATA_LOW, DATA_HIGH, STUFF = 0x80
 201 } STATE;
 202
 203 void parseTelemHubByte(uint8_t c) {
 204   static uint8_t dataId;
 205   static uint8_t byte0;
 206   static STATE state = IDLE;
 207
 208   if (c == 0x5e) {
 209     state = DATA_ID;
 210     return;
 211   }
 212   if (state == IDLE) {
 213     return;
 214   }
 215   if (state & STUFF) {
 216     c = c ^ 0x60;
 217     state = (STATE) (state - STUFF);
 218   }
 219   if (c == 0x5d) {
 220     state = (STATE) (state | STUFF);
 221     return;
 222   }
 223   if (state == DATA_ID) {
 224     if (c > 0x3f) {
 225       state = IDLE;
 226     } else {
 227       dataId = c;
 228       state = DATA_LOW;
 229     }
 230     return;
 231   }
 232   if (state == DATA_LOW) {
 233     byte0 = c;
 234     state = DATA_HIGH;
 235     return;
 236   }
 237   state = IDLE;
 238
 239   switch (dataId) {
 240   case GPS_ALT_BP_ID:
 241     alt = (int16_t)((c << 8) + byte0);
 242     gotAlt = true;
 243     break;
 244   case BARO_ALT_BP_ID:
 245     alt = (int16_t)((c << 8) + byte0);
 246     gotAlt = true;
 247     break;
 248   case GPS_LON_BP_ID:
 249     lon_bp = (c << 8) + byte0;
 250     break;
 251   case GPS_LON_AP_ID:
 252     lon_ap = (c << 8) + byte0;
 253     break;
 254   case GPS_LAT_BP_ID:
 255     lat_bp = (c << 8) + byte0;
 256     break;
 257   case GPS_LAT_AP_ID:
 258     lat_ap = (c << 8) + byte0;
 259     break;
 260   case GPS_LON_EW_ID:
 261     EW = byte0;
 262     break;
 263   case GPS_LAT_NS_ID:
 264     NS = byte0;
 265     break;
 266   case TEMP2:
 267         if(telemetry_provider==1){
 268                 sats = byte0 / 10;
 269                 fix = byte0 % 10;
 270         } else if(telemetry_provider==2){
 271                 sats = byte0 % 100;
 272         } else
 273                 sats = byte0;
 274     break;
 275   }
 276         if ((NS == 'N' || NS == 'S') && (EW == 'E' || EW == 'W')) {
 277
 278                 frskyd_setLat();
 279                 frskyd_setLon();
 280                 frskyd_setAlt();
 281                 frskyd_setSats();
 282                 gotFix = true;
 283         } else
 284                 telemetry_failed_cs++;
 285 }
 286