kopia lustrzana https://github.com/pjalocha/esp32-ogn-tracker
Update from other projects
Pawel Jalocha
rodzic
10a2845380
commit
b99cf5b913
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,412 @@
|
|||
#ifndef __ADSL_H__
|
||||
#define __ADSL_H__
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
// #include <string.h>
|
||||
// #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<uint16_t,6>(Position[6]); } // [0.25 m/s]
|
||||
void setSpeed(uint16_t Speed) { Position[6] = UnsVRencode<uint16_t,6>(Speed); } // [0.25 m/s]
|
||||
|
||||
int32_t getAlt(void) const // [m]
|
||||
{ int32_t Word=Position[8]&0x3F; Word<<=8; Word|=Position[7];
|
||||
return UnsVRdecode<int32_t,12>(Word)-316; }
|
||||
void setAlt(int32_t Alt)
|
||||
{ Alt+=316; if(Alt<0) Alt=0;
|
||||
int32_t Word=UnsVRencode<uint32_t,12>(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<int16_t,6>(getClimbWord()); }
|
||||
void setClimb(int16_t Climb) // [0.125 m/s]
|
||||
{ setClimbWord(SignVRencode<int16_t,6>(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<Bytes; Idx++)
|
||||
{ CRC = PolyPass(CRC, Byte[Idx]); }
|
||||
return CRC>>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<Bytes; Idx++)
|
||||
{ CRC = PolyPass(CRC, Byte[Idx]); }
|
||||
CRC=PolyPass(CRC, 0); CRC=PolyPass(CRC, 0); CRC=PolyPass(CRC, 0);
|
||||
return CRC>>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<Bytes; ByteIdx++) // loop over bytes
|
||||
{ uint8_t Byte=PktErr[ByteIdx];
|
||||
uint8_t Mask=0x80;
|
||||
for(uint8_t BitIdx=0; BitIdx<8; BitIdx++) // loop over bits
|
||||
{ if(Byte&Mask)
|
||||
{ if(BadBits<MaxBadBits)
|
||||
{ BadBitIdx[BadBits]=ByteIdx; // store the bad bit index
|
||||
BadBitMask[BadBits]=Mask;
|
||||
Syndrome[BadBits]=CRCsyndrome(ByteIdx*8+BitIdx); }
|
||||
BadBits++;
|
||||
}
|
||||
Mask>>=1;
|
||||
}
|
||||
if(BadBits>MaxBadBits) break;
|
||||
}
|
||||
if(BadBits>MaxBadBits) return -1; // return failure when too many bad bits
|
||||
|
||||
uint8_t Loops = 1<<BadBits; uint8_t PrevGrayIdx=0;
|
||||
for(uint8_t Idx=1; Idx<Loops; Idx++) // loop through all combination of bad bit flips
|
||||
{ uint8_t GrayIdx= Idx ^ (Idx>>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__
|
||||
145
utils/fanet.h
145
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 <map>
|
||||
|
||||
class FANET_NameList
|
||||
{ public:
|
||||
std::map<uint32_t, FANET_Name> List;
|
||||
|
|
@ -612,6 +722,7 @@ class FANET_NameList
|
|||
return 1; }
|
||||
|
||||
} ;
|
||||
#endif
|
||||
|
||||
// ===============================================================================================
|
||||
|
||||
|
|
|
|||
|
|
@ -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<Bytes; Idx++) Format_Hex(Output, Byte[Idx]); }
|
||||
|
||||
void Format_Hex( void (*Output)(char), uint16_t Word )
|
||||
{ Format_Hex(Output, (uint8_t)(Word>>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<Len; Idx++)
|
||||
{ if(Idx) (*Output)(':');
|
||||
|
|
@ -105,6 +112,24 @@ void Format_HHMMSS(void (*Output)(char), uint32_t Time)
|
|||
uint32_t HHMMSS = 10000*Hour + 100*Min + Sec;
|
||||
Format_UnsDec(Output, HHMMSS, 6); }
|
||||
|
||||
void Format_Period(void (*Output)(char), int32_t Time)
|
||||
{ if(Time<0) { (*Output)('-'); Time=(-Time); }
|
||||
else { (*Output)(' '); }
|
||||
if(Time<60) { (*Output)(' '); Format_UnsDec(Output, (uint32_t)Time, 2); (*Output)('s'); return; }
|
||||
if(Time<3600) { Format_UnsDec(Output, (uint32_t)Time/60, 2); (*Output)('m'); Format_UnsDec(Output, (uint32_t)Time%60, 2); return; }
|
||||
if(Time<86400) { Format_UnsDec(Output, (uint32_t)Time/3600, 2); (*Output)('h'); Format_UnsDec(Output, ((uint32_t)Time%3600)/60, 2); return; }
|
||||
Format_UnsDec(Output, (uint32_t)Time/86400, 2); (*Output)('d'); Format_UnsDec(Output, ((uint32_t)Time%86400)/3600, 2); }
|
||||
|
||||
uint8_t Format_Period(char *Out, int32_t Time)
|
||||
{ uint8_t Len=0;
|
||||
if(Time<0) { Out[Len++]='-'; Time=(-Time); }
|
||||
else { Out[Len++]=' '; }
|
||||
if(Time<60) { Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, (uint32_t)Time, 2); Out[Len++]='s'; return Len; }
|
||||
if(Time<3600) { Len+=Format_UnsDec(Out+Len, (uint32_t)Time/60, 2); Out[Len++]='m'; Len+=Format_UnsDec(Out+Len, (uint32_t)Time%60, 2); return Len; }
|
||||
if(Time<86400) { Len+=Format_UnsDec(Out+Len, (uint32_t)Time/3600, 2); Out[Len++]='h'; Len+=Format_UnsDec(Out+Len, ((uint32_t)Time%3600)/60, 2); return Len; }
|
||||
Len+=Format_UnsDec(Out+Len, (uint32_t)Time/86400, 2); Out[Len++]='d'; Len+=Format_UnsDec(Out+Len, ((uint32_t)Time%86400)/3600, 2);
|
||||
return Len; }
|
||||
|
||||
void Format_UnsDec( void (*Output)(char), uint16_t Value, uint8_t MinDigits, uint8_t DecPoint)
|
||||
{ uint16_t Base; uint8_t Pos;
|
||||
for( Pos=5, Base=10000; Base; Base/=10, Pos--)
|
||||
|
|
@ -188,12 +213,26 @@ uint8_t Format_SignDec(char *Out, int32_t Value, uint8_t MinDigits, uint8_t DecP
|
|||
uint8_t Format_Hex( char *Output, uint8_t Byte )
|
||||
{ (*Output++) = HexDigit(Byte>>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<Bytes; Idx++)
|
||||
Len+=Format_Hex(Output+Len, Byte[Idx]);
|
||||
return Len; }
|
||||
|
||||
uint8_t Format_Hex( char *Output, uint16_t Word )
|
||||
{ Format_Hex(Output, (uint8_t)(Word>>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)
|
||||
|
|
|
|||
|
|
@ -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 <class Type>
|
||||
|
|
@ -47,9 +56,19 @@ template <class Type>
|
|||
Word>>=4; }
|
||||
return Digits; }
|
||||
|
||||
template <class Type>
|
||||
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 <class Type>
|
||||
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 <class Type>
|
|||
{ 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 <class Type>
|
||||
int8_t Read_Float1(Type &Value, const char *Inp) // read floating point, take just one digit after decimal point
|
||||
|
|
|
|||
|
|
@ -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<Len; Idx++)
|
||||
{ uint8_t Byte=Data[Idx];
|
||||
CRC=GDL90_CRC16(Byte, CRC);
|
||||
Count+=GDL90_SendEsc(Output, Byte); }
|
||||
Count+=GDL90_SendEsc(Output, CRC&0xFF);
|
||||
Count+=GDL90_SendEsc(Output, CRC>>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<Len; Idx++)
|
||||
{ uint8_t Byte=Data[Idx];
|
||||
CRC=GDL90_CRC16(Byte, CRC);
|
||||
OutLen+=GDL90_SendEsc(Out+OutLen, Byte); }
|
||||
OutLen+=GDL90_SendEsc(Out+OutLen, CRC&0xFF);
|
||||
OutLen+=GDL90_SendEsc(Out+OutLen, CRC>>8);
|
||||
Out[OutLen++]=GDL90_Flag;
|
||||
return OutLen; }
|
||||
|
||||
|
|
|
|||
150
utils/gdl90.h
150
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; Idx<Size; Idx++) Data[Idx]=0; }
|
||||
|
||||
void setAltitude(int32_t Alt) // [5 feet] GPS altitude (ref. to Ellipsoid)
|
||||
{ if(Alt>0x7FFF) Alt=0x7FFF;
|
||||
else if(Alt<(-0x8000)) Alt=(-0x8000);
|
||||
Data[0]=Alt>>8; Data[1]=Alt&0x0FF; }
|
||||
int32_t getAltitude(void) const { int16_t Alt=Data[0]; Alt<<=8; Alt|=Data[1]; return Alt; }
|
||||
|
||||
void setWarning(bool Warn) // [bool]
|
||||
{ if(Warn) Data[2]|=0x80;
|
||||
else Data[2]&=0x7F; }
|
||||
bool getWarning(void) const { return Data[2]&0x80; }
|
||||
|
||||
void setFOM(uint16_t FOM) // [m] vertical Figure of Merit (accuracy ?)
|
||||
{ Data[2] = (Data[2]&0x80) | (FOM>>8);
|
||||
Data[3] = FOM&0xFF; }
|
||||
|
||||
uint16_t getFOM(void) const { uint16_t FOM=Data[2]&0x7F; FOM<<=8; FOM|=Data[3]; return FOM; }
|
||||
|
||||
int Send(void (*Output)(char)) const { return GDL90_Send(Output, 11, Data, Size); }
|
||||
int Send(char *Output ) const { return GDL90_Send(Output, 11, Data, Size); }
|
||||
|
||||
void Print(void) const
|
||||
{ printf("GDL90_GEOMALT: %dft (%dm) Warn:%d\n", getAltitude()*5, getFOM(), getWarning()); }
|
||||
|
||||
} ;
|
||||
|
||||
class GDL90_REPORT // Position report: Traffic: ID = 20, Ownship: ID = 10
|
||||
|
|
@ -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<Size; Idx++) Data[Idx]=0; } // clear all data (Lat/Lon = invalid)
|
||||
|
||||
uint8_t getAlertStatus(void) const { return Data[0]>>4; } // 0 = no alert, 1 = alert
|
||||
void setAlertStatus(uint8_t Status) { Data[0] = (Data[0]&0x0F) | (Status<<4); }
|
||||
|
|
@ -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<Len; Idx++)
|
||||
printf("%02X", Byte[Idx]);
|
||||
printf("\n"); }
|
||||
|
|
@ -232,11 +352,11 @@ class GDL90_RxMsg // receiver for the MAV messages
|
|||
uint8_t ProcessByte(uint8_t RxByte) // process a single byte: add to the message or reject
|
||||
{ // printf("Process[%2d] 0x%02X\n", Len, RxByte);
|
||||
if(Len==0) // if the very first byte
|
||||
{ if(RxByte==SYNC) { Byte[Len]=RxByte; return 1; } // is SYNC then store it
|
||||
else if(Byte[Len]==SYNC)
|
||||
{ Byte[Len]=RxByte; if(RxByte==ESC) return 1;
|
||||
{ if(RxByte==SYNC) { Byte[Len]=RxByte; return 1; } // if SYNC then store it
|
||||
else if(Byte[Len]==SYNC) // if the packet before was SYNC
|
||||
{ Byte[Len]=RxByte; if(RxByte==ESC) return 1; // store the byte
|
||||
Len++; Byte[Len]=0; return 1; }
|
||||
else if(Byte[Len]==ESC)
|
||||
else if(Byte[Len]==ESC) // if the previous byte was ESC
|
||||
{ RxByte^=0x20; Byte[Len++]=RxByte; Byte[Len]=0; return 1; }
|
||||
else return 0; }
|
||||
if(RxByte==SYNC) // if not the very first byte and SYNC then the packet is possibly complete
|
||||
|
|
@ -245,7 +365,7 @@ class GDL90_RxMsg // receiver for the MAV messages
|
|||
for(int Idx=0; Idx<(Len-2); Idx++)
|
||||
{ CRC=GDL90_CRC16(Byte[Idx], CRC); }
|
||||
if( (CRC&0xFF)!=Byte[Len-2] || (CRC>>8)!=Byte[Len-1] ) { Clear(); return 0; }
|
||||
return 2; }
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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; }
|
||||
|
|
|
|||
72
utils/nmea.h
72
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<MaxLen) Data[Len]=0;
|
||||
return; }
|
||||
|
|
@ -98,10 +103,22 @@ inline uint8_t NMEA_AppendCheckCRNL(char *NMEA, uint8_t Len) { return NMEA_Appen
|
|||
{ if(Data[1]!='G') return 0;
|
||||
return Data[2]=='P'; }
|
||||
|
||||
uint8_t isGL(void) const // GLONASS sentence ?
|
||||
{ if(Data[1]!='G') return 0;
|
||||
return Data[2]=='L'; }
|
||||
|
||||
uint8_t isGA(void) const // GALILEO sentence ?
|
||||
{ if(Data[1]!='G') return 0;
|
||||
return Data[2]=='A'; }
|
||||
|
||||
uint8_t isGN(void) const
|
||||
{ if(Data[1]!='G') return 0;
|
||||
return Data[2]=='N'; }
|
||||
|
||||
uint8_t isBD(void) const // for Beidou GSA and GSV
|
||||
{ if(Data[1]!='B') return 0;
|
||||
return Data[2]=='D'; }
|
||||
|
||||
uint8_t isGx(void) const // GPS or GLONASS sentence ?
|
||||
{ return Data[1]=='G'; }
|
||||
|
||||
|
|
@ -141,7 +158,19 @@ inline uint8_t NMEA_AppendCheckCRNL(char *NMEA, uint8_t Len) { return NMEA_Appen
|
|||
if(Data[4]!='G') return 0;
|
||||
return Data[5]=='A'; }
|
||||
|
||||
uint8_t isGPGSA(void) const //
|
||||
uint8_t isGxVTG(void) const // velocity relative to the ground
|
||||
{ if(!isGx()) return 0;
|
||||
if(Data[3]!='V') return 0;
|
||||
if(Data[4]!='T') return 0;
|
||||
return Data[5]=='G'; }
|
||||
|
||||
uint8_t isGxZDA(void) const // UTC time+date, time zone
|
||||
{ if(!isGx()) return 0;
|
||||
if(Data[3]!='Z') return 0;
|
||||
if(Data[4]!='D') return 0;
|
||||
return Data[5]=='A'; }
|
||||
|
||||
uint8_t isGPGSA(void) const // GPS satellite data
|
||||
{ if(!isGP()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='S') return 0;
|
||||
|
|
@ -153,14 +182,14 @@ inline uint8_t NMEA_AppendCheckCRNL(char *NMEA, uint8_t Len) { return NMEA_Appen
|
|||
if(Data[4]!='S') return 0;
|
||||
return Data[5]=='A'; }
|
||||
|
||||
uint8_t isGxGSA(void) const //
|
||||
uint8_t isGxGSA(void) const // GP=GPS, GA=Galileo, GL=Glonass, GB=BaiDou, GN=any of the systems combined
|
||||
{ if(!isGx()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='S') return 0;
|
||||
return Data[5]=='A'; }
|
||||
|
||||
uint8_t isGxGSV(void) const // GPS satellite data
|
||||
{ if(!isGx()) return 0;
|
||||
uint8_t isGxGSV(void) const // Satellite data
|
||||
{ if(!isGx() && !isBD()) return 0; // we include as well $BDGSV
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='S') return 0;
|
||||
return Data[5]=='V'; }
|
||||
|
|
@ -171,19 +200,31 @@ inline uint8_t NMEA_AppendCheckCRNL(char *NMEA, uint8_t Len) { return NMEA_Appen
|
|||
if(Data[4]!='S') return 0;
|
||||
return Data[5]=='V'; }
|
||||
|
||||
uint8_t isGNGSV(void) const // GPS satellite data
|
||||
{ if(!isGN()) return 0;
|
||||
uint8_t isGLGSV(void) const // GLONASS satellite data
|
||||
{ if(!isGL()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='S') return 0;
|
||||
return Data[5]=='V'; }
|
||||
|
||||
uint8_t isGPTXT(void) const // GPS satellite data
|
||||
uint8_t isGAGSV(void) const // GALILEO satellite data
|
||||
{ if(!isGA()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='S') return 0;
|
||||
return Data[5]=='V'; }
|
||||
|
||||
uint8_t isBDGSV(void) const // BEIDOU satellite data
|
||||
{ if(!isBD()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='S') return 0;
|
||||
return Data[5]=='V'; }
|
||||
|
||||
uint8_t isGPTXT(void) const // GPS test message
|
||||
{ if(!isGP()) return 0;
|
||||
if(Data[3]!='T') return 0;
|
||||
if(Data[4]!='X') return 0;
|
||||
return Data[5]=='T'; }
|
||||
|
||||
uint8_t isP(void) const
|
||||
uint8_t isP(void) const // Private thus specific to the euqipment
|
||||
{ return Data[1]=='P'; }
|
||||
|
||||
uint8_t isPOGN(void) const // OGN dedicated NMEA sentence
|
||||
|
|
@ -204,6 +245,13 @@ inline uint8_t NMEA_AppendCheckCRNL(char *NMEA, uint8_t Len) { return NMEA_Appen
|
|||
{ if(!isPOGN()) return 0;
|
||||
return Data[5]=='S'; }
|
||||
|
||||
uint8_t isPGRMZ(void) // barometric pressure report
|
||||
{ if(!isP()) return 0;
|
||||
if(Data[2]!='G') return 0;
|
||||
if(Data[3]!='R') return 0;
|
||||
if(Data[4]!='M') return 0;
|
||||
return Data[5]=='Z'; }
|
||||
|
||||
uint8_t isPOGNL(void) // log file list request
|
||||
{ if(!isPOGN()) return 0;
|
||||
return Data[5]=='L'; }
|
||||
|
|
|
|||
795
utils/ogn.h
795
utils/ogn.h
Plik diff jest za duży
Load Diff
232
utils/ogn1.h
232
utils/ogn1.h
|
|
@ -5,6 +5,7 @@
|
|||
|
||||
#include <string.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#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<InfoParmNum ? Name[Idx]:0; }
|
||||
|
||||
#ifndef __AVR__
|
||||
|
||||
uint32_t getTime(uint32_t RefTime, int FwdMargin=5) const
|
||||
{ if(Position.Time>=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<<Status.RxRate)-1); Out[Len++]='/'; Out[Len++]='m'; Out[Len++]='i'; Out[Len++]='n';
|
||||
return Len; }
|
||||
|
||||
uint8_t WriteDeviceStatus(char *Out)
|
||||
Out[Len]=0; return Len; }
|
||||
/*
|
||||
int WriteDeviceStatus(char *Out) const
|
||||
{ return sprintf(Out, " h%02X v%02X %dsat/%d/%ddB %ldm %3.1fhPa %+4.1fdegC %3.1f%% %4.2fV %d/%+4.1fdBm %d/min",
|
||||
Status.Hardware, Status.Firmware, Status.Satellites, Status.FixQuality, 8+Status.SatSNR, (long int)DecodeAltitude(),
|
||||
0.08*Status.Pressure, 0.1*DecodeTemperature(), 0.1*DecodeHumidity(),
|
||||
(1.0/64)*DecodeVoltage(), Status.TxPower+4, -0.5*Status.RadioNoise, (1<<Status.RxRate)-1 );
|
||||
}
|
||||
*/
|
||||
int PrintDeviceInfo(char *Out) const
|
||||
{ int Len=0;
|
||||
char Value[16];
|
||||
uint8_t InfoType;
|
||||
uint8_t Idx=0;
|
||||
for( ; ; )
|
||||
{ uint8_t Chars = readInfo(Value, InfoType, Idx);
|
||||
if(Chars==0) break;
|
||||
Out[Len++]=' ';
|
||||
if(InfoType<InfoParmNum)
|
||||
{ Len += Format_String(Out+Len, InfoParmName(InfoType)); }
|
||||
else
|
||||
{ Out[Len++]='#'; Out[Len++]=HexDigit(InfoType); }
|
||||
Out[Len++]='=';
|
||||
Len += Format_String(Out+Len, Value);
|
||||
Idx+=Chars; }
|
||||
Out[Len]=0; return Len; }
|
||||
|
||||
uint8_t WriteDeviceInfo(char *Out)
|
||||
/*
|
||||
int WriteDeviceInfo(char *Out)
|
||||
{ int Len=0;
|
||||
char Value[16];
|
||||
uint8_t InfoType;
|
||||
|
|
@ -213,12 +289,19 @@ class OGN1_Packet // Packet structure for the OGN tracker
|
|||
{ Len += sprintf(Out+Len, " #%d=%s", InfoType, Value); }
|
||||
Idx+=Chars; }
|
||||
Out[Len]=0; return Len; }
|
||||
*/
|
||||
void setStatus (void) { Status.ReportType=0; }
|
||||
void setInfo (void) { Status.ReportType=1; }
|
||||
void setManufMsg(void) { Status.ReportType=15; }
|
||||
|
||||
bool isStatus (void) const { return Status.ReportType==0; }
|
||||
bool isInfo (void) const { return Status.ReportType==1; }
|
||||
bool isManufMsg(void) const { return Status.ReportType==15; }
|
||||
|
||||
void Print(void) const
|
||||
{ if(!Header.NonPos) { PrintPosition(); return; }
|
||||
if(Status.ReportType==0) { PrintDeviceStatus(); return; }
|
||||
if(Status.ReportType==1) { PrintDeviceInfo(); return; }
|
||||
}
|
||||
{ if(!Header.NonPos) return PrintPosition();
|
||||
if(isStatus()) return PrintDeviceStatus();
|
||||
if(isInfo ()) return PrintDeviceInfo(); }
|
||||
|
||||
void PrintDeviceInfo(void) const
|
||||
{ printf("%c:%06lX R%c%c%c:",
|
||||
|
|
@ -263,7 +346,14 @@ class OGN1_Packet // Packet structure for the OGN tracker
|
|||
printf("\n");
|
||||
}
|
||||
|
||||
int WriteJSON(char *JSON) const
|
||||
int DecodePosition(float &Lat, float &Lon, int &Alt)
|
||||
{ if(Header.NonPos) return 0;
|
||||
Lat = (0.0001f/60)*DecodeLatitude();
|
||||
Lon = (0.0001f/60)*DecodeLongitude();
|
||||
Alt = DecodeAltitude();
|
||||
return 3; }
|
||||
|
||||
int WriteStxJSON(char *JSON) const // Stratux JSON message
|
||||
{ int Len=0;
|
||||
Len+=Format_String(JSON+Len, "\"addr\":\"");
|
||||
Len+=Format_Hex(JSON+Len, (uint8_t) (Header.Address>>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__
|
||||
|
||||
|
|
|
|||
28
utils/ogn2.h
28
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<<Status.RxRate)-1); Out[Len++]='/'; Out[Len++]='m'; Out[Len++]='i'; Out[Len++]='n';
|
||||
Out[Len]=0; return Len; }
|
||||
|
||||
int WriteDeviceStatus(char *Out)
|
||||
{ return sprintf(Out, " h%02X v%02X %dsat/%d %ldm %3.1fhPa %+4.1fdegC %3.1f%% %4.2fV %d/%+4.1fdBm %d/min",
|
||||
Status.Hardware, Status.Firmware, Status.Satellites, Status.FixQuality, (long int)DecodeAltitude(),
|
||||
{ return sprintf(Out, " h%02X v%02X %dsat/%d/%ddB %ldm %3.1fhPa %+4.1fdegC %3.1f%% %4.2fV %d/%+4.1fdBm %d/min",
|
||||
Status.Hardware, Status.Firmware, Status.Satellites, Status.FixQuality, 8+Status.SatSNR, (long int)DecodeAltitude(),
|
||||
0.08*Status.Pressure, 0.1*DecodeTemperature(), 0.1*DecodeHumidity(),
|
||||
(1.0/64)*DecodeVoltage(), Status.TxPower+4, -0.5*Status.RadioNoise, (1<<Status.RxRate)-1 );
|
||||
}
|
||||
|
|
@ -162,8 +182,8 @@ class OGN2_Packet // Packet structure for the OGN tracker
|
|||
void PrintDeviceStatus(void) const
|
||||
{ printf("%c:%06lX R%c%c %02ds:",
|
||||
'0'+Header.AddrType, (long int)Header.Address, '0'+Header.Relay, Header.Emergency?'E':' ', Status.Time);
|
||||
printf(" h%02X v%02X %dsat/%d %ldm %3.1fhPa %+4.1fdegC %3.1f%% %4.2fV Tx:%ddBm Rx:%+4.1fdBm %d/min",
|
||||
Status.Hardware, Status.Firmware, Status.Satellites, Status.FixQuality, (long int)DecodeAltitude(),
|
||||
printf(" h%02X v%02X %dsat/%d/%ddB %ldm %3.1fhPa %+4.1fdegC %3.1f%% %4.2fV Tx:%ddBm Rx:%+4.1fdBm %d/min",
|
||||
Status.Hardware, Status.Firmware, Status.Satellites, Status.FixQuality, 8+Status.SatSNR, (long int)DecodeAltitude(),
|
||||
0.08*Status.Pressure, 0.1*DecodeTemperature(), 0.1*DecodeHumidity(),
|
||||
(1.0/64)*DecodeVoltage(), Status.TxPower+4, -0.5*Status.RadioNoise, (1<<Status.RxRate)-1 );
|
||||
printf("\n");
|
||||
|
|
|
|||
|
|
@ -4,6 +4,22 @@
|
|||
#include "format.h"
|
||||
#include "ognconv.h"
|
||||
|
||||
// ==============================================================================================
|
||||
// Coordinate scales:
|
||||
// - uBlox GPS and FLARM: LSB = 1e-7 deg
|
||||
// - OGN-Tracker: LSB = 0.0001/60 deg
|
||||
// - FANET/ADS-L pseudo-cordic: LSB =
|
||||
// - True cordic: 2^32 = 360 deg
|
||||
|
||||
int32_t Coord_FNTtoOGN(int32_t Coord) { return ((int64_t)Coord*27000219 +(1<<28))>>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
|
||||
|
|
|
|||
|
|
@ -3,9 +3,27 @@
|
|||
|
||||
#include <stdint.h>
|
||||
|
||||
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
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue