src/main/tracker/ltm.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 of Ghettostation antenna tracker
   6  * https://github.com/KipK/Ghettostation
   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 /*
  24 LTM is an air to ground telemetry, so it's one way only; No ACK & stuffs it just stream datas to the ground at low baudrates.
  25
  26 Depending of flighcontroller implementation & configuration it starts at boot or when armed.
  27
  28 Protocol is binary, using little endian.
  29 There's 3 frames to consider:
  30
  31 .
  32
  33 G Frame (GPS position) (2hz @ 1200 bauds , 5hz >= 2400 bauds): 18BYTES 0x24 0x54 0x47 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xC0
  34 $ T G --------LAT-------- -------LON--------- SPD --------ALT-------- SAT/FIX CRC
  35 A Frame (Attitude) (5hz @ 1200bauds , 10hz >= 2400bauds): 10BYTES
  36 0x24 0x54 0x41 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xC0
  37
  38 $ T A --PITCH-- --ROLL--- -HEADING- CRC
  39
  40 S Frame (Sensors) (2hz @ 1200bauds, 5hz >= 2400bauds): 11BYTES
  41 0x24 0x54 0x53 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xC0
  42
  43 $ T S VBAT(mv) Current(ma) RSSI AIRSPEED ARM/FS/FMOD CRC
  44
  45 values are:
  46
  47 LAT/LON: int32 (cm)
  48 SPD ( speed ) : uint8 ( m/s )
  49 ALT ( alititude ) int32 ( cm )
  50 SATS ( number of sat visible ) 6bits
  51 FIX ( sat fix ) 2 bits ( sats & fix are coupled in the same byte )
  52 PITCH/ROLL/HEADING int16 ( deg , -180/180° range )
  53 VBAT( voltage ) uint16 (mv )
  54 Current uint16 ( ma )
  55 RSSI : uint8 ( in % )
  56 AIRSPEED : uint8 (m/s)
  57 ARM/FS/FMOD: uint8 ARM armed status: 1 bit ( first ) FS : failsafe status : 1 bit ( second one ) FMOD: flight mode : last 6 bits: // Flight mode(0-19): 0: Manual, 1: Rate, 2: Attitude/Angle, 3: Horizon, 4: Acro, 5: Stabilized1, 6: Stabilized2, 7: Stabilized3, // 8: Altitude Hold, 9: Loiter/GPS Hold, 10: Auto/Waypoints, 11: Heading Hold / headFree, // 12: Circle, 13: RTH, 14: FollowMe, 15: LAND, 16:FlybyWireA, 17: FlybywireB, 18: Cruise, 19: Unknown
  58 Each frame end with a CRC byte, it's a simple XOR from first payload byte to last one ( starting at 4th byte , headers are not xored )
  59
  60 Attached LTM-log with sample attitude data nothing else (changed text document into picture for attaching here)
  61  */
  62 #include "config.h"
  63 #include "telemetry.h"
  64 #include "Arduino.h"
  65
  66 //
  67 #define LTM_HEADER_START1 0x24 //$
  68 #define LTM_HEADER_START2 0x54 //T
  69 #define LTM_GFRAME 0x47 //G Frame
  70 #define LTM_AFRAME 0x41 //A Frame
  71
  72 #define LTM_GFRAME_LENGTH 18
  73 #define LTM_AFRAME_LENGTH 10
  74
  75 // machine states
  76 enum LtmDataState {
  77     IDLE,
  78     STATE_START1,
  79     STATE_START2,
  80     STATE_MSGTYPE,
  81     STATE_DATA
  82   };
  83
  84 //
  85 static uint8_t LTM_Buffer[LTM_GFRAME_LENGTH-4];
  86 static uint8_t LTM_Index;
  87 static uint8_t LTM_cmd;
  88 static uint8_t LTM_chk;
  89 static uint8_t LTM_read_index;
  90 static uint8_t LTM_frame_length;
  91 static uint8_t dataState = IDLE;
  92
  93 int32_t temp_alt;
  94 uint8_t satsfix;
  95 uint8_t fix_type;
  96
  97 void parseLTM_GFRAME(void);
  98 uint8_t ltmread_u8();
  99 uint16_t ltmread_u16();
 100 uint32_t ltmread_u32();
 101
 102 void ltm_encodeTargetData(uint8_t c) {
 103
 104             if (dataState == IDLE && c == '$') {
 105               dataState = STATE_START1;
 106             }
 107             else if (dataState == STATE_START1 && c == 'T') {
 108                 dataState=STATE_START2;
 109             }
 110             else if (dataState == STATE_START2) {
 111               switch (c) {
 112                  case 'G':
 113                    LTM_frame_length = LTM_GFRAME_LENGTH;
 114                    dataState = STATE_MSGTYPE;
 115                    break;
 116                  case 'A':
 117                        LTM_frame_length = LTM_AFRAME_LENGTH;
 118                        dataState = STATE_MSGTYPE;
 119                    break;
 120                  case 'S':
 121                          dataState=IDLE;
 122                          return;
 123                    break;
 124                  default:
 125                    dataState = IDLE;
 126               }
 127               LTM_cmd = c;
 128               LTM_Index=0;
 129             }
 130             else if (dataState == STATE_MSGTYPE) {
 131                   if(LTM_Index == 0) {
 132                   LTM_chk = c;
 133                   }
 134                   else {
 135                   LTM_chk ^= c;
 136                   }
 137               if(LTM_Index == LTM_frame_length-4) {   // received checksum byte
 138                 if(LTM_chk == 0) {
 139                         LTM_read_index = 0;
 140                         parseLTM_GFRAME();
 141                     dataState = IDLE;
 142                 }
 143                 else {                                                   // wrong checksum, drop packet
 144                         dataState = IDLE;
 145                         telemetry_failed_cs++;
 146                 }
 147               }
 148               else LTM_Buffer[LTM_Index++]=c;
 149             }
 150 }
 151
 152 void parseLTM_GFRAME(void) {
 153   if (LTM_cmd==LTM_GFRAME)
 154   {
 155     telemetry_lat = (int32_t)ltmread_u32();
 156     telemetry_lat = telemetry_lat/10;
 157     telemetry_lon = (int32_t)ltmread_u32();
 158     telemetry_lon = telemetry_lon/10;
 159     telemetry_speed = ltmread_u8();
 160     temp_alt = (int32_t)ltmread_u32();//10000000;
 161     telemetry_alt = (int16_t)(temp_alt/100);
 162     satsfix = ltmread_u8();
 163     telemetry_sats = (int16_t)((satsfix >> 2) & 0xFF);
 164     telemetry_fixtype = satsfix & 0b00000011;
 165     if(telemetry_sats>=5) gotFix = 1;
 166     gotAlt = true;
 167   }
 168   if (LTM_cmd==LTM_AFRAME)
 169     {
 170       telemetry_pitch = radians((int16_t)ltmread_u16());
 171       telemetry_roll =  radians((int16_t)ltmread_u16());
 172       telemetry_course = (int16_t)ltmread_u16() * 1.0f ;
 173       telemetry_yaw = radians(telemetry_course);
 174   }
 175 }
 176 uint8_t ltmread_u8()  {
 177   return LTM_Buffer[LTM_read_index++];
 178 }
 179
 180 uint16_t ltmread_u16() {
 181   uint16_t t = ltmread_u8();
 182   t |= (uint16_t)ltmread_u8()<<8;
 183   return t;
 184 }
 185
 186 uint32_t ltmread_u32() {
 187   uint32_t t = ltmread_u16();
 188   t |= (uint32_t)ltmread_u16()<<16;
 189   return t;
 190 }
 191
 192
 193