src/main/tracker/frskyx.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 coordToLong(int8_t neg, uint16_t bp, uint16_t ap);
  29
  30 #define START_STOP         0x7e
  31 #define BYTESTUFF          0x7d
  32 #define STUFF_MASK         0x20
  33
  34 // FrSky PRIM IDs (1 byte)
  35 #define DATA_FRAME         0x10
  36
  37 // FrSky old DATA IDs (1 byte)
  38 #define GPS_ALT_BP_ID      0x01
  39 #define GPS_ALT_AP_ID      0x09
  40 #define BARO_ALT_BP_ID     0x10
  41 #define BARO_ALT_AP_ID     0x21
  42 #define GPS_LONG_BP_ID     0x12
  43 #define GPS_LONG_AP_ID     0x1A
  44 #define GPS_LAT_BP_ID      0x13
  45 #define GPS_LAT_AP_ID      0x1B
  46 #define GPS_LONG_EW_ID     0x22
  47 #define GPS_LAT_NS_ID      0x23
  48 #define FRSKY_LAST_ID      0x3F
  49 //used for sats and fix type
  50 #define TEMP2              0x05
  51
  52 // FrSky Passthrough DATA IDs from APM
  53 #define GPS_STATUS                              0x5002
  54 #define AIRCRAFT_HOME                   0X5004
  55 #define bitmask(X,Y) (((1 << X) -1) << Y)
  56
  57 // FrSky new DATA IDs (2 bytes)
  58 #define ALT_FIRST_ID       0x0100
  59 #define ALT_LAST_ID        0x010f
  60 #define T2_FIRST_ID             0x0410
  61 #define T2_LAST_ID              0x041f
  62 #define GPS_LONG_LATI_FIRST_ID  0x0800
  63 #define GPS_LONG_LATI_LAST_ID   0x080f
  64 #define GPS_ALT_FIRST_ID        0x0820
  65 #define GPS_ALT_LAST_ID         0x082f
  66 #define GPS_SPEED_FIRST_ID      0x0830
  67 #define GPS_SPEED_LAST_ID       0x083f
  68 #define GPS_COURS_FIRST_ID      0x0840
  69 #define GPS_COURS_LAST_ID       0x084f
  70
  71 #define TELEMETRY_INIT    0
  72 #define TELEMETRY_OK      1
  73 #define TELEMETRY_KO      2
  74
  75 #define FRSKY_RX_PACKET_SIZE 9
  76
  77 #define FRSKYX_LATLON_DIVIDER 1000000
  78
  79 uint8_t frskyx_RxBuffer[FRSKY_RX_PACKET_SIZE];
  80 uint8_t telemetryState = TELEMETRY_INIT;
  81
  82 //alt in m
  83 int16_t alt;
  84 uint8_t sats;
  85 uint8_t fix;
  86
  87 uint16_t NS;
  88 uint16_t EW;
  89
  90 //lat lon in decimal degree dd.ddddd
  91 uint16_t lat_bp;
  92 uint16_t lon_bp;
  93 uint16_t lat_ap;
  94 uint16_t lon_ap;
  95
  96 int32_t frskyx_setLat() {
  97   int32_t value = coordToLong(NS == 'N' ? 1 : -1, lat_bp, lat_ap);
  98   NS = 0;
  99   return value;
 100 }
 101
 102 int32_t frskyx_setLon() {
 103   int32_t value = coordToLong(EW == 'E' ? 1 : -1, lon_bp, lon_ap);
 104   EW = 0;
 105   return value;
 106 }
 107
 108 int32_t coordToLong(int8_t neg, uint16_t bp, uint16_t ap) {
 109   uint32_t first = ((uint32_t)bp / 100) * FRSKYX_LATLON_DIVIDER;
 110   uint32_t second = ((((uint32_t)bp % 100) * FRSKYX_LATLON_DIVIDER) + ((uint32_t)ap * 100)) / 60;
 111   return ((int32_t)(first + second) * (uint32_t)neg);
 112 }
 113
 114 void processHubPacket(uint8_t id, uint16_t value)
 115 {
 116   if (id > FRSKY_LAST_ID)
 117     return;
 118   switch (id) {
 119     case GPS_ALT_BP_ID:
 120       alt = value;
 121       telemetry_alt = alt;
 122       gotAlt = true;
 123       break;
 124     case GPS_LONG_BP_ID:
 125       lon_bp = value;
 126       break;
 127     case GPS_LONG_AP_ID:
 128       lon_ap = value;
 129       break;
 130     case GPS_LAT_BP_ID:
 131       lat_bp = value;
 132       break;
 133     case GPS_LAT_AP_ID:
 134       lat_ap = value;
 135       break;
 136     case GPS_LONG_EW_ID:
 137       EW = value;
 138       break;
 139     case GPS_LAT_NS_ID:
 140       NS = value;
 141       break;
 142     case TEMP2:
 143           if(telemetry_provider == TELEMETRY_PROVIDER_DIY_GPS){
 144                         sats = value / 10;
 145                         fix = value % 10;
 146           } else if (telemetry_provider == TELEMETRY_PROVIDER_INAV){
 147                         sats = value % 100;
 148           } else
 149                         sats = value;
 150       break;
 151   }
 152   if ((NS == 'N' || NS == 'S') && (EW == 'E' || EW == 'W')) {
 153         telemetry_lat = frskyx_setLat();
 154         telemetry_lon = frskyx_setLon();
 155         telemetry_sats = sats;
 156     gotFix = true;
 157   }
 158 }
 159
 160 bool checkSportPacket(uint8_t *packet)
 161 {
 162   short crc = 0;
 163   for (int i = 1; i < FRSKY_RX_PACKET_SIZE; i++) {
 164     crc += packet[i]; //0-1FF
 165     crc += crc >> 8; //0-100
 166     crc &= 0x00ff;
 167     crc += crc >> 8; //0-0FF
 168     crc &= 0x00ff;
 169   }
 170   return (crc == 0x00ff);
 171 }
 172
 173 #define SPORT_DATA_U8(packet)   (packet[4])
 174 #define SPORT_DATA_S32(packet)  (*((int32_t *)(packet+4)))
 175 #define SPORT_DATA_U32(packet)  (*((uint32_t *)(packet+4)))
 176 #define HUB_DATA_U16(packet)    (*((uint16_t *)(packet+4)))
 177
 178 void processSportPacket(uint8_t *packet)
 179 {
 180   uint8_t  prim   = packet[1];
 181   uint16_t appId  = *((uint16_t *)(packet + 2));
 182
 183   if (!checkSportPacket(packet)){
 184           telemetry_failed_cs++;
 185           return;
 186   }
 187
 188   if(appId == GPS_STATUS) telemetry_provider = TELEMETRY_PROVIDER_APM10;
 189
 190   switch (prim)
 191   {
 192     case DATA_FRAME:
 193       if ((appId >> 8) == 0) {
 194         // The old FrSky IDs
 195         uint8_t  id = (uint8_t)appId;
 196         uint16_t value = HUB_DATA_U16(packet);
 197         processHubPacket(id, value);
 198       }
 199       else if (appId >= T2_FIRST_ID && appId <= T2_LAST_ID) {
 200                 if(telemetry_provider == TELEMETRY_PROVIDER_DIY_GPS){
 201                         sats = SPORT_DATA_S32(packet) / 10;
 202                         //fix = SPORT_DATA_S32(packet) % 10;
 203                 } else if (telemetry_provider == TELEMETRY_PROVIDER_INAV){
 204                         sats = SPORT_DATA_S32(packet) % 100;
 205                 } else {
 206                         //we assume that other systems just send the sats over temp2 and fix over temp1
 207                         sats = SPORT_DATA_S32(packet);
 208                 }
 209       }
 210       else if (appId >= GPS_SPEED_FIRST_ID && appId <= GPS_SPEED_LAST_ID) {
 211         //frskyData.hub.gpsSpeed_bp = (uint16_t) (SPORT_DATA_U32(packet) / 1000);
 212       }
 213       else if (appId >= GPS_COURS_FIRST_ID && appId <= GPS_COURS_LAST_ID) {
 214         //uint32_t course = SPORT_DATA_U32(packet)/100;
 215       }
 216       else if (appId >= GPS_ALT_FIRST_ID && appId <= GPS_ALT_LAST_ID) {
 217         alt = SPORT_DATA_S32(packet) / 100;
 218         telemetry_alt = alt;
 219         gotAlt = true;
 220       }
 221       else if (appId >= GPS_LONG_LATI_FIRST_ID && appId <= GPS_LONG_LATI_LAST_ID) {
 222         uint32_t gps_long_lati_data = SPORT_DATA_U32(packet);
 223         uint32_t gps_long_lati_b1w, gps_long_lati_a1w;
 224         gps_long_lati_b1w = (gps_long_lati_data & 0x3fffffff) / 10000;
 225         gps_long_lati_a1w = (gps_long_lati_data & 0x3fffffff) % 10000;
 226
 227         switch ((gps_long_lati_data & 0xc0000000) >> 30) {
 228           case 0:
 229             lat_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
 230             lat_ap = gps_long_lati_a1w;
 231             NS = 'N';
 232             break;
 233           case 1:
 234             lat_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
 235             lat_ap = gps_long_lati_a1w;
 236             NS = 'S';
 237             break;
 238           case 2:
 239             lon_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
 240             lon_ap = gps_long_lati_a1w;
 241             EW = 'E';
 242             break;
 243           case 3:
 244             lon_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
 245             lon_ap = gps_long_lati_a1w;
 246             EW = 'W';
 247             break;
 248         }
 249         if ((NS == 'N' || NS == 'S') && (EW == 'E' || EW == 'W')) {
 250           telemetry_lat = frskyx_setLat();
 251           telemetry_lon = frskyx_setLon();
 252           telemetry_sats = sats;
 253           gotFix = true;
 254         }
 255
 256       }
 257       else if(appId == GPS_STATUS && telemetry_provider == TELEMETRY_PROVIDER_APM10){
 258           sats = (uint8_t) (SPORT_DATA_U32(packet) & bitmask(4,0));
 259           //telemetry.gpsAlt = bit32.extract(VALUE,24,7) * (10^bit32.extract(VALUE,22,2)) * (bit32.extract(VALUE,31,1) == 1 and -1 or 1) -- dm
 260                   uint16_t msl_dc, msl_x, msl_sgn;
 261                   msl_dc = (SPORT_DATA_U32(packet) & bitmask(7,24)) >> 24;
 262                   msl_x = (SPORT_DATA_U32(packet) & bitmask(2,22)) >> 22;
 263                   msl_sgn = (SPORT_DATA_U32(packet) & bitmask(1,31)) >> 31;
 264                   telemetry_alt = (int16_t) (msl_dc * pow(10,msl_x) * 0.1 * (msl_sgn == 1?-1:1));
 265           gotAlt = true;
 266       }
 267       break;
 268   }
 269 }
 270
 271 // Receive buffer state machine state enum
 272 enum FrSkyDataState {
 273   STATE_DATA_IDLE,
 274   STATE_DATA_IN_FRAME,
 275   STATE_DATA_XOR,
 276 };
 277
 278 void frskyx_encodeTargetData(uint8_t data) {
 279   static uint8_t numPktBytes = 0;
 280   static uint8_t dataState = STATE_DATA_IDLE;
 281
 282   if (data == START_STOP) {
 283     dataState = STATE_DATA_IN_FRAME;
 284     numPktBytes = 0;
 285   }
 286   else {
 287     switch (dataState) {
 288       case STATE_DATA_XOR:
 289         frskyx_RxBuffer[numPktBytes++] = data ^ STUFF_MASK;
 290         dataState = STATE_DATA_IN_FRAME;
 291         break;
 292
 293       case STATE_DATA_IN_FRAME:
 294         if (data == BYTESTUFF)
 295           dataState = STATE_DATA_XOR; // XOR next byte
 296         else
 297           frskyx_RxBuffer[numPktBytes++] = data;
 298         break;
 299     }
 300   }
 301
 302   if (numPktBytes == FRSKY_RX_PACKET_SIZE) {
 303     processSportPacket(frskyx_RxBuffer);
 304     dataState = STATE_DATA_IDLE;
 305   }
 306 }