mirror
/
esp32-ogn-tracker
kopia lustrzana https://github.com/pjalocha/esp32-ogn-tracker
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność
esp32-ogn-tracker/utils/gdl90.h

380 wiersze
17 KiB
C++
Czysty Bezpośredni odnośnik Wina Historia

#ifndef __GDL90_H__
#define __GDL90_H__
#include <stdio.h>
#include <stdint.h>
#include "format.h"
// =================================================================================
uint16_t GDL90_CRC16(uint8_t Byte, uint16_t CRC); // pass a single byte through the CRC
uint16_t GDL90_CRC16(const uint8_t *Data, uint8_t Len, uint16_t CRC=0); // pass a packet of bytes through the CRC
int GDL90_Send(void (*Output)(char), uint8_t ID, const uint8_t *Data, int Len); // transmit GDL90 packet with proper framing and CRC
int GDL90_Send(uint8_t *Out, uint8_t ID, const uint8_t *Data, int Len);
inline int GDL90_Send(char *Out, uint8_t ID, const uint8_t *Data, int Len) { return GDL90_Send((uint8_t *)Out, ID, Data, Len); }
// =================================================================================
// https://www.faa.gov/nextgen/programs/adsb/archival/media/gdl90_public_icd_reva.pdf
// SkyRadar msg ID 101: https://github.com/etdey/gdl90/blob/master/Format%20of%20the%20SkyRadar%20receiver%20message%20ID%20101.pdf
// other msg ID: https://www.foreflight.com/connect/spec/
class GDL90_HEARTBEAT // Heart-beat packet to be send at every UTC second
{ public:
static const int Size=6;
union
{ uint8_t Status1;
struct
{ bool Initialized: 1;
bool reserved : 1;
bool RATCS : 1;
bool LowBatt : 1; // battery is LOW
bool AddrType : 1; // are we transmitting ICAO (0) or self-assigned address (1)
bool IDENT : 1;
bool MaintReq : 1;
bool PosValid : 1; // GPS position is valid
} ;
} ;
union
{ uint8_t Status2;
struct
{ bool UTCvalid : 1; // UTC timing is valid
bool reserved1 : 1;
bool reserved2 : 1;
bool reserved3 : 1;
bool reserved4 : 1;
bool CSA_Req : 1; // CSA has been requested (Conflict Situation Awareness)
bool CSA_NotAvail : 1; // CSA is not available
bool TimeStampMSB : 1; // [0x10000sec] highest TimeStamp bit
} ;
} ;
uint16_t TimeStamp; // [sec] since 0000z cut to 16-bit
uint8_t MsgCount[2]; // [/sec] counts messages received during the previous second
public:
void Clear(void)
{ Status1=0; Status2=0; TimeStamp=0; MsgCount[0]=0; MsgCount[1]=0; }
void Print(void) const
{ uint32_t Time=getTimeStamp();
int Hour=Time/3600; Time-=Hour*3600;
int Min =Time/60; Time-=Min*60;
int Sec =Time;
printf("GDL90_HEARTBEAT: %02d:%02d:%02dZ %02X:%02X %4d:%4d\n", Hour, Min, Sec, Status1, Status2, getUplinkCount(), getDownlinkCount()); }
void setTimeStamp(uint32_t Time)
{ Time%=86400; TimeStamp=Time; TimeStampMSB=Time>>16; }
uint32_t getTimeStamp(void) const
{ uint32_t Time=TimeStampMSB; Time = (Time<<16) | TimeStamp; return Time; }
uint8_t getUplinkCount(void) const { return MsgCount[0]>>3; } // Uplink messages received
void setUplinkCount(uint8_t Count) { MsgCount[0] = (MsgCount[0]&0x07) | Count<<3; }
uint16_t getDownlinkCount(void) const { uint16_t Count = MsgCount[0]&0x03; return (Count<<8) | MsgCount[1]; } // Basic and Long messages received
void setDownlinkCount(uint8_t Count) { MsgCount[0] = (MsgCount[0]&0xFC) | (Count>>8); MsgCount[1] = Count; }
int Send(void (*Output)(char)) const { return GDL90_Send(Output, 0, (const uint8_t *)this, Size); }
int Send(char *Output ) const { return GDL90_Send(Output, 0, (const uint8_t *)this, Size); }
} __attribute__((packed));
class STX_HEARTBEAT
{ public:
static const int Size=1;
union
{ uint8_t Status;
struct
{ bool AHRS : 1;
bool GPS : 1;
uint8_t ProtVer: 6;
} ;
} ;
public:
void Print(void) const
{ printf("STX_HEARTBEAT: AHRS:%d GPS:%d v%d\n", AHRS, GPS, ProtVer); }
} ;
class STX_AHRS
{ public:
static const int Size=23;
uint8_t Data[Size];
public:
void Print(void) const
{ printf("STX_AHRS:\n"); }
} ;
class STX_STATUS
{ public:
static const int Size=28;
union
{ uint8_t Data[Size];
struct
{ char X;
uint8_t MsgType;
uint8_t MsgVer;
uint8_t Firmware[4];
uint8_t Hardware[4];
union
{ uint8_t ValidFlags[2];
} ;
union
{ uint8_t ConnFlags[2];
} ;
uint8_t SatLocked;
uint8_t SatConnected;
uint8_t TrafficTargets978[2];
uint8_t TrafficTargets1090[2];
uint8_t TraffcRate978[2];
uint8_t TraffcRate1090[2];
uint8_t CPUtemperature[2];
uint8_t ADSBtowers;
// 6x ADSBtowers bytes for coordinates
} ;
} ;
public:
static uint16_t getTwoBytes(const uint8_t *Data) { uint16_t Val=Data[0]; return (Val<<8) | Data[1]; }
// uint16_t getTargets(void) const { return getTwoBytes(TrafficTargets); }
float getCPUtemp(void) const
{ uint16_t Temp=getTwoBytes(CPUtemperature);
if(Temp&0x8000) return -0.1f*(Temp&0x7FFF);
else return +0.1f*Temp; }
void Print(void) const
{ printf("STX_STATUS: %d/%dsats v%d.%d %+5.1fdegC\n", SatLocked, SatConnected, Firmware[0], Firmware[1], getCPUtemp()); }
} ;
// class GSL90_CONFIG // Initialization, ID=117
// { public:
// uint8_t Data[19];
// } ;
class GDL90_GEOMALT // Geometrical altitude: ID = 11 (GPS ref. to Ellipsoid)
{ public:
static const int Size=4;
uint8_t Data[Size];
public:
void Clear(void) { for(int Idx=0; Idx<Size; Idx++) Data[Idx]=0; }
void setAltitude(int32_t Alt) // [5 feet] GPS altitude (ref. to Ellipsoid)
{ if(Alt>0x7FFF) Alt=0x7FFF;
else if(Alt<(-0x8000)) Alt=(-0x8000);
Data[0]=Alt>>8; Data[1]=Alt&0x0FF; }
int32_t getAltitude(void) const { int16_t Alt=Data[0]; Alt<<=8; Alt|=Data[1]; return Alt; }
void setWarning(bool Warn) // [bool]
{ if(Warn) Data[2]|=0x80;
else Data[2]&=0x7F; }
bool getWarning(void) const { return Data[2]&0x80; }
void setFOM(uint16_t FOM) // [m] vertical Figure of Merit (accuracy ?)
{ Data[2] = (Data[2]&0x80) | (FOM>>8);
Data[3] = FOM&0xFF; }
uint16_t getFOM(void) const { uint16_t FOM=Data[2]&0x7F; FOM<<=8; FOM|=Data[3]; return FOM; }
int Send(void (*Output)(char)) const { return GDL90_Send(Output, 11, Data, Size); }
int Send(char *Output ) const { return GDL90_Send(Output, 11, Data, Size); }
void Print(void) const
{ printf("GDL90_GEOMALT: %dft (%dm) Warn:%d\n", getAltitude()*5, getFOM(), getWarning()); }
} ;
class GDL90_REPORT // Position report: Traffic: ID = 20, Ownship: ID = 10
{ public:
static const int Size=27;
union
{ uint8_t Data[Size]; // 27 bytes excluding the ID and framing/CRC
/*
struct
{ uint8_t AddrType : 4; //
uint8_t Alert : 4; // 1=alert
uint32_t Address :24; // byte-reversed
uint32_t Latitude :24; // byte-reversed
uint32_t Longitude :24; // byte-reversed
uint16_t Altitude :12; // garbled
uint8_t Misc : 4; // garbled
uint8_t NACp : 4;
uint8_t NIC : 4;
uint16_t Velocity :12; // garbled
uint16_t Climb :12; // garbled
uint8_t Track : 8;
uint8_t AcftCat : 8;
char Call[8];
uint8_t Spare : 4;
uint8_t Priority : 4;
} ;
*/
} ; // __attribute__((packed));
public:
static uint32_t get3bytes(const uint8_t *Byte) { uint32_t Word=Byte[0]; Word=(Word<<8) | Byte[1]; Word=(Word<<8) | Byte[2]; return Word; } // 3-byte value
static void set3bytes(uint8_t *Byte, uint32_t Word) { Byte[0]=Word>>16; Byte[1]=Word>>8; Byte[2]=Word; }
void Clear(void) { for(int Idx=0; Idx<Size; Idx++) Data[Idx]=0; } // clear all data (Lat/Lon = invalid)
uint8_t getAlertStatus(void) const { return Data[0]>>4; } // 0 = no alert, 1 = alert
void setAlertStatus(uint8_t Status) { Data[0] = (Data[0]&0x0F) | (Status<<4); }
uint8_t getAddrType(void) const { return Data[0]&0x0F; } // 0=ICAO, 1=non-ICAO, 4=surface vehicle, 5=ground beacon
void setAddrType(uint8_t AddrType) { Data[0] = (Data[0]&0xF0) | (AddrType&0x0F); }
uint32_t getAddress(void) const { return get3bytes(Data+1); }
void setAddress(uint32_t Addr) { set3bytes(Data+1, Addr); }
int32_t getLatitude(void) const { return get3bytes(Data+4)<<8; } // [cyclic]
void setLatitude(int32_t Lat) { set3bytes(Data+4, (Lat>>8)&0xFFFFFF); }
int32_t getLongitude(void) const { return get3bytes(Data+7)<<8; } // [cyclic]
void setLongitude(int32_t Lon) { set3bytes(Data+7, (Lon>>8)&0xFFFFFF); }
static int32_t CordicOGN(int32_t Coord) { return ((int64_t)Coord*83399993+(1<<21))>>22; } // [1/60000deg] => [cordic]
void setLatOGN(int32_t Lat) { setLatitude (CordicOGN(Lat)); } // [1/60000deg]
void setLonOGN(int32_t Lon) { setLongitude(CordicOGN(Lon)); } // [1/60000deg]
int32_t getAltitude(void) const { int32_t Alt=Data[10]; Alt=(Alt<<4) | (Data[11]>>4); return Alt*25-1000; } // [feet]
void setAltitude(int32_t Alt) // [feet]
{ Alt = (Alt+1000+12)/25;
if(Alt<0) Alt=0; else if(Alt>0xFFF) Alt=0xFFF;
Data[10] = Alt>>4; Alt&=0x00F; Data[11] = (Data[11]&0x0F) | (Alt<<4); }
uint8_t getMiscInd(void) const { return Data[11]&0x0F; } // Airborne | Extrapolated | TT: 00=not valid, 01=true track, 10=magnetic, 11=heading
void setMiscInd(uint8_t MiscInd) { Data[11] = (Data[11]&0xF0) | (MiscInd&0x0F); }
uint8_t getNIC(void) const { return Data[12]>>4; } // containment radius: 9=75m, 10=25m, 11=7.5m
uint8_t getNACp(void) const { return Data[12]&0x0F; } // est. pos. uncertainty: 9=30m, 10=10m, 11=3m
void setAccuracy(uint8_t NIC, uint8_t NACp) { Data[12] = (NIC<<4) | (NACp&0x0F); }
uint16_t getSpeed(void) const { uint16_t Speed=Data[13]; Speed=(Speed<<4) | (Data[14]>>4); return Speed; } // [knot]
void setSpeed(uint16_t Speed) { if(Speed>0xFFE) Speed=0xFFE; Data[13] = Speed>>4; Data[14] = (Data[14]&0x0F) | (Speed<<4); } // [knot]
void clrSpeed(void) { Data[13] = 0xFF; Data[14] = (Data[14]&0x0F) | 0xF0; } // Speed = invalid
bool hasSpeed(void) const { return Data[13]!=0xFF || (Data[14]&0xF0)!=0xF0; }
int32_t getClimbRate(void) const
{ int16_t Climb=Data[14]&0x0F; Climb=(Climb<<8)|Data[15]; Climb<<=4; return (int32_t)Climb<<2; } // [fpm]
void setClimbRate(int32_t Climb) // [fpm]
{ Climb = (Climb+32)>>6; if(Climb<(-510)) Climb=(-510); else if(Climb>510) Climb=510; // full 12-bit range is not being used
Data[15] = Climb&0xFF; Data[14] = (Data[14]&0xF0) | ((Climb>>8)&0x0F); }
void clrClimbRate(void) { Data[15]=0x00; Data[14] = (Data[14]&0xF0) | 0x08; } // set vertical rate = not available
bool hasClimbRate(void) const { return Data[15]!=0x00 || (Data[14]&0x0F)!=0x08; }
uint8_t getHeading(void) const { return Data[16]; } // [cyclic]
void setHeading(uint8_t Heading) { Data[16]=Heading; } // [cyclic]
uint8_t getAcftCat(void) const { return Data[17]; } // 1=light, 2=small, 3=large, 4=high vortex, 5=heavy, 6=high-G, 7=rotor, 9=glider, 10=airship, 11=parachute, 12=ULM/para/hang, 14=UAV, 15=space, 17=surf, 18=service
uint8_t getAcftCatADSB(void) const { uint8_t Cat=getAcftCat(); Cat = (((Cat&0x18)<<1)+0xA0) | (Cat&7); return Cat; }
void setAcftCat(uint8_t Cat) { Data[17]=Cat; }
void setAcftCatADSB(uint8_t Cat) { if(Cat>=0xA0) Cat-=0xA0; Cat = ((Cat>>1)&0x18) | (Cat&7); setAcftCat(Cat); }
void setAcftTypeOGN(uint8_t AcftType) // set OGN-type aircraft-type
{ // 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, A, B, C, D, E, F
const uint8_t OGNtype[16] = { 0, 9, 1, 7, 11, 1, 12, 12, 1, 3,15,10,10,14,18,19 } ; // conversion table from OGN aricraft-type
setAcftCat(OGNtype[AcftType&0x0F]); }
const char *getAcftCall(void) const { return (const char *)(Data+18); } // is not null-terminated
void setAcftCall(const char *Call)
{ int Idx=0;
for( ; Idx<8; Idx++)
{ char ch=Call[Idx]; if(ch==0) break;
Data[18+Idx]=ch; }
for( ; Idx<8; Idx++)
Data[18+Idx]=' ';
}
void setAcftCall(uint32_t ID)
{ const char *AddrName[4] = { "RN", "IC", "FL", "OG" } ;
char *Call = (char *)(Data+18);
const char *Name = AddrName[(ID>>24)&3]; Call[0]=Name[0]; Call[1]=Name[1];
Format_Hex(Call+2, (uint8_t)((ID>>16)&0xFF)); Format_Hex(Call+4, (uint16_t)(ID&0xFFFF)); }
uint8_t getPriority(void) const { return Data[26]>>4; } // 1=general, 2=medical, 3=fuel, 4=comms, 5=interference, 6=downed
void setPriority(uint8_t Prior) { Data[26] = (Data[26]&0x0F) | (Prior<<4); }
int Send(void (*Output)(char), uint8_t ID=10) const { return GDL90_Send(Output, ID, Data, Size); }
int Send(char *Output , uint8_t ID=10) const { return GDL90_Send(Output, ID, Data, Size); }
void Print(void) const
{ printf("%X:%06X %02X/%8s NIC:%X NACp:%X %5dft",
getAddrType(), getAddress(), getAcftCatADSB(), getAcftCall(),
getNIC(), getNACp(), getAltitude());
if(hasClimbRate()) printf(" %+4dfpm", getClimbRate());
printf(" [%+09.5f,%+010.5f] %03.0f/%dkt MI:%X Alrt:%X Prio:%X\n",
(90.0/0x40000000)*getLatitude(), (90.0/0x40000000)*getLongitude(),
(360.0/256)*getHeading(), getSpeed(),
getMiscInd(), getAlertStatus(), getPriority()); }
} ;
// =================================================================================
class GDL90_RxMsg // receiver for the MAV messages
{ public:
static const uint8_t MaxBytes = 60; // max. number of bytes
static const uint8_t SYNC = 0x7E; // GDL90 sync byte
static const uint8_t ESC = 0x7D; // GDL90 escape byte
uint8_t Byte[MaxBytes];
uint8_t Len;
public:
void Clear(void) { Len=0; Byte[Len]=0; }
uint8_t getID(void) const { return Byte[0]; }
bool isHeartBeat(void) const { return getID()== 0 && Len== 9; } // regular GDL90 heart-beat sent at 1Hz
bool isGeomAlt(void) const { return getID()==11 && Len== 7; } // own height-above-Ellipsoid
bool isOwnReport(void) const { return getID()==10 && Len==30; } // own position
bool isTrafReport(void) const { return getID()==20 && Len==30; } // other aircraft position
bool isForeFlight(void) const { return getID()==0x65; } // ForeFlight: can be software ID or AHRS
bool isSkyLink(void) const { return getID()==0x31; } // SkyLink:
bool isStxHeartBeat(void) const { return getID()==0xCC && Len== 4; } // Stratux heart-beat
bool isStxAHRS(void) const { return getID()==0x4C && Len==26; } // Stratux AHRS
bool isStxStatus(void) const // Stratux status
{ if(getID()!='S') return 0;
if(Len<31) return 0;
uint8_t Towers=Byte[31];
return Len == 31+Towers*6; }
void Print(void) const
{ printf("GDL90[%2d:%3d] ", Len, Byte[0]);
for(int Idx=0; Idx<Len; Idx++)
printf("%02X", Byte[Idx]);
printf("\n"); }
uint8_t ProcessByte(uint8_t RxByte) // process a single byte: add to the message or reject
{ // printf("Process[%2d] 0x%02X\n", Len, RxByte);
if(Len==0) // if the very first byte
{ if(RxByte==SYNC) { Byte[Len]=RxByte; return 1; } // if SYNC then store it
else if(Byte[Len]==SYNC) // if the packet before was SYNC
{ Byte[Len]=RxByte; if(RxByte==ESC) return 1; // store the byte
Len++; Byte[Len]=0; return 1; }
else if(Byte[Len]==ESC) // if the previous byte was ESC
{ RxByte^=0x20; Byte[Len++]=RxByte; Byte[Len]=0; return 1; }
else return 0; }
if(RxByte==SYNC) // if not the very first byte and SYNC then the packet is possibly complete
{ if(Len<3 || Byte[Len]!=0) { Clear(); return 0; }
uint16_t CRC=0;
for(int Idx=0; Idx<(Len-2); Idx++)
{ CRC=GDL90_CRC16(Byte[Idx], CRC); }
if( (CRC&0xFF)!=Byte[Len-2] || (CRC>>8)!=Byte[Len-1] ) { Clear(); return 0; }
return 2; } // packet complete and good CRC
if(Byte[Len]==ESC) { RxByte^=0x20; } // if after an ESC then xor with 0x20
Byte[Len]=RxByte; if(RxByte==ESC) return 1;
Len++; // advance
if(Len>=MaxBytes) { Clear(); return 0; }
Byte[Len]=0; return 1; }
} ;
// =================================================================================
#endif // __GDL90_H__
Reference in New Issue View Git Blame Kopiuj bezpośredni odnośnik