diff --git a/main/ognconv.cpp b/main/ognconv.cpp index 0b4dfac..2b2c6b2 100644 --- a/main/ognconv.cpp +++ b/main/ognconv.cpp @@ -45,7 +45,7 @@ uint8_t AcftType_ADSBtoOGN(uint8_t AcftCat) { if(Low==1) return 8; if(Low==7) return 3; return 9; } - if(Upp=0xB) + if(Upp==0xB) { const uint8_t Map[8] = { 0, 0xB, 1, 4, 7, 0, 0xD, 0 }; return Map[Low]; } if(Upp==0xC) diff --git a/utils/adsl.h b/utils/adsl.h new file mode 100644 index 0000000..05db3fb --- /dev/null +++ b/utils/adsl.h @@ -0,0 +1,412 @@ +#ifndef __ADSL_H__ +#define __ADSL_H__ + +#include +#include +// #include +// #include "radiodemod.h" +// #include "intmath.h" +#include "ognconv.h" +#include "bitcount.h" +// #include "format.h" +// #include "crc1021.h" + +class ADSL_Packet +{ public: + + const static uint8_t TxBytes = 27; // including SYNC, Length, actual packet content (1+20 bytes) and 3-byte CRC + const static uint8_t SYNC1 = 0x72; // two SYNC bytes - Lemgth byte can be considered the 3rd SYNC byte as it is fixed + const static uint8_t SYNC2 = 0x4B; + + uint8_t SYNC[2]; // two bytes for correct alignment: can contain the last two SYNC bytes + uint8_t Length; // [bytes] packet length = 24 = 0x18 (excluding length but including the 24-bit CRC) + uint8_t Version; // Version[4]/Sigmature[1]/Key[2]/Reserved[1] + union + { uint32_t Word[5]; // this part to be scrambled/encrypted, is aligned to 32-bit + struct // this is aligned to 32-bit + { uint8_t Type; // 2=iConspicuity, bit #7 = Unicast + uint8_t Address [4]; // Address[30]/Reserved[1]/RelayForward[1] (not aligned to 32-bit !) + union + { uint8_t Meta [2]; // Time[6]/Cat[5]/Emerg[3]/FlightState[2] + struct + { uint8_t TimeStamp :6; // [0.25sec] + uint8_t FlightState :2; // 0=unknown, 1=ground, 2=airborne + uint8_t AcftCat :5; // 1=light, 2=small-heavy, 3=heli, 4=glider, 5=baloon/airship, 6=para/hang-glider, 7=skydiver, + uint8_t Emergency :3; // 1=OK + } ; + } ; + uint8_t Position[11]; // Lat[24]/Lon[24]/Speed[8]/Alt[14]/Climb[9]/Track[9] + union + { uint8_t Integrity[2]; // SourceInteg[2]/DesignAssurance[2]/NavigationIntegrity[4]/NorizAccuracy[3]/VertAccuracy[2]/ValocityAccuracy[2]/Reserved[1] + struct + { uint8_t SourceIntegrity:2; // 3=1e-7/h, 2=1e-5/h, 1=1e-3/h + uint8_t DesignAssurance:2; // 3=B, 2=C, 1=D + uint8_t NavigIntegrity :4; // 12=7.5m, 11=25m, 10=75m + uint8_t HorizAccuracy :3; // 7=3m, 6=10m, 5=30m + uint8_t VertAccuracy :2; // 3=15m, 2=45m, 1=150m + uint8_t VelAccuracy :2; // 3=1m/s 2=3m/s 3=10m/s + uint8_t Reserved :1; // + } ; + } ; + } ; + } ; + uint8_t CRC[3]; // 24-bit (is aligned to 32-bit) + +// -------------------------------------------------------------------------------------------------------- + + public: + void Init(void) + { SYNC[0]=SYNC1; SYNC[1]=SYNC2; + Length=TxBytes-3; Version=0x00; + for(int Idx=0; Idx<5; Idx++) + Word[Idx]=0; + Type=0x02; } + + void Print(void) const + { printf(" v%02X %4.1fs: %02X:%06X [%+09.5f,%+010.5f]deg %dm %+4.1fm/s %05.1fdeg %3.1fm/s\n", + Version, 0.25*TimeStamp, getAddrTable(), getAddress(), FNTtoFloat(getLat()), FNTtoFloat(getLon()), + getAlt(), 0.125*getClimb(), (45.0/0x40)*getTrack(), 0.25*getSpeed()); } + + int Print(char *Out) const + { return sprintf(Out, "%02X:%06X %4.1fs [%+09.5f,%+010.5f]deg %dm %+4.1fm/s %05.1fdeg %3.1fm/s", + getAddrTable(), getAddress(), 0.25*TimeStamp, FNTtoFloat(getLat()), FNTtoFloat(getLon()), + getAlt(), 0.125*getClimb(), (45.0/0x40)*getTrack(), 0.25*getSpeed()); } + +/* + uint32_t getAddress(void) const { return get3bytes(Address); } + uint8_t getAddrTable(void) const { return Address[3]&0x3F; } + + void setAddress(uint32_t Addr) { set3bytes(Address, Addr); } + void setAddrTable(uint8_t Table) { Address[3] = (Address[3]&0xC0) | Table; } +*/ + + uint8_t getRelay(void) const { return Address[3]&0x80; } + void setRelay(uint8_t Relay) { Address[3] = (Address[3]&0x7F) | (Relay<<7); } + + static uint32_t get3bytes(const uint8_t *Byte) { int32_t Word=Byte[2]; Word<<=8; Word|=Byte[1]; Word<<=8; Word|=Byte[0]; return Word; } + static void set3bytes(uint8_t *Byte, uint32_t Word) { Byte[0]=Word; Byte[1]=Word>>8; Byte[2]=Word>>16; } + + static uint32_t get4bytes(const uint8_t *Byte) + { uint32_t A = Byte[0]; + uint32_t B = Byte[1]; + uint32_t C = Byte[2]; + uint32_t D = Byte[3]; + return A | (B<<8) | (C<<16) | (D<<24); } + // { uint32_t Word =Byte[3]; Word<<=8; + // Word|=Byte[2]; Word<<=8; + // Word|=Byte[1]; Word<<=8; + // Word|=Byte[0]; + // return Word; } + static void set4bytes(uint8_t *Byte, uint32_t Word) { Byte[0]=Word; Byte[1]=Word>>8; Byte[2]=Word>>16; Byte[3]=Word>>24; } + + uint32_t getAddress(void) const + { uint32_t Addr = get4bytes(Address); return (Addr>>6)&0x00FFFFFF; } + + void setAddress(uint32_t NewAddr) + { uint32_t Addr = get4bytes(Address); + Addr = (Addr&0xC000003F) | (NewAddr<<6); + set4bytes(Address, Addr); } + + uint8_t getAddrTable(void) const { return Address[0]&0x3F; } + void setAddrTable(uint8_t Table) { Address[0] = (Address[0]&0xC0) | Table; } + + uint8_t getAddrTypeOGN(void) const + { uint8_t Table=getAddrTable(); + if(Table==0x05) return 1; // ICAO + if(Table==0x06) return 2; // FLARM + if(Table==0x07) return 3; // OGN + if(Table==0x08) return 2; // FANET => FLARM ? + return 0; } + + void setAddrTypeOGN(uint8_t AddrType) + { if(AddrType==0) setAddrTable(0); + else setAddrTable(AddrType+4); } + + void setAcftTypeOGN(uint8_t AcftType) // set OGN aircraft-type + { const uint8_t Map[16] = { 0, 4, 1, 3, // unknown, glider, tow-plane, helicopter + 8, 1, 7, 7, // sky-diver, drop plane, hang-glider, para-glider + 1, 2, 0, 5, // motor airplane, jet, UFO, balloon + 5,11, 0, 0 } ; // airship, UAV, ground vehicle, static object + if(AcftType<16) AcftCat=Map[AcftType]; + else AcftCat=0; } + uint8_t getAcftTypeOGN(void) const // get OGN aircraft-type + { const uint8_t Map[32] = { 0, 8, 9, 3, 1,12, 2, 7, + 4,13, 3,13,13,13, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0 } ; + return Map[AcftCat]; } + +// -------------------------------------------------------------------------------------------------------- + + uint32_t getTime(uint16_t &msTime, uint32_t RefTime, int FwdMargin=3) const + { msTime=250*(TimeStamp&3); + if(TimeStamp>=60) return 0; + int Sec=RefTime%15; + int DiffSec=(TimeStamp>>2)-Sec; + if(DiffSec>FwdMargin) DiffSec-=15; + else if(DiffSec<=(-15+FwdMargin)) DiffSec+=15; + return RefTime+DiffSec; } // get out the correct position timestamp + + uint8_t getHorAccur(void) const + { const uint8_t Map[8] = { 63, 63, 63, 63, 63, 30, 10, 3 } ; + return Map[HorizAccuracy]; } + void setHorAccur(uint8_t Prec) + { if(Prec<= 3) HorizAccuracy=7; + else if(Prec<=10) HorizAccuracy=6; + else if(Prec<=30) HorizAccuracy=5; + else HorizAccuracy=4; + VelAccuracy = HorizAccuracy-4; } + + uint8_t getVerAccur(void) const // [m] vertical accuracy + { const uint8_t Map[8] = { 63, 63, 45, 15 } ; + return Map[VertAccuracy]; } + void setVerAccur(uint8_t Prec) + { if(Prec<=15) HorizAccuracy=3; + else if(Prec<=45) HorizAccuracy=2; + else HorizAccuracy=1; } + + static int32_t FNTtoOGN(int32_t Coord) { return ((int64_t)Coord*27000219 +(1<<28))>>29; } // [FANET cordic] => [0.0001/60 deg] + static int32_t OGNtoFNT(int32_t Coord) { return ((int64_t)Coord*83399317 +(1<<21))>>22; } // [0.0001/60 deg] => [FANET cordic] + static int32_t FNTtoUBX(int32_t Coord) { return ((int64_t)Coord*900007296+(1<<29))>>30; } // [FANET-cordic ] => [1e-7 deg] + static int32_t UBXtoFNT(int32_t Coord) { return ((int64_t)Coord*5003959 +(1<<21))>>22; } // [1e-7 deg] => [FANET cordic] + static float FNTtoFloat(int32_t Coord) // convert from FANET cordic units to float degrees + { const float Conv = 90.0007295677/0x40000000; // FANET cordic conversion factor (not exactly cordic) + return Conv*Coord; } + + int32_t getLatOGN(void) const { return FNTtoOGN(getLat()); } + int32_t getLonOGN(void) const { return FNTtoOGN(getLon()); } + + int32_t getLatUBX(void) const { return FNTtoUBX(getLat()); } + int32_t getLonUBX(void) const { return FNTtoUBX(getLon()); } + + int32_t getLat(void) const { int32_t Lat=get3bytes(Position ); Lat<<=8; Lat>>=1; return Lat; } // FANET-cordic + int32_t getLon(void) const { int32_t Lon=get3bytes(Position+3); Lon<<=8; return Lon; } // FANET-cordic + + void setLatOGN(int32_t Lat) { setLat(OGNtoFNT(Lat)); } + void setLonOGN(int32_t Lon) { setLon(OGNtoFNT(Lon)); } + + void setLat(int32_t Lat) { Lat = (Lat+0x40)>>7; set3bytes(Position , Lat); } // FANET-cordic + void setLon(int32_t Lon) { Lon = (Lon+0x80)>>8; set3bytes(Position+3, Lon); } // FANET-cordic + + uint16_t getSpeed(void) const { return UnsVRdecode(Position[6]); } // [0.25 m/s] + void setSpeed(uint16_t Speed) { Position[6] = UnsVRencode(Speed); } // [0.25 m/s] + + int32_t getAlt(void) const // [m] + { int32_t Word=Position[8]&0x3F; Word<<=8; Word|=Position[7]; + return UnsVRdecode(Word)-316; } + void setAlt(int32_t Alt) + { Alt+=316; if(Alt<0) Alt=0; + int32_t Word=UnsVRencode(Alt); + Position[7]=Word; + Position[8] = (Position[8]&0xC0) | (Word>>8); } + + int16_t getClimbWord(void) const // + { int16_t Word=Position[9]&0x7F; Word<<=2; Word|=Position[8]>>6; return Word; } + int16_t getClimb(void) const // [0.125 m/s] + { return SignVRdecode(getClimbWord()); } + void setClimb(int16_t Climb) // [0.125 m/s] + { setClimbWord(SignVRencode(Climb)); } + void setClimbWord(int16_t Word) + { Position[8] = (Position[8]&0x3F) | ((Word&0x03)<<6); + Position[9] = (Position[9]&0x80) | (Word>>2); } + bool hasClimb(void) { return getClimbWord()!=0x100; } // climb-rate present or absent + void clrClimb(void) { setClimbWord(0x100); } // declare climb-rate as absent + + uint16_t getTrack(void) const // 9-bit cordic + { int16_t Word=Position[10]; Word<<=1; Word|=Position[9]>>7; return Word; } + void setTrack(int16_t Word) + { Position[9] = (Position[9]&0x7F) | ((Word&0x01)<<7); + Position[10] = Word>>1; } + +// -------------------------------------------------------------------------------------------------------- + + // calculate distance vector [LatDist, LonDist] from a given reference [RefLat, Reflon] + int calcDistanceVectorOGN(int32_t &LatDist, int32_t &LonDist, int32_t RefLat, int32_t RefLon, uint16_t LatCos=3000, int32_t MaxDist=0x7FFF) + { LatDist = ((getLatOGN()-RefLat)*1517+0x1000)>>13; // convert from 1/600000deg to meters (40000000m = 360deg) => x 5/27 = 1517/(1<<13) + if(abs(LatDist)>MaxDist) return -1; + LonDist = ((getLonOGN()-RefLon)*1517+0x1000)>>13; + if(abs(LonDist)>(4*MaxDist)) return -1; + LonDist = (LonDist*LatCos+0x800)>>12; + if(abs(LonDist)>MaxDist) return -1; + return 1; } + + // sets position [Lat, Lon] according to given distance vector [LatDist, LonDist] from a reference point [RefLat, RefLon] + void setDistanceVectorOGN(int32_t LatDist, int32_t LonDist, int32_t RefLat, int32_t RefLon, uint16_t LatCos=3000) + { setLatOGN(RefLat+(LatDist*27)/5); + LonDist = (LonDist<<12)/LatCos; // LonDist/=cosine(Latitude) + setLonOGN(RefLon+(LonDist*27)/5); } + +// -------------------------------------------------------------------------------------------------------- + + void Scramble(void) + { XXTEA_Encrypt_Key0(Word, 5, 6); } + + void Descramble(void) + { XXTEA_Decrypt_Key0(Word, 5, 6); } + +// -------------------------------------------------------------------------------------------------------- + + static uint32_t PolyPass(uint32_t CRC, uint8_t Byte) // pass a single byte through the CRC polynomial + { const uint32_t Poly = 0xFFFA0480; + CRC |= Byte; + for(uint8_t Bit=0; Bit<8; Bit++) + { if(CRC&0x80000000) CRC ^= Poly; + CRC<<=1; } + return CRC; } + + static uint32_t checkPI(const uint8_t *Byte, uint8_t Bytes) // run over data bytes and the three CRC bytes + { uint32_t CRC = 0; + for(uint8_t Idx=0; Idx>8; } // should be all zero for a correct packet + + static uint32_t calcPI(const uint8_t *Byte, uint8_t Bytes) // calculate PI for the given packet data excluding the three CRC bytes + { uint32_t CRC = 0; + for(uint8_t Idx=0; Idx>8; } // + + void setCRC(void) + { uint32_t Word = calcPI((const uint8_t *)&Version, TxBytes-6); + CRC[0]=Word>>16; CRC[1]=Word>>8; CRC[2]=Word; } + + uint32_t checkCRC(void) const + { return checkPI((const uint8_t *)&Version, TxBytes-3); } + + static int Correct(uint8_t *PktData, uint8_t *PktErr, const int MaxBadBits=6) // correct the manchester-decoded packet with dead/weak bits marked + { const int Bytes=TxBytes-3; + uint32_t CRC = checkPI(PktData, Bytes); if(CRC==0) return 0; + uint8_t ErrBit=FindCRCsyndrome(CRC); + if(ErrBit!=0xFF) { FlipBit(PktData, ErrBit); return 1; } + + uint8_t BadBitIdx[MaxBadBits]; // bad bit index + uint8_t BadBitMask[MaxBadBits]; // bad bit mask + uint32_t Syndrome[MaxBadBits]; // bad bit mask + uint8_t BadBits=0; // count the bad bits + for(uint8_t ByteIdx=0; ByteIdx>=1; + } + if(BadBits>MaxBadBits) break; + } + if(BadBits>MaxBadBits) return -1; // return failure when too many bad bits + + uint8_t Loops = 1<>1); // use Gray code to change flip just one bit at a time + uint8_t BitExp = GrayIdx^PrevGrayIdx; + uint8_t Bit=0; while(BitExp>>=1) Bit++; + PktData[BadBitIdx[Bit]]^=BadBitMask[Bit]; + CRC^=Syndrome[Bit]; if(CRC==0) return Count1s(GrayIdx); + uint8_t ErrBit=FindCRCsyndrome(CRC); + if(ErrBit!=0xFF) + { FlipBit(PktData, ErrBit); + return Count1s(GrayIdx)+1; } + PrevGrayIdx=GrayIdx; } + + return -1; } + + static void FlipBit(uint8_t *Byte, int BitIdx) + { int ByteIdx=BitIdx>>3; + BitIdx&=7; BitIdx=7-BitIdx; + uint8_t Mask=1; Mask<<=BitIdx; + Byte[ByteIdx]^=Mask; } + + static uint32_t CRCsyndrome(uint8_t Bit) + { const uint16_t PacketBytes = TxBytes-3; + const uint16_t PacketBits = PacketBytes*8; + const uint32_t Syndrome[PacketBits] = { + 0x7ABEE1, 0xC2A574, 0x6152BA, 0x30A95D, 0xE7AEAA, 0x73D755, 0xC611AE, 0x6308D7, + 0xCE7E6F, 0x98C533, 0xB3989D, 0xA6364A, 0x531B25, 0xD67796, 0x6B3BCB, 0xCA67E1, + 0x9AC9F4, 0x4D64FA, 0x26B27D, 0xECA33A, 0x76519D, 0xC4D2CA, 0x626965, 0xCECEB6, + 0x67675B, 0xCC49A9, 0x99DED0, 0x4CEF68, 0x2677B4, 0x133BDA, 0x099DED, 0xFB34F2, + 0x7D9A79, 0xC13738, 0x609B9C, 0x304DCE, 0x1826E7, 0xF3E977, 0x860EBF, 0xBCFD5B, + 0xA184A9, 0xAF3850, 0x579C28, 0x2BCE14, 0x15E70A, 0x0AF385, 0xFA83C6, 0x7D41E3, + 0xC15AF5, 0x9F577E, 0x4FABBF, 0xD82FDB, 0x93EDE9, 0xB60CF0, 0x5B0678, 0x2D833C, + 0x16C19E, 0x0B60CF, 0xFA4A63, 0x82DF35, 0xBE959E, 0x5F4ACF, 0xD05F63, 0x97D5B5, + 0xB410DE, 0x5A086F, 0xD2FE33, 0x96851D, 0xB4B88A, 0x5A5C45, 0xD2D426, 0x696A13, + 0xCB4F0D, 0x9A5D82, 0x4D2EC1, 0xD96D64, 0x6CB6B2, 0x365B59, 0xE4D7A8, 0x726BD4, + 0x3935EA, 0x1C9AF5, 0xF1B77E, 0x78DBBF, 0xC397DB, 0x9E31E9, 0xB0E2F0, 0x587178, + 0x2C38BC, 0x161C5E, 0x0B0E2F, 0xFA7D13, 0x82C48D, 0xBE9842, 0x5F4C21, 0xD05C14, + 0x682E0A, 0x341705, 0xE5F186, 0x72F8C3, 0xC68665, 0x9CB936, 0x4E5C9B, 0xD8D449, + 0x939020, 0x49C810, 0x24E408, 0x127204, 0x093902, 0x049C81, 0xFDB444, 0x7EDA22, + 0x3F6D11, 0xE04C8C, 0x702646, 0x381323, 0xE3F395, 0x8E03CE, 0x4701E7, 0xDC7AF7, + 0x91C77F, 0xB719BB, 0xA476D9, 0xADC168, 0x56E0B4, 0x2B705A, 0x15B82D, 0xF52612, + 0x7A9309, 0xC2B380, 0x6159C0, 0x30ACE0, 0x185670, 0x0C2B38, 0x06159C, 0x030ACE, + 0x018567, 0xFF38B7, 0x80665F, 0xBFC92B, 0xA01E91, 0xAFF54C, 0x57FAA6, 0x2BFD53, + 0xEA04AD, 0x8AF852, 0x457C29, 0xDD4410, 0x6EA208, 0x375104, 0x1BA882, 0x0DD441, + 0xF91024, 0x7C8812, 0x3E4409, 0xE0D800, 0x706C00, 0x383600, 0x1C1B00, 0x0E0D80, + 0x0706C0, 0x038360, 0x01C1B0, 0x00E0D8, 0x00706C, 0x003836, 0x001C1B, 0xFFF409, + 0x800000, 0x400000, 0x200000, 0x100000, 0x080000, 0x040000, 0x020000, 0x010000, + 0x008000, 0x004000, 0x002000, 0x001000, 0x000800, 0x000400, 0x000200, 0x000100, + 0x000080, 0x000040, 0x000020, 0x000010, 0x000008, 0x000004, 0x000002, 0x000001 } ; + return Syndrome[Bit]; } + + static uint8_t FindCRCsyndrome(uint32_t Syndr) // quick search for a single-bit CRC syndrome + { const uint16_t PacketBytes = TxBytes-3; + const uint16_t PacketBits = PacketBytes*8; + const uint32_t Syndrome[PacketBits] = { + 0x000001BF, 0x000002BE, 0x000004BD, 0x000008BC, 0x000010BB, 0x000020BA, 0x000040B9, 0x000080B8, + 0x000100B7, 0x000200B6, 0x000400B5, 0x000800B4, 0x001000B3, 0x001C1BA6, 0x002000B2, 0x003836A5, + 0x004000B1, 0x00706CA4, 0x008000B0, 0x00E0D8A3, 0x010000AF, 0x01856788, 0x01C1B0A2, 0x020000AE, + 0x030ACE87, 0x038360A1, 0x040000AD, 0x049C816D, 0x06159C86, 0x0706C0A0, 0x080000AC, 0x0939026C, + 0x099DED1E, 0x0AF3852D, 0x0B0E2F5A, 0x0B60CF39, 0x0C2B3885, 0x0DD44197, 0x0E0D809F, 0x100000AB, + 0x1272046B, 0x133BDA1D, 0x15B82D7E, 0x15E70A2C, 0x161C5E59, 0x16C19E38, 0x1826E724, 0x18567084, + 0x1BA88296, 0x1C1B009E, 0x1C9AF551, 0x200000AA, 0x24E4086A, 0x2677B41C, 0x26B27D12, 0x2B705A7D, + 0x2BCE142B, 0x2BFD538F, 0x2C38BC58, 0x2D833C37, 0x304DCE23, 0x30A95D03, 0x30ACE083, 0x34170561, + 0x365B594D, 0x37510495, 0x38132373, 0x3836009D, 0x3935EA50, 0x3E44099A, 0x3F6D1170, 0x400000A9, + 0x457C2992, 0x4701E776, 0x49C81069, 0x4CEF681B, 0x4D2EC14A, 0x4D64FA11, 0x4E5C9B66, 0x4FABBF32, + 0x531B250C, 0x56E0B47C, 0x579C282A, 0x57FAA68E, 0x58717857, 0x5A086F41, 0x5A5C4545, 0x5B067836, + 0x5F4ACF3D, 0x5F4C215E, 0x609B9C22, 0x6152BA02, 0x6159C082, 0x62696516, 0x6308D707, 0x67675B18, + 0x682E0A60, 0x696A1347, 0x6B3BCB0E, 0x6CB6B24C, 0x6EA20894, 0x70264672, 0x706C009C, 0x726BD44F, + 0x72F8C363, 0x73D75505, 0x76519D14, 0x78DBBF53, 0x7A930980, 0x7ABEE100, 0x7C881299, 0x7D41E32F, + 0x7D9A7920, 0x7EDA226F, 0x800000A8, 0x80665F8A, 0x82C48D5C, 0x82DF353B, 0x860EBF26, 0x8AF85291, + 0x8E03CE75, 0x91C77F78, 0x93902068, 0x93EDE934, 0x96851D43, 0x97D5B53F, 0x98C53309, 0x99DED01A, + 0x9A5D8249, 0x9AC9F410, 0x9CB93665, 0x9E31E955, 0x9F577E31, 0xA01E918C, 0xA184A928, 0xA476D97A, + 0xA6364A0B, 0xADC1687B, 0xAF385029, 0xAFF54C8D, 0xB0E2F056, 0xB3989D0A, 0xB410DE40, 0xB4B88A44, + 0xB60CF035, 0xB719BB79, 0xBCFD5B27, 0xBE959E3C, 0xBE98425D, 0xBFC92B8B, 0xC1373821, 0xC15AF530, + 0xC2A57401, 0xC2B38081, 0xC397DB54, 0xC4D2CA15, 0xC611AE06, 0xC6866564, 0xCA67E10F, 0xCB4F0D48, + 0xCC49A919, 0xCE7E6F08, 0xCECEB617, 0xD05C145F, 0xD05F633E, 0xD2D42646, 0xD2FE3342, 0xD677960D, + 0xD82FDB33, 0xD8D44967, 0xD96D644B, 0xDC7AF777, 0xDD441093, 0xE04C8C71, 0xE0D8009B, 0xE3F39574, + 0xE4D7A84E, 0xE5F18662, 0xE7AEAA04, 0xEA04AD90, 0xECA33A13, 0xF1B77E52, 0xF3E97725, 0xF526127F, + 0xF9102498, 0xFA4A633A, 0xFA7D135B, 0xFA83C62E, 0xFB34F21F, 0xFDB4446E, 0xFF38B789, 0xFFF409A7 } ; + + uint16_t Bot=0; + uint16_t Top=PacketBits; + uint32_t MidSyndr=0; + for( ; ; ) + { uint16_t Mid=(Bot+Top)>>1; + MidSyndr = Syndrome[Mid]>>8; + if(Syndr==MidSyndr) return (uint8_t)Syndrome[Mid]; + if(Mid==Bot) break; + if(Syndr< MidSyndr) Top=Mid; + else Bot=Mid; } + return 0xFF; } + +} __attribute__((packed)); + +class ADSL_RxPacket: public ADSL_Packet +{ public: + uint32_t sTime; // [ s] reception time + uint16_t msTime; // [ms] + int8_t RSSI; // [dBm] + uint8_t BitErr; // number of bit errors + + public: + void setTime(double RxTime) { sTime=floor(RxTime); msTime=floor(1000.0*(RxTime-sTime)); } + double getTime(void) const { return (double)sTime+0.001*msTime; } + uint32_t SlotTime(void) const { uint32_t Slot=sTime; if(msTime<=300) Slot--; return Slot; } + +}; + +#endif // __ADSL_H__ diff --git a/utils/fanet.h b/utils/fanet.h index c5914f2..e334839 100644 --- a/utils/fanet.h +++ b/utils/fanet.h @@ -44,14 +44,14 @@ class FANET_Packet uint8_t getAddrType(void) const // address-type based on prefix { uint8_t Pref=getAddrPref(); - if(Pref==0x11 || Pref==0x20 || Pref==0xDD || Pref==0xDE || Pref==0xDF) return 2; + if(Pref==0x08 || Pref==0x11 || Pref==0x20 || Pref==0xDD || Pref==0xDE || Pref==0xDF) return 2; return 3; } - void setAddress(uint32_t Addr) { setAddrPref(Addr>>16); setAddrLow(Addr); } - void setAddrPref(uint8_t Prefix) { Byte[1]=Prefix; } - void setAddrLow(uint16_t Addr ) { Byte[2]=Addr; Byte[3]=Addr>>8; } + void setAddress(uint32_t Addr) { setAddrPref(Addr>>16); setAddrLow(Addr); } // full 24-bit address + void setAddrPref(uint8_t Prefix) { Byte[1]=Prefix; } // address prefix + void setAddrLow(uint16_t Addr ) { Byte[2]=Addr; Byte[3]=Addr>>8; } // lower 16-bits of the address void setHeader(uint8_t Type) { Byte[0] = 0x40 | (Type&0x3F); } - void setType(uint8_t Type) { Byte[0] = (Byte[0]&0xC0) | (Type&0x3F); } + void setType(uint8_t Type) { Byte[0] = (Byte[0]&0xC0) | (Type&0x3F); } // packet-type: 1=air-position uint8_t ExtHeaderLen(void) const // length ot the extended header (zero in most cases) { if(!ExtHeader()) return 0; @@ -62,7 +62,8 @@ class FANET_Packet uint8_t MsgOfs(void) const { return 4+ExtHeaderLen(); } // offset to the actual message (past the header and ext. header) uint8_t MsgLen(void) const { return Len-4-ExtHeaderLen(); } // length of the actual message - const uint8_t *Msg(void) const { return Byte+MsgOfs(); } + const uint8_t *Msg(void) const { return Byte+MsgOfs(); } // pointer to the message, past the header + uint8_t *Msg(void) { return Byte+MsgOfs(); } void setName(const char *Name) { setHeader(2); @@ -192,6 +193,20 @@ class FANET_Packet uint8_t WriteFNNGB(char *Out) { return 0; } + int DecodePosition(float &Lat, float &Lon, int &Alt) + { uint8_t Idx=MsgOfs(); + if(Type()==1) + { Lat = FloatCoord(getLat(Byte+Idx)); + Lon = FloatCoord(getLon(Byte+Idx+3)); + Alt = getAltitude(Byte+Idx+6); + return 3; } + if(Type()==7) + { Lat = FloatCoord(getLat(Byte+Idx)); + Lon = FloatCoord(getLon(Byte+Idx+3)); + Alt = 0; + return 2; } + return 0; } + void Print(const char *Name=0) const { if(Name) printf("%s ", Name); printf("[%2d:%d:%2d] FNT%06X", Len, Type(), MsgLen(), getAddr()); @@ -262,7 +277,7 @@ class FANET_Packet Byte[Len] = (Upp<<4) | Low; Inp+=2; } // new byte, count input return Len; } // return number of bytes read = packet length - static int32_t CoordUBX(int32_t Coord) { return ((int64_t)900007296*Coord+0x20000000)>>30; } // convert FANET-cordic to UBX 10e-7deg units + static int32_t CoordUBX(int32_t Coord) { return ((int64_t)900007296*Coord+0x20000000)>>30; } // convert FANET-cordic to UBX 1e-7deg units // ((int64_t)900000000*Coord+0x20000000)>>30; // this is the exact formula, but FANET is not exact here static int Format_Lat(char *Str, int32_t Lat, char &HighRes) // format latitude after APRS @@ -313,11 +328,12 @@ class FANET_RxPacket: public FANET_Packet int8_t RSSI; // [dBm] uint8_t BitErr; // number of bit errors uint8_t CodeErr; // number of block errors + uint8_t Sync; // sync symbols used: 0xF1 for sx127x but 0x12 for sx1262 ? public: void setTime(double RxTime) { sTime=floor(RxTime); msTime=floor(1000.0*(RxTime-sTime)); } double getTime(void) const { return (double)sTime+0.001*msTime; } - uint32_t SlotTime(void) const { uint32_t Slot=sTime; if(msTime<=300) Slot--; return Slot; } + uint32_t SlotTime(void) const { uint32_t Slot=sTime; if(msTime<100) Slot--; return Slot; } void Print(char *Name=0) const { char HHMMSS[8]; @@ -325,7 +341,72 @@ class FANET_RxPacket: public FANET_Packet printf("%s CR%c%c%c %3.1fdB/%de %+3.1fkHz ", HHMMSS, '0'+CR, hasCRC?'c':'_', badCRC?'-':'+', 0.25*SNR, BitErr, 1e-2*FreqOfs); FANET_Packet::Print(Name); } - int WriteJSON(char *JSON) const + int PrintJSON(char *JSON, uint8_t AddrType=0) const + { const uint8_t *Msg = this->Msg(); + uint8_t MsgLen = this->MsgLen(); + uint8_t Type = this->Type(); + if(Type!=1 && Type!=7) { JSON[0]=0; return 0; } + int Len=0; + JSON[Len++]='{'; + uint32_t Address = getAddr(); + if(AddrType==0) AddrType = getAddrType(); + Len+=Format_String(JSON+Len, "\"Address\":\""); + Len+=Format_Hex(JSON+Len, Byte[1]); + Len+=Format_Hex(JSON+Len, Byte[3]); + Len+=Format_Hex(JSON+Len, Byte[2]); + Len+=Format_String(JSON+Len, "\", \"AddrType\":"); + JSON[Len++] = '0'+AddrType; + Len+=Format_String(JSON+Len, "\", \"ID\":\""); + Len+=Format_Hex(JSON+Len, Address | ((uint32_t)AddrType<<24)); + uint32_t Time = SlotTime(); // sTime; if(msTime<100) Time--; + Len+=Format_String(JSON+Len, "\", \"Time\":"); + Len+=Format_UnsDec(JSON+Len, Time); + if(Type==1) + { const uint8_t OGNtype[8] = { 0, 7, 6, 0xB, 1, 8, 3, 0xD } ; // OGN aircraft types + uint8_t AcftType=Msg[7]>>4; // get the aircraft-type and online-track flag + Len+=Format_String(JSON+Len, ", \"AcftType\":"); + Len+=Format_UnsDec(JSON+Len, OGNtype[AcftType&0x7]); + uint32_t Alt=getAltitude(Msg+6); // [m] decode the altitude + uint32_t Speed=getSpeed(Msg[8]); // [0.5km/h] ground speed + Speed = (Speed*355+0x80)>>8; // [0.5km/h] => [0.1m/s] convert + int32_t Climb=getClimb(Msg[9]); // [0.1m/s] climb rate + uint16_t Dir=getDir(Msg[10]); // [deg] + Len+=Format_String(JSON+Len, ", \"Alt\":"); + Len+=Format_UnsDec(JSON+Len, Alt); + Len+=Format_String(JSON+Len, ", \"Track\":"); + Len+=Format_UnsDec(JSON+Len, Dir); + Len+=Format_String(JSON+Len, ", \"Speed\":"); + Len+=Format_UnsDec(JSON+Len, Speed, 2, 1); + Len+=Format_String(JSON+Len, ", \"Climb\":"); + Len+=Format_SignDec(JSON+Len, Climb, 2, 1, 1); + if(MsgLen>11) + { int16_t Turn=getTurnRate(Msg[11]); + Len+=Format_String(JSON+Len, ", \"Turn\":"); + Len+=Format_SignDec(JSON+Len, Turn*10/4, 2, 1, 1); } + if(MsgLen>12) + { int32_t AltStd=Alt; Alt+=getQNE(Msg[12]); + Len+=Format_String(JSON+Len, ", \"StdAlt\":"); + Len+=Format_SignDec(JSON+Len, AltStd, 1, 0, 1); } + } + if(Type==1 || Type==7) + { int32_t Lat = getLat(Msg); // [cordic] decode the latitude + int32_t Lon = getLon(Msg+3); // [cordic] decode the longitude + Len+=Format_String(JSON+Len, ", \"Lat\":"); + Len+=Format_SignDec(JSON+Len, CoordUBX(Lat), 8, 7, 1); + Len+=Format_String(JSON+Len, ", \"Lon\":"); + Len+=Format_SignDec(JSON+Len, CoordUBX(Lon), 8, 7, 1); } + Len+=Format_String(JSON+Len, ", \"RxProt\":\"FNT\""); + if(SNR>0) + { Len+=Format_String(JSON+Len, ", \"RxSNR\":"); + Len+=Format_SignDec(JSON+Len, ((int16_t)SNR*10+2-843)>>2, 2, 1, 1); } + Len+=Format_String(JSON+Len, ", \"RxErr\":"); + Len+=Format_UnsDec(JSON+Len, BitErr); + Len+=Format_String(JSON+Len, ", \"RxFreqOfs\":"); + Len+=Format_SignDec(JSON+Len, FreqOfs/10, 1, 1); + JSON[Len++]=' '; JSON[Len++]='}'; + JSON[Len]=0; return Len; } + + int WriteStxJSON(char *JSON, uint8_t AddrType=0) const { int Len=0; Len+=Format_String(JSON+Len, "\"addr\":\""); Len+=Format_Hex(JSON+Len, Byte[1]); @@ -334,11 +415,12 @@ class FANET_RxPacket: public FANET_Packet JSON[Len++]='\"'; JSON[Len++]=','; Len+=Format_String(JSON+Len, "\"addr_type\":"); - JSON[Len++] = HexDigit(getAddrType()); + if(AddrType==0) AddrType = getAddrType(); + JSON[Len++] = '0'+AddrType; const uint8_t *Msg = this->Msg(); uint8_t MsgLen = this->MsgLen(); uint8_t Type = this->Type(); - uint32_t Time=sTime; if(msTime<300) Time--; + uint32_t Time = SlotTime(); // sTime; if(msTime<100) Time--; Len+=Format_String(JSON+Len, ",\"time\":"); Len+=Format_UnsDec(JSON+Len, Time); int64_t RxTime=(int64_t)sTime-Time; RxTime*=1000; RxTime+=msTime; @@ -378,7 +460,29 @@ class FANET_RxPacket: public FANET_Packet Len+=Format_String(JSON+Len, ",\"lat_deg\":"); Len+=Format_SignDec(JSON+Len, CoordUBX(Lat), 8, 7, 1); Len+=Format_String(JSON+Len, ",\"lon_deg\":"); - Len+=Format_SignDec(JSON+Len, CoordUBX(Lon), 8, 7, 1); } + Len+=Format_SignDec(JSON+Len, CoordUBX(Lon), 8, 7, 1); + int Idx=7; + if(Service&0x40) + { Len+=Format_String(JSON+Len, ",\"temp_deg\":"); + Len+=Format_SignDec(JSON+Len, (int16_t)5*((int8_t)Msg[Idx++]), 2, 1, 1); } + if(Service&0x20) + { uint16_t Dir = Msg[Idx++]; // [cordic] + Len+=Format_String(JSON+Len, ",\"wind_deg\":"); + Len+=Format_UnsDec(JSON+Len, (45*Dir+16)>>5, 2, 1); + uint16_t Wind = getSpeed(Msg[Idx++]); // [0.2km/h] + Len+=Format_String(JSON+Len, ",\"wind_kmh\":"); + Len+=Format_UnsDec(JSON+Len, 2*Wind, 2, 1); + uint16_t Gust = getSpeed(Msg[Idx++]); + Len+=Format_String(JSON+Len, ",\"gust_kmh\":"); + Len+=Format_UnsDec(JSON+Len, 2*Gust, 2, 1); } + if(Service&0x10) + { Len+=Format_String(JSON+Len, ",\"hum_perc\":"); + Len+=Format_UnsDec(JSON+Len, (uint16_t)4*Msg[Idx++], 2, 1); } + if(Service&0x08) + { Len+=Format_String(JSON+Len, ",\"press_hpa\":"); + Len+=Format_UnsDec(JSON+Len, getPressure(Msg+Idx), 2, 1); + Idx+=2; } + } if(Type==1 || Type==7) // airborne or ground position { int32_t Lat = getLat(Msg); // [cordic] decode the latitude int32_t Lon = getLon(Msg+3); // [cordic] decode the longitude @@ -416,8 +520,9 @@ class FANET_RxPacket: public FANET_Packet Len+=Format_String(JSON+Len, ",\"on_ground\":1"); } return Len; } - int WriteAPRS(char *Out) + int WriteAPRS(char *Out, uint8_t AddrType=0) { bool Report=0; + if(AddrType==0) AddrType = getAddrType(); // 2 (FLARM) or 3 (OGN) int Len=0; bool isPosition = Type()==1 || Type()==4 || Type()==7; Len+=Format_String(Out+Len, "FNT"); @@ -469,7 +574,8 @@ class FANET_RxPacket: public FANET_Packet } else Len+=Format_String(Out+Len, ".../...g..."); Out[Len++]='t'; if(Service&0x40) - { int16_t Fahr=Temp; Fahr+=4*Temp/5; Fahr+=32; + { // int16_t Fahr=Temp; Fahr+=4*Temp/5; Fahr/=2; Fahr+=32; // + int16_t Fahr = (((int16_t)Temp*115+64)>>7) + 32; // [0.5degC] => [degF] if(Fahr>=0) Len+=Format_UnsDec(Out+Len, Fahr, 3); else Len+=Format_SignDec(Out+Len, Fahr, 2); } else Len+=Format_String(Out+Len, "..."); @@ -486,7 +592,6 @@ class FANET_RxPacket: public FANET_Packet const uint8_t OGNtype[8] = { 0, 7, 6, 0xB, 1, 8, 3, 0xD } ; // OGN aircraft types uint8_t AcftType=Msg[7]>>4; // aircraft-type and online-tracking flag const char *Icon = AcftIcon[AcftType&7]; // APRS icon - uint8_t AddrType = getAddrType(); // 2 (FLARM) or 3 (OGN) uint32_t ID = (OGNtype[AcftType&7]<<2) | AddrType; // acft-type and addr-type bool Track = AcftType&0x08; // online tracking flag if(!Track) ID|=0x80; // if no online tracking the set as stealth flag @@ -542,7 +647,6 @@ class FANET_RxPacket: public FANET_Packet const char *Icon = "\\n"; // static object if(Status>=13) Icon = "\\!"; // Emergency // const char *StatMsg = StatusMsg[Status]; - uint8_t AddrType = getAddrType(); // uint8_t AcftType = 15; // uint32_t ID = (AcftType<<2) | AddrType; // acft-type and addr-type if(!Track) ID|=0x80; // stealth flag @@ -565,9 +669,11 @@ class FANET_RxPacket: public FANET_Packet Len+=Format_String(Out+Len, " FNT7"); Out[Len++]=HexDigit(Status); Report=1; break; } } + Out[Len++]=' '; Out[Len++]='s'; + Len+=Format_Hex(Out+Len, Sync); if(SNR>0) { Out[Len++]=' '; - Len+=Format_UnsDec(Out+Len, ((uint16_t)SNR*10+2)/4, 2, 1); + Len+=Format_SignDec(Out+Len, ((int16_t)SNR*10+2-843)>>2, 2, 1, 1); Out[Len++]='d'; Out[Len++]='B'; } Out[Len++]=' '; Len+=Format_SignDec(Out+Len, FreqOfs/10, 2, 1); @@ -581,6 +687,8 @@ class FANET_RxPacket: public FANET_Packet // ========================================================================================= +#ifndef ARDUINO + class FANET_Name { public: static const int MaxSize = 32; @@ -598,6 +706,8 @@ class FANET_Name } ; +#include + class FANET_NameList { public: std::map List; @@ -612,6 +722,7 @@ class FANET_NameList return 1; } } ; +#endif // =============================================================================================== diff --git a/utils/format.cpp b/utils/format.cpp index 37140f0..98aa6ef 100644 --- a/utils/format.cpp +++ b/utils/format.cpp @@ -66,6 +66,9 @@ uint8_t Format_String(char *Out, const char *String, uint8_t MinLen, uint8_t Max void Format_Hex( void (*Output)(char), uint8_t Byte ) { (*Output)(HexDigit(Byte>>4)); (*Output)(HexDigit(Byte&0x0F)); } +void Format_HexBytes( void (*Output)(char), const uint8_t *Byte, uint8_t Bytes) +{ for(uint8_t Idx=0; Idx>8)); Format_Hex(Output, (uint8_t)Word); } @@ -73,6 +76,10 @@ void Format_Hex( void (*Output)(char), uint32_t Word ) { Format_Hex(Output, (uint8_t)(Word>>24)); Format_Hex(Output, (uint8_t)(Word>>16)); Format_Hex(Output, (uint8_t)(Word>>8)); Format_Hex(Output, (uint8_t)Word); } +void Format_Hex( void (*Output)(char), uint64_t Word ) +{ Format_Hex(Output, (uint32_t)(Word>>32)); + Format_Hex(Output, (uint32_t)(Word )); } + void Format_MAC( void (*Output)(char), uint8_t *MAC, uint8_t Len) { for(uint8_t Idx=0; Idx>4); (*Output++)=HexDigit(Byte&0x0F); return 2; } +uint8_t Format_HexBytes(char *Output, const uint8_t *Byte, uint8_t Bytes) +{ uint8_t Len=0; + for(uint8_t Idx=0; Idx>8)); Format_Hex(Output+2, (uint8_t)Word); return 4; } +{ Format_Hex(Output, (uint8_t)(Word>>8)); + Format_Hex(Output+2, (uint8_t)Word); + return 4; } uint8_t Format_Hex( char *Output, uint32_t Word ) -{ Format_Hex(Output , (uint8_t)(Word>>24)); Format_Hex(Output+2, (uint8_t)(Word>>16)); - Format_Hex(Output+4, (uint8_t)(Word>> 8)); Format_Hex(Output+6, (uint8_t) Word ); return 8; } +{ Format_Hex(Output , (uint16_t)(Word>>16)); + Format_Hex(Output+4, (uint16_t)(Word )); + return 8; } + +uint8_t Format_Hex( char *Output, uint64_t Word ) +{ Format_Hex(Output , (uint32_t)(Word>>32)); + Format_Hex(Output+8, (uint32_t)(Word )); + return 16; } uint8_t Format_Hex( char *Output, uint32_t Word, uint8_t Digits) { for(uint8_t Idx=Digits; Idx>0; ) @@ -249,11 +288,16 @@ int16_t Read_Dec3(const char *Inp) // convert three digit decimal nu int8_t Low=Read_Dec1(Inp[2]); if(Low<0) return -1; return (int16_t)Low + (int16_t)10*(int16_t)Mid + (int16_t)100*(int16_t)High; } -int16_t Read_Dec4(const char *Inp) // convert three digit decimal number into an integer +int16_t Read_Dec4(const char *Inp) // convert four digit decimal number into an integer { int16_t High=Read_Dec2(Inp ); if(High<0) return -1; int16_t Low =Read_Dec2(Inp+2); if(Low<0) return -1; return Low + (int16_t)100*(int16_t)High; } +int32_t Read_Dec5(const char *Inp) // convert four digit decimal number into an integer +{ int16_t High=Read_Dec2(Inp ); if(High<0) return -1; + int16_t Low =Read_Dec3(Inp+2); if(Low<0) return -1; + return (int32_t)Low + (int32_t)1000*(int32_t)High; } + // ------------------------------------------------------------------------------------------ int8_t Read_Coord(int32_t &Lat, const char *Inp) diff --git a/utils/format.h b/utils/format.h index 808c311..f0c3ad1 100644 --- a/utils/format.h +++ b/utils/format.h @@ -16,7 +16,11 @@ void Format_String( void (*Output)(char), const char *String, uint8_t MinLen, void Format_Hex( void (*Output)(char), uint8_t Byte ); void Format_Hex( void (*Output)(char), uint16_t Word ); void Format_Hex( void (*Output)(char), uint32_t Word ); -void Format_MAC( void (*Output)(char), uint8_t *MAC, uint8_t Len=6); +void Format_Hex( void (*Output)(char), uint64_t Word ); +// void Format_Hex( void (*Output)(char), uint32_t Word, uint8_t Digits); +void Format_MAC( void (*Output)(char), const uint8_t *MAC, uint8_t Len=6); + +void Format_HexBytes( void (*Output)(char), const uint8_t *Byte, uint8_t Bytes); void Format_UnsDec ( void (*Output)(char), uint16_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0); void Format_SignDec( void (*Output)(char), int16_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0, uint8_t NoPlus=0); @@ -33,11 +37,16 @@ uint8_t Format_String(char *Out, const char *String, uint8_t MinLen, uint8_t Max uint8_t Format_UnsDec (char *Out, uint32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0); uint8_t Format_SignDec(char *Out, int32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0, uint8_t NoPlus=0); -uint8_t Format_Hex( char *Output, uint8_t Byte ); -uint8_t Format_Hex( char *Output, uint16_t Word ); -uint8_t Format_Hex( char *Output, uint32_t Word ); -uint8_t Format_Hex( char *Output, uint32_t Word, uint8_t Digits); -uint8_t Format_Hex( char *Output, uint64_t Word ); +uint8_t Format_Hex(char *Output, uint8_t Byte ); +uint8_t Format_Hex(char *Output, uint16_t Word ); +uint8_t Format_Hex(char *Output, uint32_t Word ); +uint8_t Format_Hex(char *Output, uint32_t Word, uint8_t Digits); +uint8_t Format_Hex(char *Output, uint64_t Word ); + +uint8_t Format_HexBytes(char *Output, const uint8_t *Byte, uint8_t Bytes); + +// uint8_t Format_Hex(char *Output, const uint8_t *Bytes, uint8_t Len ); + // uint8_t Format_Hex( char *Output, uint64_t Word, uint8_t Digits); template @@ -47,9 +56,19 @@ template Word>>=4; } return Digits; } +template + void Format_Bin( void (*Output)(char), Type Word) +{ const uint8_t Digits = sizeof(Type)<<3; + for( Type Mask = (Type)1<<(Digits-1); Mask; Mask>>=1) + { bool Bit = Word&Mask; + (*Output)('0'+Bit); } +} + uint8_t Format_HHcMMcSS(char *Out, uint32_t Time); uint8_t Format_HHMMSS(char *Out, uint32_t Time); void Format_HHMMSS(void (*Output)(char), uint32_t Time); +uint8_t Format_Period(char *Out, int32_t Time); +void Format_Period(void (*Output)(char), int32_t Time); uint8_t Format_Latitude (char *Out, int32_t Lat); // [1/600000deg] => DDMM.MMMMs uint8_t Format_Longitude(char *Out, int32_t Lon); // [1/600000deg] => DDDMM.MMMMs @@ -60,7 +79,8 @@ int8_t Read_Dec1(char Digit); // convert single digit into an inline int8_t Read_Dec1(const char *Inp) { return Read_Dec1(Inp[0]); } int8_t Read_Dec2(const char *Inp); // convert two digit decimal number into an integer int16_t Read_Dec3(const char *Inp); // convert three digit decimal number into an integer -int16_t Read_Dec4(const char *Inp); // convert three digit decimal number into an integer +int16_t Read_Dec4(const char *Inp); // convert four digit decimal number into an integer +int32_t Read_Dec5(const char *Inp); // convert five digit decimal number into an integer template int8_t Read_Hex(Type &Int, const char *Inp, uint8_t MaxDig=0) // convert variable number of digits hexadecimal number into an integer @@ -102,7 +122,8 @@ template { Dig=Read_UnsDec(Value, Inp+Len); } if(Dig<=0) return Dig; Len+=Dig; - if(Sign=='-') Value=(-Value); return Len; } + if(Sign=='-') Value=(-Value); + return Len; } template int8_t Read_Float1(Type &Value, const char *Inp) // read floating point, take just one digit after decimal point diff --git a/utils/gdl90.cpp b/utils/gdl90.cpp index 1f547f8..61bb8ae 100644 --- a/utils/gdl90.cpp +++ b/utils/gdl90.cpp @@ -104,16 +104,35 @@ static int GDL90_SendEsc(void (*Output)(char), uint8_t Byte) // shall we escap (*Output)((char)Byte); return 1; } int GDL90_Send(void (*Output)(char), uint8_t ID, const uint8_t *Data, int Len) -{ int Count=0; uint16_t CRC=0; - (*Output)((char)GDL90_Flag); Count++; +{ int OutLen=0; uint16_t CRC=0; + (*Output)((char)GDL90_Flag); OutLen++; CRC=GDL90_CRC16(ID, CRC); - Count+=GDL90_SendEsc(Output, ID); + OutLen+=GDL90_SendEsc(Output, ID); for( int Idx=0; Idx>8); - (*Output)((char)GDL90_Flag); Count++; - return Count; } + OutLen+=GDL90_SendEsc(Output, Byte); } + OutLen+=GDL90_SendEsc(Output, CRC&0xFF); + OutLen+=GDL90_SendEsc(Output, CRC>>8); + (*Output)((char)GDL90_Flag); OutLen++; + return OutLen; } + +static int GDL90_SendEsc(uint8_t *Out, uint8_t Byte) // shall we escape control characters as well ? +{ // if(Byte<0x20 || Byte==GDL90_Flag || Byte==GDL90_Esc) { Out[0]=GDL90_Esc; Byte^=0x20; Out[]=Byte; return 2; } + if(Byte==GDL90_Flag || Byte==GDL90_Esc) { Out[0]=GDL90_Esc; Byte^=0x20; Out[1]=Byte; return 2; } // ESCape some characters + Out[0]=Byte; return 1; } + +int GDL90_Send(uint8_t *Out, uint8_t ID, const uint8_t *Data, int Len) +{ int OutLen=0; uint16_t CRC=0; + Out[OutLen++]=GDL90_Flag; + CRC=GDL90_CRC16(ID, CRC); + OutLen+=GDL90_SendEsc(Out+OutLen, ID); + for( int Idx=0; Idx>8); + Out[OutLen++]=GDL90_Flag; + return OutLen; } diff --git a/utils/gdl90.h b/utils/gdl90.h index 1829833..25df3a0 100644 --- a/utils/gdl90.h +++ b/utils/gdl90.h @@ -12,6 +12,8 @@ uint16_t GDL90_CRC16(uint8_t Byte, uint16_t CRC); // pass 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); } // ================================================================================= @@ -55,6 +57,13 @@ class GDL90_HEARTBEAT // Heart-beat packet to be send at every UTC second 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; } @@ -68,9 +77,81 @@ class GDL90_HEARTBEAT // Heart-beat packet to be send at every UTC second 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]; @@ -82,17 +163,31 @@ class GDL90_GEOMALT // Geometrical altitude: ID = 11 (GPS ref. to Ellipsoid) uint8_t Data[Size]; public: + void Clear(void) { for(int Idx=0; Idx0x7FFF) 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 @@ -126,7 +221,7 @@ class GDL90_REPORT // Position report: Traffic: ID = 20, Ownship: ID = 10 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<27; Idx++) Data[Idx]=0; } // clear all data (Lat/Lon = invalid) + void Clear(void) { for(int Idx=0; Idx>4; } // 0 = no alert, 1 = alert void setAlertStatus(uint8_t Status) { Data[0] = (Data[0]&0x0F) | (Status<<4); } @@ -162,20 +257,25 @@ class GDL90_REPORT // Position report: Traffic: ID = 20, Ownship: ID = 10 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*4; } // [fpm] + { 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 setAcftType(uint8_t AcftType) // set OGN-type aricrraft-type + 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]); } @@ -199,13 +299,17 @@ class GDL90_REPORT // Position report: Traffic: ID = 20, Ownship: ID = 10 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 %X/%X %dft %+dfpm [%+09.5f,%+010.5f] %03.0f/%dkt\n", - getAddrType(), getAddress(), getAcftCat(), getAcftCall(), - getNIC(), getNACp(), getAltitude(), getClimbRate(), + { 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()); } + (360.0/256)*getHeading(), getSpeed(), + getMiscInd(), getAlertStatus(), getPriority()); } } ; @@ -213,7 +317,7 @@ class GDL90_REPORT // Position report: Traffic: ID = 20, Ownship: ID = 10 class GDL90_RxMsg // receiver for the MAV messages { public: - static const uint8_t MaxBytes = 32; // max. number of bytes + 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 @@ -223,8 +327,24 @@ class GDL90_RxMsg // receiver for the MAV messages public: void Clear(void) { Len=0; Byte[Len]=0; } - void Print(void) - { printf("GDL90[%d] ", Len); + 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>8)!=Byte[Len-1] ) { Clear(); return 0; } - return 2; } + 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 diff --git a/utils/nmea.cpp b/utils/nmea.cpp index 239b271..52f0910 100644 --- a/utils/nmea.cpp +++ b/utils/nmea.cpp @@ -7,13 +7,13 @@ uint8_t NMEA_Check(uint8_t *NMEA, uint8_t Len) // NMEA check-sum return Check; } uint8_t NMEA_AppendCheck(uint8_t *NMEA, uint8_t Len) -{ uint8_t Check=NMEA_Check(NMEA+1, Len-1); - NMEA[Len ]='*'; - uint8_t Digit=Check>>4; NMEA[Len+1] = Digit<10 ? Digit+'0':Digit+'A'-10; +{ uint8_t Check=NMEA_Check(NMEA+1, Len-1); // exclude the starting '$' + NMEA[Len ]='*'; // add '*' + uint8_t Digit=Check>>4; NMEA[Len+1] = Digit<10 ? Digit+'0':Digit+'A'-10; // and checksum in hex Digit=Check&0xF; NMEA[Len+2] = Digit<10 ? Digit+'0':Digit+'A'-10; - return 3; } + return 3; } // return number of characters added uint8_t NMEA_AppendCheckCRNL(uint8_t *NMEA, uint8_t Len) -{ uint8_t CheckLen=NMEA_AppendCheck(NMEA, Len); - Len+=CheckLen; NMEA[Len]='\n'; - return CheckLen+1; } +{ uint8_t CheckLen=NMEA_AppendCheck(NMEA, Len); // add *XY + Len+=CheckLen; NMEA[Len++]='\r'; NMEA[Len++]='\n'; NMEA[Len]=0; // add CR, LF, NL + return CheckLen+2; } diff --git a/utils/nmea.h b/utils/nmea.h index 94c71cd..40adf12 100644 --- a/utils/nmea.h +++ b/utils/nmea.h @@ -9,9 +9,9 @@ inline uint8_t NMEA_AppendCheck(char *NMEA, uint8_t Len) { return NMEA_AppendChe uint8_t NMEA_AppendCheckCRNL(uint8_t *NMEA, uint8_t Len); inline uint8_t NMEA_AppendCheckCRNL(char *NMEA, uint8_t Len) { return NMEA_AppendCheckCRNL((uint8_t*)NMEA, Len); } - class NMEA_RxMsg // receiver for the NMEA sentences + class NMEA_RxMsg // receiver for the NMEA sentences { public: - static const uint8_t MaxLen=104; // maximum length + static const uint8_t MaxLen=120; // maximum length static const uint8_t MaxParms=24; // maximum number of parameters (commas) uint8_t Data[MaxLen]; // the message itself uint8_t Len; // number of bytes @@ -35,8 +35,13 @@ inline uint8_t NMEA_AppendCheckCRNL(char *NMEA, uint8_t Len) { return NMEA_Appen if(Len==0) // if data is empty { if(Byte!='$') return; // then ignore all bytes but '$' Data[Len++]=Byte; // start storing the frame - setLoading(); Check=0x00; Parms=0; // set state to "isLoading", clear checksum - } else // if not empty (being loaded) + setLoading(); Check=0x00; Parms=0; } // set state to "isLoading", clear checksum +/* + if(Byte=='$') // should '$' sign restart an ongoing NMEA sentence ? + { Len=0; Data[Len++]=Byte; + setLoading(); Check=0x00; Parms=0; } +*/ + else { if((Byte=='\r')||(Byte=='\n')) // if CR (or NL ?) then frame is complete { setComplete(); if(Len if(abs(DistDeltaH)>=200) return 1; // if extrapolation error more than 50m int16_t Turn = Packet->DecodeTurnRate(); // [0.1deg/s] int16_t CFaccel = ((int32_t)Turn*Speed*229+0x10000)>>17; // [0.1m/s^2] centrifugal acceleration in turn - if(abs(CFaccel)>=50) return 1; // CFaccel at or above 5m/s^2 (0.5g) + if(abs(CFaccel)>=25) return 1; // CFaccel at or above 5m/s^2 (0.5g) int16_t PrevTurn = PrevPacket->DecodeTurnRate(); // [0.1deg/s] int16_t PrevCFaccel = ((int32_t)PrevTurn*PrevSpeed*229+0x10000)>>17; // [0.1m/s^2] - int32_t DistDeltaR = abs(CFaccel-PrevCFaccel)*TimeDelta*TimeDelta/2; // [0.1m] + int32_t DistDeltaR = abs(CFaccel-PrevCFaccel)*(int32_t)TimeDelta*TimeDelta/2; // [0.1m] if(abs(DistDeltaR)>=200) return 1; // [0.1m] return 0; } @@ -63,12 +64,12 @@ template // --------------------------------------------------------------------------------------------------------------------- template - class OGN_TxPacket // OGN packet with FEC code, like for transmission + class OGN_TxPacket // OGN packet with FEC code, ready for transmission { public: static const int Words = 7; static const int Bytes = 26; - OGNx_Packet Packet; // OGN packet + OGNx_Packet Packet; // OGN packet [20 bytes = 160 bits] uint32_t FEC[2]; // Gallager code: 48 check bits for 160 user bits @@ -82,7 +83,7 @@ template Len+=Format_Hex(Out+Len, (uint8_t)(Addr>>16)); Len+=Format_Hex(Out+Len, (uint16_t)Addr); Out[Len++]=' '; - Len+=Format_UnsDec(Out+Len, (uint16_t)Packet.Position.Time, 2); + Len+=Format_UnsDec(Out+Len, (uint32_t)Packet.Position.Time, 2); Out[Len++]=' '; Len+=Format_Latitude(Out+Len, Packet.DecodeLatitude()); Out[Len++]=' '; @@ -180,13 +181,15 @@ template uint32_t FEC[2]; // Gallager code: 48 check bits for 160 user bits union - { uint8_t State; // + { uint8_t State; // state bits and small values struct - { bool Saved :1; // has been already saved in internal storage - bool Ready :1; // is ready for transmission - bool Sent :1; // has already been transmitted out + { // bool Saved :1; // has been already saved in internal storage + // bool Ready :1; // is ready for transmission + // bool Sent :1; // has already been transmitted out + bool Alloc :1; // allocated in a queue or list bool Correct :1; // correctly received or corrected by FEC uint8_t RxErr:4; // number of bit errors corrected upon reception + uint8_t Warn :2; // LookOut warning level } ; } ; @@ -194,6 +197,10 @@ template uint8_t RxRSSI; // [-0.5dBm] uint8_t Rank; // rank: low altitude and weak signal => high rank + int16_t LatDist; // [m] distance along the latitude + int16_t LonDist; // [m] distance along the longitude + // int16_t AltDist; // [m] + public: OGN_RxPacket() { Clear(); } @@ -229,7 +236,7 @@ template +Count1s(FEC[0]^RefPacket.FEC[0]) +Count1s((FEC[1]^RefPacket.FEC[1])&0xFFFF); } - void calcRelayRank(int32_t RxAltitude) // [0.1m] altitude of reception + void calcRelayRank(int32_t RxAltitude) // [m] altitude of reception { if(Packet.Header.Emergency) { Rank=0xFF; return; } // emergency packets always highest rank Rank=0; if(Packet.Header.NonPos) return; // only relay position packets @@ -237,9 +244,10 @@ template if(Packet.Header.Relay) return; // no rank for relayed packets (only single relay) if(RxRSSI>128) // [-0.5dB] weaker signal => higher rank Rank += (RxRSSI-128)>>2; // 1point/2dB less signal - RxAltitude -= 10*Packet.DecodeAltitude(); // [0.1m] lower altitude => higher rank - if(RxAltitude>0) - Rank += RxAltitude>>9; // 2points/100m of altitude below + if(Packet.Header.Encrypted) return; // for exncrypted packets we only take signal strength + RxAltitude -= Packet.DecodeAltitude(); // [m] receiver altitude - target altitude + if(RxAltitude>0) // + Rank += RxAltitude>>6; // 1points/64m of altitude below int16_t ClimbRate = Packet.DecodeClimbRate(); // [0.1m/s] higher sink rate => higher rank if(ClimbRate<0) Rank += (-ClimbRate)>>3; // 1point/0.8m/s of sink @@ -375,7 +383,7 @@ template { uint8_t Len=0; Len+=Format_String(NMEA+Len, "$POGNT,"); // sentence name if(Packet.Position.Time<60) - Len+=Format_UnsDec(NMEA+Len, (uint16_t)Packet.Position.Time, 2); // [sec] time + Len+=Format_UnsDec(NMEA+Len, (uint32_t)Packet.Position.Time, 2); // [sec] time NMEA[Len++]=','; NMEA[Len++]=HexDigit(Packet.Position.AcftType); // [0..F] aircraft-type: 1=glider, 2=tow plane, etc. NMEA[Len++]=','; @@ -390,7 +398,7 @@ template NMEA[Len++]='0'+Packet.Position.FixQuality; // [] fix quality NMEA[Len++]='0'+Packet.Position.FixMode; // [] fix mode NMEA[Len++]=','; - Len+=Format_UnsDec(NMEA+Len, (uint16_t)(Packet.DecodeDOP()+10),2,1); // [] Dilution of Precision + Len+=Format_UnsDec(NMEA+Len, (uint32_t)(Packet.DecodeDOP()+10),2,1); // [] Dilution of Precision NMEA[Len++]=','; Len+=Format_Latitude(NMEA+Len, Packet.DecodeLatitude()); // [] Latitude NMEA[Len++]=','; @@ -409,9 +417,9 @@ template NMEA[Len++]=','; Len+=Format_SignDec(NMEA+Len, Packet.DecodeTurnRate(), 2, 1); // [deg/s] turning rate (by GPS) NMEA[Len++]=','; - Len+=Format_SignDec(NMEA+Len, -(int16_t)RxRSSI/2); // [dBm] received signal level + Len+=Format_SignDec(NMEA+Len, -(int32_t)RxRSSI/2); // [dBm] received signal level NMEA[Len++]=','; - Len+=Format_UnsDec(NMEA+Len, (uint16_t)RxErr); // [bits] corrected transmisison errors + Len+=Format_UnsDec(NMEA+Len, (uint32_t)RxErr); // [bits] corrected transmisison errors Len+=NMEA_AppendCheckCRNL(NMEA, Len); NMEA[Len]=0; return Len; } @@ -425,19 +433,23 @@ template uint8_t WritePFLAA(char *NMEA, uint8_t Status, int32_t LatDist, int32_t LonDist, int32_t AltDist) { uint8_t Len=0; - Len+=Format_String(NMEA+Len, "$PFLAA,"); // sentence name and alarm-level (but no alarms for trackers) + Len+=Format_String(NMEA+Len, "$PFLAA,"); // sentence name and alarm-level (but no alarms for trackers) NMEA[Len++]='0'+Status; NMEA[Len++]=','; Len+=Format_SignDec(NMEA+Len, LatDist); NMEA[Len++]=','; Len+=Format_SignDec(NMEA+Len, LonDist); NMEA[Len++]=','; - Len+=Format_SignDec(NMEA+Len, AltDist); // [m] relative altitude + Len+=Format_SignDec(NMEA+Len, AltDist); // [m] relative altitude NMEA[Len++]=','; - NMEA[Len++]='0'+Packet.Header.AddrType; // address-type (3=OGN) + uint8_t AddrType = Packet.Header.AddrType; +#ifdef WITH_SKYDEMON + if(AddrType!=1) AddrType=2; // SkyDemon only accepts 1 or 2 +#endif + NMEA[Len++]='0'+AddrType; // address-type (3=OGN) NMEA[Len++]=','; uint32_t Addr = Packet.Header.Address; // [24-bit] address - Len+=Format_Hex(NMEA+Len, (uint8_t)(Addr>>16)); // XXXXXX 24-bit address: RND, ICAO, FLARM, OGN + Len+=Format_Hex(NMEA+Len, (uint8_t)(Addr>>16)); // XXXXXX 24-bit address: RND, ICAO, FLARM, OGN Len+=Format_Hex(NMEA+Len, (uint16_t)Addr); NMEA[Len++]=','; Len+=Format_UnsDec(NMEA+Len, Packet.DecodeHeading(), 4, 1); // [deg] heading (by GPS) @@ -451,7 +463,7 @@ template NMEA[Len++]=HexDigit(Packet.Position.AcftType); // [0..F] aircraft-type: 1=glider, 2=tow plane, etc. Len+=NMEA_AppendCheckCRNL(NMEA, Len); NMEA[Len]=0; - return Len; } // return number of formatted characters + return Len; } // return number of formatted characters void Print(void) const { printf("[%02d/%+6.1fdBm/%2d] ", RxChan, -0.5*RxRSSI, RxErr); @@ -465,19 +477,22 @@ template Len+=Format_Hex(Out+Len, (uint8_t)(Addr>>16)); Len+=Format_Hex(Out+Len, (uint16_t)Addr); Out[Len++]=' '; - Len+=Format_SignDec(Out+Len, -(int16_t)RxRSSI/2); Out[Len++]='d'; Out[Len++]='B'; Out[Len++]='m'; - Out[Len++]=' '; - Len+=Format_UnsDec(Out+Len, (uint16_t)Packet.Position.Time, 2); + Len+=Format_SignDec(Out+Len, -(int32_t)RxRSSI/2); Out[Len++]='d'; Out[Len++]='B'; Out[Len++]='m'; Out[Len++]=' '; + Len+=Format_UnsDec(Out+Len, (uint32_t)Packet.Position.Time, 2); + Out[Len++]='s'; Out[Len++]=' '; Len+=Format_Latitude(Out+Len, Packet.DecodeLatitude()); Out[Len++]=' '; Len+=Format_Longitude(Out+Len, Packet.DecodeLongitude()); Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, (uint32_t)Packet.DecodeAltitude()); Out[Len++]='m'; Out[Len++]=' '; - Len+=Format_UnsDec(Out+Len, Packet.DecodeSpeed(), 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s'; + Len+=Format_UnsDec(Out+Len, (uint32_t)Packet.DecodeSpeed(), 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s'; Out[Len++]=' '; - Len+=Format_SignDec(Out+Len, Packet.DecodeClimbRate(), 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s'; + Len+=Format_SignDec(Out+Len, (int32_t)Packet.DecodeClimbRate(), 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s'; + Out[Len++]=' '; + Out[Len++]='r'; + Len+=Format_Hex(Out+Len, Rank); Out[Len++]='\n'; Out[Len]=0; return Len; } @@ -577,13 +592,20 @@ template uint8_t getNew(void) // get (index of) a free or lowest rank packet { Sum-=Packet[LowIdx].Rank; Packet[LowIdx].Rank=0; Low=0; return LowIdx; } // remove old packet from the rank sum - OGN_RxPacket *addNew(uint8_t NewIdx) // add the new packet to the queue + uint8_t size(void) // count all slots with Alloc flag set + { uint8_t Count=0; + for(uint8_t Idx=0; Idx *addNew(uint8_t NewIdx) // add the new packet to the queue { OGN_RxPacket *Prev = 0; + Packet[NewIdx].Alloc=1; // mark this clot as allocated uint32_t AddressAndType = Packet[NewIdx].Packet.getAddressAndType(); // get ID of this packet: ID is address-type and address (2+24 = 26 bits) for(uint8_t Idx=0; Idx uint16_t RankIdx = Rand%Sum; uint8_t Idx; uint16_t RankSum=0; for(Idx=0; IdxRankIdx) return Idx; } return Rand%Size; } void reCalc(void) // find the lowest rank and calc. the sum of all ranks { Sum=Low=Packet[0].Rank; LowIdx=0; // take minimum at the first slot for(uint8_t Idx=1; Idx void cleanTime(uint8_t Time) // clean up slots of given Time { for(int Idx=0; Idx=60) clean(Idx); } } - void clean(uint8_t Idx) // clean given slot - { Sum-=Packet[Idx].Rank; Packet[Idx].Rank=0; Low=0; LowIdx=Idx; } + void clean(uint8_t Idx) // clean given slot, remove it from the sum + { Sum-=Packet[Idx].Rank; Packet[Idx].Rank=0; Packet[Idx].Alloc=0; Low=0; LowIdx=Idx; } void decrRank(uint8_t Idx, uint8_t Decr=1) // decrement rank of given slot { uint8_t Rank=Packet[Idx].Rank; if(Rank==0) return; // if zero already: do nothing @@ -632,11 +656,15 @@ template uint8_t Print(char *Out) { uint8_t Len=0; for(uint8_t Idx=0; Idx=0) && (Min>=0) && (Sec>=0); } // all data must have been correctly read: negative means not correctly read) bool isDateValid(void) const // is the GPS date valid ? - { return (Year>=0) && (Month>=0) && (Day>=0); } + { return (Year<70) && (Year>0) && (Month>0) && (Day>0); } // MTK GPS can produce fake date with year 1980, so we treat it as invalid - uint8_t incrTime(void) // increment HH:MM:SS by one second + int8_t incrTimeFrac(int16_t msFrac) // [ms] + { mSec+=msFrac; + if(mSec>=1000) { mSec-=1000; return incrTime(); } + else if(mSec<0) { mSec+=1000; return decrTime(); } + return 0; } + + int8_t incrTime(void) // increment HH:MM:SS by one second { Sec++; if(Sec<60) return 0; Sec=0; Min++; if(Min<60) return 0; @@ -671,20 +724,20 @@ class GPS_Time Hour=0; return 1; } // return 1 if date needs to be incremented - uint8_t decrTime(void) // decrement HH:MM:SS by one second + int8_t decrTime(void) // decrement HH:MM:SS by one second { if(Sec>0) { Sec--; return 0; } Sec=60; if(Min>60) { Min--; return 0; } Min=60; if(Hour>0) { Hour--; return 0; } Hour=24; - return 1; } // return 1 if date needs to be decremented + return -1; } // return 1 if date needs to be decremented - int32_t calcTimeDiff(GPS_Time &RefTime) const - { int32_t TimeDiff = ((int32_t)Min*6000+(int16_t)Sec*100+FracSec) - ((int32_t)RefTime.Min*6000+(int16_t)RefTime.Sec*100+RefTime.FracSec); - if(TimeDiff<(-180000)) TimeDiff+=360000; // wrap-around 60min - else if(TimeDiff>=180000) TimeDiff-=360000; - return TimeDiff; } // [0.01s] + int32_t calcTimeDiff(const GPS_Time &RefTime) const + { int32_t TimeDiff = msDayTime() - RefTime.msDayTime(); // [ms] + if(TimeDiff<(-(int32_t)mSecsPerDay/2)) TimeDiff+=mSecsPerDay; // wrap-around one day + else if(TimeDiff>= (int32_t)mSecsPerDay/2 ) TimeDiff-=mSecsPerDay; + return TimeDiff; } // [ms] uint8_t MonthDays(void) // number of days per month { const uint16_t Table = 0x0AD5; // 1010 1101 0101 0=30days, 1=31days @@ -708,7 +761,7 @@ class GPS_Time void decrTimeDate(void) { if(decrTime()) decrDate(); } void copyTime(GPS_Time &RefTime) // copy HH:MM:SS.SSS from another record - { FracSec = RefTime.FracSec; + { mSec = RefTime.mSec; Sec = RefTime.Sec; Min = RefTime.Min; Hour = RefTime.Hour; } @@ -719,18 +772,26 @@ class GPS_Time Year = RefTime.Year; } void copyTimeDate(GPS_Time &RefTime) { copyTime(RefTime); copyDate(RefTime); } - uint32_t getDayTime(void) const - { return Times60((uint32_t)(Times60((uint16_t)Hour) + Min)) + Sec; } // this appears to save about 100 bytes of code + + uint32_t msDayTime(void) const // [ms] + { return getDayTime()*1000+mSec; } + + uint32_t getDayTime(void) const // [sec] time within the day + { return Times60((uint32_t)(Times60((uint16_t)Hour) + Min)) + Sec; } // this appears to save about 100 bytes of code on STM32 // return (uint32_t)Hour*SecsPerHour + (uint16_t)Min*SecsPerMin + Sec; } // compared to this line uint32_t getUnixTime(void) const // return the Unix timestamp (tested 2000-2037) { uint16_t Days = DaysSinceYear2000() + DaysSimce1jan(); return Times60(Times60(Times24((uint32_t)(Days+10957)))) + getDayTime(); } - uint32_t getFatTime(void) const // return timestamp in FAT format - { uint16_t Date = ((uint16_t)(Year+20)<<9) | ((uint16_t)Month<<5) | Day; - uint16_t Time = ((uint16_t)Hour<<11) | ((uint16_t)Min<<5) | (Sec>>1); - return ((uint32_t)Date<<16) | Time; } + uint16_t getFatDate(void) const // return date in FAT format + { return ((uint16_t)(Year+20)<<9) | ((uint16_t)Month<<5) | Day; } + + uint16_t getFatTime(void) const // return time in FAT format + { return ((uint16_t)Hour<<11) | ((uint16_t)Min<<5) | (Sec>>1); } + + uint32_t getFatDateTime(void) const // return date+time in FAT format + { return ((uint32_t)getFatDate()<<16) | getFatTime(); } void setUnixTime(uint32_t Time) // works except for the 1.1.2000 { uint32_t Days = Time/SecsPerDay; // [day] since 1970 @@ -738,30 +799,68 @@ class GPS_Time Hour = DayTime/SecsPerHour; DayTime -= (uint32_t)Hour*SecsPerHour; // Min = DayTime/SecsPerMin; DayTime -= (uint16_t)Min*SecsPerMin; Sec = DayTime; - FracSec=0; + mSec=0; Days -= 10957+1; // [day] since 2000 minus 1 day Year = (Days*4)/((365*4)+1); // [year] since 1970 Days -= 365*Year + (Year/4); Month = Days/31; Day = Days-(uint16_t)Month*31+1; Month++; + uint8_t MaxDay=MonthDays(); + if(Day>MaxDay) { Day-=MaxDay; Month++; } uint32_t CheckTime = getUnixTime(); - if(CheckTimemin; Sec = TIMEUTC->sec; if(TIMEUTC->nano<0) { decrTimeDate(); TIMEUTC->nano+=1000000000; } - FracSec = (TIMEUTC->nano+5000000)/10000000; // [ms] - if(FracSec>=100) { incrTimeDate(); FracSec-=100; } + mSec = (TIMEUTC->nano+500000)/1000000; // [ms] + if(mSec>=1000) { incrTimeDate(); mSec-=1000; } hasTime = (TIMEUTC->valid&0x02)!=0; return hasTime; } @@ -987,12 +1110,13 @@ class GPS_Position: public GPS_Time return 1; } int8_t ReadNMEA(NMEA_RxMsg &RxMsg) - { if(RxMsg.isGPGGA()) return ReadGGA(RxMsg); - if(RxMsg.isGNGGA()) return ReadGGA(RxMsg); - if(RxMsg.isGPRMC()) return ReadRMC(RxMsg); - if(RxMsg.isGNRMC()) return ReadRMC(RxMsg); - if(RxMsg.isGPGSA()) return ReadGSA(RxMsg); - if(RxMsg.isGNGSA()) return ReadGSA(RxMsg); + { // if(RxMsg.isGPGGA()) return ReadGGA(RxMsg); + if(RxMsg.isGxGGA()) return ReadGGA(RxMsg); + // if(RxMsg.isGPRMC()) return ReadRMC(RxMsg); + if(RxMsg.isGxRMC()) return ReadRMC(RxMsg); + // if(RxMsg.isGPGSA()) return ReadGSA(RxMsg); + if(RxMsg.isGxGSA()) return ReadGSA(RxMsg); + if(RxMsg.isPGRMZ()) return ReadPGRMZ(RxMsg); // (pressure) altitude // if(RxMsg.isGxGSV()) return ReadGSV(RxMsg); return 0; } @@ -1004,6 +1128,17 @@ class GPS_Position: public GPS_Time // Err=ReadGSV(NMEA); if(Err!=(-1)) return Err; return 0; } + int8_t ReadPGRMZ(NMEA_RxMsg &RxMsg) + { if(RxMsg.Parms<3) return -2; + int8_t Ret=Read_Float1(StdAltitude, (const char *)(RxMsg.ParmPtr(0))); + if(Ret<=0) return -1; + char Unit=RxMsg.ParmPtr(1)[0]; + hasBaro=1; Pressure=0; Temperature=0; + if(Unit=='m' || Unit=='M') return 1; + if(Unit!='f' && Unit!='F') return -1; + StdAltitude = (StdAltitude*312+512)>>10; + return 1; } + int8_t ReadGGA(NMEA_RxMsg &RxMsg) { if(RxMsg.Parms<14) return -2; // no less than 14 paramaters hasGPS = ReadTime((const char *)RxMsg.ParmPtr(0))>0; // read time and check if same as the RMC says @@ -1017,41 +1152,106 @@ class GPS_Position: public GPS_Time ReadAltitude(*RxMsg.ParmPtr(9), (const char *)RxMsg.ParmPtr(8)); // Altitude ReadGeoidSepar(*RxMsg.ParmPtr(11), (const char *)RxMsg.ParmPtr(10)); // Geoid separation calcLatitudeCosine(); - return 1; } + NMEAframes++; return 1; } - uint8_t WriteGGA(char *GGA) - { uint8_t Len=0; - Len+=Format_String(GGA+Len, "$GPGGA,"); - Len+=Format_UnsDec(GGA+Len, (uint16_t)Hour, 2); - Len+=Format_UnsDec(GGA+Len, (uint16_t)Min, 2); - Len+=Format_UnsDec(GGA+Len, (uint16_t)Sec, 2); - GGA[Len++]='.'; - Len+=Format_UnsDec(GGA+Len, (uint16_t)FracSec, 2); - GGA[Len++]=','; - Len+=Format_Latitude(GGA+Len, Latitude); - GGA[Len]=GGA[Len-1]; GGA[Len-1]=','; Len++; - GGA[Len++]=','; - Len+=Format_Longitude(GGA+Len, Longitude); - GGA[Len]=GGA[Len-1]; GGA[Len-1]=','; Len++; - GGA[Len++]=','; - GGA[Len++]='0'+FixQuality; - GGA[Len++]=','; - Len+=Format_UnsDec(GGA+Len, (uint16_t)Satellites); - GGA[Len++]=','; - Len+=Format_UnsDec(GGA+Len, (uint16_t)HDOP, 2, 1); - GGA[Len++]=','; - Len+=Format_SignDec(GGA+Len, Altitude, 3, 1); - GGA[Len++]=','; - GGA[Len++]='M'; - GGA[Len++]=','; - Len+=Format_SignDec(GGA+Len, GeoidSeparation, 3, 1); - GGA[Len++]=','; - GGA[Len++]='M'; - GGA[Len++]=','; - GGA[Len++]=','; - Len += NMEA_AppendCheckCRNL(GGA, Len); - GGA[Len]=0; - return Len; } + uint8_t WritePGRMZ(char *NMEA) + { uint8_t Len=Format_String(NMEA, "$PGRMZ,"); + if(hasBaro) Len+=Format_SignDec(NMEA+Len, MetersToFeet(StdAltitude)/10, 1, 0, 1); + Len+=Format_String(NMEA+Len, ",f,"); // normally f for feet, but metres and m works with XcSoar + NMEA[Len++]='0'+FixMode; // 1 = no fix, 2 = 2D, 3 = 3D + Len+=NMEA_AppendCheckCRNL(NMEA, Len); + NMEA[Len]=0; return Len; } + + uint8_t WriteGSA(char *NMEA) const + { uint8_t Len=Format_String(NMEA+Len, "$GPGSA,A,"); + NMEA[Len++]='0'+FixMode; + Len+=Format_String(NMEA+Len, ",,,,,,,,,,,,,"); + if(isValid()) Len+=Format_UnsDec(NMEA+Len, (uint32_t)PDOP, 2, 1); + NMEA[Len++]=','; + if(isValid()) Len+=Format_UnsDec(NMEA+Len, (uint32_t)HDOP, 2, 1); + NMEA[Len++]=','; + if(isValid()) Len+=Format_UnsDec(NMEA+Len, (uint32_t)VDOP, 2, 1); + Len += NMEA_AppendCheckCRNL(NMEA, Len); + NMEA[Len]=0; return Len; } + + uint8_t WriteRMC(char *NMEA) const + { uint8_t Len=Format_String(NMEA+Len, "$GPRMC,"); + if(isTimeValid()) + { Len+=Format_UnsDec(NMEA+Len, (uint32_t)Hour, 2); + Len+=Format_UnsDec(NMEA+Len, (uint32_t)Min, 2); + Len+=Format_UnsDec(NMEA+Len, (uint32_t)Sec, 2); + NMEA[Len++]='.'; + Len+=Format_UnsDec(NMEA+Len, (uint32_t)mSec, 3); + Len--; } + NMEA[Len++]=','; + NMEA[Len++] = isValid()?'A':'V'; + NMEA[Len++]=','; + if(isValid()) + { Len+=Format_Latitude(NMEA+Len, Latitude); + // NMEA[Len+1]=NMEA[Len-1]; NMEA[Len-1]='0'; NMEA[Len]=','; Len+=2; } + NMEA[Len]=NMEA[Len-1]; NMEA[Len-1]=','; Len++; } + else NMEA[Len++]=','; + NMEA[Len++]=','; + if(isValid()) + { Len+=Format_Longitude(NMEA+Len, Longitude); + // NMEA[Len+1]=NMEA[Len-1]; NMEA[Len-1]='0'; NMEA[Len]=','; Len+=2; } + NMEA[Len]=NMEA[Len-1]; NMEA[Len-1]=','; Len++; } + else NMEA[Len++]=','; + NMEA[Len++]=','; + if(isValid()) Len+=Format_UnsDec(NMEA+Len, ((uint32_t)Speed*1985+512)>>10, 2, 1); // [0.1m/s] => [0.1kt] + NMEA[Len++]=','; + if(isValid()) Len+=Format_UnsDec(NMEA+Len, (uint32_t)Heading, 2, 1); + NMEA[Len++]=','; + if(isDateValid()) + { Len+=Format_UnsDec(NMEA+Len, (uint32_t)Day, 2); + Len+=Format_UnsDec(NMEA+Len, (uint32_t)Month, 2); + Len+=Format_UnsDec(NMEA+Len, (uint32_t)Year, 2); } + NMEA[Len++]=','; + NMEA[Len++]=','; + NMEA[Len++]=','; + NMEA[Len++]=isValid()?'A':'N'; + Len += NMEA_AppendCheckCRNL(NMEA, Len); + NMEA[Len]=0; return Len; } + + uint8_t WriteGGA(char *NMEA) const + { uint8_t Len=Format_String(NMEA+Len, "$GPGGA,"); + if(isTimeValid()) + { Len+=Format_UnsDec(NMEA+Len, (uint32_t)Hour, 2); + Len+=Format_UnsDec(NMEA+Len, (uint32_t)Min, 2); + Len+=Format_UnsDec(NMEA+Len, (uint32_t)Sec, 2); + NMEA[Len++]='.'; + Len+=Format_UnsDec(NMEA+Len, (uint32_t)mSec, 3); + Len--; } + NMEA[Len++]=','; + if(isValid()) + { Len+=Format_Latitude(NMEA+Len, Latitude); + // NMEA[Len+1]=NMEA[Len-1]; NMEA[Len-1]='0'; NMEA[Len]=','; Len+=2; } + NMEA[Len]=NMEA[Len-1]; NMEA[Len-1]=','; Len++; } + else NMEA[Len++]=','; + NMEA[Len++]=','; + if(isValid()) + { Len+=Format_Longitude(NMEA+Len, Longitude); + // NMEA[Len+1]=NMEA[Len-1]; NMEA[Len-1]='0'; NMEA[Len]=','; Len+=2; } + NMEA[Len]=NMEA[Len-1]; NMEA[Len-1]=','; Len++; } + else NMEA[Len++]=','; + NMEA[Len++]=','; + if(isValid()) NMEA[Len++]='0'+FixQuality; + NMEA[Len++]=','; + if(isValid()) Len+=Format_UnsDec(NMEA+Len, (uint32_t)Satellites); + NMEA[Len++]=','; + if(isValid()) Len+=Format_UnsDec(NMEA+Len, (uint32_t)HDOP, 2, 1); + NMEA[Len++]=','; + if(isValid()) Len+=Format_SignDec(NMEA+Len, Altitude, 3, 1, 1); + NMEA[Len++]=','; + NMEA[Len++]='M'; + NMEA[Len++]=','; + if(isValid()) Len+=Format_SignDec(NMEA+Len, GeoidSeparation, 3, 1, 1); + NMEA[Len++]=','; + NMEA[Len++]='M'; + NMEA[Len++]=','; + NMEA[Len++]=','; + Len += NMEA_AppendCheckCRNL(NMEA, Len); + NMEA[Len]=0; return Len; } int8_t ReadGGA(const char *GGA) { if( (memcmp(GGA, "$GPGGA", 6)!=0) && (memcmp(GGA, "$GNGGA", 6)!=0) ) return -1; // check if the right sequence @@ -1067,7 +1267,7 @@ class GPS_Position: public GPS_Time ReadAltitude(GGA[Index[9]], GGA+Index[8]); // Altitude ReadGeoidSepar(GGA[Index[11]], GGA+Index[10]); // Geoid separation calcLatitudeCosine(); - return 1; } + NMEAframes++; return 1; } int8_t ReadGSA(NMEA_RxMsg &RxMsg) { if(RxMsg.Parms<17) return -1; @@ -1075,7 +1275,7 @@ class GPS_Position: public GPS_Time ReadPDOP((const char *)RxMsg.ParmPtr(14)); // total dilution of precision ReadHDOP((const char *)RxMsg.ParmPtr(15)); // horizontal dilution of precision ReadVDOP((const char *)RxMsg.ParmPtr(16)); // vertical dilution of precision - return 1; } + NMEAframes++; return 1; } int8_t ReadGSA(const char *GSA) { if( (memcmp(GSA, "$GPGSA", 6)!=0) && (memcmp(GSA, "$GNGSA", 6)!=0) ) return -1; // check if the right sequence @@ -1084,7 +1284,7 @@ class GPS_Position: public GPS_Time ReadPDOP(GSA+Index[14]); ReadHDOP(GSA+Index[15]); ReadVDOP(GSA+Index[16]); - return 1; } + NMEAframes++; return 1; } /* int8_t ReadGSV(NMEA_RxMsg &RxMsg) { // @@ -1105,7 +1305,7 @@ class GPS_Position: public GPS_Time ReadSpeed((const char *)RxMsg.ParmPtr(6)); // Speed ReadHeading((const char *)RxMsg.ParmPtr(7)); // Heading calcLatitudeCosine(); - return 1; } + NMEAframes++; return 1; } int8_t ReadRMC(const char *RMC) { if( (memcmp(RMC, "$GPRMC", 6)!=0) && (memcmp(RMC, "$GNRMC", 6)!=0) ) return -1; // check if the right sequence @@ -1117,7 +1317,7 @@ class GPS_Position: public GPS_Time ReadSpeed(RMC+Index[6]); ReadHeading(RMC+Index[7]); calcLatitudeCosine(); - return 1; } + NMEAframes++; return 1; } /* int32_t calcTimeDiff(GPS_Position &RefPos) const { int32_t TimeDiff = ((int32_t)Min*6000+(int16_t)Sec*100+FracSec) - ((int32_t)RefPos.Min*6000+(int16_t)RefPos.Sec*100+RefPos.FracSec); @@ -1125,39 +1325,53 @@ class GPS_Position: public GPS_Time else if(TimeDiff>=180000) TimeDiff-=360000; return TimeDiff; } // [0.01s] */ - int16_t calcDifferentials(GPS_Position &RefPos) // calculate climb rate and turn rate with an earlier reference position - { ClimbRate=0; TurnRate=0; - if(RefPos.FixQuality==0) return 0; - int16_t TimeDiff = calcTimeDiff(RefPos); - if(TimeDiff<5) return 0; - TurnRate = Heading-RefPos.Heading; - if(TurnRate>1800) TurnRate-=3600; else if(TurnRate<(-1800)) TurnRate+=3600; - ClimbRate = Altitude-RefPos.Altitude; - if(hasBaro && RefPos.hasBaro && (abs(Altitude-StdAltitude)<2500) ) - { ClimbRate = StdAltitude-RefPos.StdAltitude; } - Accel = Speed-RefPos.Speed; - if(TimeDiff==20) - { ClimbRate*=5; - TurnRate *=5; - Accel *=5; } - else if(TimeDiff==50) - { ClimbRate*=2; - TurnRate *=2; - Accel *=2; } - else if(TimeDiff==100) + int16_t calcDifferentials(GPS_Position &RefPos, bool useBaro=1) // calculate climb rate and turn rate with an earlier reference position + { // ClimbRate=0; hasClimb=0; + // TurnRate=0; hasTurn=0; + // LongAccel=0; hasAccel=0; + if(RefPos.FixQuality==0) return 0; // give up if no fix on the reference position + int16_t TimeDiff = calcTimeDiff(RefPos); // [ms] time difference between positions + if(TimeDiff<10) return 0; // [ms] give up if smaller than 10ms (as well when negative) + TurnRate = Heading-RefPos.Heading; // [0.1deg/s] turn rate + if(TurnRate>1800) TurnRate-=3600; else if(TurnRate<(-1800)) TurnRate+=3600; // wrap-around + ClimbRate = Altitude-RefPos.Altitude; // [0.1m/s] climb rate as altitude difference + if(useBaro && hasBaro && RefPos.hasBaro && (abs(Altitude-StdAltitude)<2500) ) // if there is baro data then + { ClimbRate += StdAltitude-RefPos.StdAltitude; // [0.1m/s] on pressure altitude + ClimbRate = (ClimbRate+1)>>1; } // take average of the GPS and baro climb rate + LongAccel = Speed-RefPos.Speed; // longitual acceleration + if(TimeDiff==100) // [ms] if 0.1sec difference + { ClimbRate *=10; + TurnRate *=10; + LongAccel *=10; } + if(TimeDiff==200) // [ms] if 0.2sec difference + { ClimbRate *=5; + TurnRate *=5; + LongAccel *=5; } + if(TimeDiff==250) // [ms] if 0.25sec difference + { ClimbRate *=4; + TurnRate *=4; + LongAccel *=4; } + else if(TimeDiff==500) + { ClimbRate *=2; + TurnRate *=2; + LongAccel *=2; } + else if(TimeDiff==1000) { } - else if(TimeDiff==200) - { ClimbRate=(ClimbRate+1)>>1; - TurnRate=( TurnRate+1)>>1; - Accel =( Accel +1)>>1; } + else if(TimeDiff==2000) + { ClimbRate = (ClimbRate+1)>>1; + TurnRate = ( TurnRate+1)>>1; + LongAccel = (LongAccel+1)>>1; } else if(TimeDiff!=0) - { ClimbRate = ((int32_t)ClimbRate*100)/TimeDiff; - TurnRate = ((int32_t) TurnRate*100)/TimeDiff; - Accel = ((int32_t) Accel *100)/TimeDiff; } - return TimeDiff; } // [0.01s] - + { ClimbRate = ((int32_t)ClimbRate*1000)/TimeDiff; + TurnRate = ((int32_t) TurnRate*1000)/TimeDiff; + LongAccel = ((int32_t)LongAccel*1000)/TimeDiff; } + // printf("calcDifferences( , %d) %02d.%03ds hasBaro:%d:%d %4dms %3.1f/%3.1f m %+4.1f m/s\n", + // useBaro, Sec, mSec, hasBaro, RefPos.hasBaro, TimeDiff, 0.1*Altitude, 0.1*StdAltitude, 0.1*ClimbRate); + hasClimb=1; hasTurn=1; hasAccel=1; + return TimeDiff; } // [ms] +/* void Write(MAV_GPS_RAW_INT *MAV) const - { MAV->time_usec = (int64_t)1000000*getUnixTime()+10000*FracSec; + { MAV->time_usec = (int64_t)1000000*getUnixTime()+1000*mSec; // [usec] MAV->lat = ((int64_t)50*Latitude+1)/3; MAV->lon = ((int64_t)50*Longitude+1)/3; MAV->alt = 100*Altitude; @@ -1199,7 +1413,7 @@ class GPS_Position: public GPS_Time Pressure = 100*4*MAV->press_abs; Temperature = MAV->temperature/10; hasBaro=1; } - +*/ static int32_t getCordic(int32_t Coord) { return ((int64_t)Coord*83399993+(1<<21))>>22; } // [0.0001/60 deg] => [cordic] int32_t getCordicLatitude (void) const { return getCordic(Latitude ); } int32_t getCordicLongitude(void) const { return getCordic(Longitude); } @@ -1208,7 +1422,7 @@ class GPS_Position: public GPS_Time // 180 0x066FF300 0x80000000 0x7FFFBC00 static int32_t getFANETcordic(int32_t Coord) { return ((int64_t)Coord*83399317+(1<<21))>>22; } // [0.0001/60 deg] => [FANET cordic] - void EncodeAirPos(FANET_Packet &Packet, uint8_t AcftType=1, bool Track=1) + void EncodeAirPos(FANET_Packet &Packet, uint8_t AcftType=1, bool Track=1) const { int32_t Alt = Altitude; if(Alt<0) Alt=0; else Alt=(Alt+5)/10; int32_t Lat = getFANETcordic(Latitude); // Latitude: [0.0001/60deg] => [cordic] int32_t Lon = getFANETcordic(Longitude); // Longitude: [0.0001/60deg] => [cordic] @@ -1218,7 +1432,7 @@ class GPS_Position: public GPS_Time if(hasBaro) { Packet.setQNE((StdAltitude+5)/10); } } - void Encode(GDL90_REPORT &Report) + void Encode(GDL90_REPORT &Report) const { Report.setAccuracy(9, 9); int32_t Lat = getCordicLatitude(); // Latitude: [0.0001/60deg] => [cordic] int32_t Lon = getCordicLongitude(); // Longitude: [0.0001/60deg] => [cordic] @@ -1233,18 +1447,40 @@ class GPS_Position: public GPS_Time Report.setHeading((HeadAngle+0x80)>>8); // [8-bit cordic] Report.setMiscInd(0x2); // Report.setSpeed((SpeedKts+5)/10); // [knot] - Report.setClimbRate(6*MetersToFeet(ClimbRate)); - } + Report.setClimbRate(6*MetersToFeet(ClimbRate)); } - template - void Encode(OGNx_Packet &Packet) const + void Encode(GDL90_GEOMALT &GeomAlt) const + { GeomAlt.Clear(); + int32_t Alt = MetersToFeet(Altitude+GeoidSeparation); // [0.1feet] + GeomAlt.setAltitude((Alt+25)/50); // [5feet] + GeomAlt.setFOM((HDOP*3+5)/10); } // [m] + + void Encode(ADSL_Packet &Packet) const + { Packet.setAlt((Altitude+GeoidSeparation+5)/10); + Packet.setLatOGN(Latitude); + Packet.setLonOGN(Longitude); + Packet.TimeStamp = (Sec*4+mSec/250)&0x3F; + Packet.setSpeed(((uint32_t)Speed*4+5)/10); + Packet.setClimb(((int32_t)ClimbRate*8+5)/10); + // if(hasClimb) Packet.setClimb(((int32_t)ClimbRate*8+5)/10); + // else Packet.clrClimb(); + Packet.setTrack(((uint32_t)Heading*32+112)/225); + Packet.Integrity[0]=0; Packet.Integrity[1]=0; + if((FixQuality>0)&&(FixMode>=2)) + { Packet.setHorAccur((HDOP*2+5)/10); + Packet.setVerAccur((VDOP*3+5)/10); } + } + + // template + // void Encode(OGNx_Packet &Packet) const + void Encode(OGN1_Packet &Packet) const { Packet.Position.FixQuality = FixQuality<3 ? FixQuality:3; // if((FixQuality>0)&&(FixMode>=2)) Packet.Position.FixMode = FixMode-2; // else Packet.Position.FixMode = 0; if(PDOP>0) Packet.EncodeDOP(PDOP-10); // encode PDOP from GSA else Packet.EncodeDOP(HDOP-10); // or if no GSA: use HDOP int8_t ShortTime=Sec; // the 6-bit time field in the OGN packet - if(FracSec>=50) { ShortTime+=1; if(ShortTime>=60) ShortTime-=60; } // round to the closest full second + if(mSec>=500) { ShortTime+=1; if(ShortTime>=60) ShortTime-=60; } // round to the closest full second Packet.Position.Time=ShortTime; // Time Packet.EncodeLatitude(Latitude); // Latitude Packet.EncodeLongitude(Longitude); // Longitude @@ -1279,7 +1515,7 @@ class GPS_Position: public GPS_Time void EncodeStatus(OGNx_Packet &Packet) const { Packet.Status.ReportType=0; int ShortTime=Sec; - if(FracSec>=50) { ShortTime+=1; if(ShortTime>=60) ShortTime-=60; } + if(mSec>=500) { ShortTime+=1; if(ShortTime>=60) ShortTime-=60; } Packet.Status.Time=ShortTime; Packet.Status.FixQuality = FixQuality<3 ? FixQuality:3; Packet.Status.Satellites = Satellites<15 ? Satellites:15; @@ -1287,9 +1523,11 @@ class GPS_Position: public GPS_Time if(hasBaro) { Packet.EncodeTemperature(Temperature); Packet.Status.Pressure = (Pressure+16)>>5; - Packet.EncodeHumidity(Humidity); } + if(hasHum) Packet.EncodeHumidity(Humidity); + else Packet.clrHumidity(); } else - { Packet.Status.Pressure = 0; + { Packet.Status.Pressure=0; + Packet.clrTemperature(); Packet.clrHumidity(); } } @@ -1300,6 +1538,26 @@ class GPS_Position: public GPS_Time // return 3; } // => Australia + South America: upper half of 915MHz band // return 2; } // => USA/Canada: full 915MHz band + template + void Decode(const OGNx_Packet &Packet) + { FixQuality = Packet.Position.FixQuality; + FixMode = Packet.Position.FixMode+2; + PDOP = 10+Packet.DecodeDOP(); + HDOP = PDOP; VDOP = PDOP+PDOP/2; + mSec=0; Sec=Packet.Position.Time; + Speed = Packet.DecodeSpeed(); + ClimbRate = Packet.DecodeClimbRate(); + TurnRate = Packet.DecodeTurnRate(); + Heading = Packet.DecodeHeading(); // Heading = track-over-ground + Latitude = Packet.DecodeLatitude(); + Longitude = Packet.DecodeLongitude(); + Altitude = Packet.DecodeAltitude()*10; + hasBaro=0; + if(Packet.hasBaro()) + { StdAltitude = Altitude + 10*Packet.getBaroAltDiff(); + hasBaro=1; } + } + template void Encode(OGNx_Packet &Packet, int16_t dTime) const // Encode position which is extrapolated by the given fraction of a second { Packet.Position.FixQuality = FixQuality<3 ? FixQuality:3; // @@ -1310,9 +1568,9 @@ class GPS_Position: public GPS_Time int32_t Lat, Lon, Alt; int16_t Head; calcExtrapolation(Lat, Lon, Alt, Head, dTime); int16_t ShortTime=Sec; // the 6-bit time field in the OGN packet - dTime += FracSec; - while(dTime>= 50 ) { dTime-=100; ShortTime++; if(ShortTime>=60) ShortTime-=60; } - while(dTime<(-50)) { dTime+=100; ShortTime--; if(ShortTime< 0) ShortTime+=60; } + dTime += mSec; + while(dTime>= 500 ) { dTime-=1000; ShortTime++; if(ShortTime>=60) ShortTime-=60; } + while(dTime<(-500)) { dTime+=1000; ShortTime--; if(ShortTime< 0) ShortTime+=60; } Packet.Position.Time=ShortTime; // Time Packet.EncodeLatitude(Lat); // Latitude Packet.EncodeLongitude(Lon); // Longitude @@ -1325,55 +1583,61 @@ class GPS_Position: public GPS_Time else Packet.clrBaro(); //or no-baro if pressure sensor data not there } - void Extrapolate(int32_t dTime) // [0.01sec] extrapolate the position by dTime - { int16_t dSpeed = ((int32_t)Accel*dTime)/100; + void Extrapolate(int32_t dTime) // [ms] extrapolate the position by dTime + { int16_t dSpeed = ((int32_t)LongAccel*dTime)/1000; Speed += dSpeed/2; int16_t HeadAngle = ((int32_t)Heading<<12)/225; // [cordic] heading angle - int16_t TurnAngle = (((dTime*TurnRate)/25)<<9)/225; // [cordic] + int16_t TurnAngle = (((dTime*TurnRate)/250)<<9)/225; // [cordic] HeadAngle += TurnAngle/2; int32_t LatSpeed = ((int32_t)Speed*Icos(HeadAngle)+0x800)>>12; // [0.1m/s] int32_t LonSpeed = ((int32_t)Speed*Isin(HeadAngle)+0x800)>>12; // [0.1m/s] HeadAngle += TurnAngle-TurnAngle/2; Speed += dSpeed-dSpeed/2; if(Speed<0) Speed=0; - Latitude += calcLatitudeExtrapolation (dTime, LatSpeed); - Longitude += calcLongitudeExtrapolation(dTime, LonSpeed); + Latitude += calcLatitudeExtrapolation (dTime, LatSpeed); // + Longitude += calcLongitudeExtrapolation(dTime, LonSpeed); // int32_t dAlt = calcAltitudeExtrapolation(dTime); // [0.1m] Altitude += dAlt; // [0.1m] if(hasBaro) { StdAltitude += dAlt; // [0.1m] Pressure += 4000*dAlt/Atmosphere::PressureLapseRate(Pressure/4, Temperature); } // [0.25Pa] ([Pa], [0.1degC]) - Heading += (dTime*TurnRate)/100; // [0.1deg] + Heading += (dTime*TurnRate)/1000; // [0.1deg] if(Heading<0) Heading+=3600; else if(Heading>=3600) Heading-=3600; // [0.1deg] - int16_t fTime = FracSec+dTime; // [0.01sec] - while(fTime>=100) { incrTimeDate(); fTime-=100; } - while(fTime< 0) { decrTimeDate(); fTime+=100; } - FracSec=fTime; } + int16_t fTime = mSec+dTime; // [msec] + while(fTime>=1000) { incrTimeDate(); fTime-=1000; } + while(fTime< 0) { decrTimeDate(); fTime+=1000; } + mSec=fTime; } // extrapolate GPS position by a fraction of a second - void calcExtrapolation(int32_t &Lat, int32_t &Lon, int32_t &Alt, int16_t &Head, int32_t dTime) const // [0.01sec] + void calcExtrapolation(int32_t &Lat, int32_t &Lon, int32_t &Alt, int16_t &Head, int32_t dTime) const // [msec] { int16_t HeadAngle = ((int32_t)Heading<<12)/225; // [] - int16_t TurnAngle = (((dTime*TurnRate)/25)<<9)/225; // [] + int16_t TurnAngle = (((dTime*TurnRate)/250)<<9)/225; // [] HeadAngle += TurnAngle; int32_t LatSpeed = ((int32_t)Speed*Icos(HeadAngle)+0x800)>>12; // [0.1m/s] int32_t LonSpeed = ((int32_t)Speed*Isin(HeadAngle)+0x800)>>12; // [0.1m/s] Lat = Latitude + calcLatitudeExtrapolation (dTime, LatSpeed); Lon = Longitude + calcLongitudeExtrapolation(dTime, LonSpeed); Alt = Altitude + calcAltitudeExtrapolation(dTime); - Head = Heading + (dTime*TurnRate)/100; + Head = Heading + (dTime*TurnRate)/1000; if(Head<0) Head+=3600; else if(Head>=3600) Head-=3600; } - int32_t calcAltitudeExtrapolation(int32_t Time) const // [0.01s] - { return Time*ClimbRate/100; } // [0.1m] + int32_t calcAltitudeExtrapolation(int32_t Time) const // [ms] + { return Time*ClimbRate/1000; } // [0.1m] - int32_t calcLatitudeExtrapolation(int32_t Time, int32_t LatSpeed) const // [0.01s] [0.1m/s] - { return (Time*LatSpeed*177+0x4000)>>15; } // [0.1m] + // int32_t calcLatitudeExtrapolation(int32_t Time, int32_t LatSpeed) const // [ms] [0.1m/s] + // { return (Time/10*LatSpeed*177+0x4000)>>15; } // [0.1m] - int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed) const // [0.01s] + int32_t calcLatitudeExtrapolation(int32_t Time, int32_t LatSpeed) const // [ms] [0.1m/s] + { return (Time*LatSpeed*283+0x40000)>>19; } // [0.1m] + + int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed) const // [ms] { int16_t LatCosine = calcLatCosine(calcLatAngle16(Latitude)); return calcLongitudeExtrapolation(Time, LonSpeed, LatCosine); } - int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed, int16_t LatCosine) const // [0.01s] - { return ((((int32_t)Time*LonSpeed*177+4)>>3))/LatCosine; } + // int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed, int16_t LatCosine) const // [ms] + // { return (((Time/10*LonSpeed*177+4)>>3))/LatCosine; } + + int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed, int16_t LatCosine) const // [ms] + { return (((Time*LonSpeed*283+64)>>7))/LatCosine; } // static int32_t calcLatDistance(int32_t Lat1, int32_t Lat2) // [m] distance along latitude // { return ((int64_t)(Lat2-Lat1)*0x2f684bda+0x80000000)>>32; } @@ -1391,7 +1655,9 @@ class GPS_Position: public GPS_Time // { return IntSine((uint16_t)(LatAngle+0x4000)); } static int16_t calcLatCosine(int16_t LatAngle) - { return Icos(LatAngle); } + { int16_t LatCos=Icos(LatAngle); + if(LatCos<=0) LatCos=1; // protect against zero as it is used for division + return LatCos; } // int32_t getLatDistance(int32_t RefLatitude) const // [m] distance along latitude // { return calcLatDistance(RefLatitude, Latitude); } @@ -1421,27 +1687,27 @@ class GPS_Position: public GPS_Time char LonW = Out[Len-2]; Out[Len-2]=Out[Len-3]; Out[Len-3]=Out[Len-4]; Out[Len-4]='.'; Out[Len++]=Icon[1]; - Len+=Format_UnsDec(Out+Len, Heading/10, 3); // [deg] Heading + Len+=Format_UnsDec(Out+Len, (uint32_t)Heading/10, 3); // [deg] Heading Out[Len++]='/'; Len+=Format_UnsDec(Out+Len, ((uint32_t)Speed*199+512)>>10, 3); // [kt] speed - Out[Len++] = '/'; Out[Len++] = 'A'; Out[Len++] = '='; Len+=Format_UnsDec(Out+Len, (MetersToFeet(Altitude)+5)/10, 6); // [feet] altitude + Out[Len++] = '/'; Out[Len++] = 'A'; Out[Len++] = '='; Len+=Format_UnsDec(Out+Len, ((uint32_t)MetersToFeet(Altitude)+5)/10, 6); // [feet] altitude Out[Len++]=' '; Out[Len++]='!'; Out[Len++]='W'; Out[Len++]=LatW; Out[Len++]=LonW; Out[Len++]='!'; // more accurate Lat/Lon Out[Len++]=' '; Out[Len++]='i'; Out[Len++]='d'; Len+=Format_Hex(Out+Len, ID); // ID Out[Len++] = ' '; Len+=Format_SignDec(Out+Len, ((int32_t)ClimbRate*10079+256)>>9, 3); Out[Len++] = 'f'; Out[Len++] = 'p'; Out[Len++] = 'm'; // [fpm] - Out[Len++] = ' '; Len+=Format_SignDec(Out+Len, TurnRate/3, 2, 1); Out[Len++] = 'r'; Out[Len++] = 'o'; Out[Len++] = 't'; // [ROT] + Out[Len++] = ' '; Len+=Format_SignDec(Out+Len, (int32_t)TurnRate/3, 2, 1); Out[Len++] = 'r'; Out[Len++] = 'o'; Out[Len++] = 't'; // [ROT] if(hasBaro) { int32_t Alt=(StdAltitude+5)/10; // [m] standard pressure altitude if(Alt<0) Alt=0; Out[Len++] = ' '; Out[Len++] = 'F'; Out[Len++] = 'L'; - Len+=Format_UnsDec(Out+Len, MetersToFeet((uint32_t)Alt), 5, 2); } // [feet] "Flight Level" + Len+=Format_UnsDec(Out+Len, (uint32_t)MetersToFeet((uint32_t)Alt), 5, 2); } // [feet] "Flight Level" uint16_t DOP=PDOP; if(DOP==0) DOP=HDOP; uint16_t HorPrec=(DOP*2+5)/10; if(HorPrec>63) HorPrec=63; // [m] uint16_t VerPrec=(DOP*3+5)/10; if(VerPrec>63) VerPrec=63; // [m] Out[Len++] = ' '; Out[Len++] = 'g'; Out[Len++] = 'p'; Out[Len++] = 's'; - Len+=Format_UnsDec(Out+Len, HorPrec); Out[Len++] = 'x'; Len+=Format_UnsDec(Out+Len, VerPrec); + Len+=Format_UnsDec(Out+Len, (uint32_t)HorPrec); Out[Len++] = 'x'; Len+=Format_UnsDec(Out+Len, (uint32_t)VerPrec); Out[Len]=0; return Len; } @@ -1449,40 +1715,40 @@ class GPS_Position: public GPS_Time { int Len=0; bool Neg = Coord<0; if(Neg) Coord=(-Coord); int32_t Deg = Coord/600000; - Len+=Format_UnsDec(Out+Len, Deg, DegSize); + Len+=Format_UnsDec(Out+Len, (uint32_t)Deg, DegSize); Coord-=Deg*600000; Coord/=10; - Len+=Format_UnsDec(Out+Len, Coord, 5); + Len+=Format_UnsDec(Out+Len, (uint32_t)Coord, 5); Out[Len++]=SignChar[Neg]; return Len; } - int WriteHHMMSS(char *Out) - { Format_UnsDec(Out , Hour, 2); - Format_UnsDec(Out+2, Min , 2); - Format_UnsDec(Out+4, Sec , 2); + int WriteHHMMSS(char *Out) const + { Format_UnsDec(Out , (uint32_t)Hour, 2); + Format_UnsDec(Out+2, (uint32_t)Min , 2); + Format_UnsDec(Out+4, (uint32_t)Sec , 2); return 6; } - int WriteIGC(char *Out) + int WriteIGC(char *Out) const // write IGC B-record { // if(!isValid()) return 0; int Len=0; Out[Len++] = 'B'; - if(isTimeValid()) Len+=WriteHHMMSS(Out+Len); - else Len+=Format_String(Out+Len, " "); - if(isValid()) - { Len+=WriteIGCcoord(Out+Len, Latitude, 2, "NS"); - Len+=WriteIGCcoord(Out+Len, Longitude, 3, "EW"); - Out[Len++] = FixMode>2 ? 'A':'V'; } - else Len+=Format_String(Out+Len, " "); - if(hasBaro) - { int32_t Alt = StdAltitude/10; // [m] - if(Alt<0) { Alt = (-Alt); Out[Len++] = '-'; Len+=Format_UnsDec(Out+Len, (uint32_t)Alt, 4); } - else { Len+=Format_UnsDec(Out+Len, (uint32_t)Alt, 5); } + if(isTimeValid()) Len+=WriteHHMMSS(Out+Len); // if time is valid + else Len+=Format_String(Out+Len, " "); // or leave empty + if(isValid()) // if position valid + { Len+=WriteIGCcoord(Out+Len, Latitude, 2, "NS"); // DDMM.MMM latitude + Len+=WriteIGCcoord(Out+Len, Longitude, 3, "EW"); // DDDMM.MMM longitude + Out[Len++] = FixMode>2 ? 'A':'V'; } // fix mode + else Len+=Format_String(Out+Len, " "); // is position not valid then leave empty + if(hasBaro) // if pressure data is there + { int32_t Alt = StdAltitude/10; // [m] pressure altitude + if(Alt<0) { Alt = (-Alt); Out[Len++] = '-'; Len+=Format_UnsDec(Out+Len, (uint32_t)Alt, 4); } // -AAAA (when negative) + else { Len+=Format_UnsDec(Out+Len, (uint32_t)Alt, 5); } // AAAAA } else Len+=Format_String(Out+Len, " "); - if(isValid()) - { int32_t Alt = (Altitude+GeoidSeparation)/10; // [m] - if(Alt<0) { Alt = (-Alt); Out[Len++] = '-'; Len+=Format_UnsDec(Out+Len, (uint32_t)Alt, 4); } - else { Len+=Format_UnsDec(Out+Len, (uint32_t)Alt, 5); } + if(isValid()) // if position is valid + { int32_t Alt = (Altitude+GeoidSeparation)/10; // [m] HAE GPS altitude + if(Alt<0) { Alt = (-Alt); Out[Len++] = '-'; Len+=Format_UnsDec(Out+Len, (uint32_t)Alt, 4); } // -AAAA (when negative) + else { Len+=Format_UnsDec(Out+Len, (uint32_t)Alt, 5); } // AAAAA } else Len+=Format_String(Out+Len, " "); - Out[Len]=0; return Len; } + Out[Len++]='\n'; Out[Len]=0; return Len; } private: @@ -1548,29 +1814,6 @@ class GPS_Position: public GPS_Time else if(DOP>255) DOP=255; VDOP=DOP; return 0; } - int8_t ReadTime(const char *Value) // read the Time field: HHMMSS.ss and check if it is a new one or the same one - { int8_t Prev; int8_t Same=1; - Prev=Hour; - Hour=Read_Dec2(Value); if(Hour<0) return -1; // read hour (two digits), return when invalid - if(Prev!=Hour) Same=0; - Prev=Min; - Min=Read_Dec2(Value+2); if(Min<0) return -1; // read minute (two digits), return when invalid - if(Prev!=Min) Same=0; - Prev=Sec; - Sec=Read_Dec2(Value+4); if(Sec<0) return -1; // read second (two digits), return when invalid - if(Prev!=Sec) Same=0; - Prev=FracSec; - if(Value[6]=='.') // is there a fraction - { FracSec=Read_Dec2(Value+7); if(FracSec<0) return -1; } // read the fraction, return when invalid - if(Prev!=FracSec) Same=0; // return 0 when time is valid but did not change - return Same; } // return 1 when time did not change (both RMC and GGA were for same time) - - int8_t ReadDate(const char *Param) // read the field DDMMYY - { Day=Read_Dec2(Param); if(Day<0) return -1; // read calendar year (two digits - thus need to be extended to four) - Month=Read_Dec2(Param+2); if(Month<0) return -1; // read calendar month - Year=Read_Dec2(Param+4); if(Year<0) return -1; // read calendar day - return 0; } // return 0 when field valid and was read correctly - public: int8_t static IndexNMEA(uint8_t Index[20], const char *Seq) // index parameters and verify the NMEA checksum diff --git a/utils/ogn1.h b/utils/ogn1.h index 08ae411..a180083 100644 --- a/utils/ogn1.h +++ b/utils/ogn1.h @@ -5,6 +5,7 @@ #include #include +#include #include "ognconv.h" @@ -132,6 +133,13 @@ class OGN1_Packet // Packet structure for the OGN tracker unsigned int BaroAltDiff: 8; // [m] // lower 8 bits of the altitude difference between baro and GPS } Wind; + struct + { uint8_t Data[14]; // up to 14 bytes od specific data + unsigned int DataLen : 4; // 0..14 number of bytes in the message + unsigned int ReportType: 4; // 15 for the manufacturer specific mesage + unsigned int ManufID : 8; // Manufacturer identification: 0 for Cube-Board + } ManufMsg; // manufacturer-specific message + } ; uint8_t *Byte(void) const { return (uint8_t *)&HeaderWord; } // packet as bytes @@ -139,14 +147,22 @@ class OGN1_Packet // Packet structure for the OGN tracker // void recvBytes(const uint8_t *SrcPacket) { memcpy(Byte(), SrcPacket, Bytes); } // load data bytes e.g. from a demodulator - static const uint8_t InfoParmNum = 14; // [int] number of info-parameters and their names + static const uint8_t InfoParmNum = 15; // [int] number of info-parameters and their names static const char *InfoParmName(uint8_t Idx) { static const char *Name[InfoParmNum] = { "Pilot", "Manuf", "Model", "Type", "SN", "Reg", "ID", "Class", - "Task" , "Base" , "ICE" , "PilotID", "Hard", "Soft" } ; + "Task" , "Base" , "ICE" , "PilotID", "Hard", "Soft", "Crew" } ; return Idx=60) return 0; + int Sec=RefTime%60; + int DiffSec=Position.Time-Sec; + if(DiffSec>FwdMargin) DiffSec-=60; // difference should always be zero or negative, but can be small positive for predicted positions + else if(DiffSec<=(-60+FwdMargin)) DiffSec+=60; + return RefTime+DiffSec; } // get out the correct position time + void Dump(void) const { printf("%08lX: %08lX %08lX %08lX %08lX\n", (long int)HeaderWord, (long int)Data[0], (long int)Data[1], @@ -175,31 +191,91 @@ class OGN1_Packet // Packet structure for the OGN tracker { printf(" %02X", Byte()[Idx]); } printf("\n"); } - uint8_t WriteStatus(char *Out) - { uint8_t Len=0; + int Print(char *Out) const + { int Len=0; + Out[Len++]='0'+Header.AddrType; Out[Len++]=':'; + uint32_t Addr = Header.Address; + Len+=Format_Hex(Out+Len, (uint8_t)(Addr>>16)); + Len+=Format_Hex(Out+Len, (uint16_t)Addr); + Out[Len++]=' '; + Out[Len++]='R'; + Out[Len++]='0'+Header.Relay; + if(!Header.NonPos) return Len+PrintPosition(Out+Len); + if(isStatus()) return Len+PrintDeviceStatus(Out+Len); + if(isInfo ()) return Len+PrintDeviceInfo(Out+Len); + Out[Len]=0; return Len; } + + int PrintPosition(char *Out) const + { int Len=0; + Out[Len++]=' '; + // Out[Len++]=HexDigit(Position.AcftType); Out[Len++]=':'; + // Len+=Format_SignDec(Out+Len, -(int16_t)RxRSSI/2); Out[Len++]='d'; Out[Len++]='B'; Out[Len++]='m'; + // Out[Len++]=' '; + Len+=Format_UnsDec(Out+Len, (uint16_t)Position.Time, 2); + Len+=Format_String(Out+Len, "s ["); + Len+=Format_SignDec(Out+Len, DecodeLatitude()/6, 7, 5); + Out[Len++]=','; + Len+=Format_SignDec(Out+Len, DecodeLongitude()/6, 8, 5); + Len+=Format_String(Out+Len, "]deg "); + // Len+=Format_Latitude(Out+Len, DecodeLatitude()); + // Out[Len++]=' '; + // Len+=Format_Longitude(Out+Len, DecodeLongitude()); + // Out[Len++]=' '; + Len+=Format_UnsDec(Out+Len, (uint32_t)DecodeAltitude()); Out[Len++]='m'; + Out[Len++]=' '; + Len+=Format_UnsDec(Out+Len, DecodeSpeed(), 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s'; + Out[Len++]=' '; + Len+=Format_SignDec(Out+Len, DecodeClimbRate(), 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s'; + Out[Len]=0; + return Len; } + + int PrintDeviceStatus(char *Out) const + { int Len=0; Out[Len++]=' '; Out[Len++]='h'; Len+=Format_Hex(Out+Len, (uint8_t)Status.Hardware); Out[Len++]=' '; Out[Len++]='v'; Len+=Format_Hex(Out+Len, (uint8_t)Status.Firmware); Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Status.Satellites); Out[Len++]='s'; Out[Len++]='a'; Out[Len++]='t'; Out[Len++]='/'; Out[Len++]='0'+Status.FixQuality; Out[Len++]='/'; Len+=Format_UnsDec(Out+Len, Status.SatSNR+8); Out[Len++]='d'; Out[Len++]='B'; Out[Len++]=' '; Len+=Format_SignDec(Out+Len, DecodeAltitude(), 1, 0, 1); Out[Len++]='m'; - Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, (((uint32_t)Status.Pressure<<3)+5)/10, 2, 1); Out[Len++]='h'; Out[Len++]='P'; Out[Len++]='a'; - Out[Len++]=' '; Len+=Format_SignDec(Out+Len, DecodeTemperature(), 2, 1); Out[Len++]='d'; Out[Len++]='e'; Out[Len++]='g'; Out[Len++]='C'; - Out[Len++]=' '; Len+=Format_SignDec(Out+Len, DecodeHumidity(), 2, 1); Out[Len++]='%'; + if(Status.Pressure>0) + { Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, (((uint32_t)Status.Pressure<<3)+5)/10, 2, 1); Out[Len++]='h'; Out[Len++]='P'; Out[Len++]='a'; } + if(hasTemperature()) + { Out[Len++]=' '; Len+=Format_SignDec(Out+Len, DecodeTemperature(), 2, 1); Out[Len++]='d'; Out[Len++]='e'; Out[Len++]='g'; Out[Len++]='C'; } + if(hasHumidity()) + { Out[Len++]=' '; Len+=Format_SignDec(Out+Len, DecodeHumidity(), 2, 1); Out[Len++]='%'; } Out[Len++]=' '; Len+=Format_SignDec(Out+Len, ((uint32_t)DecodeVoltage()*100+32)>>6, 3, 2); Out[Len++]='V'; Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Status.TxPower+4); Out[Len++]='/'; Out[Len++]='-'; Len+=Format_UnsDec(Out+Len, 5*Status.RadioNoise, 2, 1); Out[Len++]='d'; Out[Len++]='B'; Out[Len++]='m'; Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, (1<>16)); @@ -298,16 +388,18 @@ class OGN1_Packet // Packet structure for the OGN tracker Len+=Format_UnsDec(JSON+Len, DecodeHeading(), 2, 1); Len+=Format_String(JSON+Len, ",\"speed_mps\":"); Len+=Format_UnsDec(JSON+Len, DecodeSpeed(), 2, 1); - Len+=Format_String(JSON+Len, ",\"climb_mps\":"); - Len+=Format_SignDec(JSON+Len, DecodeClimbRate(), 2, 1, 1); - Len+=Format_String(JSON+Len, ",\"turn_dps\":"); - Len+=Format_SignDec(JSON+Len, DecodeTurnRate(), 2, 1, 1); + if(hasClimbRate()) + { Len+=Format_String(JSON+Len, ",\"climb_mps\":"); + Len+=Format_SignDec(JSON+Len, DecodeClimbRate(), 2, 1, 1); } + if(hasTurnRate()) + { Len+=Format_String(JSON+Len, ",\"turn_dps\":"); + Len+=Format_SignDec(JSON+Len, DecodeTurnRate(), 2, 1, 1); } Len+=Format_String(JSON+Len, ",\"DOP\":"); Len+=Format_UnsDec(JSON+Len, 10+DecodeDOP(), 2, 1); } - if(!Header.Encrypted && Header.NonPos) // non-encrypted status and info - { if(Status.ReportType==0) // status + else if(!Header.Encrypted && Header.NonPos) // non-encrypted status and info + { if(isStatus()) // status { } - if(Status.ReportType==1) // info + else if(isInfo()) // info { char Value[16]; uint8_t InfoType; uint8_t Idx=0; @@ -382,7 +474,7 @@ class OGN1_Packet // Packet structure for the OGN tracker return AcftType<16 ? AprsIcon[AcftType]:0; } - int8_t ReadAPRS(const char *Msg) // read an APRS position message + int ReadAPRS(const char *Msg) // read an APRS position message { Clear(); const char *Data = strchr(Msg, ':'); if(Data==0) return -1; // where the time/position data starts @@ -410,14 +502,21 @@ class OGN1_Packet // Packet structure for the OGN tracker } if(Data[0]!='/') return -1; - int8_t Time; + int Sec, Min, Hour; if(Data[7]=='h') // HHMMSS UTC time - { Time=Read_Dec2(Data+5); if(Time<0) return -1; } + { Sec =Read_Dec2(Data+5); if(Sec<0) return -1; + Min =Read_Dec2(Data+3); if(Min<0) return -1; + Hour=Read_Dec2(Data+1); if(Hour<0) return -1; + } else if(Data[7]=='z') // DDHHMM UTC time - { Time=0; } + { Sec =0; + Min =Read_Dec2(Data+5); if(Min<0) return -1; + Hour=Read_Dec2(Data+3); if(Hour<0) return -1; + } else return -1; - Position.Time=Time; + int Time = Sec + Min*60 + Hour*3600; + Position.Time=Sec; Data+=8; Position.FixMode=1; @@ -503,7 +602,7 @@ class OGN1_Packet // Packet structure for the OGN tracker if(LonSign=='W') Longitude=(-Longitude); else if(LonSign!='E') return -1; EncodeLongitude(Longitude); - return 0; } + return Time; } // [sec] return Time-of-Day uint8_t WriteAPRS(char *Msg, uint32_t Time, const char *ProtName="APRS") // write an APRS position message { uint8_t Len=0; @@ -518,17 +617,27 @@ class OGN1_Packet // Packet structure for the OGN tracker { memcpy(Msg+Len, ",RELAY*", 7); Len+=7; } Msg[Len++] = ':'; - if(Header.NonPos && Status.ReportType!=0) return 0; // give up if neither position nor status + if(Header.NonPos && Status.ReportType>1) { Msg[Len]=0; return 0; } // give up if neither position nor status nor info if(Position.Time<60) - { uint32_t DayTime=Time%86400; int Sec=DayTime%60; // second of the time the packet was recevied - int DiffSec=Position.Time-Sec; if(DiffSec>4) DiffSec-=60; // difference should always be zero or negative, but can be small positive for predicted positions - Time+=DiffSec; } // get out the correct position time - Msg[Len++] = Header.NonPos?'>':'/'; + // { uint32_t DayTime=Time%86400; int Sec=DayTime%60; // second of the time the packet was recevied + // int DiffSec=Position.Time-Sec; if(DiffSec>4) DiffSec-=60; // difference should always be zero or negative, but can be small positive for predicted positions + // Time+=DiffSec; } // get out the correct position time + Time = getTime(Time, 5); + Msg[Len++] = Header.NonPos || Header.Encrypted?'>':'/'; Len+=Format_HHMMSS(Msg+Len, Time); Msg[Len++] = 'h'; - if(Header.NonPos) { Len+=WriteStatus(Msg+Len); Msg[Len++]='\n'; Msg[Len]=0; return Len; } + if(Header.NonPos) // status and info packets + { if(isStatus()) Len+=PrintDeviceStatus(Msg+Len); + else if(isInfo() ) Len+=PrintDeviceInfo(Msg+Len); + Msg[Len]=0; return Len; } + + if(Header.Encrypted) // encrypted packets + { Msg[Len++]=' '; + for(int Idx=0; Idx<4; Idx++) + { Len+=EncodeAscii85(Msg+Len, Data[Idx]); } + /* Msg[Len++]='\n'; */ Msg[Len]=0; return Len; } const char *Icon = getAprsIcon(Position.AcftType); @@ -568,8 +677,11 @@ class OGN1_Packet // Packet structure for the OGN tracker Msg[Len++] = ' '; Msg[Len++] = 'i'; Msg[Len++] = 'd'; Len+=Format_Hex(Msg+Len, ((uint32_t)Position.AcftType<<26) | ((uint32_t)Header.AddrType<<24) | Header.Address); - Msg[Len++] = ' '; Len+=Format_SignDec(Msg+Len, ((int32_t)DecodeClimbRate()*10079+256)>>9, 3); Msg[Len++] = 'f'; Msg[Len++] = 'p'; Msg[Len++] = 'm'; - Msg[Len++] = ' '; Len+=Format_SignDec(Msg+Len, DecodeTurnRate()/3, 2, 1); Msg[Len++] = 'r'; Msg[Len++] = 'o'; Msg[Len++] = 't'; + if(hasClimbRate()) + { Msg[Len++] = ' '; Len+=Format_SignDec(Msg+Len, ((int32_t)DecodeClimbRate()*10079+256)>>9, 3); Msg[Len++] = 'f'; Msg[Len++] = 'p'; Msg[Len++] = 'm'; } + + if(hasTurnRate()) + { Msg[Len++] = ' '; Len+=Format_SignDec(Msg+Len, DecodeTurnRate()/3, 2, 1); Msg[Len++] = 'r'; Msg[Len++] = 'o'; Msg[Len++] = 't'; } if(hasBaro()) { int32_t Alt = DecodeStdAltitude(); @@ -583,7 +695,7 @@ class OGN1_Packet // Packet structure for the OGN tracker Msg[Len++] = ' '; Msg[Len++] = 'g'; Msg[Len++] = 'p'; Msg[Len++] = 's'; Len+=Format_UnsDec(Msg+Len, HorPrec); Msg[Len++] = 'x'; Len+=Format_UnsDec(Msg+Len, VerPrec); - Msg[Len++]='\n'; + // Msg[Len++]='\n'; Msg[Len]=0; return Len; } @@ -617,7 +729,7 @@ class OGN1_Packet // Packet structure for the OGN tracker if(abs(Radius)>MaxRadius) return 0; return Radius; } int16_t calcTurnRadius(int16_t MaxRadius=0x7FFF) { return calcTurnRadius(DecodeSpeed(), DecodeTurnRate(), MaxRadius); } - +/* uint8_t Print(char *Out) const { uint8_t Len=0; Out[Len++]=HexDigit(Position.AcftType); Out[Len++]=':'; @@ -641,7 +753,7 @@ class OGN1_Packet // Packet structure for the OGN tracker Len+=Format_SignDec(Out+Len, DecodeClimbRate(), 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s'; Out[Len++]='\n'; Out[Len]=0; return Len; } - +*/ // OGN1_Packet() { Clear(); } void Clear(void) { HeaderWord=0; Data[0]=0; Data[1]=0; Data[2]=0; Data[3]=0; } @@ -714,17 +826,17 @@ class OGN1_Packet // Packet structure for the OGN tracker uint16_t getHeadingAngle(void) const { return (uint16_t)Position.Heading<<6; } - void clrTurnRate(void) { Position.TurnRate=0x80; } - bool hasTurnRate(void) const { return Position.TurnRate==0x80; } + void clrTurnRate(void) { Position.TurnRate=0x80; } // mark turn-rate as absent + bool hasTurnRate(void) const { return Position.TurnRate!=0x80; } - void EncodeTurnRate(int16_t Turn) // [0.1 deg/sec] + void EncodeTurnRate(int16_t Turn) // [0.1 deg/sec] { Position.TurnRate = EncodeSR2V5(Turn); } int16_t DecodeTurnRate(void) const { return DecodeSR2V5(Position.TurnRate); } - void clrClimbRate(void) { Position.ClimbRate=0x100; } - bool hasClimbRate(void) const { return Position.ClimbRate==0x100; } + void clrClimbRate(void) { Position.ClimbRate=0x100; } // mark climb rate as absent + bool hasClimbRate(void) const { return Position.ClimbRate!=0x100; } void EncodeClimbRate(int16_t Climb) { Position.ClimbRate = EncodeSR2V6(Climb); } @@ -842,5 +954,35 @@ class OGN1_Packet // Packet structure for the OGN tracker } ; +/* +class OGN1_DiffPacket +{ public: + union + { uint32_t Word; + struct + { uint8_t dTime:4; // [0..15sec] time difference + int32_t dLat :6; // [-32..+31] + int32_t dLon :6; // [-32..+31] + int32_t dAlt :5; // [-16..+15] + int32_t dVel :5; // [-16..+15] + int32_t dHead:6; // [-32..+31] + } ; + } ; + + public: + bool Encode(const OGN1_Packet &Pos, const OGN1_Packet &RefPos) + { int8_t dT = RefPos.Position.Time - Pos.Position.Time; if(dT<0) dT+=60; + if(dT>15) return 0; + int32_t dLat = (int32_t)RefPos.Position.Latitude - (int32_t)Pos.Position.Latitude; + int32_t dLon = (int32_t)RefPos.Position.Longitude - (int32_t)Pos.Position.Longitude; + int16_t dAlt = (int16_t)RefPos.Position.Altitude - (int16_t)Pos.Position.Altitude; + int16_t dVel = (int16_t)RefPos.Position.Speed - (int16_t)Pos.Position.Speed; // [0.1m/s] difference in speed + int16_t dHead = (int16_t)RefPos.Position.Heading - (int16_t)Pos.Position.Heading; // [10bit cordic] difference in heading + dHead&=0x03FF; if(dHead&0x0200) dHead|=0xFC00; + return 1; } + +} ; +*/ + #endif // of __OGN1_H__ diff --git a/utils/ogn2.h b/utils/ogn2.h index dfa8512..e0e8b99 100644 --- a/utils/ogn2.h +++ b/utils/ogn2.h @@ -132,9 +132,29 @@ class OGN2_Packet // Packet structure for the OGN tracker { printf(" %02X", Byte()[Idx]); } printf("\n"); } + int WriteStatus(char *Out) + { uint8_t Len=0; + Out[Len++]=' '; Out[Len++]='h'; Len+=Format_Hex(Out+Len, (uint8_t)Status.Hardware); + Out[Len++]=' '; Out[Len++]='v'; Len+=Format_Hex(Out+Len, (uint8_t)Status.Firmware); + Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Status.Satellites); Out[Len++]='s'; Out[Len++]='a'; Out[Len++]='t'; + Out[Len++]='/'; Out[Len++]='0'+Status.FixQuality; + Out[Len++]='/'; Len+=Format_UnsDec(Out+Len, Status.SatSNR+8); Out[Len++]='d'; Out[Len++]='B'; + Out[Len++]=' '; Len+=Format_SignDec(Out+Len, DecodeAltitude(), 1, 0, 1); Out[Len++]='m'; + if(Status.Pressure>0) + { Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, (((uint32_t)Status.Pressure<<3)+5)/10, 2, 1); Out[Len++]='h'; Out[Len++]='P'; Out[Len++]='a'; } + if(hasTemperature()) + { Out[Len++]=' '; Len+=Format_SignDec(Out+Len, DecodeTemperature(), 2, 1); Out[Len++]='d'; Out[Len++]='e'; Out[Len++]='g'; Out[Len++]='C'; } + if(hasHumidity()) + { Out[Len++]=' '; Len+=Format_SignDec(Out+Len, DecodeHumidity(), 2, 1); Out[Len++]='%'; } + Out[Len++]=' '; Len+=Format_SignDec(Out+Len, ((uint32_t)DecodeVoltage()*100+32)>>6, 3, 2); Out[Len++]='V'; + Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Status.TxPower+4); + Out[Len++]='/'; Out[Len++]='-'; Len+=Format_UnsDec(Out+Len, 5*Status.RadioNoise, 2, 1); Out[Len++]='d'; Out[Len++]='B'; Out[Len++]='m'; + Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, (1<>29; } // [FANET cordic] => [0.0001/60 deg] +int32_t Coord_OGNtoFNT(int32_t Coord) { return ((int64_t)Coord*83399317 +(1<<21))>>22; } // [0.0001/60 deg] => [FANET cordic] + +int32_t Coord_FNTtoUBX(int32_t Coord) { return ((int64_t)Coord*900007296+(1<<29))>>30; } // [FANET cordic ] => [1e-7 deg] +int32_t Coord_UBXtoFNT(int32_t Coord) { return ((int64_t)Coord*5003959 +(1<<21))>>22; } // [1e-7 deg] => [FANET cordic] + +int32_t Coord_CRDtoOGN(int32_t Coord) { return ((int64_t)Coord*421875 +(1<<22))>>23; } // [32-bit cordic] => [0.0001/60 deg] +int32_t Coord_OGNtoCRD(int32_t Coord) { return ((int64_t)Coord*83399993 +(1<<21))>>22; } // [0.0001/60 deg] => [32-bit cordic] + // ============================================================================================== int32_t FeetToMeters(int32_t Altitude) { return (Altitude*312+512)>>10; } // [feet] => [m] @@ -11,6 +27,64 @@ int32_t MetersToFeet(int32_t Altitude) { return (Altitude*3360+512)>>10; } // [m // ============================================================================================== +uint8_t AcftType_OGNtoADSB(uint8_t AcftType) + // no-inf0, glider, tow, heli, parachute, drop-plane, hang-glider, para-glider, powered, jet, UFO, balloon, Zeppelin, UAV, ground vehicle, fixed object +{ const uint8_t AcftCat[16] = { 0x00, 0xB1, 0xA1, 0xA7, 0xB3, 0xA1, 0xB4, 0xB4, 0xA1, 0xA2, 0x00, 0xB2, 0xB2, 0xB6, 0xC3, 0xC4 }; + return AcftCat[AcftType]; } + +uint8_t AcftType_FNTtoADSB(uint8_t AcftType) + // no-info, para-glider, hang-glider, balloon, glider, powered, heli, UAV +{ const uint8_t AcftCat[8] = { 0, 0xB4, 0xB4, 0xB2, 0xB1, 0xA1, 0xA7, 0xB6 } ; + return AcftCat[AcftType]; } + +uint8_t AcftType_ADSBtoOGN(uint8_t AcftCat) +{ // if(AcftCat&0x38) return 0; + uint8_t Upp = AcftCat>>4; + uint8_t Low = AcftCat&7; + if(Upp==0xA) + { if(Low==1) return 8; + if(Low==7) return 3; + return 9; } + if(Upp==0xB) + { const uint8_t Map[8] = { 0, 0xB, 1, 4, 7, 0, 0xD, 0 }; + return Map[Low]; } + if(Upp==0xC) + { if(Low>=4) return 0xF; + if(Low==3) return 0xE; + return 0; } + return 0; } + +uint8_t AcftType_OGNtoGDL(uint8_t AcftType) + // no-info, glider, tow, heli, parachute, drop-plane, hang-glider, para-glider, powered, jet, UFO, balloon, Zeppelin, UAV, ground vehicle, static-object +{ const uint8_t AcftCat[16] = { 0, 9, 1, 7, 11, 1, 12, 12, 1, 2, 0, 10, 10, 14, 18, 19 } ; + return AcftCat[AcftType]; } + +uint8_t AcftType_OGNtoADSL(uint8_t AcftType) // OGN to ADS-L aircraft-type +{ const uint8_t Map[16] = { 0, 4, 1, 3, // unknown, glider, tow-plane, helicopter + 8, 1, 7, 7, // sky-diver, drop plane, hang-glider, para-glider + 1, 2, 0, 5, // motor airplane, jet, UFO, balloon + 5,11, 0, 0 } ; // airship, UAV, ground vehicle, static object + return Map[AcftType]; } + +uint8_t AcftType_ADSLtoOGN(uint8_t AcftCat) // ADS-L to OGN aircraft-type +{ const uint8_t Map[32] = { 0, 8, 9, 3, 1,12, 2, 7, + 4,13, 3,13,13,13, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0 } ; + return Map[AcftCat]; } + +uint8_t AcftType_FNTtoOGN(uint8_t AcftType) + // no-info, para-glider, hang-glider, balloon, glider, powered, heli, UAV +{ const uint8_t OGNtype[8] = { 0, 7, 6, 0xB, 1, 8, 3, 0xD } ; + return OGNtype[AcftType]; } + +uint8_t AcftType_FNTtoADSL(uint8_t AcftType) + // no-info, para-glider, hang-glider, balloon, glider, powered, heli, UAV +{ const uint8_t AcftCat[8] = { 0, 12, 12, 10, 9, 1, 7, 14 } ; + return AcftCat[AcftType]; } + +// ============================================================================================== + uint16_t EncodeUR2V8(uint16_t Value) // Encode unsigned 12bit (0..3832) as 10bit { if(Value<0x100) { } else if(Value<0x300) Value = 0x100 | ((Value-0x100)>>1); @@ -229,6 +303,35 @@ void XXTEA_Decrypt(uint32_t *Data, uint8_t Words, const uint32_t Key[4], uint8_t } } +static uint32_t XXTEA_MX_KEY0(uint32_t Y, uint32_t Z, uint32_t Sum) +{ return ((((Z>>5) ^ (Y<<2)) + ((Y>>3) ^ (Z<<4))) ^ ((Sum^Y) + Z)); } + +void XXTEA_Encrypt_Key0(uint32_t *Data, uint8_t Words, uint8_t Loops) +{ const uint32_t Delta = 0x9e3779b9; + uint32_t Sum = 0; + uint32_t Z = Data[Words-1]; uint32_t Y; + for( ; Loops; Loops--) + { Sum += Delta; + for (uint8_t P=0; P<(Words-1); P++) + { Y = Data[P+1]; + Z = Data[P] += XXTEA_MX_KEY0(Y, Z, Sum); } + Y = Data[0]; + Z = Data[Words-1] += XXTEA_MX_KEY0(Y, Z, Sum); } +} + +void XXTEA_Decrypt_Key0(uint32_t *Data, uint8_t Words, uint8_t Loops) +{ const uint32_t Delta = 0x9e3779b9; + uint32_t Sum = Loops*Delta; + uint32_t Y = Data[0]; uint32_t Z; + for( ; Loops; Loops--) + { for (uint8_t P=Words-1; P; P--) + { Z = Data[P-1]; + Y = Data[P] -= XXTEA_MX_KEY0(Y, Z, Sum); } + Z = Data[Words-1]; + Y = Data[0] -= XXTEA_MX_KEY0(Y, Z, Sum); + Sum -= Delta; } +} + // ============================================================================================== void XorShift32(uint32_t &Seed) // simple random number generator @@ -236,7 +339,7 @@ void XorShift32(uint32_t &Seed) // simple random number generator Seed ^= Seed >> 17; Seed ^= Seed << 5; } -void xorshift64(uint64_t &Seed) +void XorShift64(uint64_t &Seed) { Seed ^= Seed >> 12; Seed ^= Seed << 25; Seed ^= Seed >> 27; } @@ -277,7 +380,7 @@ int APRS2IGC(char *Out, const char *Inp, int GeoidSepar) // convert Msg++; // where message starts if(Msg[0]!='/' || Msg[7]!='h') return 0; const char *Pos = Msg+8; if(Pos[4]!='.' || Pos[14]!='.') return 0; // where position starts - const char *ExtPos = strstr(Pos+18, " !W"); if(ExtPos && ExtPos[5]=='!') ExtPos+=3; else ExtPos=0; + const char *ExtPos = strstr(Pos+18, " !W"); if(ExtPos[5]=='!') ExtPos+=3; else ExtPos=0; Out[Len++]='B'; // B-record memcpy(Out+Len, Msg+1, 6); Len+=6; // copy UTC time memcpy(Out+Len, Pos, 4); Len+=4; // copy DDMM diff --git a/utils/ognconv.h b/utils/ognconv.h index 5854e96..3de1106 100644 --- a/utils/ognconv.h +++ b/utils/ognconv.h @@ -3,9 +3,27 @@ #include +int32_t Coord_FNTtoOGN(int32_t Coord); +int32_t Coord_OGNtoFNT(int32_t Coord); + +int32_t Coord_FNTtoUBX(int32_t Coord); +int32_t Coord_UBXtoFNT(int32_t Coord); + +int32_t Coord_CRDtoOGN(int32_t Coord); +int32_t Coord_OGNtoCRD(int32_t Coord); + int32_t FeetToMeters(int32_t Altitude); // int32_t MetersToFeet(int32_t Altitude); // +uint8_t AcftType_OGNtoADSB(uint8_t AcftType); +uint8_t AcftType_FNTtoADSB(uint8_t AcftType); +uint8_t AcftType_ADSBtoOGN(uint8_t AcftCat); +uint8_t AcftType_OGNtoGDL(uint8_t AcftType); +uint8_t AcftType_OGNtoADSL(uint8_t AcftType); +uint8_t AcftType_ADSLtoOGN(uint8_t AcftCat); +uint8_t AcftType_FNTtoOGN(uint8_t AcftType); +uint8_t AcftType_FNTtoADSL(uint8_t AcftType); + uint16_t EncodeUR2V8(uint16_t Value); // Encode unsigned 12bit (0..3832) as 10bit uint16_t DecodeUR2V8(uint16_t Value); // Decode 10bit 0..0x3FF @@ -76,8 +94,13 @@ void TEA_Decrypt_Key0 (uint32_t* Data, int Loops); void XXTEA_Encrypt(uint32_t *Data, uint8_t Words, const uint32_t Key[4], uint8_t Loops); void XXTEA_Decrypt(uint32_t *Data, uint8_t Words, const uint32_t Key[4], uint8_t Loops); +void XXTEA_Encrypt_Key0(uint32_t *Data, uint8_t Words, uint8_t Loops); +void XXTEA_Decrypt_Key0(uint32_t *Data, uint8_t Words, uint8_t Loops); + void XorShift32(uint32_t &Seed); // simple random number generator -void xorshift64(uint64_t &Seed); +void XorShift64(uint64_t &Seed); +uint64_t inline XorShift64star(uint64_t &Seed) +{ XorShift64(Seed); return Seed*UINT64_C(0x2545f4914f6cdd1d); } uint8_t EncodeAscii85( char *Ascii, uint32_t Word ); // Encode 32-bit Word into 5-char Ascii-85 string uint8_t DecodeAscii85(uint32_t &Word, const char *Ascii); // Decode 5-char Ascii-85 to 32-bit Word