diff --git a/utils/aprs2igc.cc b/utils/aprs2igc.cc new file mode 100644 index 0000000..03230b6 --- /dev/null +++ b/utils/aprs2igc.cc @@ -0,0 +1,83 @@ +#include +#include +#include +#include +#include + +#include "format.h" + +static int APRS2IGC(char *Out, const char *Inp, int GeoidSepar) // convert APRS positon message into IGC B-record +{ int Len=0; + const char *Msg = strchr(Inp, ':'); if(Msg==0) return 0; // colon: separates header and message + 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[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 + memcpy(Out+Len, Pos+5, 2); Len+=2; // copy fractional MM + Out[Len++] = ExtPos?ExtPos[0]:'0'; // extended precision + Out[Len++] = Pos[7]; // copy N/S sign + memcpy(Out+Len, Pos+9, 5); Len+=5; // copy DDMM + memcpy(Out+Len, Pos+15,2); Len+=2; // copy fractional MM + Out[Len++] = ExtPos?ExtPos[1]:'0'; // extended precision + Out[Len++] = Pos[17]; // copy E/W sign + Out[Len++] = 'A'; + memcpy(Out+Len, " ", 10); + const char *FL = strstr(Pos+18, " FL"); + if(FL) // pressure altitude + { float PressAlt=atof(FL+3); PressAlt*=30.4; int32_t Alt=floor(PressAlt+0.5); + if(Alt<0) { Alt = (-Alt); Out[Len] = '-'; Format_UnsDec(Out+Len+1, (uint32_t)Alt, 4); } + else { Format_UnsDec(Out+Len, (uint32_t)Alt, 5); } + } + Len+=5; + if(Pos[27]=='A' && Pos[28]=='=') // geometrical altitude + { int32_t Alt=atol(Pos+29); Alt=(Alt*3+5)/10; Alt+=GeoidSepar; // convert to meters and add GeoidSepar for HAE + if(Alt<0) { Alt = (-Alt); Out[Len] = '-'; Format_UnsDec(Out+Len+1, (uint32_t)Alt, 4); } + else { Format_UnsDec(Out+Len, (uint32_t)Alt, 5); } + } + Len+=5; + Out[Len]=0; return Len; } + +static int Verbose = 1; +static int GeoidSepar = 40; + +static FILE *OutFile = 0; + +int main(int argc, char *argv[]) +{ if(argc<3) + { printf("Usage: %s \n", argv[0]); + return 0; } + + const char *OwnAcft = argv[1]; int OwnAcftLen = strlen(OwnAcft); + char OutFileName[32]; strcpy(OutFileName, OwnAcft); strcat(OutFileName, ".IGC"); + OutFile=fopen(OutFileName, "wt"); + if(OutFile==0) { printf("Cannot open %s for write\n", OutFileName); return 0; } + + const char *InpFileName = argv[2]; + + FILE *InpFile=fopen(InpFileName, "rt"); + if(InpFile==0) { printf("Cannot open %s for read\n", InpFileName); return 0; } + + char InpLine[256]; + char OutLine[256]; + int InpLines=0; + int OutLines=0; + for( ; ; ) + { if(fgets(InpLine, 256, InpFile)==0) break; + char *EOL = strchr(InpLine, '\n'); if(EOL==0) break; + *EOL = 0; + InpLines++; + if(memcmp(InpLine, OwnAcft, OwnAcftLen)) continue; + int OutLen=APRS2IGC(OutLine, InpLine, GeoidSepar); + if(OutLen>0) + { if(Verbose) printf("%s => %s [%d]\n", InpLine, OutLine, OutLen); + fprintf(OutFile, "%s\n", OutLine); OutLines++; } + } + fclose(InpFile); + printf("%d lines from %s\n", InpLines, InpFileName); + fclose(OutFile); + printf("%d lines to %s\n", OutLines, OutFileName); + + return 0; } diff --git a/utils/atmosphere.cpp b/utils/atmosphere.cpp new file mode 100644 index 0000000..57e55bd --- /dev/null +++ b/utils/atmosphere.cpp @@ -0,0 +1,5 @@ +#include "atmosphere.h" + +// 200, 300, 400, 500, 600, 700, 800, 900, 1000, 1100 // [ hPa] + const int32_t Atmosphere::StdAltTable[10] = { 117764, 91652, 71864, 55752, 42070, 30126, 19493, 9886, 1109, -6984 } ; // [0.1 m] + diff --git a/utils/atmosphere.h b/utils/atmosphere.h new file mode 100644 index 0000000..0914981 --- /dev/null +++ b/utils/atmosphere.h @@ -0,0 +1,63 @@ +#ifndef __ATMOSPHERE_H__ +#define __ATMOSPHERE_H__ + +#include +#include + +class Atmosphere +{ public: + // int32_t Pressure; // [ Pa ] + // int32_t Altitude; // [0.1 m ] + // int32_t Temperature; // [0.1 degC] + + // altitude vs. pressure // 20000, 30000, 40000, 50000, 60000, 70000, 80000, 90000, 100000, 110000 // [ Pa] + static const int32_t StdAltTable[10]; // = { 117764, 91652, 71864, 55752, 42070, 30126, 19493, 9886, 1109, -6984 } ; // [0.1 m] + + public: + // dH/dP = -R/g*T => R = 287.04m^2/K/sec^2, g = 9.80655m/s^2 + + static int32_t PressureLapseRate(int32_t Pressure, int32_t Temperature=150) // [Pa], [0.1 degC] => [0.0001 m/Pa] + { return -((int32_t)29270*(Temperature+2732)+Pressure/2)/Pressure; } + // int32_t PressureLapseRate(void) { return PressureLapseRate(Pressure, Temperature); } + + static const int32_t StdPressureAtSeaLevel = 101325; // [Pa] + static const int32_t StdPressureAt11km = 22632; // [Pa] + static const int32_t StdPressureAt20km = 5475; // [Pa] + static const int32_t StdTemperatureLapseRate = -65; // [0.1 degC/1000m] valid till 11km + static const int32_t StdTemperatureAtSeaLevel = 150; // [0.1 degC ] + static const int32_t StdTemperatureAt11km = -565; // [0.1 degC ] + + static int32_t StdTemperature(int32_t Altitude) // [0.1 m ] valid till 20km + { if(Altitude>110000) return StdTemperatureAt11km; + return StdTemperatureAtSeaLevel+(StdTemperatureLapseRate*Altitude-5000)/10000; } + + static int32_t AltitudeDelta(int32_t PressureDelta, int32_t PressureLapseRate) // [Pa], [0.0001 m/Pa] + { return (PressureDelta*PressureLapseRate)/100; } // [0.01m] + + static int32_t AltitudeDelta(int32_t PressureDelta, int32_t Pressure, int32_t Temperature) // [Pa], [Pa], [0.1degC] + { int32_t PLR=PressureLapseRate(Pressure, Temperature); return AltitudeDelta(PressureDelta, PLR); } // [0.01m] + + static int32_t StdAltitude(int32_t Pressure, int32_t PressStep=100) // [Pa] + { int32_t Idx=(Pressure+5000)/10000; Idx-=2; + if(Idx<0) Idx=0; else if(Idx>9) Idx=9; + int32_t Press = 10000*(Idx+2); + int32_t Altitude = 10*StdAltTable[Idx]; + for( ; ; ) + { int32_t Temp=StdTemperature(Altitude/10); + int32_t Delta=Pressure-Press; if(Delta==0) break; + if(Delta>PressStep) Delta=PressStep; + else if(Delta<(-PressStep)) Delta=(-PressStep); + Altitude+=AltitudeDelta(Delta, Press, Temp); + Press+=Delta; + } + return Altitude/10; } // [0.1m] + +#ifdef NO_RTOS + static int32_t StdAltitude_float(int32_t Pressure) + { return floor(443300*(1-powf((float)Pressure/(float)101325.0, (float)0.190295))+0.5); } +#endif + + +} ; + +#endif // __ATMOSPHERE_H__ diff --git a/utils/bitcount.cpp b/utils/bitcount.cpp new file mode 100644 index 0000000..0b08323 --- /dev/null +++ b/utils/bitcount.cpp @@ -0,0 +1,33 @@ +#include "bitcount.h" + +#ifndef BITCOUNT_USE_BUILTIN +#ifdef BITCOUNT_SAVE_FLASH +const uint8_t ByteCount1s[ 16] = { 0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4 } ; +#else +const uint8_t ByteCount1s[256] = { + 0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4, + 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, + 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, + 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, + 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, + 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, + 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, + 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, + 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, + 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, + 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, + 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, + 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, + 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, + 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, + 4, 5, 5, 6, 5, 6, 6, 7, 5, 6, 6, 7, 6, 7, 7, 8 +} ; +#endif +#endif + +int Count1s(const uint8_t *Byte, int Bytes) +{ int Count=0; + for( ; Bytes>0; Bytes--) + { Count += Count1s(*Byte++); } + return Count; } + diff --git a/utils/bitcount.h b/utils/bitcount.h new file mode 100644 index 0000000..ce3683f --- /dev/null +++ b/utils/bitcount.h @@ -0,0 +1,71 @@ +// Fast bit counting for (forward) error correction codes +// (c) 2003, Pawel Jalocha, Pawel.Jalocha@cern.ch + +#ifndef __BITCOUNT_H__ +#define __BITCOUNT_H__ + +#include + +// #define BITCOUNT_USE_BUILTIN +#define BITCOUNT_SAVE_FLASH + +// ========================================================================== +// a table for fast bit counting + +#ifdef BITCOUNT_SAVE_FLASH +extern const uint8_t ByteCount1s[16]; +#else +extern const uint8_t ByteCount1s[256]; +#endif + +// ========================================================================== + +#ifdef BITCOUNT_USE_BUILTIN +inline uint8_t Count1s(uint8_t Byte) { return __builtin_popcount(Byte); } +#else +#ifdef BITCOUNT_SAVE_FLASH +inline uint8_t Count1s(uint8_t Byte) { return ByteCount1s[Byte&0x0F] + ByteCount1s[Byte>>4]; } +#else +inline uint8_t Count1s(uint8_t Byte) { return ByteCount1s[Byte]; } +#endif +#endif + +inline uint8_t Count1s(int8_t Byte) { return Count1s((uint8_t)Byte); } + +#ifdef BITCOUNT_USE_BUILTIN +inline uint8_t Count1s(uint16_t Word) { return __builtin_popcount(Word); } +#else +inline uint8_t Count1s(uint16_t Word) +{ return Count1s((uint8_t)Word) + +Count1s((uint8_t)(Word>>8)); } +#endif + +inline uint8_t Count1s(int16_t Word) { return Count1s((uint16_t)Word); } + +#ifdef BITCOUNT_USE_BUILTIN +inline uint8_t Count1s(uint32_t LongWord) { return __builtin_popcountl(LongWord); } +#else +inline uint8_t Count1s(uint32_t LongWord) +{ return Count1s((uint16_t)LongWord) + +Count1s((uint16_t)(LongWord>>16)); } +#endif + +inline uint8_t Count1s(int32_t LongWord) { return Count1s((uint32_t)LongWord); } + +#ifdef BITCOUNT_USE_BUILTIN +inline uint8_t Count1s(uint64_t LongWord) { return __builtin_popcountll(LongWord); } +#else +inline uint8_t Count1s(uint64_t LongWord) +{ return Count1s((uint32_t)LongWord) + +Count1s((uint32_t)(LongWord>>32)); } +#endif + +inline uint8_t Count1s(int64_t LongWord) { return Count1s((uint64_t)LongWord); } + +int Count1s(const uint8_t *Byte, int Bytes); + +// ========================================================================== + +// use __builtin_popcount(unsigned int) ? http://stackoverflow.com/questions/109023/how-to-count-the-number-of-set-bits-in-a-32-bit-integer + +#endif // of __BITCOUNT_H__ diff --git a/utils/fanet.h b/utils/fanet.h new file mode 100644 index 0000000..c5914f2 --- /dev/null +++ b/utils/fanet.h @@ -0,0 +1,618 @@ +#ifndef __FANET_H__ +#define __FANET_H__ + +#include +#include +#include + +#include + +#include "format.h" +#include "intmath.h" + +// =============================================================================================== + +class FANET_Packet +{ public: + union + { uint8_t Flags; + struct + { uint8_t CR:3; // Coding rate used (RX) or to be used (TX) + bool hasCRC:1; // CRC was there (RX) + bool badCRC:1; // CRC was bad (RX) + bool Done:1; + } ; + } ; + uint8_t Len; // [bytes] packet length + static const int MaxBytes = 40; + uint8_t Byte[MaxBytes+2]; + + public: + FANET_Packet() { Len=0; } + + uint8_t Dump(char *Out) + { uint8_t Len=0; + for(int Idx=0; IdxLen; Idx++) + { Len+=Format_Hex(Out+Len, Byte[Idx]); } + return Len; } + + bool ExtHeader(void) const { return Byte[0]&0x80; } // is there extended header ? + bool Forward(void) const { return Byte[0]&0x40; } // forward flag + uint8_t Type(void) const { return Byte[0]&0x3F; } // message type + uint32_t getAddr(void) const { uint32_t Addr=Byte[1]; Addr<<=8; Addr|=Byte[3]; Addr<<=8; Addr|=Byte[2]; return Addr; } // 24-bit source address + uint8_t getAddrPref(void) const { return Byte[1]; } + + 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; + 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 setHeader(uint8_t Type) { Byte[0] = 0x40 | (Type&0x3F); } + void setType(uint8_t Type) { Byte[0] = (Byte[0]&0xC0) | (Type&0x3F); } + + uint8_t ExtHeaderLen(void) const // length ot the extended header (zero in most cases) + { if(!ExtHeader()) return 0; + uint8_t Len=1; + if(Byte[4]&0x20) Len+=3; // if Unicast + if(Byte[4]&0x10) Len+=4; // if Signature + return Len; } + + 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(); } + + void setName(const char *Name) + { setHeader(2); + uint8_t Ofs; + for(Ofs=MsgOfs(); Ofs>7; Byte[0]=Lat; Byte[1]=Lat>>8; Byte[2]=Lat>>16; } + static int32_t getLon(const uint8_t *Byte) // FANET cordic units + { int32_t Longitude=Byte[2]; Longitude<<=8; Longitude|=Byte[1]; Longitude<<=8; Longitude|=Byte[0]; Longitude<<=8; return Longitude; } + static void setLon(uint8_t *Byte, int32_t Lon) + { Lon = (Lon+0x80)>>8; Byte[0]=Lon; Byte[1]=Lon>>8; Byte[2]=Lon>>16; } + + static uint16_t getSpeed(uint8_t Byte) // [0.5km/h] for Type=1 + { if(Byte<0x80) return Byte; + return (uint16_t)5*(Byte-0x80); } + static void setSpeed(uint8_t *Byte, uint16_t Speed) + { if(Speed>=128) { Speed=(Speed+2)/5; if(Speed>=0x80) Speed=0x7F; Speed|=0x80; } + Byte[0]=Speed; } + + static int16_t getClimb(uint8_t Byte) // [0.1m/s] + { int16_t Climb = Byte&0x3F; if(Byte&0x40) Climb|=0xFFC0; + if(Byte&0x80) return Climb*5; + return Climb; } + static void setClimb(uint8_t *Byte, int16_t Climb) // [0.1m/s] + { if(Climb>63) + { Climb = (Climb+2)/5; if(Climb>63) Climb=63; + Byte[0] = 0x80 | Climb; } + else if(Climb<(-63)) + { Climb = (Climb-2)/5; if(Climb<(-63)) Climb=(-63); + Byte[0] = 0x80 | Climb; } + else + { Byte[0] = Climb&0x7F; } + } + + static uint16_t getDir(uint8_t Byte) // [deg] + { uint16_t Dir = ((uint16_t)45*Byte+0x10)>>5; return Dir; } + + static uint16_t getPressure(const uint8_t *Byte) // [0.1hPa] + { uint16_t Press = Byte[1]; Press<<=8; Press|=Byte[0]; return Press+4300; } + + static int16_t getTurnRate(uint8_t Byte) // [0.25deg/s] + { int16_t Rate = Byte&0x7F; if(Byte&0x40) Rate|=0xFF80; + if(Byte&0x80) return Rate<<2; + return Rate; } + static void setTurnRate(uint8_t *Byte, int16_t Rate) + { if(Rate>= 64 ) { if(Rate> 254 ) Rate= 254 ; Byte[0] = 0x80 | ((Rate+2)>>2); return; } + if(Rate<(-64)) { if(Rate<(-254)) Rate=(-254); Byte[0] = 0x80 | (((Rate+2)>>2)&0x7F); return; } + Byte[0] = Rate&0x7F; } + + static int16_t getQNE(uint8_t Byte) // [m] difference between pressure altitude and GPS altitude + { int16_t QNE = Byte&0x7F; if(Byte&0x40) QNE|=0xFF80; + if(Byte&0x80) return QNE<<2; + return QNE; } + static void setQNE(uint8_t *Byte, int16_t QNE) + { if(QNE>= 64 ) { if(QNE> 254 ) QNE= 254 ; Byte[0] = 0x80 | ((QNE+2)>>2); return; } + if(QNE<(-64)) { if(QNE<(-254)) QNE=(-254); Byte[0] = 0x80 | (((QNE+2)>>2)&0x7F); return; } + Byte[0] = QNE&0x7F; } + + static uint16_t getAltitude(const uint8_t *Byte) // [m] + { uint16_t Alt = Byte[1]; Alt<<=8; Alt|=Byte[0]; Alt&=0x0FFF; + if(Alt<0x800) return Alt; + return (Alt-0x800)<<2; } + void setAltitude(uint8_t *Byte, uint16_t Alt) // [m] + { if(Alt>=0x800) { Alt>>=2; if(Alt>0x800) Alt=0x800; Alt|=0x800; } + Byte[0]=Alt; Byte[1] = (Byte[1]&0xF0) | (Alt>>8); } + + // [0..7] [0..1] [FANET cordic] [FANET cordic] [m] [cordic] [0.1m/s] [0.1m/s] [0.1deg/s] + void setAirPos(uint8_t AcftType, uint8_t Track, int32_t Lat, int32_t Lon, int16_t Alt, uint8_t Dir, uint16_t Speed, int16_t Climb, int16_t Turn) + { setHeader(1); + uint8_t Ofs=MsgOfs(); + Len=Ofs+12; + setLat(Byte+Ofs, Lat); // [cordic] + setLon(Byte+Ofs+3, Lon); // [cordic] + Byte[Ofs+7]=(AcftType<<4) | (Track<<7); + if(Alt<0) Alt=0; + setAltitude(Byte+Ofs+6, Alt); // [m] + Speed = (Speed*23+16)>>5; // [0.1m/s] => [0.5km/h] + setSpeed(Byte+Ofs+8, Speed); // [0.5km/h] + setClimb(Byte+Ofs+9, Climb); // [0.1m/s] + Byte[Ofs+10] = Dir; // [cordic] + setTurnRate(Byte+Ofs+11, Turn*2/5); } // [0.25deg/s] + + void setQNE(int32_t StdAltitude) // [m] only for air-position + { uint8_t Ofs=MsgOfs(); + int32_t Alt=getAltitude(Byte+Ofs+6); + if(Len<(Ofs+13)) Len=Ofs+13; + setQNE(Byte+Ofs+12, StdAltitude-Alt); } + + // [0..15] [0..1] [FANET cordic] [FANET cordic] + void setGndPos(uint8_t Status, uint8_t Track, int32_t Lat, int32_t Lon) + { setHeader(7); + uint8_t Ofs=MsgOfs(); + Len=11; + setLat(Byte+Ofs, Lat); + setLon(Byte+Ofs+3, Lon); + Byte[Ofs+6] = (Status<<4) | Track; } + +/* + * $FNNGB,manufacturer(hex),id(hex),name(up to 32bytes),type/status,latitude,longitude,altitude,climb,speed,heading*checksum + * manufacturer: 1-2 chars hex + * id: 1-4 chars hex + * name: string up to 32 chars + * type/status: while airborne: aircraft type: 0-7 (3D tracking), else: status: 0-15 (2D tracking) +10 -> 10-25 + * latitude: %.5f in degree + * longitude: %.5f in degree + * altitude: %.f in meter, -1000 for ground + * climb: %.1f in m/s + * speed: %.1f in km/h + * heading: %.f in degree + * + * for the types please see: https://github.com/3s1d/fanet-stm32/blob/master/Src/fanet/radio/protocol.txt + * + */ + + uint8_t WriteFNNGB(char *Out) + { return 0; } + + void Print(const char *Name=0) const + { if(Name) printf("%s ", Name); + printf("[%2d:%d:%2d] FNT%06X", Len, Type(), MsgLen(), getAddr()); + if(Type()==2) // Name + { printf(" "); + for(uint8_t Idx=MsgOfs(); Idx>4; + int32_t Lat=getLat(Byte+Idx); // [FANET cordic] + int32_t Lon=getLon(Byte+Idx+3); // [FANET cordic] + uint16_t Alt=getAltitude(Byte+Idx+6); // [m] + uint16_t Speed=getSpeed(Byte[Idx+8]); // [0.5km/h] + int16_t Climb=getClimb(Byte[Idx+9]); // [0.1m/s] + printf(" [%+09.5f,%+010.5f] %dm %3.1fkm/h %03.0fdeg %+4.1fm/s a%X%c", + FloatCoord(Lat), FloatCoord(Lon), Alt, 0.5*Speed, (180.0/128)*Byte[Idx+10], 0.1*Climb, + AcftType&0x07, AcftType&0x08?'T':'H'); + if((Idx+11)>5)&0x0F, 2019+((Fw>>9)&0x3F), Fw&0x8000?'d':'r'); + printf("\n"); return; } + printf("\n"); + } + + uint8_t Read(const char *Inp) // read packet from a hex dump + { for( Len=0; Len>30; } // convert FANET-cordic to UBX 10e-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 + { Lat = CoordUBX(Lat); // convert from FANET cordic to UBX 1e-7 deg + char Sign; + if(Lat>0) { Sign='N'; } + else { Sign='S'; Lat=(-Lat); } + int32_t Dig; + Dig=Lat/100000000; (*Str++)='0'+Dig; Lat-=Dig*100000000; + Dig=Lat/10000000; (*Str++)='0'+Dig; Lat-=Dig*10000000; + Lat*=60; + Dig=Lat/100000000; (*Str++)='0'+Dig; Lat-=Dig*100000000; + Dig=Lat/10000000; (*Str++)='0'+Dig; Lat-=Dig*10000000; + (*Str++)='.'; + Dig=Lat/1000000; (*Str++)='0'+Dig; Lat-=Dig*1000000; + Dig=Lat/100000; (*Str++)='0'+Dig; Lat-=Dig*100000; + Dig=Lat/10000; HighRes ='0'+Dig; Lat-=Dig*10000; + (*Str++)=Sign; + return 8; } + + static int Format_Lon(char *Str, int32_t Lon, char &HighRes) // format longitude after APRS + { Lon = CoordUBX(Lon); // convert from FANET cordic to UBX 1e-7 deg + char Sign; + if(Lon>0) { Sign='E'; } + else { Sign='W'; Lon=(-Lon); } + int32_t Dig; + Dig=Lon/1000000000; (*Str++)='0'+Dig; Lon-=Dig*1000000000; + Dig=Lon/100000000; (*Str++)='0'+Dig; Lon-=Dig*100000000; + Dig=Lon/10000000; (*Str++)='0'+Dig; Lon-=Dig*10000000; + Lon*=60; + Dig=Lon/100000000; (*Str++)='0'+Dig; Lon-=Dig*100000000; + Dig=Lon/10000000; (*Str++)='0'+Dig; Lon-=Dig*10000000; + (*Str++)='.'; + Dig=Lon/1000000; (*Str++)='0'+Dig; Lon-=Dig*1000000; + Dig=Lon/100000; (*Str++)='0'+Dig; Lon-=Dig*100000; + Dig=Lon/10000; HighRes ='0'+Dig; Lon-=Dig*10000; + (*Str++)=Sign; + return 9; } + +} ; + +class FANET_RxPacket: public FANET_Packet +{ public: + uint32_t sTime; // [ s] reception time + uint16_t msTime; // [ms] + int16_t FreqOfs; // [ 10Hz] + int8_t SNR; // [0.25dB] + int8_t RSSI; // [dBm] + uint8_t BitErr; // number of bit errors + uint8_t CodeErr; // number of block 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; } + + void Print(char *Name=0) const + { char HHMMSS[8]; + Format_HHMMSS(HHMMSS, SlotTime()); HHMMSS[6]='h'; HHMMSS[7]=0; + 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 Len=0; + Len+=Format_String(JSON+Len, "\"addr\":\""); + Len+=Format_Hex(JSON+Len, Byte[1]); + Len+=Format_Hex(JSON+Len, Byte[3]); + Len+=Format_Hex(JSON+Len, Byte[2]); + JSON[Len++]='\"'; + JSON[Len++]=','; + Len+=Format_String(JSON+Len, "\"addr_type\":"); + JSON[Len++] = HexDigit(getAddrType()); + const uint8_t *Msg = this->Msg(); + uint8_t MsgLen = this->MsgLen(); + uint8_t Type = this->Type(); + uint32_t Time=sTime; if(msTime<300) Time--; + Len+=Format_String(JSON+Len, ",\"time\":"); + Len+=Format_UnsDec(JSON+Len, Time); + int64_t RxTime=(int64_t)sTime-Time; RxTime*=1000; RxTime+=msTime; + Len+=Format_String(JSON+Len, ",\"rx_time\":"); + Len+=Format_SignDec(JSON+Len, RxTime, 4, 3, 1); + if(Type==2) + { Len+=Format_String(JSON+Len, ",\"Name\":\""); + for(int Idx=0; Idx>4; // get the aircraft-type and online-track flag + Len+=Format_String(JSON+Len, ",\"acft_type\":\""); + JSON[Len++] = HexDigit(AcftType&0x7); + JSON[Len++]='\"'; + Len+=Format_String(JSON+Len, ",\"acft_cat\":\""); // GDL90 aircraft category + // no-info, para-glider, hang-glider, balloon, glider, powered, heli, UAV + const uint8_t AcftCat[8] = { 0, 12, 12, 10, 9, 1, 7, 14 } ; + Len+=Format_Hex(JSON+Len, AcftCat[AcftType&0x07]); + JSON[Len++]='\"'; + Len+=Format_String(JSON+Len, ",\"no_track\":"); + JSON[Len++]='0' + (AcftType>>3); } + if(Type==4) // for service/weather + { uint8_t Service=Msg[0]; // + Len+=Format_String(JSON+Len, ",\"service\":\""); + if(Service&0x80) JSON[Len++]='I'; + if(Service&0x40) JSON[Len++]='T'; + if(Service&0x20) JSON[Len++]='W'; + if(Service&0x10) JSON[Len++]='H'; + if(Service&0x08) JSON[Len++]='B'; + // if(Service&0x04) JSON[Len++]='R'; + if(Service&0x02) JSON[Len++]='C'; + JSON[Len++]='\"'; + int32_t Lat = getLat(Msg+1); // [cordic] decode the latitude + int32_t Lon = getLon(Msg+4); // [cordic] decode the longitude + 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); } + 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 + 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); } + if(Type==1) // for airpborne position + { 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_msl_m\":"); + Len+=Format_UnsDec(JSON+Len, Alt); + Len+=Format_String(JSON+Len, ",\"track_deg\":"); + Len+=Format_UnsDec(JSON+Len, Dir); + Len+=Format_String(JSON+Len, ",\"speed_mps\":"); + Len+=Format_UnsDec(JSON+Len, Speed, 2, 1); + Len+=Format_String(JSON+Len, ",\"climb_mps\":"); + Len+=Format_SignDec(JSON+Len, Climb, 2, 1, 1); + if(MsgLen>11) + { int16_t Turn=getTurnRate(Msg[11]); + Len+=Format_String(JSON+Len, ",\"turn_dps\":"); + 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, ",\"alt_std_m\":"); + Len+=Format_SignDec(JSON+Len, AltStd, 1, 0, 1); } + Len+=Format_String(JSON+Len, ",\"on_ground\":0"); } + if(Type==7) // for ground position + { uint8_t Status = Msg[6]; + Len+=Format_String(JSON+Len, ",\"no_track\":1"); + JSON[Len++]='0' + (Status&1); + Len+=Format_String(JSON+Len, ",\"on_ground\":1"); } + return Len; } + + int WriteAPRS(char *Out) + { bool Report=0; + int Len=0; + bool isPosition = Type()==1 || Type()==4 || Type()==7; + Len+=Format_String(Out+Len, "FNT"); + Len+=Format_Hex(Out+Len, Byte[1]); + Len+=Format_Hex(Out+Len, Byte[3]); + Len+=Format_Hex(Out+Len, Byte[2]); + Len+=Format_String(Out+Len, ">OGNFNT,qOR:"); + Out[Len++]=isPosition?'/':'>'; + Len+=Format_HHMMSS(Out+Len, SlotTime()); + Out[Len++]='h'; + const uint8_t *Msg = this->Msg(); + uint8_t MsgLen = this->MsgLen(); + switch(Type()) + { case 2: // Name: pilot or weather station + { Len+=Format_String(Out+Len, " Name=\""); + for(int Idx=0; Idx>3, 3); // [0.2km/h -> mph] + Out[Len++]='g'; + Len+=Format_UnsDec(Out+Len, (Gust+4)>>3, 3); // [0.2km/h -> mph] + } else Len+=Format_String(Out+Len, ".../...g..."); + Out[Len++]='t'; + if(Service&0x40) + { int16_t Fahr=Temp; Fahr+=4*Temp/5; Fahr+=32; + if(Fahr>=0) Len+=Format_UnsDec(Out+Len, Fahr, 3); + else Len+=Format_SignDec(Out+Len, Fahr, 2); + } else Len+=Format_String(Out+Len, "..."); + if(Service&0x10) + { Out[Len++]='h'; + Hum = (Hum*4+5)/10; if(Hum>=100) Hum=00; + Len+=Format_UnsDec(Out+Len, Hum, 2); } + if(Service&0x08) + { Out[Len++]='b'; + Len+=Format_UnsDec(Out+Len, Press, 5); } + Report=Byte[1]!=0x06; break; } // don't report Burnair weather reports + case 1: // airborne position + { const char *AcftIcon[8] = { "/z", "/g", "/g", "/O", "/'", "\\^", "/X", "/'" } ; // APRS icons for aircraft types + 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 + ID<<=24; ID |= getAddr(); // address + int32_t Lat = getLat(Msg); // [cordic] + int32_t Lon = getLon(Msg+3); // [cordic] + uint32_t Alt=getAltitude(Msg+6); // [m] + uint32_t Feet = ((int32_t)3360*Alt+512)>>10; // [feet] + uint32_t Speed=getSpeed(Msg[8]); // [0.5km/h] + uint32_t Knots=(Speed*553+1024)>>11; // knots + int32_t Climb=getClimb(Msg[9]); // [0.1m/s] + int32_t ClimbFeet = ((int32_t)1968*Climb+50)/100; // [fpm] + uint16_t Dir=getDir(Msg[10]); // [deg] + int16_t Turn=getQNE(Msg[11]); // [0.25deg/s] + int16_t QNE=getQNE(Msg[12]); // [m] + int32_t StdAlt=Alt+QNE; if(StdAlt<0) StdAlt=0; // [m] + uint32_t StdFeet = ((int32_t)3360*StdAlt+512)>>10; // [feet] + char hLat, hLon; + Len+=Format_Lat(Out+Len, Lat, hLat); + Out[Len++]=Icon[0]; + Len+=Format_Lon(Out+Len, Lon, hLon); + Out[Len++]=Icon[1]; + Len+=Format_UnsDec(Out+Len, Dir, 3); + Out[Len++]='/'; + Len+=Format_UnsDec(Out+Len, Knots, 3); + Len+=Format_String(Out+Len, "/A="); + Len+=Format_UnsDec(Out+Len, Feet, 6); + Len+=Format_String(Out+Len, " !W"); + Out[Len++]=hLat; + Out[Len++]=hLon; + Out[Len++]='!'; + Len+=Format_String(Out+Len, " id"); + Len+=Format_Hex(Out+Len, ID); + Out[Len++]=' '; + Len+=Format_SignDec(Out+Len, ClimbFeet); + Len+=Format_String(Out+Len, "fpm"); + if(MsgLen>11) + { Out[Len++]=' '; + Len+=Format_SignDec(Out+Len, Turn*5/6, 2, 1); + Len+=Format_String(Out+Len, "rot"); } + if(MsgLen>12) + { Len+=Format_String(Out+Len, " FL"); + Len+=Format_UnsDec(Out+Len, StdFeet, 5, 2); } + Len+=Format_String(Out+Len, " FNT1"); Out[Len++]='0'+(AcftType&7); + Report=1; break; } + case 7: // ground position + { // const char *StatusMsg[16] = { 0, "Walking", "Vehicle", "Bike", "Boot", 0, 0, 0, + // "Need-ride", "Landed-well", 0, 0, "Need-technical", + // "Need-medical", "Distress(man)", "Distress(auto)" } ; + uint8_t Status = Msg[6]; + bool Track = Status&1; + Status>>=4; + 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 + ID<<=24; ID |= getAddr(); // address + int32_t Lat = getLat(Msg); // [cordic] + int32_t Lon = getLon(Msg+3); // [cordic] + char hLat, hLon; + Len+=Format_Lat(Out+Len, Lat, hLat); + Out[Len++]=Icon[0]; + Len+=Format_Lon(Out+Len, Lon, hLon); + Out[Len++]=Icon[1]; + Len+=Format_String(Out+Len, " !W"); + Out[Len++]=hLat; + Out[Len++]=hLon; + Out[Len++]='!'; + Len+=Format_String(Out+Len, " id"); + Len+=Format_Hex(Out+Len, ID); + // if(StatMsg) + // { Out[Len++]=' '; Len+=Format_String(Out+Len, StatMsg); } + Len+=Format_String(Out+Len, " FNT7"); Out[Len++]=HexDigit(Status); + Report=1; break; } + } + if(SNR>0) + { Out[Len++]=' '; + Len+=Format_UnsDec(Out+Len, ((uint16_t)SNR*10+2)/4, 2, 1); + Out[Len++]='d'; Out[Len++]='B'; } + Out[Len++]=' '; + Len+=Format_SignDec(Out+Len, FreqOfs/10, 2, 1); + Len+=Format_String(Out+Len,"kHz"); + if(BitErr) + { Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, BitErr); Out[Len++]='e'; } + if(!Report) Len=0; // if not to be reported + Out[Len]=0; return Len; } + +} ; + +// ========================================================================================= + +class FANET_Name +{ public: + static const int MaxSize = 32; + uint32_t Time; + // uint8_t Type; + char Name[MaxSize]; + + public: + FANET_Name() { Time=0; Name[0]=0; } + + int Copy(const uint8_t *Src, int Size) + { if(Size>=MaxSize) Size=MaxSize-1; + memcpy(Name, Src, Size); Name[Size]=0; + return Size; } + +} ; + +class FANET_NameList +{ public: + std::map List; + + public: + int Update(FANET_RxPacket &Packet) + { if(Packet.Type()!=2) return 0; + uint32_t Addr = Packet.getAddr(); + FANET_Name &Name = List[Addr]; + Name.Time = Packet.SlotTime(); + Name.Copy(Packet.Msg(), Packet.MsgLen()); + return 1; } + +} ; + +// =============================================================================================== + +#endif // __FANET_H__ diff --git a/utils/format.cpp b/utils/format.cpp new file mode 100644 index 0000000..37140f0 --- /dev/null +++ b/utils/format.cpp @@ -0,0 +1,316 @@ +#include "format.h" + +// ------------------------------------------------------------------------------------------ + +char HexDigit(uint8_t Val) { return Val+(Val<10?'0':'A'-10); } + +// ------------------------------------------------------------------------------------------ + +void Format_Bytes( void (*Output)(char), const uint8_t *Bytes, uint8_t Len) +{ for( ; Len; Len--) + (*Output)(*Bytes++); +} + +void Format_String( void (*Output)(char), const char *String) +{ if(String==0) return; + for( ; ; ) + { uint8_t ch = (*String++); if(ch==0) break; +#ifdef WITH_AUTOCR + if(ch=='\n') (*Output)('\r'); +#endif + (*Output)(ch); } +} + +uint8_t Format_String(char *Out, const char *String) +{ if(String==0) return 0; + uint8_t OutLen=0; + for( ; ; ) + { char ch = (*String++); if(ch==0) break; +#ifdef WITH_AUTOCR + if(ch=='\n') Out[OutLen++]='\r'; +#endif + Out[OutLen++]=ch; } + // Out[OutLen]=0; + return OutLen; } + +void Format_String( void (*Output)(char), const char *String, uint8_t MinLen, uint8_t MaxLen) +{ if(String==0) return; + if(MaxLen>4)); (*Output)(HexDigit(Byte&0x0F)); } + +void Format_Hex( void (*Output)(char), uint16_t Word ) +{ Format_Hex(Output, (uint8_t)(Word>>8)); Format_Hex(Output, (uint8_t)Word); } + +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_MAC( void (*Output)(char), uint8_t *MAC, uint8_t Len) +{ for(uint8_t Idx=0; Idx=Base) + { Dig=Value/Base; Value-=Dig*Base; } + else + { Dig=0; } + if(Pos==DecPoint) (*Output)('.'); + if( (Pos<=MinDigits) || (Dig>0) || (Pos<=DecPoint) ) + { (*Output)('0'+Dig); MinDigits=Pos; } + } +} + +void Format_SignDec( void (*Output)(char), int16_t Value, uint8_t MinDigits, uint8_t DecPoint, uint8_t NoPlus) +{ if(Value<0) { (*Output)('-'); Value=(-Value); } + else if(!NoPlus) { (*Output)('+'); } + Format_UnsDec(Output, (uint16_t)Value, MinDigits, DecPoint); } + +void Format_UnsDec( void (*Output)(char), uint32_t Value, uint8_t MinDigits, uint8_t DecPoint) +{ uint32_t Base; uint8_t Pos; + for( Pos=10, Base=1000000000; Base; Base/=10, Pos--) + { uint8_t Dig; + if(Value>=Base) + { Dig=Value/Base; Value-=Dig*Base; } + else + { Dig=0; } + if(Pos==DecPoint) (*Output)('.'); + if( (Pos<=MinDigits) || (Dig>0) || (Pos<=DecPoint) ) + { (*Output)('0'+Dig); MinDigits=Pos; } + } +} + +void Format_SignDec( void (*Output)(char), int32_t Value, uint8_t MinDigits, uint8_t DecPoint, uint8_t NoPlus) +{ if(Value<0) { (*Output)('-'); Value=(-Value); } + else if(!NoPlus) { (*Output)('+'); } + Format_UnsDec(Output, (uint32_t)Value, MinDigits, DecPoint); } + +void Format_UnsDec( void (*Output)(char), uint64_t Value, uint8_t MinDigits, uint8_t DecPoint) +{ uint64_t Base; uint8_t Pos; + for( Pos=20, Base=10000000000000000000llu; Base; Base/=10, Pos--) + { uint8_t Dig; + if(Value>=Base) + { Dig=Value/Base; Value-=Dig*Base; } + else + { Dig=0; } + if(Pos==DecPoint) (*Output)('.'); + if( (Pos<=MinDigits) || (Dig>0) || (Pos<=DecPoint) ) + { (*Output)('0'+Dig); MinDigits=Pos; } + } +} + +void Format_SignDec( void (*Output)(char), int64_t Value, uint8_t MinDigits, uint8_t DecPoint, uint8_t NoPlus) +{ if(Value<0) { (*Output)('-'); Value=(-Value); } + else if(!NoPlus) { (*Output)('+'); } + Format_UnsDec(Output, (uint32_t)Value, MinDigits, DecPoint); } + +// ------------------------------------------------------------------------------------------ + +uint8_t Format_UnsDec(char *Out, uint32_t Value, uint8_t MinDigits, uint8_t DecPoint) +{ uint32_t Base; uint8_t Pos, Len=0; + for( Pos=10, Base=1000000000; Base; Base/=10, Pos--) + { uint8_t Dig; + if(Value>=Base) + { Dig=Value/Base; Value-=Dig*Base; } + else + { Dig=0; } + if(Pos==DecPoint) { (*Out++)='.'; Len++; } + if( (Pos<=MinDigits) || (Dig>0) || (Pos<=DecPoint) ) + { (*Out++)='0'+Dig; Len++; MinDigits=Pos; } + // (*Out)=0; + } + return Len; } + +uint8_t Format_SignDec(char *Out, int32_t Value, uint8_t MinDigits, uint8_t DecPoint, uint8_t NoPlus) +{ uint8_t Len=0; + if(Value<0) { (*Out++)='-'; Len++; Value=(-Value); } + else if(!NoPlus) { (*Out++)='+'; Len++; } + return Len+Format_UnsDec(Out, Value, MinDigits, DecPoint); } + +uint8_t Format_Hex( char *Output, uint8_t Byte ) +{ (*Output++) = HexDigit(Byte>>4); (*Output++)=HexDigit(Byte&0x0F); return 2; } + +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; } + +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; } + +uint8_t Format_Hex( char *Output, uint32_t Word, uint8_t Digits) +{ for(uint8_t Idx=Digits; Idx>0; ) + { Output[--Idx]=HexDigit(Word&0x0F); + Word>>=4; } + return Digits; } + +// ------------------------------------------------------------------------------------------ + +uint8_t Format_Latitude(char *Out, int32_t Lat) +{ uint8_t Len=0; + char Sign='N'; + if(Lat<0) { Sign='S'; Lat=(-Lat); } + uint32_t Deg=Lat/600000; + Lat -= 600000*Deg; + Len+=Format_UnsDec(Out+Len, Deg, 2, 0); + Len+=Format_UnsDec(Out+Len, Lat, 6, 4); + Out[Len++]=Sign; + return Len; } + +uint8_t Format_Longitude(char *Out, int32_t Lon) +{ uint8_t Len=0; + char Sign='E'; + if(Lon<0) { Sign='W'; Lon=(-Lon); } + uint32_t Deg=Lon/600000; + Lon -= 600000*Deg; + Len+=Format_UnsDec(Out+Len, Deg, 3, 0); + Len+=Format_UnsDec(Out+Len, Lon, 6, 4); + Out[Len++]=Sign; + return Len; } + +// ------------------------------------------------------------------------------------------ + +int8_t Read_Hex1(char Digit) +{ int8_t Val=Read_Dec1(Digit); if(Val>=0) return Val; + if( (Digit>='A') && (Digit<='F') ) return Digit-'A'+10; + if( (Digit>='a') && (Digit<='f') ) return Digit-'a'+10; + return -1; } + +int8_t Read_Dec1(char Digit) // convert single digit into an integer +{ if(Digit<'0') return -1; // return -1 if not a decimal digit + if(Digit>'9') return -1; + return Digit-'0'; } + +int8_t Read_Dec2(const char *Inp) // convert two digit decimal number into an integer +{ int8_t High=Read_Dec1(Inp[0]); if(High<0) return -1; + int8_t Low =Read_Dec1(Inp[1]); if(Low<0) return -1; + return Low+10*High; } + +int16_t Read_Dec3(const char *Inp) // convert three digit decimal number into an integer +{ int8_t High=Read_Dec1(Inp[0]); if(High<0) return -1; + int8_t Mid=Read_Dec1(Inp[1]); if(Mid<0) return -1; + 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 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; } + +// ------------------------------------------------------------------------------------------ + +int8_t Read_Coord(int32_t &Lat, const char *Inp) +{ uint16_t Deg; int8_t Min, Sec; + Lat=0; + const char *Start=Inp; + int8_t Len=Read_UnsDec(Deg, Inp); if(Len<0) return -1; + Inp+=Len; + Lat=(uint32_t)Deg*36000; + if(Inp[0]!=(char)0xC2) return -1; + if(Inp[1]!=(char)0xB0) return -1; + Inp+=2; + Min=Read_Dec2(Inp); if(Min<0) return -1; + Inp+=2; + Lat+=(uint32_t)Min*600; + if(Inp[0]!=(char)'\'') return -1; + Inp++; + Sec=Read_Dec2(Inp); if(Sec<0) return -1; + Inp+=2; + Lat+=(uint32_t)Sec*10; + if(Inp[0]=='.') + { Sec=Read_Dec1(Inp+1); if(Sec<0) return -1; + Inp+=2; Lat+=Sec; } + if(Inp[0]==(char)'\"') { Inp++; } + else if( (Inp[0]==(char)'\'') && (Inp[1]==(char)'\'') ) { Inp+=2; } + else return -1; + return Inp-Start; } + +int8_t Read_LatDDMMSS(int32_t &Lat, const char *Inp) +{ Lat=0; + const char *Start=Inp; + int8_t Sign=0; + if(Inp[0]=='N') { Sign= 1 ; Inp++; } + else if(Inp[0]=='S') { Sign=(-1); Inp++; } + int8_t Len=Read_Coord(Lat, Inp); if(Len<0) return -1; + Inp+=Len; + if(Sign==0) + { if(Inp[0]=='N') { Sign= 1 ; Inp++; } + else if(Inp[0]=='S') { Sign=(-1); Inp++; } + } + if(Sign==0) return -1; + if(Sign<0) Lat=(-Lat); + return Inp-Start; } + +int8_t Read_LonDDMMSS(int32_t &Lon, const char *Inp) +{ Lon=0; + const char *Start=Inp; + int8_t Sign=0; + if(Inp[0]=='E') { Sign= 1 ; Inp++; } + else if(Inp[0]=='W') { Sign=(-1); Inp++; } + int8_t Len=Read_Coord(Lon, Inp); if(Len<0) return -1; + Inp+=Len; + if(Sign==0) + { if(Inp[0]=='E') { Sign= 1 ; Inp++; } + else if(Inp[0]=='W') { Sign=(-1); Inp++; } + } + if(Sign==0) return -1; + if(Sign<0) Lon=(-Lon); + return Inp-Start; } + diff --git a/utils/format.h b/utils/format.h new file mode 100644 index 0000000..808c311 --- /dev/null +++ b/utils/format.h @@ -0,0 +1,124 @@ +#ifndef __FORMAT_H__ +#define __FORMAT_H__ + +#include + +#define WITH_AUTOCR + +char HexDigit(uint8_t Val); + + void Format_Bytes ( void (*Output)(char), const uint8_t *Bytes, uint8_t Len); +inline void Format_Bytes ( void (*Output)(char), const char *Bytes, uint8_t Len) { Format_Bytes(Output, (const uint8_t *)Bytes, Len); } + +void Format_String( void (*Output)(char), const char *String); +void Format_String( void (*Output)(char), const char *String, uint8_t MinLen, uint8_t MaxLen); + +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_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); + +void Format_UnsDec ( void (*Output)(char), uint32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0); +void Format_SignDec( void (*Output)(char), int32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0, uint8_t NoPlus=0); + +void Format_UnsDec ( void (*Output)(char), uint64_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0); +void Format_SignDec( void (*Output)(char), int64_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0, uint8_t NoPlus=0); + +uint8_t Format_String(char *Out, const char *String); +uint8_t Format_String(char *Out, const char *String, uint8_t MinLen, uint8_t MaxLen); + +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, uint64_t Word, uint8_t Digits); + +template + uint8_t Format_Hex( char *Output, Type Word, uint8_t Digits) +{ for(uint8_t Idx=Digits; Idx>0; ) + { Output[--Idx]=HexDigit(Word&0x0F); + Word>>=4; } + return Digits; } + +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_Latitude (char *Out, int32_t Lat); // [1/600000deg] => DDMM.MMMMs +uint8_t Format_Longitude(char *Out, int32_t Lon); // [1/600000deg] => DDDMM.MMMMs + +int8_t Read_Hex1(char Digit); + +int8_t Read_Dec1(char Digit); // convert single digit into an integer +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 + + template + int8_t Read_Hex(Type &Int, const char *Inp, uint8_t MaxDig=0) // convert variable number of digits hexadecimal number into an integer + { if(Inp==0) return 0; + if(MaxDig==0) MaxDig=2*sizeof(Type); + Int=0; int8_t Len=0; + for( ; MaxDig; MaxDig--) + { int8_t Dig=Read_Hex1(Inp[Len]); if(Dig<0) break; + Int = (Int<<4) + Dig; Len++; } + return Len; } // return number of characters read + +template + int8_t Read_UnsDec(Type &Int, const char *Inp) // convert variable number of digits unsigned decimal number into an integer + { Int=0; int8_t Len=0; + if(Inp==0) return 0; + for( ; ; ) + { int8_t Dig=Read_Dec1(Inp[Len]); if(Dig<0) break; + Int = 10*Int + Dig; Len++; } + return Len; } // return number of characters read + +template + int8_t Read_SignDec(Type &Int, const char *Inp) // convert signed decimal number into in16_t or int32_t + { Int=0; int8_t Len=0; + if(Inp==0) return 0; + char Sign=Inp[0]; + if((Sign=='+')||(Sign=='-')) Len++; + Len+=Read_UnsDec(Int, Inp+Len); if(Sign=='-') Int=(-Int); + return Len; } // return number of characters read + +template + int8_t Read_Int(Type &Value, const char *Inp) + { Value=0; int8_t Len=0; + if(Inp==0) return 0; + char Sign=Inp[0]; int8_t Dig; + if((Sign=='+')||(Sign=='-')) Len++; + if((Inp[Len]=='0')&&(Inp[Len+1]=='x')) + { Len+=2; Dig=Read_Hex(Value, Inp+Len); } + else + { Dig=Read_UnsDec(Value, Inp+Len); } + if(Dig<=0) return Dig; + Len+=Dig; + if(Sign=='-') Value=(-Value); return Len; } + +template + int8_t Read_Float1(Type &Value, const char *Inp) // read floating point, take just one digit after decimal point + { Value=0; int8_t Len=0; + if(Inp==0) return 0; + char Sign=Inp[0]; int8_t Dig; + if((Sign=='+')||(Sign=='-')) Len++; + Len+=Read_UnsDec(Value, Inp+Len); Value*=10; + if(Inp[Len]!='.') goto Ret; + Len++; + Dig=Read_Dec1(Inp[Len]); if(Dig<0) goto Ret; + Value+=Dig; Len++; + Dig=Read_Dec1(Inp[Len]); if(Dig>=5) Value++; + Ret: if(Sign=='-') Value=(-Value); return Len; } + +int8_t Read_LatDDMMSS(int32_t &Lat, const char *Inp); +int8_t Read_LonDDMMSS(int32_t &Lon, const char *Inp); + +#endif // __FORMAT_H__ diff --git a/utils/gdl90.cpp b/utils/gdl90.cpp new file mode 100644 index 0000000..1f547f8 --- /dev/null +++ b/utils/gdl90.cpp @@ -0,0 +1,119 @@ +#include "gdl90.h" + +static const uint16_t CRC16_CCITT_Table[256] = { + 0x0000U, 0x1021U, 0x2042U, 0x3063U, 0x4084U, 0x50A5U, 0x60C6U, 0x70E7U, + 0x8108U, 0x9129U, 0xA14AU, 0xB16BU, 0xC18CU, 0xD1ADU, 0xE1CEU, 0xF1EFU, + 0x1231U, 0x0210U, 0x3273U, 0x2252U, 0x52B5U, 0x4294U, 0x72F7U, 0x62D6U, + 0x9339U, 0x8318U, 0xB37BU, 0xA35AU, 0xD3BDU, 0xC39CU, 0xF3FFU, 0xE3DEU, + 0x2462U, 0x3443U, 0x0420U, 0x1401U, 0x64E6U, 0x74C7U, 0x44A4U, 0x5485U, + 0xA56AU, 0xB54BU, 0x8528U, 0x9509U, 0xE5EEU, 0xF5CFU, 0xC5ACU, 0xD58DU, + 0x3653U, 0x2672U, 0x1611U, 0x0630U, 0x76D7U, 0x66F6U, 0x5695U, 0x46B4U, + 0xB75BU, 0xA77AU, 0x9719U, 0x8738U, 0xF7DFU, 0xE7FEU, 0xD79DU, 0xC7BCU, + 0x48C4U, 0x58E5U, 0x6886U, 0x78A7U, 0x0840U, 0x1861U, 0x2802U, 0x3823U, + 0xC9CCU, 0xD9EDU, 0xE98EU, 0xF9AFU, 0x8948U, 0x9969U, 0xA90AU, 0xB92BU, + 0x5AF5U, 0x4AD4U, 0x7AB7U, 0x6A96U, 0x1A71U, 0x0A50U, 0x3A33U, 0x2A12U, + 0xDBFDU, 0xCBDCU, 0xFBBFU, 0xEB9EU, 0x9B79U, 0x8B58U, 0xBB3BU, 0xAB1AU, + 0x6CA6U, 0x7C87U, 0x4CE4U, 0x5CC5U, 0x2C22U, 0x3C03U, 0x0C60U, 0x1C41U, + 0xEDAEU, 0xFD8FU, 0xCDECU, 0xDDCDU, 0xAD2AU, 0xBD0BU, 0x8D68U, 0x9D49U, + 0x7E97U, 0x6EB6U, 0x5ED5U, 0x4EF4U, 0x3E13U, 0x2E32U, 0x1E51U, 0x0E70U, + 0xFF9FU, 0xEFBEU, 0xDFDDU, 0xCFFCU, 0xBF1BU, 0xAF3AU, 0x9F59U, 0x8F78U, + 0x9188U, 0x81A9U, 0xB1CAU, 0xA1EBU, 0xD10CU, 0xC12DU, 0xF14EU, 0xE16FU, + 0x1080U, 0x00A1U, 0x30C2U, 0x20E3U, 0x5004U, 0x4025U, 0x7046U, 0x6067U, + 0x83B9U, 0x9398U, 0xA3FBU, 0xB3DAU, 0xC33DU, 0xD31CU, 0xE37FU, 0xF35EU, + 0x02B1U, 0x1290U, 0x22F3U, 0x32D2U, 0x4235U, 0x5214U, 0x6277U, 0x7256U, + 0xB5EAU, 0xA5CBU, 0x95A8U, 0x8589U, 0xF56EU, 0xE54FU, 0xD52CU, 0xC50DU, + 0x34E2U, 0x24C3U, 0x14A0U, 0x0481U, 0x7466U, 0x6447U, 0x5424U, 0x4405U, + 0xA7DBU, 0xB7FAU, 0x8799U, 0x97B8U, 0xE75FU, 0xF77EU, 0xC71DU, 0xD73CU, + 0x26D3U, 0x36F2U, 0x0691U, 0x16B0U, 0x6657U, 0x7676U, 0x4615U, 0x5634U, + 0xD94CU, 0xC96DU, 0xF90EU, 0xE92FU, 0x99C8U, 0x89E9U, 0xB98AU, 0xA9ABU, + 0x5844U, 0x4865U, 0x7806U, 0x6827U, 0x18C0U, 0x08E1U, 0x3882U, 0x28A3U, + 0xCB7DU, 0xDB5CU, 0xEB3FU, 0xFB1EU, 0x8BF9U, 0x9BD8U, 0xABBBU, 0xBB9AU, + 0x4A75U, 0x5A54U, 0x6A37U, 0x7A16U, 0x0AF1U, 0x1AD0U, 0x2AB3U, 0x3A92U, + 0xFD2EU, 0xED0FU, 0xDD6CU, 0xCD4DU, 0xBDAAU, 0xAD8BU, 0x9DE8U, 0x8DC9U, + 0x7C26U, 0x6C07U, 0x5C64U, 0x4C45U, 0x3CA2U, 0x2C83U, 0x1CE0U, 0x0CC1U, + 0xEF1FU, 0xFF3EU, 0xCF5DU, 0xDF7CU, 0xAF9BU, 0xBFBAU, 0x8FD9U, 0x9FF8U, + 0x6E17U, 0x7E36U, 0x4E55U, 0x5E74U, 0x2E93U, 0x3EB2U, 0x0ED1U, 0x1EF0U +}; + +uint16_t GDL90_CRC16(uint8_t Byte, uint16_t CRC) +{ return CRC16_CCITT_Table[CRC>>8] ^ (CRC<<8) ^ Byte; } + +uint16_t GDL90_CRC16(const uint8_t *Data, uint8_t Len, uint16_t CRC) +{ for(int Idx=0; Idx>8) ^ Byte ) & 0xFF; +// X ^= X>>4; +// CRC = (CRC<<8) ^ (X<<12) ^ (X<<5) ^ X; +// return CRC; } + +/* +inline uint16_t CRC16_CCITT(uint8_t Byte, uint16_t CRC) +{ CRC = (CRC>>8) | (CRC<<8); + CRC ^= Byte; + CRC ^= (CRC&0xFF)>>4; + CRC ^= CRC<<12; + CRC ^= (CRC&0xFF)<<5; + return CRC; } +*/ +/* +inline uint16_t CRC16_CCITT(uint8_t Byte, uint16_t CRC) +{ // CRC = (CRC>>8) | (CRC<<8); + CRC ^= (uint16_t)Byte<<8; + CRC ^= (CRC&0xFF00)<<4; + CRC ^= CRC>>12; + CRC ^= (CRC&0xFF00)>>5; + // CRC = (CRC>>8) | (CRC<<8); + return CRC; } +*/ + +/* +uint16_t CRC16_CCITT(const uint8_t *Data, uint8_t Len, uint16_t CRC) +{ while (Len--) + { uint8_t X = (CRC>>8) ^ (*Data++); + CRC = (CRC<<8) ^ CRC16_CCITT_Table[X]; } + return CRC; } + +uint16_t CRC16_CCITT(const uint8_t *Data, uint8_t Len, uint16_t CRC) +{ while (Len--) + { uint16_t X = ( (CRC>>8) ^ (*Data++) ) & 0xFF; + X ^= X>>4; + CRC = (CRC<<8) ^ (X<<12) ^ (X<<5) ^ X; } + return CRC; } +*/ + +/* +uint16_t CRC16_CCITT(const uint8_t *Data, uint8_t Len, uint16_t CRC) +{ while (Len--) + { CRC = (CRC>>8) | (CRC<<8); + CRC ^= (*Data++); + CRC ^= (CRC&0xFF)>>4; + CRC ^= CRC<<12; + CRC ^= (CRC&0xFF)<<5; } + return CRC; } +*/ + +const uint8_t GDL90_Flag = 0x7E; +const uint8_t GDL90_Esc = 0x7D; + +static int GDL90_SendEsc(void (*Output)(char), uint8_t Byte) // shall we escape control characters as well ? +{ // if(Byte<0x20 || Byte==GDL90_Flag || Byte==GDL90_Esc) { (*Output)((char)GDL90_Esc); Byte^=0x20; (*Output)((char)Byte); return 2; } + if(Byte==GDL90_Flag || Byte==GDL90_Esc) { (*Output)((char)GDL90_Esc); Byte^=0x20; (*Output)((char)Byte); return 2; } // ESCape some characters + (*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++; + CRC=GDL90_CRC16(ID, CRC); + Count+=GDL90_SendEsc(Output, ID); + for( int Idx=0; Idx>8); + (*Output)((char)GDL90_Flag); Count++; + return Count; } + diff --git a/utils/gdl90.h b/utils/gdl90.h new file mode 100644 index 0000000..1829833 --- /dev/null +++ b/utils/gdl90.h @@ -0,0 +1,259 @@ +#ifndef __GDL90_H__ +#define __GDL90_H__ + +#include +#include + +#include "format.h" + +// ================================================================================= + +uint16_t GDL90_CRC16(uint8_t Byte, uint16_t CRC); // pass a single byte through the CRC +uint16_t GDL90_CRC16(const uint8_t *Data, uint8_t Len, uint16_t CRC=0); // pass a packet of bytes through the CRC + +int GDL90_Send(void (*Output)(char), uint8_t ID, const uint8_t *Data, int Len); // transmit GDL90 packet with proper framing and CRC + +// ================================================================================= + +// https://www.faa.gov/nextgen/programs/adsb/archival/media/gdl90_public_icd_reva.pdf +// SkyRadar msg ID 101: https://github.com/etdey/gdl90/blob/master/Format%20of%20the%20SkyRadar%20receiver%20message%20ID%20101.pdf +// other msg ID: https://www.foreflight.com/connect/spec/ + +class GDL90_HEARTBEAT // Heart-beat packet to be send at every UTC second +{ public: + static const int Size=6; + union + { uint8_t Status1; + struct + { bool Initialized: 1; + bool reserved : 1; + bool RATCS : 1; + bool LowBatt : 1; // battery is LOW + bool AddrType : 1; // are we transmitting ICAO (0) or self-assigned address (1) + bool IDENT : 1; + bool MaintReq : 1; + bool PosValid : 1; // GPS position is valid + } ; + } ; + union + { uint8_t Status2; + struct + { bool UTCvalid : 1; // UTC timing is valid + bool reserved1 : 1; + bool reserved2 : 1; + bool reserved3 : 1; + bool reserved4 : 1; + bool CSA_Req : 1; // CSA has been requested (Conflict Situation Awareness) + bool CSA_NotAvail : 1; // CSA is not available + bool TimeStampMSB : 1; // [0x10000sec] highest TimeStamp bit + } ; + } ; + uint16_t TimeStamp; // [sec] since 0000z cut to 16-bit + uint8_t MsgCount[2]; // [/sec] counts messages received during the previous second + + public: + void Clear(void) + { Status1=0; Status2=0; TimeStamp=0; MsgCount[0]=0; MsgCount[1]=0; } + + void setTimeStamp(uint32_t Time) + { Time%=86400; TimeStamp=Time; TimeStampMSB=Time>>16; } + + uint32_t getTimeStamp(void) const + { uint32_t Time=TimeStampMSB; Time = (Time<<16) | TimeStamp; return Time; } + + uint8_t getUplinkCount(void) const { return MsgCount[0]>>3; } // Uplink messages received + void setUplinkCount(uint8_t Count) { MsgCount[0] = (MsgCount[0]&0x07) | Count<<3; } + + uint16_t getDownlinkCount(void) const { uint16_t Count = MsgCount[0]&0x03; return (Count<<8) | MsgCount[1]; } // Basic and Long messages received + void setDownlinkCount(uint8_t Count) { MsgCount[0] = (MsgCount[0]&0xFC) | (Count>>8); MsgCount[1] = Count; } + + int Send(void (*Output)(char)) const { return GDL90_Send(Output, 0, (const uint8_t *)this, Size); } + +} __attribute__((packed)); + +// class GSL90_CONFIG // Initialization, ID=117 +// { public: +// uint8_t Data[19]; +// } ; + +class GDL90_GEOMALT // Geometrical altitude: ID = 11 (GPS ref. to Ellipsoid) +{ public: + static const int Size=4; + uint8_t Data[Size]; + + public: + void 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; } + void setWarning(bool Warn) // [bool] + { if(Warn) Data[2]|=0x80; + else Data[2]&=0x7F; } + void setFOM(uint16_t FOM) // [m] vertical Figure of Merit (accuracy ?) + { Data[2] = (Data[2]&0x80) | (FOM>>8); + Data[3] = FOM&0xFF; } + +} ; + +class GDL90_REPORT // Position report: Traffic: ID = 20, Ownship: ID = 10 +{ public: + static const int Size=27; + union + { uint8_t Data[Size]; // 27 bytes excluding the ID and framing/CRC +/* + struct + { uint8_t AddrType : 4; // + uint8_t Alert : 4; // 1=alert + uint32_t Address :24; // byte-reversed + uint32_t Latitude :24; // byte-reversed + uint32_t Longitude :24; // byte-reversed + uint16_t Altitude :12; // garbled + uint8_t Misc : 4; // garbled + uint8_t NACp : 4; + uint8_t NIC : 4; + uint16_t Velocity :12; // garbled + uint16_t Climb :12; // garbled + uint8_t Track : 8; + uint8_t AcftCat : 8; + char Call[8]; + uint8_t Spare : 4; + uint8_t Priority : 4; + } ; +*/ + } ; // __attribute__((packed)); + + public: + static uint32_t get3bytes(const uint8_t *Byte) { uint32_t Word=Byte[0]; Word=(Word<<8) | Byte[1]; Word=(Word<<8) | Byte[2]; return Word; } // 3-byte value + static void set3bytes(uint8_t *Byte, uint32_t Word) { Byte[0]=Word>>16; Byte[1]=Word>>8; Byte[2]=Word; } + + void Clear(void) { for(int Idx=0; Idx<27; Idx++) Data[Idx]=0; } // clear all data (Lat/Lon = invalid) + + uint8_t getAlertStatus(void) const { return Data[0]>>4; } // 0 = no alert, 1 = alert + void setAlertStatus(uint8_t Status) { Data[0] = (Data[0]&0x0F) | (Status<<4); } + + uint8_t getAddrType(void) const { return Data[0]&0x0F; } // 0=ICAO, 1=non-ICAO, 4=surface vehicle, 5=ground beacon + void setAddrType(uint8_t AddrType) { Data[0] = (Data[0]&0xF0) | (AddrType&0x0F); } + + uint32_t getAddress(void) const { return get3bytes(Data+1); } + void setAddress(uint32_t Addr) { set3bytes(Data+1, Addr); } + int32_t getLatitude(void) const { return get3bytes(Data+4)<<8; } // [cyclic] + void setLatitude(int32_t Lat) { set3bytes(Data+4, (Lat>>8)&0xFFFFFF); } + int32_t getLongitude(void) const { return get3bytes(Data+7)<<8; } // [cyclic] + void setLongitude(int32_t Lon) { set3bytes(Data+7, (Lon>>8)&0xFFFFFF); } + + static int32_t CordicOGN(int32_t Coord) { return ((int64_t)Coord*83399993+(1<<21))>>22; } // [1/60000deg] => [cordic] + + void setLatOGN(int32_t Lat) { setLatitude (CordicOGN(Lat)); } // [1/60000deg] + void setLonOGN(int32_t Lon) { setLongitude(CordicOGN(Lon)); } // [1/60000deg] + + int32_t getAltitude(void) const { int32_t Alt=Data[10]; Alt=(Alt<<4) | (Data[11]>>4); return Alt*25-1000; } // [feet] + void setAltitude(int32_t Alt) // [feet] + { Alt = (Alt+1000+12)/25; + if(Alt<0) Alt=0; else if(Alt>0xFFF) Alt=0xFFF; + Data[10] = Alt>>4; Alt&=0x00F; Data[11] = (Data[11]&0x0F) | (Alt<<4); } + + uint8_t getMiscInd(void) const { return Data[11]&0x0F; } // Airborne | Extrapolated | TT: 00=not valid, 01=true track, 10=magnetic, 11=heading + void setMiscInd(uint8_t MiscInd) { Data[11] = (Data[11]&0xF0) | (MiscInd&0x0F); } + + uint8_t getNIC(void) const { return Data[12]>>4; } // containment radius: 9=75m, 10=25m, 11=7.5m + uint8_t getNACp(void) const { return Data[12]&0x0F; } // est. pos. uncertainty: 9=30m, 10=10m, 11=3m + void setAccuracy(uint8_t NIC, uint8_t NACp) { Data[12] = (NIC<<4) | (NACp&0x0F); } + + uint16_t getSpeed(void) const { uint16_t Speed=Data[13]; Speed=(Speed<<4) | (Data[14]>>4); return Speed; } // [knot] + void setSpeed(uint16_t Speed) { if(Speed>0xFFE) Speed=0xFFE; Data[13] = Speed>>4; Data[14] = (Data[14]&0x0F) | (Speed<<4); } // [knot] + void clrSpeed(void) { Data[13] = 0xFF; Data[14] = (Data[14]&0x0F) | 0xF0; } // Speed = invalid + + int32_t getClimbRate(void) const + { int16_t Climb=Data[14]&0x0F; Climb=(Climb<<8)|Data[15]; Climb<<=4; return (int32_t)Climb*4; } // [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 + + 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 + void setAcftCat(uint8_t Cat) { Data[17]=Cat; } + void setAcftType(uint8_t AcftType) // set OGN-type aricrraft-type + { // 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, A, B, C, D, E, F + const uint8_t OGNtype[16] = { 0, 9, 1, 7, 11, 1, 12, 12, 1, 3,15,10,10,14,18,19 } ; // conversion table from OGN aricraft-type + setAcftCat(OGNtype[AcftType&0x0F]); } + + const char *getAcftCall(void) const { return (const char *)(Data+18); } // is not null-terminated + void setAcftCall(const char *Call) + { int Idx=0; + for( ; Idx<8; Idx++) + { char ch=Call[Idx]; if(ch==0) break; + Data[18+Idx]=ch; } + for( ; Idx<8; Idx++) + Data[18+Idx]=' '; + } + void setAcftCall(uint32_t ID) + { const char *AddrName[4] = { "RN", "IC", "FL", "OG" } ; + char *Call = (char *)(Data+18); + const char *Name = AddrName[(ID>>24)&3]; Call[0]=Name[0]; Call[1]=Name[1]; + Format_Hex(Call+2, (uint8_t)((ID>>16)&0xFF)); Format_Hex(Call+4, (uint16_t)(ID&0xFFFF)); } + + uint8_t getPriority(void) const { return Data[26]>>4; } // 1=general, 2=medical, 3=fuel, 4=comms, 5=interference, 6=downed + void setPriority(uint8_t Prior) { Data[26] = (Data[26]&0x0F) | (Prior<<4); } + + int Send(void (*Output)(char), uint8_t ID=10) const { return GDL90_Send(Output, ID, Data, Size); } + + 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(), + (90.0/0x40000000)*getLatitude(), (90.0/0x40000000)*getLongitude(), + (360.0/256)*getHeading(), getSpeed()); } + +} ; + +// ================================================================================= + +class GDL90_RxMsg // receiver for the MAV messages +{ public: + static const uint8_t MaxBytes = 32; // max. number of bytes + static const uint8_t SYNC = 0x7E; // GDL90 sync byte + static const uint8_t ESC = 0x7D; // GDL90 escape byte + + uint8_t Byte[MaxBytes]; + uint8_t Len; + + public: + void Clear(void) { Len=0; Byte[Len]=0; } + + void Print(void) + { printf("GDL90[%d] ", Len); + for(int Idx=0; Idx>8)!=Byte[Len-1] ) { Clear(); return 0; } + return 2; } + if(Byte[Len]==ESC) { RxByte^=0x20; } // if after an ESC then xor with 0x20 + Byte[Len]=RxByte; if(RxByte==ESC) return 1; + Len++; // advance + if(Len>=MaxBytes) { Clear(); return 0; } + Byte[Len]=0; return 1; } + +} ; + +// ================================================================================= + +#endif // __GDL90_H__ diff --git a/utils/intmath.cpp b/utils/intmath.cpp new file mode 100644 index 0000000..30b793c --- /dev/null +++ b/utils/intmath.cpp @@ -0,0 +1,157 @@ +#include "intmath.h" + +static const uint32_t SinePoints=256; +// static const uint32_t SineScale=0x80000000; +static const int32_t SineTable[SinePoints/4+1] = +{ 0x00000000, 0x03242ABF, 0x0647D97C, 0x096A9049, 0x0C8BD35E, 0x0FAB272B, 0x12C8106F, 0x15E21445, + 0x18F8B83C, 0x1C0B826A, 0x1F19F97B, 0x2223A4C5, 0x25280C5E, 0x2826B928, 0x2B1F34EB, 0x2E110A62, + 0x30FBC54D, 0x33DEF287, 0x36BA2014, 0x398CDD32, 0x3C56BA70, 0x3F1749B8, 0x41CE1E65, 0x447ACD50, + 0x471CECE7, 0x49B41533, 0x4C3FDFF4, 0x4EBFE8A5, 0x5133CC94, 0x539B2AF0, 0x55F5A4D2, 0x5842DD54, + 0x5A82799A, 0x5CB420E0, 0x5ED77C8A, 0x60EC3830, 0x62F201AC, 0x64E88926, 0x66CF8120, 0x68A69E81, + 0x6A6D98A4, 0x6C242960, 0x6DCA0D14, 0x6F5F02B2, 0x70E2CBC6, 0x72552C85, 0x73B5EBD1, 0x7504D345, + 0x7641AF3D, 0x776C4EDB, 0x78848414, 0x798A23B1, 0x7A7D055B, 0x7B5D039E, 0x7C29FBEE, 0x7CE3CEB2, + 0x7D8A5F40, 0x7E1D93EA, 0x7E9D55FC, 0x7F0991C4, 0x7F62368F, 0x7FA736B4, 0x7FD8878E, 0x7FF62182, + 0x7FFFFFFF } ; + +// get Sine from the SineTable +// Angle is 0..255 which corresponds to <0..2*PI) +int32_t IntSine(uint8_t Angle) +{ uint8_t Idx=Angle; + if(Angle&0x80) { Angle^=0x40; Idx=(-Idx); } + if(Angle&0x40) Idx=0x80-Idx; + int32_t Val=SineTable[Idx]; + if(Angle&0x80) Val=(-Val); + return Val; } + +// precise Sine with for 16-bit angles 2nd derivative interpolation +// max. result error is about 2.3e-7 +int32_t IntSine(uint16_t Angle) +{ uint8_t Int = Angle>>8; + int32_t Frac = Angle&0x00FF; + int32_t Value = IntSine(Int); Int+=1; + int32_t Delta = (IntSine(Int)-Value)>>8; + Value += Frac*Delta; + // printf(" [%02X %02X %+11.8f] ", Int-1, Frac, (double)Value/(uint32_t)0x80000000); + int32_t Frac2 = (Frac*(Frac-0x100)); + const int32_t Coeff = (int32_t)floor(2*M_PI*M_PI*0x80+0.5); + int32_t Deriv2 = (Coeff*(Value>>12)); + int32_t Corr = ((Deriv2>>16)*Frac2)>>11; + Value -= Corr; + // printf("[%04X %+11.8f %+11.8f] ", -Frac2, (double)Deriv2/(uint32_t)0x80000000, (double)Corr/(uint32_t)0x80000000); + return Value; } + +// precise Sine for 32-bit angles with 2nd derivative interpolation +// max. result error is about 2.3e-7 +int32_t IntSine(uint32_t Angle) +{ uint8_t Int = Angle>>24; + int32_t Frac = Angle&0x00FFFFFF; + int32_t Value = IntSine(Int); Int+=1; + int32_t Delta = (IntSine(Int)-Value); + Value += ((int64_t)Frac*(int64_t)Delta)>>24; + // printf(" [%02X %06X %+11.8f] ", Int-1, Frac, (double)Value/(uint32_t)0x80000000); + int64_t Frac2 = ((int64_t)Frac*(Frac-0x1000000))>>32; + const int64_t Coeff = (int64_t)floor(2*M_PI*M_PI*0x4000000+0.5); + int64_t Deriv2 = (Coeff*Value)>>26; + int64_t Corr = (Deriv2*Frac2)>>32; + Value -= Corr; + // printf(" [%04X %+11.8f %+11.8f] ", -Frac2, (double)Deriv2/(uint32_t)0x80000000, (double)Corr/(uint32_t)0x80000000); + return Value; } + +// Less precise sine for 16-bit angles +// source: http://www.coranac.com/2009/07/sines/ +/// A sine approximation via a fourth-order cosine approx. +/// @param x angle (with 2^16 units/circle) +/// @return Sine value (Q12) +int16_t Isin(int16_t Angle) // input: full angle = 16-bit range +{ + int32_t x=Angle; + int32_t c, y; + static const int qN= 14, qA= 12, B=19900, C=3516; + + c= x<<(30-qN); // Semi-circle info into carry. + x -= 1< cosine calc + + x= x<<(31-qN); // Mask with PI + x= x>>(31-qN); // Note: SIGNED shift! (to qN) + x= (x*x)>>(2*qN-14); // x=x^2 To Q14 + + y= B - ((x*C)>>14); // B - x^2*C + y= (1<>16); // A - x^2*(B-x^2*C) + + return c>=0 ? y : -y; } // result: -4096..+4096 (max. error = +/-12) + + +/* +int16_t IntAtan2(int16_t Y, int16_t X) +{ uint16_t Angle=0; // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle); + if(Y<0) { Angle+=0x8000; X=(-X); Y=(-Y); } // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle); + if(X<0) { Angle+=0x4000; int16_t tmp=Y; Y=(-X); X=tmp; } // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle); + if(X>1))/ X; + int16_t DD = ((int32_t)D*(int32_t)D)>>14; + int16_t DDD = ((int32_t)DD*(int32_t)D)>>14; + // printf(" %08X %08X %08X\n", D, DD, DDD); + Angle += ((5*D)>>3) - (DDD>>3); return Angle; } // good to about 1/2 degree +*/ + +int16_t IntAtan2(int16_t Y, int16_t X) +{ uint16_t Angle=0; // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle); + const int32_t CosPi8 = 30274; // cos(PI/8)*32768 + const int32_t SinPi8 = 12540; // sin(PI/8)*32768 + if(Y<0) { Angle+=0x8000; X=(-X); Y=(-Y); } // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle); + if(X<0) { Angle+=0x4000; int16_t tmp=Y; Y=(-X); X=tmp; } // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle); + if(X>1); + if(Y<0) + { if(X<(-Yc)) + { int32_t NewX = CosPi8*X - SinPi8*Y; + int32_t NewY = SinPi8*X + CosPi8*Y; + X=NewX>>15; if(NewX&0x4000) X+=1; + Y=NewY>>15; if(NewY&0x4000) Y+=1; + Angle-=0x1000; } + } else // Y>=0 + { if(X>15; if(NewX&0x4000) X+=1; + Y=NewY>>15; if(NewY&0x4000) Y+=1; + Angle+=0x1000; } + } // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle); + int16_t D = (((int32_t)Y<<14) + (X>>1))/ X; + // int16_t D = ((int32_t)Y<<14) / X; + int16_t DD = ((int32_t)D*(int32_t)D)>>14; + int16_t DDD = ((int32_t)DD*(int32_t)D)>>14; + // printf(" %08X %08X %08X\n", D, DD, DDD); + Angle += ((5*D)>>3) - (DDD>>3); + return Angle; } // good to about 1/6 degree + +/* +// integer square root +uint32_t IntSqrt(uint32_t Inp) +{ uint32_t Out = 0; + uint32_t Mask = 0x40000000; + + while(Mask>Inp) Mask>>=2; + while(Mask) + { if(Inp >= (Out+Mask)) + { Inp -= Out+Mask; Out += Mask<<1; } + Out>>=1; Mask>>=2; } + if(Inp>Out) Out++; + + return Out; } + +uint64_t IntSqrt(uint64_t Inp) +{ uint64_t Out = 0; + uint64_t Mask = 0x4000000000000000; + + while(Mask>Inp) Mask>>=2; + while(Mask) + { if(Inp >= (Out+Mask)) + { Inp -= Out+Mask; Out += Mask<<1; } + Out>>=1; Mask>>=2; } + if(Inp>Out) Out++; + + return Out; } +*/ diff --git a/utils/intmath.h b/utils/intmath.h new file mode 100644 index 0000000..7426e23 --- /dev/null +++ b/utils/intmath.h @@ -0,0 +1,68 @@ +#include +#include + +#ifndef __INTMATH_H__ +#define __INTMATH_H__ + +const uint32_t IntSine_Scale=0x80000000; + +// get Sine from the SineTable +// Angle is 0..255 which corresponds to <0..2*PI) +int32_t IntSine(uint8_t Angle); + +// precise Sine with for 16-bit angles 2nd derivative interpolation +// max. result error is about 2.3e-7 +int32_t IntSine(uint16_t Angle); + +// precise Sine for 32-bit angles with 2nd derivative interpolation +// max. result error is about 2.3e-7 +int32_t IntSine(uint32_t Angle); + +// less precise sine for 16-bit angles +const int16_t Isine_Scale=0x4000; +int16_t Isin(int16_t Angle); +int16_t inline Icos(int16_t Angle) { return Isin(Angle+0x4000); } + +// atan2(Y, X) +// max. result error is 1/6 degree +int16_t IntAtan2(int16_t Y, int16_t X); + +// integer square root +// uint32_t IntSqrt(uint32_t Inp); +// uint64_t IntSqrt(uint64_t Inp); + +template // integer square root for 16-bit or 32-bit + Type IntSqrt(Type Inp) // must be made with unsigned type or signed types with _positive_ values +{ Type Out = 0; + Type Mask = 1; Mask<<=(sizeof(Type)*8-2); + + while(Mask>Inp) Mask>>=2; + while(Mask) + { if(Inp >= (Out+Mask)) + { Inp -= Out+Mask; Out += Mask<<1; } + Out>>=1; Mask>>=2; } + if(Inp>Out) Out++; + + return Out; } + +// Distance = sqrt(dX*dX+dY*dY) + +inline uint32_t IntDistance(int32_t dX, int32_t dY) { return IntSqrt((uint64_t)((int64_t)dX*dX + (int64_t)dY*dY)); } +inline uint16_t IntDistance(int16_t dX, int16_t dY) { return IntSqrt((uint32_t)((int32_t)dX*dX + (int32_t)dY*dY)); } + +template + IntType IntFastDistance(IntType dX, IntType dY) // after: http://www.flipcode.com/archives/Fast_Approximate_Distance_Functions.shtml + { IntType min, max, approx; + + if(dX<0) dX = -dX; + if(dY<0) dY = -dY; + + if(dX>10; } + +#endif // of __INTMATH_H__ diff --git a/utils/ldpc.cpp b/utils/ldpc.cpp new file mode 100644 index 0000000..c445032 --- /dev/null +++ b/utils/ldpc.cpp @@ -0,0 +1,751 @@ +#include +#include + +#include "ldpc.h" + +#ifndef __AVR__ +#include +#endif + +#ifdef __AVR__ +#include +#endif + +// =================================================================================================================== + +// FindVectors65432bit(10,20,23, 500) => 208, Delta=2579 + +// every row represents a parity check to be performed on the received codeword +static const uint32_t LDPC_ParityCheck_n208k160[48][7] +#ifdef __AVR__ +PROGMEM +#endif += { // parity check vectors: 48 vectors for 48 parity checks + // Eaech vector applied to the data packet should yield even number of bits + { 0x00000805, 0x00000020, 0x04000000, 0x20000000, 0x00000040, 0x00044020, 0x00000000 }, + { 0x00000001, 0x00800800, 0x00000000, 0x00000000, 0x00000000, 0x10010000, 0x00008C98 }, + { 0x00004001, 0x01000080, 0x80000400, 0x00000000, 0x08000200, 0x00200000, 0x00000005 }, + { 0x00000101, 0x20000200, 0x00000022, 0x00000000, 0x00000000, 0xCC008000, 0x00005002 }, + { 0x00000401, 0x00000000, 0x00004900, 0x00000020, 0x00000000, 0x20C00349, 0x00000020 }, + { 0x03140001, 0x00000002, 0x00000000, 0x40000001, 0x41534100, 0x00102C00, 0x00002000 }, + { 0x04008800, 0x82000642, 0x00000000, 0x00000020, 0x88040020, 0x03000010, 0x00000400 }, + { 0x00000802, 0x20000000, 0x02000014, 0x01200000, 0x04000403, 0x00800004, 0x0000A004 }, + { 0x02020820, 0x00000000, 0x80020820, 0x10190040, 0x30000000, 0x00000002, 0x00000900 }, + { 0x40804950, 0x00090000, 0x00000000, 0x00021204, 0x40001000, 0x10001100, 0x00000000 }, + { 0x08000A00, 0x00020008, 0x00040000, 0x02400010, 0x01002000, 0x40280280, 0x00000010 }, + { 0x00000000, 0x00008010, 0x118000A0, 0x00040080, 0x01000084, 0x00040100, 0x00000444 }, + { 0x20040108, 0x18000000, 0x08608800, 0x0000000A, 0x08000010, 0x00040080, 0x00008000 }, + { 0x00004080, 0x00422201, 0x00010000, 0x0000A400, 0x00400800, 0x00840000, 0x00000800 }, + { 0x00000000, 0x60200000, 0x80100240, 0x08000021, 0x02800000, 0x100C0000, 0x00000000 }, + { 0x00001000, 0x01010002, 0x00082001, 0x04000000, 0x00000001, 0x00040002, 0x00004030 }, + { 0x00002300, 0x04000000, 0xA0080000, 0x20004000, 0x00028000, 0x00800000, 0x00000400 }, + { 0x00004000, 0x00104100, 0x40041028, 0x24000020, 0x00200000, 0x00100000, 0x00008000 }, + { 0x08011000, 0x20040000, 0x00000000, 0xA0800000, 0x08090000, 0x00000100, 0x00000A00 }, + { 0x10180000, 0x00000204, 0x00002800, 0x20400800, 0x00000000, 0x10000000, 0x00000004 }, + { 0x00000000, 0xC0000000, 0x10200000, 0x20028000, 0x20000000, 0x80000008, 0x00002011 }, + { 0x82004000, 0x20000000, 0x04202000, 0x00000000, 0x00000000, 0x00020200, 0x00000400 }, + { 0x08600000, 0x00001200, 0x94000000, 0x00000000, 0x40000008, 0x00000000, 0x00008020 }, + { 0x04040000, 0x04010000, 0x04100000, 0x00000100, 0x00200000, 0x40000008, 0x00000804 }, + { 0x00000200, 0x00000110, 0x04000100, 0x00000000, 0x28400400, 0x10000000, 0x00004000 }, + { 0x00080000, 0x00000080, 0x04001000, 0x01882007, 0x00008024, 0x04000001, 0x00000010 }, + { 0x20200000, 0x00000020, 0x00010040, 0x81000800, 0x10001000, 0x00300008, 0x00004400 }, + { 0x90000010, 0x89841021, 0x00000118, 0x08080000, 0x00020000, 0x40000000, 0x00000040 }, + { 0x04C20000, 0x10404034, 0x00000000, 0x00004000, 0x00810001, 0x04000200, 0x00000009 }, + { 0x40102000, 0x020020A0, 0x40100000, 0x00100080, 0x00080400, 0x80030080, 0x00000020 }, + { 0x00010000, 0x04020920, 0x00000200, 0x00060000, 0x00000218, 0x01002007, 0x00001000 }, + { 0x00020008, 0x00A08040, 0x00080000, 0x40001400, 0x04200040, 0x80200001, 0x00000200 }, + { 0x40000402, 0x01100000, 0x20808000, 0x00008000, 0x10100060, 0x00080000, 0x00001008 }, + { 0x200010A0, 0x00000000, 0x01040100, 0x00000104, 0x02040042, 0x08012000, 0x00000001 }, + { 0x01000000, 0x50000880, 0x00000092, 0x14400000, 0x00001840, 0x02400000, 0x00000000 }, + { 0x00000010, 0x02000000, 0x00014000, 0x00200018, 0x00000240, 0x04000800, 0x00000180 }, + { 0x00008000, 0x00880008, 0x08000044, 0x00100000, 0x00000004, 0x00400820, 0x00001001 }, + { 0x01000000, 0x00002000, 0x02004001, 0x00000042, 0x00000000, 0x09201020, 0x00000048 }, + { 0x00800000, 0x01000400, 0x00400002, 0xC0002000, 0x00002080, 0x00010064, 0x00000100 }, + { 0x00000400, 0x08400840, 0x00000400, 0x00000890, 0x00008102, 0x00000020, 0x00000002 }, + { 0x00200040, 0x00000081, 0x00000000, 0x02050000, 0x04940000, 0x20008020, 0x00000080 }, + { 0x00000404, 0x00800000, 0x00001000, 0x00014000, 0x00082200, 0x0A000400, 0x00000000 }, + { 0x0000A024, 0x00000000, 0x00000402, 0x08A01000, 0x00004010, 0x20000000, 0x00000008 }, + { 0x00480046, 0x00008000, 0x00000208, 0x00000048, 0x00000000, 0x00410010, 0x00000002 }, + { 0x0000008C, 0x00044C00, 0x00824004, 0x00000200, 0x00000000, 0x00028000, 0x00000000 }, + { 0x10010004, 0x00080000, 0x43008000, 0x10000400, 0x80000100, 0x00000040, 0x00000080 }, + { 0x80000000, 0x0020000C, 0x20420480, 0x00000100, 0x00000008, 0x00005410, 0x00000080 }, + { 0x00000000, 0x00101000, 0x08000001, 0x02000200, 0x82004A80, 0x00004000, 0x00000202 } +} ; + + +const uint8_t LDPC_ParityCheckIndex_n208k160[48][24] +#ifdef __AVR__ +PROGMEM +#endif += { // number of, indicies to bits to be taken for parity checks + { 10, 0, 2, 11, 37, 90, 125, 134, 165, 174, 178 }, + { 11, 0, 43, 55, 176, 188, 195, 196, 199, 202, 203, 207 }, + { 11, 0, 14, 39, 56, 74, 95, 137, 155, 181, 192, 194, }, + { 14, 0, 8, 41, 61, 65, 69, 175, 186, 187, 190, 191, 193, 204, 206 }, + { 15, 0, 10, 72, 75, 78, 101, 160, 163, 166, 168, 169, 182, 183, 189, 197 }, + { 21, 0, 18, 20, 24, 25, 33, 96, 126, 136, 142, 144, 145, 148, 150, 152, 158, 170, 171, 173, 180, 205 }, + { 18, 11, 15, 26, 33, 38, 41, 42, 57, 63, 101, 133, 146, 155, 159, 164, 184, 185, 202 }, + { 17, 1, 11, 61, 66, 68, 89, 117, 120, 128, 129, 138, 154, 162, 183, 194, 205, 207 }, + { 18, 5, 11, 17, 25, 69, 75, 81, 95, 102, 112, 115, 116, 124, 156, 157, 161, 200, 203 }, + { 18, 4, 6, 8, 11, 14, 23, 30, 48, 51, 98, 105, 108, 113, 140, 158, 168, 172, 188 }, + { 17, 9, 11, 27, 35, 49, 82, 100, 118, 121, 141, 152, 167, 169, 179, 181, 190, 196 }, + { 17, 36, 47, 69, 71, 87, 88, 92, 103, 114, 130, 135, 152, 168, 178, 194, 198, 202 }, + { 18, 3, 8, 18, 29, 59, 60, 75, 79, 85, 86, 91, 97, 99, 132, 155, 167, 178, 207 }, + { 16, 7, 14, 32, 41, 45, 49, 54, 80, 106, 109, 111, 139, 150, 178, 183, 203 }, + { 15, 53, 61, 62, 70, 73, 84, 95, 96, 101, 123, 151, 153, 178, 179, 188 }, + { 14, 12, 33, 48, 56, 64, 77, 83, 122, 128, 161, 178, 196, 197, 206 }, + { 13, 8, 9, 13, 58, 83, 93, 95, 110, 125, 143, 145, 183, 202 }, + { 15, 14, 40, 46, 52, 67, 69, 76, 82, 94, 101, 122, 125, 149, 180, 207 }, + { 14, 12, 16, 27, 50, 61, 119, 125, 127, 144, 147, 155, 168, 201, 203 }, + { 12, 19, 20, 28, 34, 41, 75, 77, 107, 118, 125, 188, 194 }, + { 13, 62, 63, 85, 92, 111, 113, 125, 157, 163, 191, 192, 196, 205 }, + { 10, 14, 25, 31, 61, 77, 85, 90, 169, 177, 202 }, + { 12, 21, 22, 27, 41, 44, 90, 92, 95, 131, 158, 197, 207 }, + { 12, 18, 26, 48, 58, 84, 90, 104, 149, 163, 190, 194, 203 }, + { 11, 9, 36, 40, 72, 90, 138, 150, 155, 157, 188, 206 }, + { 17, 19, 39, 76, 90, 96, 97, 98, 109, 115, 119, 120, 130, 133, 143, 160, 186, 196 }, + { 15, 21, 29, 37, 70, 80, 107, 120, 127, 140, 156, 163, 180, 181, 202, 206 }, + { 19, 4, 28, 31, 32, 37, 44, 50, 55, 56, 59, 63, 67, 68, 72, 115, 123, 145, 190, 198 }, + { 18, 17, 22, 23, 26, 34, 36, 37, 46, 54, 60, 110, 128, 144, 151, 169, 186, 192, 195 }, + { 18, 13, 20, 30, 37, 39, 45, 57, 84, 94, 103, 116, 138, 147, 167, 176, 177, 191, 197 }, + { 18, 16, 37, 40, 43, 49, 58, 73, 113, 114, 131, 132, 137, 160, 161, 162, 173, 184, 204 }, + { 17, 3, 17, 38, 47, 53, 55, 83, 106, 108, 126, 134, 149, 154, 160, 181, 191, 201 }, + { 16, 1, 10, 30, 52, 56, 79, 87, 93, 111, 133, 134, 148, 156, 179, 195, 204 }, + { 17, 5, 7, 12, 29, 72, 82, 88, 98, 104, 129, 134, 146, 153, 173, 176, 187, 192 }, + { 16, 24, 39, 43, 60, 62, 65, 68, 71, 118, 122, 124, 134, 139, 140, 182, 185 }, + { 13, 4, 57, 78, 80, 99, 100, 117, 134, 137, 171, 186, 199, 200 }, + { 14, 15, 35, 51, 55, 66, 70, 91, 116, 130, 165, 171, 182, 192, 204 }, + { 14, 24, 45, 64, 78, 89, 97, 102, 165, 172, 181, 184, 187, 195, 198 }, + { 15, 23, 42, 56, 65, 86, 109, 126, 127, 135, 141, 162, 165, 166, 176, 200 }, + { 14, 10, 38, 43, 54, 59, 74, 100, 103, 107, 129, 136, 143, 165, 193 }, + { 15, 6, 21, 32, 39, 112, 114, 121, 146, 148, 151, 154, 165, 175, 189, 199 }, + { 12, 2, 10, 55, 76, 110, 112, 137, 141, 147, 170, 185, 187 }, + { 14, 2, 5, 13, 15, 65, 74, 108, 117, 119, 123, 132, 142, 189, 195 }, + { 14, 1, 2, 6, 19, 22, 47, 67, 73, 99, 102, 164, 176, 182, 193 }, + { 14, 2, 3, 7, 42, 43, 46, 50, 66, 78, 81, 87, 105, 175, 177 }, + { 14, 2, 16, 28, 51, 79, 88, 89, 94, 106, 124, 136, 159, 166, 199 }, + { 16, 31, 34, 35, 53, 71, 74, 81, 86, 93, 104, 131, 164, 170, 172, 174, 199 }, + { 15, 44, 52, 64, 91, 105, 121, 135, 137, 139, 142, 153, 159, 174, 193, 201 } + } ; + +static const uint8_t LDPC_BitWeight_n208k160[208] // weight of parity checks for every codeword bit +#ifdef __AVR__ +PROGMEM +#endif += { 6, 3, 6, 3, 3, 3, 3, 3, 4, 3, 4, 6, 3, 3, 5, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, + 3, 3, 3, 3, 3, 6, 3, 5, 3, 5, 3, 5, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 5, 5, 3, 3, 3, 3, 5, 3, 3, + 3, 4, 3, 3, 3, 4, 3, 3, 4, 3, 4, 4, 3, 3, 4, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 6, 3, 3, 3, 3, 5, + 3, 3, 3, 3, 3, 4, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 6, 3, 3, + 3, 3, 3, 3, 3, 3, 6, 3, 3, 5, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 5, 3, 3, 3, 3, + 4, 3, 3, 4, 3, 6, 3, 3, 4, 4, 3, 3, 3, 3, 3, 3, 5, 3, 6, 3, 3, 5, 4, 4, 3, 3, 4, 4, 5, 3, 4, 4, + 5, 4, 5, 5, 5, 4, 3, 5, 3, 3, 6, 5, 4, 3, 4, 5 } ; + +// every row represents the generator for a parity bit +static const uint32_t LDPC_ParityGen_n208k160[48][5] +#ifdef __AVR__ +PROGMEM +#endif += { // Parity bits generator: 48 vectors to generate 48 parity bits + // Each vector applied to the user data yields a corresponding parity bit + { 0x40A90281, 0x9159D249, 0xCE9D516B, 0x2FDEED0B, 0xD9267CD4 }, + { 0xCCBC0FC3, 0xCC4FA4BC, 0x811EC3D0, 0xB07EC1B3, 0xA3B8E8D8 }, + { 0x66418D56, 0x3B85ADFF, 0xD2A6532E, 0x48CF52E4, 0x6A16586D }, + { 0x44C71240, 0x2C94631F, 0x15F15A4A, 0x7459D901, 0x037863CC }, + { 0x7386D718, 0x7F6C9623, 0x738E2E0C, 0xD2351593, 0xEF358669 }, + { 0x7BF87232, 0x9E4CCD68, 0xBB82590E, 0x9C9292EA, 0x4CE2AEB9 }, + { 0xAA8436BC, 0x94A61C4D, 0x1DA89B11, 0x72EAF204, 0x34D3A041 }, + { 0xBCE77760, 0x229935B2, 0xAAF85CE3, 0xFFE7B602, 0xF26BCC64 }, + { 0xD0C371D0, 0xA553D12F, 0xA0685BF2, 0x5C553C81, 0x0218EB48 }, + { 0x8D29034D, 0xEB20A394, 0x1A8C82A3, 0x41B4DA0C, 0x8632F81E }, + { 0x15A50876, 0x9BC10F59, 0xF979D1E0, 0xCFF6BD88, 0x88FE5895 }, + { 0x037A9ED5, 0xFA5DB837, 0x61395ACA, 0xE65B5839, 0x9A2D9D02 }, + { 0xEE70D18F, 0x8AE909C6, 0x8AF5BECA, 0x66968559, 0x1BD9B5E7 }, + { 0xC56397AC, 0xD8FF6A30, 0x8E165AF3, 0xC01686B9, 0xEC26BEDC }, + { 0xB2D63859, 0xFACA8CFD, 0x7EE85EB7, 0x19BFDA46, 0xC8C1CA52 }, + { 0xD7EB4D94, 0x426104DA, 0x124FBD54, 0xBF610A1D, 0x0E615094 }, + { 0x68EFE180, 0x15A1549C, 0x18D20289, 0xBD28AD44, 0x8DADDAEC }, + { 0xD7EB4D18, 0x426548DA, 0x12CDFD50, 0xBF61081D, 0x0E615094 }, + { 0xC92E426E, 0x648641B5, 0xC16A07B9, 0xA52D48AC, 0x842364AB }, + { 0xB71DAB61, 0x2B15995C, 0x6BE7E0C1, 0x97ECE351, 0xDF622A04 }, + { 0x55CD7406, 0x5E0F3507, 0x23F6C372, 0x7ECAFE84, 0x7E68A8DF }, + { 0x97DB831C, 0xD46D648F, 0x14FA22B3, 0x4F875648, 0x94C23936 }, + { 0x60D940EC, 0xFCC18797, 0xD0DE7383, 0xF38F22E5, 0x2E7A733E }, + { 0xD8C22D55, 0x8D45EB4E, 0xAC695FF3, 0xDED59211, 0x8851288A }, + { 0xCE9D11A1, 0xD8E8F438, 0xAF3102EE, 0xCB2FE547, 0xC11845BD }, + { 0x61D940EC, 0xACC18F17, 0xD0DE7311, 0xE7CF22E5, 0x2E7A6B7E }, + { 0x66AD8025, 0x493D883C, 0x538E9261, 0x5F0E116B, 0xB17492FA }, + { 0x747C4C9E, 0x3780804E, 0x29A7B2F1, 0x2838DF6D, 0xA68C11EB }, + { 0x7E33E90F, 0x2FB3D8E9, 0x2A9DE538, 0x3AC1ABDC, 0x59C14EAF }, + { 0x16B6095E, 0x4883D57E, 0xF765FF4B, 0x431C6EF3, 0xF2C45F6C }, + { 0x3F04D4F4, 0xEEA73108, 0x567ECF38, 0x15200560, 0x56AB6942 }, + { 0x1E5ECFFE, 0x29426F53, 0x17057060, 0xA774ED7F, 0x4FE7EACB }, + { 0xF9F02A12, 0xFADEBEE2, 0xBE67EB8B, 0x5506F594, 0xC5037599 }, + { 0x7BF87632, 0x960CC528, 0xBB825D0E, 0x9C929A7A, 0x4CE22FBB }, + { 0x6E2BE90F, 0x2FB3DAED, 0x2A9DCD38, 0x1A81A3DC, 0x59C14EAF }, + { 0x16B6A97A, 0x4883D57E, 0xF765FB49, 0x4BBC7EF3, 0xF2C41F7C }, + { 0x260C82A4, 0xD8645AF5, 0x9913D30A, 0x7158DC67, 0x68526E0A }, + { 0x5DAD3406, 0x5E1F6607, 0xF7F2D35A, 0x5ACAFEA4, 0x3E48A8D7 }, + { 0xAF04D4E4, 0x67232129, 0x567ECE20, 0x1D280560, 0x56A96942 }, + { 0xBA8536B8, 0x94AE1C4D, 0x5EA81B11, 0x62EAF604, 0xB4D3A141 }, + { 0xDF522858, 0x25CE2C46, 0x6C1E93BA, 0xDB9FBF4E, 0x9F8AACF9 }, + { 0xC92E4E6B, 0x6CD659D5, 0xCD6A03B8, 0x872D423C, 0x0623AF69 }, + { 0xD8C20E55, 0x8945EB4E, 0x0C615FF3, 0xFED5D211, 0x8853A88A }, + { 0x11EC2FBB, 0xE98188FA, 0x6D02584A, 0x7BF87EBD, 0x0C324421 }, + { 0xE1AB0619, 0x62864C22, 0xBC029B88, 0xDC501DA2, 0x3DB63518 }, + { 0x85657508, 0xE76CE85B, 0x35A012AB, 0xD7719D8D, 0xC1CE9294 }, + { 0x7E33EB0F, 0x2FB3D9F9, 0x2E9DE438, 0x3AC1ABDC, 0x71814AAF }, + { 0x55CD3406, 0x5E1F7407, 0x63F2D35A, 0x5ACAFEA4, 0x7E48A8DF } +} ; + + +// =================================================================================================================== + +#ifdef WITH_PPM + +const uint32_t LDPC_ParityCheck_n354k160[194][12] // 354 codeword = 160 user bits + 194 parity checks +#ifdef __AVR__ +PROGMEM +#endif += { // parity check vectors + { 0x00000000, 0x00000000, 0x00000000, 0x28000000, 0x00000000, 0x00000000, 0x00000400, 0x00E00006, 0x00000000, 0x00000080, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x80000000, 0x000C0040, 0x00010000, 0x00180000, 0x00000000, 0x40000040, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x20003000, 0x58000000, 0x00000000, 0x00000000, 0x00010000, 0x00000000, 0x30000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00200000, 0x18000000, 0x00000400, 0x00000000, 0x00000C00, 0x00008000, 0x00000000, 0x20000020, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00003000, 0x00002030, 0x10000800, 0x00000008, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x20000004, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000C00, 0x00000000, 0x0C000000, 0x00000000, 0x40000001, 0x00000008, 0x00004000, 0x00000000, 0x10000000, 0x00000001 }, + { 0x00040000, 0x00800001, 0x00000380, 0x00100000, 0x04000000, 0x00000000, 0x00000000, 0x00000000, 0x00004000, 0x00000000, 0x00000010, 0x00000000 }, + { 0x80048000, 0x00002200, 0x00000000, 0x00080200, 0x00000000, 0x00000000, 0x00010001, 0x00000800, 0x00000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000020, 0x00000000, 0x00000000, 0x01000000, 0x00000400, 0x00000000, 0x80000000, 0x00000010, 0x00000000, 0x00000000, 0x0D000100, 0x00000002 }, + { 0x00000000, 0x00000400, 0x00000000, 0x00000440, 0x00002000, 0x20000000, 0x20000000, 0x00000000, 0x00000000, 0x00000000, 0x02000082, 0x00000000 }, + { 0x80000000, 0x00000400, 0x00004200, 0x00000000, 0x00000000, 0x00000000, 0x00014000, 0x00000800, 0x00003000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x81000040, 0x00040000, 0x00002000, 0x8A000000, 0x00000000, 0x00000002 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00801000, 0x04060000, 0x00000800, 0x10000000, 0x01000200, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x20000100, 0x00000000, 0x0400050C, 0x00000000, 0x00000000, 0x00001000, 0x00000040, 0x00000000, 0x00000000 }, + { 0x40000000, 0x00000000, 0x00000000, 0x00000088, 0x03000000, 0x04000000, 0x00002000, 0x00000000, 0x00000008, 0x04000000, 0x00000000, 0x00000000 }, + { 0x40000000, 0x00008000, 0x00000070, 0x00000080, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000100, 0x00000000, 0x02200000, 0x00000000 }, + { 0x00004000, 0x00004000, 0x01000000, 0x00000000, 0x00002400, 0x00000000, 0x80008000, 0x00000000, 0x00000000, 0x02000000, 0x00100008, 0x00000000 }, + { 0x00002000, 0x00000000, 0x01000002, 0x00010040, 0x10000200, 0x00000400, 0x00000000, 0x00000000, 0x00000000, 0x01000000, 0x00000000, 0x00000001 }, + { 0x40040000, 0x00000000, 0x00000006, 0x00000020, 0x02000000, 0x02000000, 0x00000800, 0x00000000, 0x20000000, 0x00800000, 0x00000000, 0x00000000 }, + { 0x00021800, 0x80000000, 0x00000000, 0x10000000, 0x00000000, 0x00000100, 0x00000000, 0x08000008, 0x00000000, 0x00000000, 0x00000014, 0x00000000 }, + { 0x00000601, 0x00000002, 0x00000000, 0x00000000, 0x00000000, 0x02800000, 0x00010000, 0x00000000, 0x00000008, 0x00400020, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000140, 0x08000000, 0x01800000, 0x00000000, 0x00000008, 0x00000000, 0x00000002, 0x20100000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000800, 0x0A004000, 0x00000000, 0x00000003, 0x02001004, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00800A00, 0x00000000, 0x00800240, 0x00008080, 0x00000000, 0x00000000, 0x00000000, 0x00200000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00010020, 0x00000400, 0x00010004, 0x00000000, 0x01008000, 0x00000008, 0x00000000, 0x10000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x04000000, 0x20000000, 0x08000000, 0x01400000, 0x00000000, 0x00000800, 0x00000000, 0x00004000, 0x00000082, 0x00000000 }, + { 0x00000000, 0x40000000, 0x00400100, 0x00800000, 0x08000000, 0x00008000, 0x00000000, 0x00000008, 0x00000000, 0x00000002, 0x00000002, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00400100, 0x05000000, 0x00000000, 0x00000000, 0x00000440, 0x00010000, 0x00000000, 0x00080000, 0x00000001 }, + { 0x00000000, 0x10000000, 0x00200080, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x80010000, 0x02002000, 0x000C0000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00100000, 0x04000000, 0x00000200, 0x00000004, 0x00040000, 0x01000000, 0x40000004, 0x00000000, 0x00000004, 0x00000000 }, + { 0x00000000, 0x00000000, 0x41100000, 0x00000000, 0x00000220, 0x00000000, 0x40000080, 0x00000000, 0x80000000, 0x00000000, 0x00000000, 0x00000002 }, + { 0x00000000, 0x00000000, 0x00142000, 0x00008000, 0x00000000, 0x00000000, 0x00000080, 0x00004000, 0x00000000, 0x01000000, 0x01001000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x80000000, 0x00080000, 0x00000000, 0x00100400, 0x00000080, 0x08800000, 0x00000000, 0x00000000, 0x80000008, 0x00000000 }, + { 0x00000000, 0x20000000, 0x00002000, 0x02800000, 0x00000000, 0x00000040, 0x00000001, 0x00000000, 0x40000000, 0x00000002, 0x00000080, 0x00000000 }, + { 0x00000000, 0x08000000, 0x00000400, 0x00004000, 0x00000000, 0x00000000, 0x00480000, 0x00000010, 0x00000005, 0x01000000, 0x00000000, 0x00000000 }, + { 0x40000000, 0x00000404, 0x80000000, 0x00000000, 0x00000000, 0x40000010, 0x00000000, 0x80000000, 0x00000000, 0x00400000, 0x10000000, 0x00000000 }, + { 0x40000000, 0x00000003, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00200301, 0x20000100, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00008000, 0x00008010, 0x00000100, 0x40401000, 0x00080080, 0x00000000 }, + { 0x30000000, 0x00000000, 0x00000000, 0x00004000, 0x00008000, 0x08000420, 0x00000000, 0x08000000, 0x00000000, 0x00002000, 0x00000000, 0x00000000 }, + { 0x24000000, 0x00001000, 0x04000000, 0x01010020, 0x00000000, 0x00000000, 0x00000000, 0x01000000, 0x00200000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x0C0C0000, 0x00000000, 0x00000000, 0x21000000, 0x00000500, 0x00000000, 0x00000000, 0x00200000, 0x00008000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x21000000, 0x00000000, 0x00000000, 0x01000000, 0x00080000, 0x20000000, 0x00020000, 0x60000010, 0x00000000 }, + { 0x00000000, 0x00000000, 0x04000200, 0x00000002, 0x00000100, 0x40008000, 0x00000040, 0x00000400, 0x10000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00010000, 0x0C000400, 0x00000040, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x400000C1, 0x00001000, 0x00000000, 0x00000000 }, + { 0x21010000, 0x00000000, 0x00000000, 0x10000800, 0x00000100, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x40000800, 0x00080000, 0x00000000 }, + { 0x00004800, 0x00000000, 0x40080000, 0x00000008, 0x00000000, 0x00000050, 0x00000000, 0x00000000, 0x00000000, 0x00000008, 0x00040000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x01080000, 0x00000098, 0x00002000, 0x00000000, 0x00000000, 0x00000000, 0x00000800, 0x80001000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00002000, 0x00002000, 0x00008000, 0x00000000, 0x00082000, 0x04080000, 0x00000000, 0x00000000, 0x00000A00, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00086000, 0x00000008, 0x00000000, 0x00000002, 0x00042040, 0x00000200, 0x00000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000040, 0x00000000, 0x00000000, 0x00000002, 0x00000010, 0x00080000, 0x10000001, 0x00000800, 0x80020000, 0x00000000 }, + { 0x00000000, 0x00000400, 0x00000000, 0x02000002, 0x00000000, 0x00000000, 0x00020400, 0x00000000, 0x10200000, 0x00000800, 0x00000002, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00002000, 0x00000000, 0x00081201, 0x00000000, 0x00000000, 0x00300000, 0x00020400, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00080000, 0x00000004, 0x00000000, 0x00000000, 0x04000000, 0x10000004, 0x00400080, 0x00010000, 0x00010000, 0x00000000 }, + { 0x00000400, 0x00400000, 0x00001800, 0x00002000, 0x00000002, 0x02000000, 0x00000000, 0x10000000, 0x18000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00080000, 0x0080A000, 0x00000080, 0x00000000, 0x00100000, 0x00000000, 0x00010000, 0x00002100, 0x00000000 }, + { 0x00000000, 0x00000900, 0x00000000, 0x00000000, 0x00001000, 0x40080001, 0x00000000, 0x00000000, 0x00000000, 0x02000500, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00080B00, 0x20000208, 0x00000000, 0x00000000, 0x00040000, 0x00000000, 0x00004000, 0x00000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x10000002, 0x00000000, 0x00000000, 0x00000200, 0x00800040, 0x40000000, 0x20000008, 0x00000040, 0x00000000 }, + { 0x00000000, 0x80000000, 0x10000000, 0x00000420, 0x00000080, 0x00000020, 0x00000080, 0x00000000, 0x04000000, 0x00000200, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00008400, 0x00000010, 0x00028000, 0x00000000, 0x00000000, 0x00000000, 0x80010000, 0x00040100, 0x00000000 }, + { 0x80000000, 0x00000020, 0x80000000, 0x10000010, 0x80000000, 0x00000000, 0x24000000, 0x00000000, 0x80000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00010000, 0x00400000, 0x00000000, 0x40000010, 0x00000001, 0x00000100, 0x00000008, 0x08000008, 0x00000000 }, + { 0x00000000, 0x00000084, 0x00000010, 0x00000100, 0x00000002, 0x00000000, 0x00000000, 0x00000000, 0x00200000, 0x30000000, 0x02000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x801D0000, 0x0020C000, 0x00000002 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x40000004, 0x00000000, 0x0082000A, 0x40020000, 0x00000000, 0x00000800, 0x00000000, 0x00000000 }, + { 0x30000000, 0x04020000, 0x00000000, 0x00000080, 0x00401000, 0x00000000, 0x00000000, 0x20000000, 0x00000000, 0x00000000, 0x01000000, 0x00000000 }, + { 0x00088000, 0x00000010, 0x00000000, 0x00000000, 0x10000800, 0x40000000, 0x00000000, 0x04000000, 0x00060000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00006000, 0x00000000, 0x00000000, 0x00004002, 0x00200000, 0x00000000, 0x00000200, 0x00000000, 0x00000080, 0x00000204, 0x00000000, 0x00000000 }, + { 0x10000000, 0x00000000, 0x00000000, 0x00A00000, 0x00000000, 0x00000080, 0x00040000, 0x00000200, 0x00400000, 0x02000000, 0x00040000, 0x00000000 }, + { 0x00000000, 0x00100000, 0x01000001, 0x00000000, 0x00000000, 0x00000000, 0x00008206, 0x00001000, 0x00000000, 0x00008000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x10000008, 0x00000000, 0x00008000, 0x00000000, 0x00000010, 0x00020000, 0x04000002, 0x04040000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00040000, 0x00000000, 0x00000001, 0x00000000, 0x00010001, 0x00000000, 0x00000040, 0x00000000, 0x00200100, 0x00010000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x20000000, 0x00000000, 0x20000000, 0x40002000, 0x00000004, 0x00100000, 0x00000028, 0x00000000 }, + { 0x00000000, 0x00000000, 0x20000000, 0x00000000, 0x02000000, 0x00000000, 0x44000001, 0x00000000, 0x00000004, 0x00408000, 0x00000000, 0x00000000 }, + { 0x11200000, 0x00010000, 0x40000000, 0x00000000, 0x00000010, 0x00000000, 0x00000004, 0x00000000, 0x00000000, 0x00000000, 0x08000000, 0x00000000 }, + { 0x00000000, 0x00000040, 0x00100000, 0x00400000, 0x00000000, 0x80000008, 0x00800000, 0x00002080, 0x00000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00400000, 0x80000010, 0x00000000, 0x00000000, 0x00000026, 0x00000000, 0x00000000, 0x00000000, 0x00002000, 0x00000400, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000042, 0x00000000, 0x00000080, 0x01000000, 0x01000000, 0x00400000, 0x00000000, 0x00000000, 0x00000400, 0x00000020, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000200, 0x00000000, 0x00000400, 0x00000000, 0x00002080, 0x00002000, 0x00008024, 0x00000000 }, + { 0x00000000, 0x02000000, 0x00800002, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000002, 0x00004012, 0x00008000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x02800000, 0x10000000, 0x00000000, 0x00000000, 0x00000000, 0x80002000, 0x00100000, 0x00200000, 0x00000000, 0x00008000, 0x00000000 }, + { 0x00080100, 0x00000000, 0x00080000, 0x00000010, 0x00000000, 0x08000000, 0x10000001, 0x00000400, 0x00000000, 0x00000000, 0x40000000, 0x00000000 }, + { 0x00010080, 0x00000000, 0x00000000, 0x00040000, 0x00000000, 0x00000000, 0x00000000, 0x11000000, 0x00040001, 0x80200000, 0x00000000, 0x00000000 }, + { 0x00002010, 0x00000000, 0x00000000, 0x00400014, 0x00004000, 0x00000000, 0x20000000, 0x20000000, 0x00000000, 0x00000000, 0x00000010, 0x00000000 }, + { 0x00000440, 0x04000020, 0x08010000, 0x00000000, 0x00000040, 0x08000000, 0x00400000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000002, 0x48000000, 0x00000000, 0x00200000, 0x04001000, 0x02000000, 0x00000000, 0x00000000, 0x00001000, 0x00000000, 0x00000000 }, + { 0x00200400, 0x00000000, 0x00800000, 0x02000000, 0x00000000, 0x00000020, 0x04000000, 0x00000000, 0x00000000, 0x00000004, 0x01000000, 0x00000000 }, + { 0x01200000, 0x01000000, 0x00000800, 0x00004004, 0x00000000, 0x10000000, 0x00000000, 0x00000000, 0x00000008, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000001, 0x00400001, 0x00000000, 0x00000201, 0x00000020, 0x20000000, 0x08000000, 0x00000000 }, + { 0x02000000, 0x00000000, 0x00000800, 0x00000001, 0x00000001, 0x00000000, 0x00000000, 0x01000000, 0x00000020, 0x20000080, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000500, 0x00000000, 0x00000000, 0x00040000, 0x00000000, 0x00000000, 0x11000210, 0x00000000, 0x40000000, 0x00000000 }, + { 0x00000020, 0x00000000, 0x00000000, 0x00000000, 0x02000000, 0x20001000, 0x00000000, 0x00000020, 0x41000000, 0x00000400, 0x00000400, 0x00000000 }, + { 0x00000000, 0x08000000, 0x00000001, 0x00000040, 0x00000000, 0x01000020, 0x02000000, 0x00008000, 0x80000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00100000, 0x80000100, 0x00000001, 0x00000000, 0x00104040, 0x00001000, 0x00000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000100, 0x00000008, 0x10000000, 0x00100000, 0x00000000, 0x00001000, 0x00008000, 0x00000000, 0x00000002, 0x00100001, 0x00000000, 0x00000000 }, + { 0xA0000100, 0x10004000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00008000, 0x00000000, 0x00400000, 0x00000000, 0x00002400, 0x00000000 }, + { 0x01800004, 0x00000010, 0x00000000, 0x00020000, 0x00200000, 0x00000040, 0x00000200, 0x00008000, 0x00000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00900000, 0x00000000, 0x00400000, 0x00000000, 0x00000020, 0x00000002, 0x00000000, 0x00100000, 0x00200000, 0x00000080, 0x02000000, 0x00000000 }, + { 0x00000000, 0x20000008, 0x00014000, 0x00000000, 0x00000000, 0x00080000, 0x00000000, 0x00000020, 0x00020000, 0x08000000, 0x00000000, 0x00000000 }, + { 0x00100000, 0x1000A000, 0x00000100, 0x80000000, 0x00000000, 0x00000000, 0x00020000, 0x80000000, 0x00000000, 0x00000000, 0x00100000, 0x00000000 }, + { 0x00020000, 0x00000000, 0x00000000, 0x00000000, 0x00100000, 0x00000000, 0x00200000, 0x00410040, 0x00000400, 0x00080000, 0x00000010, 0x00000000 }, + { 0x00000000, 0x08000000, 0x00010000, 0x00400000, 0x40000000, 0x00800800, 0x02000000, 0x00000000, 0x00001000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00120200, 0x00800010, 0x20000000, 0x00000000, 0x00000000, 0x00000300, 0x00000000 }, + { 0x00000000, 0x00020000, 0x00000000, 0x00000004, 0x00000080, 0x00000100, 0x00000000, 0x00000020, 0x00000020, 0x00000000, 0x80000040, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000008, 0x00002400, 0x00100800, 0x00000000, 0x00020000, 0x10000002, 0x00000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x10000000, 0x00000220, 0x80000000, 0x00000000, 0x00000000, 0x20000000, 0x00200000, 0x00000020, 0x00080000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x04000000, 0x80008000, 0x00004000, 0x00000000, 0x00000000, 0x00000000, 0x00200200, 0x00000004, 0x00000000, 0x00040000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x40100000, 0x80000800, 0x00000080, 0x00000000, 0x00000000, 0x00000000, 0x00120000, 0x00000008, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00080000, 0x00000018, 0x00100000, 0x00040000, 0x00004000, 0x00800000, 0x00000000, 0x00080000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00C00820, 0x00000000, 0x08000000, 0x00000000, 0x00000000, 0x00000408, 0x00000000, 0x00000000, 0x00000000, 0x00000100, 0x00000000 }, + { 0x00000000, 0x00000000, 0x20000020, 0x00000000, 0x00002000, 0x00000008, 0x00001000, 0x00004000, 0x00000000, 0x00004000, 0x00800000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00020001, 0x00000000, 0x00000040, 0x00000800, 0x00000000, 0x00001020, 0x00000000, 0x00400200, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00010000, 0x00001010, 0x00000040, 0x00000004, 0x00000000, 0x00000000, 0x00000011, 0x00010000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000400, 0x00000040, 0x00000000, 0x00000000, 0x00001000, 0x00000000, 0x02000000, 0x00002080, 0x00000000, 0x00010000, 0x04001000, 0x00000000 }, + { 0x00220040, 0x20000044, 0x00000001, 0x00000000, 0x00000000, 0x00000200, 0x00010000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00008204, 0x00001000, 0x00000000, 0x00000000, 0x80000000, 0x00000000, 0x80000400, 0x80000000, 0x02000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00401300, 0x00000000, 0x00040000, 0x00000000, 0x05000000, 0x00000000, 0x00080000, 0x00000000, 0x00000100, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000020, 0x00000000, 0x00000000, 0x08000000, 0x00080000, 0x00008000, 0x00000900, 0x00000240, 0x04000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x10000000, 0x1C000000, 0x00000200, 0x00000000, 0x20000000, 0x00000100, 0x04000000, 0x01000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00200201, 0x00000000, 0x00000800, 0x00000000, 0x00010800, 0x00001000, 0x00000000, 0x00000008, 0x00040000, 0x00000000, 0x00000000 }, + { 0x00080000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x08000000, 0x40010000, 0x00014040, 0x20000000, 0x00000400, 0x00000000 }, + { 0x00000000, 0x00840000, 0x00200000, 0x04001000, 0x00000000, 0x00000000, 0x00004000, 0x00000000, 0x00000802, 0x00000000, 0x40000000, 0x00000000 }, + { 0x00000800, 0x00014000, 0x00000000, 0x00000000, 0x00080100, 0x00000000, 0x00000000, 0x00000000, 0x00080000, 0x40200000, 0x00800000, 0x00000000 }, + { 0x00400000, 0x00000000, 0x00400000, 0x00000000, 0x00080000, 0x00000000, 0x00000000, 0x0001000A, 0x4A000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x08000000, 0x00200000, 0x00000000, 0x80000000, 0x00000100, 0x02002004, 0x20000000, 0x04000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00280000, 0x00000024, 0x00060000, 0x00000000, 0x00000000, 0x00000000, 0x00040001, 0x00000000, 0x04000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000080, 0x00000000, 0x00010000, 0x00000000, 0x00000000, 0x00480000, 0x00000040, 0x000080A0, 0x04000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x10000000, 0x01000002, 0x000A0000, 0x00000000, 0x00000100, 0x00400000, 0x00000004, 0x00020000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00010000, 0x00440000, 0x00004000, 0x00200000, 0x02000000, 0x80000000, 0x00800010, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x20404000, 0x08000100, 0x00100000, 0x80000000, 0x00000200, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00020000, 0x40000000, 0x40000040, 0x00002000, 0x00000000, 0x00300000, 0x80000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000001, 0x00000040, 0x00000000, 0x00000100, 0x00000000, 0x08000000, 0x00000000, 0x0C000002, 0x00000000, 0x00020000, 0x00000000 }, + { 0x00010000, 0x00000010, 0x00000000, 0x00040000, 0x00400008, 0x00020000, 0x00000000, 0x00000000, 0x00020000, 0x00800000, 0x00000000, 0x00000000 }, + { 0x02404000, 0x00000002, 0x04000000, 0x40000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x01000080, 0x00000000, 0x00000000 }, + { 0x00002000, 0x00000000, 0x00010000, 0x00000000, 0x00000000, 0x10000000, 0x00020020, 0x00000100, 0x00000000, 0x10000000, 0x00000020, 0x00000000 }, + { 0x00000880, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000100, 0x00080004, 0x00000000, 0x00080010, 0x00000000, 0x00800000, 0x00000000 }, + { 0x00008070, 0x00000000, 0x00000002, 0x00000000, 0x00000008, 0x00000002, 0x00000000, 0x00000000, 0x00420000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000200, 0x00000008, 0x80000000, 0x80010000, 0x80400000, 0x00000000, 0x00010000, 0x00000004, 0x00000000, 0x00000000, 0x00000000 }, + { 0x03000000, 0x00020004, 0x22000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000020, 0x10000000, 0x00100000, 0x00000000 }, + { 0x00141000, 0x00080000, 0x00000000, 0x00000000, 0x00000000, 0x80000000, 0x00000010, 0x00000000, 0x00020800, 0x00004000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00280000, 0x48000000, 0x80000080, 0x00000400, 0x00100100, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x24000800, 0x00001020, 0x00802000, 0x02000000, 0x00000000, 0x00002000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x40030010, 0x00000084, 0x00000002, 0x00000080, 0x00000000, 0x00000200, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00010000, 0x02120000, 0x00000000, 0x00004001, 0x00100000, 0x00000000, 0x00000001, 0x00000040, 0x00000000, 0x00000000 }, + { 0x00000000, 0x40800000, 0x002080A0, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x20000000, 0x00820000, 0x00000000, 0x00000000 }, + { 0x00000080, 0x00003000, 0x00000000, 0x00000000, 0x00000000, 0x00800000, 0x00000000, 0x00200000, 0x00000800, 0x04000040, 0x00010200, 0x00000000 }, + { 0x00000000, 0x00000100, 0x00000000, 0x00000000, 0x00000000, 0x00000010, 0x10000000, 0x42100010, 0x00100000, 0x00000020, 0x00400000, 0x00000000 }, + { 0x04080004, 0x00102008, 0x00000400, 0x08000000, 0x00000000, 0x00000000, 0x00000020, 0x00000000, 0x00000000, 0x40000000, 0x00000000, 0x00000000 }, + { 0x00000081, 0x00000008, 0x00000000, 0x00000002, 0x00000000, 0x00000020, 0x00000000, 0x00020400, 0x00140000, 0x00000001, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00088000, 0x00000000, 0x10040000, 0x00000000, 0x00000000, 0x02000010, 0x00000060, 0x10000000, 0x00000002 }, + { 0x08000000, 0x00000000, 0x00000000, 0x00000800, 0x00000000, 0x10010000, 0x00050000, 0x00000000, 0x09000800, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00002008, 0x00000000, 0x02008080, 0x00004000, 0x00000000, 0x04020040, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00400000, 0x00200000, 0x00020000, 0x00000000, 0x00100000, 0x40000000, 0x00000000, 0x01080101, 0x00000000, 0x00000000 }, + { 0x00000002, 0x00000000, 0x00068000, 0x08000000, 0x00000000, 0x00004042, 0x10000000, 0x00000000, 0x00000000, 0x00000000, 0x00000800, 0x00000000 }, + { 0x00000000, 0x01400000, 0x02004000, 0x00000000, 0x00000020, 0x00000010, 0x00000000, 0x00001000, 0x00000200, 0x00000000, 0x80000080, 0x00000000 }, + { 0x00000000, 0x62300000, 0x00000000, 0x00000200, 0x00000000, 0x00000000, 0x00000000, 0x00400000, 0x08800080, 0x00000000, 0x00000000, 0x00000000 }, + { 0x04000000, 0x000091A0, 0x02000000, 0x00200000, 0x00000000, 0x00000000, 0x00000000, 0x00000100, 0x00000000, 0x00200000, 0x00000000, 0x00000000 }, + { 0x00730000, 0x00000040, 0x00000000, 0x00000000, 0x20000000, 0x04000000, 0x00000000, 0x00000000, 0x00000400, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000020, 0x00102000, 0x00202000, 0x00000000, 0x02000000, 0x08040010, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000100, 0x00400000, 0x00000201, 0x00000000, 0x02000200, 0x00000000, 0x00040000, 0x00001000, 0x00000010, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00008000, 0x00000000, 0x00000000, 0x00000000, 0x10100000, 0x00800000, 0x00008000, 0x00081008, 0x00200000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x40828000, 0x00800000, 0x00100000, 0x00120000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00008000, 0x00000000 }, + { 0x80020000, 0x01100880, 0x00000000, 0x00000000, 0x00000000, 0x00100010, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000001, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000400, 0x08000000, 0x0000080A, 0x00000000, 0x00000200, 0x00020010, 0x04000000, 0x00000000 }, + { 0x00800000, 0x00000000, 0x00000000, 0x06000000, 0x00010001, 0x00000800, 0x00000080, 0x00000000, 0x00000000, 0x08000000, 0x00000040, 0x00000000 }, + { 0x00000000, 0x02200000, 0x00000000, 0x00008008, 0x00200000, 0x00000000, 0x00000000, 0x00004000, 0x00000000, 0x40000004, 0x00002000, 0x00000000 }, + { 0x00800000, 0x00000000, 0x88000000, 0x84000000, 0x00040000, 0x00000000, 0x00000000, 0x00000000, 0x00800000, 0x00000000, 0x00014000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x40000001, 0x00040000, 0x00001000, 0x01200000, 0x02400004, 0x00000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00004808, 0x04000000, 0x00200000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00080000, 0x00080040, 0x00000040, 0x00000000, 0x00000000 }, + { 0x00000310, 0x00002000, 0x00000000, 0x00800200, 0x00000000, 0x00010000, 0x00400000, 0x00000000, 0x01000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x0000100E, 0x00000018, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000002, 0x00000002, 0x00000000, 0x00000000, 0x00000002, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00820000, 0x00200100, 0x09000020, 0x00000800, 0x00000000, 0x00008000, 0x00000000, 0x00000000 }, + { 0x08000000, 0x00000000, 0x02200000, 0x00001400, 0x80004000, 0x00000000, 0x00000004, 0x00000000, 0x00001000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00008000, 0x00000000, 0x01000000, 0x00000000, 0x20000080, 0x00000040, 0x10000000, 0x10000000, 0x00000000, 0x00000020, 0x00400000, 0x00000000 }, + { 0x00000000, 0x40000080, 0x00020000, 0x00000000, 0x00001000, 0x00004000, 0x00000100, 0x20000000, 0x00000000, 0x00004000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00004800, 0x00000000, 0x04000800, 0x00000000, 0x00400000, 0x00000000, 0x00000000, 0x00000000, 0x00000010, 0x00400000, 0x00000001 }, + { 0x00000000, 0x00040004, 0x00041000, 0x00000000, 0x00000008, 0x00000000, 0x00000024, 0x00000000, 0x00000000, 0x00000000, 0x00004000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x02000000, 0x00000000, 0x00040000, 0x00000000, 0x00100008, 0x00000000, 0x20000020, 0x08000040, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00001004, 0x00100000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00008000, 0x00804108, 0x00000000, 0x00000000 }, + { 0x00000000, 0x01010000, 0x00800000, 0x00001001, 0x00800000, 0x00000000, 0x00000000, 0x00240000, 0x00000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000200, 0x00000000, 0x00000000, 0x00002200, 0x00000002, 0x08200800, 0x00000001 }, + { 0x00000000, 0x00000000, 0x00100000, 0x0000C000, 0x00804010, 0x00004000, 0x00000000, 0x00000000, 0x00000400, 0x00000000, 0x00000000, 0x00000000 }, + { 0x08000000, 0x00480000, 0x00008004, 0x00000000, 0x00040000, 0x00000000, 0x00000100, 0x00000000, 0x00000000, 0x10000000, 0x00000000, 0x00000000 }, + { 0x08800000, 0x00040001, 0x00000000, 0x00000000, 0x00080080, 0x00000000, 0x04000000, 0x00000000, 0x04000000, 0x00000000, 0x00000000, 0x00000000 }, + { 0x00003200, 0x00000000, 0x00000000, 0x00000000, 0x10420000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00002000, 0x00000400, 0x00000000 }, + { 0x00000040, 0x00000000, 0x00000000, 0x00000004, 0x00000008, 0x00000000, 0x00000000, 0x00000000, 0x00000600, 0x00000000, 0x00004801, 0x00000000 }, + { 0x00000020, 0x00000000, 0x00000410, 0x00000000, 0x00000004, 0x00000000, 0x00800000, 0x00000000, 0x00900000, 0x00000001, 0x00000000, 0x00000000 }, + { 0x02000000, 0x00000000, 0x00000000, 0x00000000, 0x02000040, 0x00000000, 0x00000000, 0x00000000, 0x00840040, 0x00800000, 0x00000001, 0x00000000 }, + { 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x44000800, 0x00000000, 0x08001000, 0x20001000, 0x00000000, 0x00000000, 0x00800000, 0x00000000 }, + { 0x00000001, 0x00000000, 0x00000000, 0x00000880, 0x00000000, 0x00002000, 0x00000000, 0x00420000, 0x00040000, 0x00002020, 0x00000000, 0x00000000 }, + { 0x00000010, 0x00000000, 0x00040000, 0x00000000, 0x20000000, 0x00040000, 0x01000000, 0x04000000, 0x00000000, 0x00000080, 0x00400000, 0x00000001 }, + { 0x00000008, 0x00020000, 0x00000000, 0x00021000, 0x00000000, 0x00000000, 0x00000008, 0x00000000, 0x00080000, 0x00000210, 0x00000001, 0x00000000 }, + { 0x0000000A, 0x00001080, 0x00000000, 0x00000000, 0x00004000, 0x00200004, 0x00000000, 0x00000000, 0x00008000, 0x00000004, 0x00000000, 0x00000000 }, + { 0x00100082, 0x00000000, 0x20008000, 0x00000000, 0x00000000, 0x00004000, 0x00000800, 0x00000000, 0x00000000, 0x00001000, 0x00000000, 0x00000002 } +} ; + +const uint32_t LDPC_ParityGen_n354k160[194][5] +#ifdef __AVR__ +PROGMEM +#endif += { // parity bits generator + { 0xBF3B0B3C, 0x314E4CB2, 0x1C4219B8, 0xF87E8234, 0x6D121C05 }, + { 0x43C31BD3, 0xA49EE0D2, 0xB613CA26, 0x4A06EB31, 0xBBBC4B69 }, + { 0xA8E96C9C, 0x5C4BD117, 0x8877A3B7, 0x2C28FCBB, 0xC20F5C52 }, + { 0x18E91F00, 0xABA26C2A, 0x20AFA0D3, 0x781A54FD, 0x7AAD3CD7 }, + { 0xE2CE0C54, 0x0BBBE7EC, 0xBA8B6D72, 0x3CF41BBE, 0x9847B7BA }, + { 0x2119517F, 0xDA38A438, 0x6A3A2341, 0x3C6B3CEE, 0xC45138E0 }, + { 0xA03B5560, 0xB650F4D1, 0xF79542B5, 0x8B8A9EE2, 0x0008FAD3 }, + { 0x5D2624EF, 0x10DBF749, 0x3CA1F2A0, 0x6EC49CB4, 0xE180DDB9 }, + { 0xBFBC0710, 0xD0FA2A89, 0xFE1E22BF, 0x8DB41BAE, 0xE2A0DE6F }, + { 0x7C97D8D6, 0xD03734C4, 0x3630E03C, 0xBC0E99D2, 0x5B1663C3 }, + { 0xA8CB16AD, 0x006106C3, 0x64F41A4D, 0x98C281B0, 0x7E8055C4 }, + { 0x8C2CD493, 0x2CD876DC, 0x00C39BE8, 0x0CC58836, 0xE9B55589 }, + { 0x9ECCEF2A, 0xA648F8D7, 0xDB12CECF, 0xDECF8910, 0xFABA64FD }, + { 0x2DD164C6, 0xE4E44F87, 0x425F5945, 0xEE48E56B, 0xE7F4349D }, + { 0x5916345A, 0xDA049129, 0xCEEF739D, 0x47902FB7, 0x709F8AB4 }, + { 0x987D6D27, 0x3448F4E7, 0x20DAF291, 0xF82BCE50, 0x8B591390 }, + { 0x4DA99CD3, 0x1A13CC5E, 0xC3CE1A7E, 0xF460FD61, 0x8BA9FE66 }, + { 0x72CED441, 0x90AAAB8D, 0xC48C6831, 0x9B52333B, 0xD835B6FC }, + { 0x989EAEC0, 0xD9F69D6C, 0x66022DD7, 0x778FD6A5, 0x8B526FEE }, + { 0x90861033, 0xCA18E3FB, 0x996E893B, 0x24185E8D, 0x3FD66C7C }, + { 0xB8810387, 0x44833D00, 0xD0E0E7B3, 0xC4D91AA6, 0x6B5215F9 }, + { 0x1FDA72C6, 0xCBD4BB6B, 0x04BE862D, 0xE1B9EA26, 0x30C01CA7 }, + { 0xDAC99950, 0x66FC1E65, 0x734D0861, 0x8C4E7985, 0xBFCD3AD1 }, + { 0x17BF566C, 0x26A04744, 0x2F60189D, 0x373BFCA1, 0xA08EBFF8 }, + { 0x7EA9713E, 0x6A9A3324, 0x0E59E2A7, 0xCE7C57D0, 0xCBA40287 }, + { 0xA442B46F, 0x182CFF0A, 0x9568B31D, 0x82015F2C, 0x4F3AB3A7 }, + { 0x5965345A, 0xDA049169, 0xCEFF739D, 0x4790EFB7, 0x501FCAA4 }, + { 0x5ACF0524, 0x09664861, 0xC26B69ED, 0x041EC4B6, 0xD3BE71E7 }, + { 0xECEA5D89, 0x4C7AB36D, 0x9FA963C5, 0xFD416742, 0x1D331757 }, + { 0x1256E1C1, 0x5C2D5E39, 0xFA349FC5, 0x98F81F1D, 0x4921E241 }, + { 0x2AF78B26, 0xC3B88A6B, 0xE441147E, 0x68ED4E24, 0x2CD189AB }, + { 0x899F3FCE, 0x74270F2B, 0x50C13F29, 0xD7359EEA, 0xDA12BA16 }, + { 0x3F5A73D8, 0x71423A62, 0xD2957F27, 0xFE0F86A7, 0xAD98760B }, + { 0x17E138A8, 0x77EA7AE4, 0xB08FEC00, 0xF51D5B10, 0x50616E89 }, + { 0x9DEA1BA4, 0x864AA36F, 0x15AD6853, 0x4B734C20, 0xF2C935C0 }, + { 0x1C61D24D, 0x36304D45, 0x6935994B, 0xCBB771C3, 0x74B76058 }, + { 0x9C173562, 0xFF1735DA, 0x0C41BCB7, 0xC3195ADC, 0x60999718 }, + { 0x30C81C35, 0x60EE7A6E, 0x9DFC8572, 0xBD28D328, 0xFCD88B0E }, + { 0xECB8E63E, 0x8EA3EE2A, 0x61AE3F32, 0xB6B5E43C, 0xEA2DD6DE }, + { 0x03219881, 0x126EA7A6, 0x6A130444, 0xA5313241, 0x5DD972B0 }, + { 0xBCD7FF20, 0xB4C020BD, 0x9B77A60E, 0x91CA8631, 0xD7D1ACD2 }, + { 0xFF80C91E, 0x5BCA2B6E, 0xF8D17E27, 0x770DC19A, 0xB1EEEDA0 }, + { 0x7EF4C4F1, 0xC77292D4, 0x2E555DD7, 0x33DE6349, 0x6D79A577 }, + { 0x89A41EFF, 0xE7455A6E, 0x4431B80C, 0xB47CB0B1, 0x7A2CD62A }, + { 0x9F800F19, 0x2E497949, 0xD321D8DA, 0xF74C666B, 0x8EA103C2 }, + { 0x46149176, 0xA17E679C, 0xC6481C0D, 0xBA6BA8B7, 0xDD9E0E80 }, + { 0x5FE0198D, 0xE51C0C94, 0x80DA07B0, 0x8BCE8FAC, 0x119400C3 }, + { 0x7D66BF7E, 0x75045431, 0x6927222B, 0x7FEF3B1C, 0xB2E5D39D }, + { 0x7CB5D896, 0xF0373480, 0x3630E03D, 0xBC0E99D2, 0x5B1663C3 }, + { 0x78DC56FF, 0x4967B31E, 0x06291302, 0xB3B4A072, 0x8BB88CEC }, + { 0xA0354BC6, 0xD06A06D7, 0x4E724381, 0x3F07A3FD, 0x73AFE6FC }, + { 0xF30D48DA, 0x676E5492, 0x177F288F, 0x79E936BC, 0x2016EAC9 }, + { 0x8EF77970, 0xBD662325, 0x23C36963, 0xC4AA7264, 0x0A9BE530 }, + { 0xFEF2A35E, 0x77C86351, 0x89A87585, 0x8F39F81C, 0xB3853AC4 }, + { 0x5ACF0164, 0x0D664841, 0xCA6A69ED, 0x041EC4B6, 0xD3BE71A7 }, + { 0xFC14B7B4, 0x4B5C63C5, 0xA72CC3B5, 0x6CD2FE00, 0x57EA6966 }, + { 0x3E1E6D7B, 0x79532AE8, 0xA51A3C1B, 0xFC6FF9BD, 0x6BA67425 }, + { 0x0E79995B, 0x843292F7, 0x382FEB26, 0x70CD2CB7, 0x7BF29FB1 }, + { 0x6425F72F, 0xDFDC3624, 0xC593E31E, 0xE113249B, 0xB7490AFD }, + { 0x5819FC79, 0x8A418AC9, 0xD1900B5E, 0xA01E332A, 0xE3937E73 }, + { 0x0A475305, 0x7943BCA0, 0x666BBDE0, 0xF88290EA, 0x03436E21 }, + { 0xEBD7D24F, 0x0ED6EC40, 0x169BF54D, 0x8F4C3C3A, 0xF288B858 }, + { 0x15A1D5ED, 0x065B4DEE, 0xB7170FF2, 0xC110FFE6, 0x43DCAF43 }, + { 0xF2717717, 0xD28DAD36, 0x43329E2C, 0xF244AB0A, 0x1D79E529 }, + { 0x52FA7334, 0x7CF51534, 0xC14F1120, 0x0FB016BD, 0x11A8EDF7 }, + { 0x63BBB512, 0xE79B813B, 0x8329901D, 0xB51B64C4, 0xE88F0253 }, + { 0xB6BD7090, 0xF9DABA98, 0xFEFC792B, 0x0A9C05FC, 0xF3E5681E }, + { 0xE3E8F94B, 0xB416DFF4, 0x18EA246F, 0xC4847023, 0x520C4290 }, + { 0x9D999726, 0x6E454BA5, 0x424A1C3F, 0x9378A8C4, 0x3529A57B }, + { 0xD96B5D54, 0xC0FDACD7, 0xC7BC13F2, 0x26604362, 0xB730C113 }, + { 0x88F15CAA, 0xE33CD7A2, 0x6580C515, 0x4D51D804, 0x6E4D16DE }, + { 0xACB34E0A, 0xACF46901, 0xD1E3790C, 0xCFB8150B, 0x1D24AFDF }, + { 0xC15B49C8, 0x2493920E, 0x1EFB0A31, 0x96CF52E4, 0x6A59CC69 }, + { 0x495A275D, 0x5B296FB3, 0x5F8FCA98, 0x79DF044F, 0xFFA075CB }, + { 0xCB57CA6C, 0x96E56516, 0x34B373AB, 0x1DF6A1FF, 0x55FACA55 }, + { 0xC3EB2B4E, 0x81752CE2, 0xE4A59F1A, 0x42091D75, 0xF68E15C8 }, + { 0x3DAF86C3, 0xD6D9C19B, 0xC2A751E1, 0x79D118F4, 0xF122332C }, + { 0xC1D1D970, 0x382D6985, 0x06B12543, 0x0C05211C, 0xEA714078 }, + { 0x989EAEC0, 0xD9FE966C, 0x46022FDF, 0x778FD6A5, 0x8B526FEE }, + { 0x5E3B9C7A, 0xED9ADFAF, 0x0F443C92, 0xFC855F78, 0xB1C61773 }, + { 0x0EBF15FE, 0x1ECB1B82, 0x848E30D5, 0xE117B243, 0x6F3CE271 }, + { 0x424E825C, 0x096A4205, 0xABF29FA4, 0x854E07BF, 0x5E998352 }, + { 0xE0418B91, 0x4ADD50AC, 0x56511551, 0x0F0C7673, 0x821A3E84 }, + { 0x267BDCEF, 0x04734BB4, 0xF7B4BC80, 0xCC12BA7D, 0x36C612CF }, + { 0xC262CA37, 0x7E32C56F, 0x47845A47, 0xDF1B9BA8, 0xE0AFB8FC }, + { 0xE0418B91, 0x4BDC50AC, 0x56D11551, 0x0F0C6672, 0x829A3E84 }, + { 0x91F50DDE, 0x49E45E51, 0x6428D337, 0x31BCC161, 0x75402FCB }, + { 0xB296A10D, 0xF646A5FD, 0x470B0F51, 0x1AFBB1A8, 0xDCE8A51D }, + { 0x9848FB90, 0xDB688948, 0x8210D7E4, 0xC93EB1A8, 0x933FF050 }, + { 0x79685C41, 0x18ED55A7, 0x6D742D4D, 0xD6B94C2D, 0xA4386DC9 }, + { 0xB9E78046, 0xEF98DA9B, 0x74E74A58, 0xF9CCA0FB, 0xC6E7C152 }, + { 0x1C42C120, 0x35F81A6E, 0xC633A9F9, 0x8F0AD0F1, 0xBB3588CF }, + { 0x1B67E3ED, 0xAEFC3225, 0x85008317, 0x06AFE0B6, 0x632786BF }, + { 0x2E623791, 0xDDA5B84C, 0x7531F094, 0x3CE5681A, 0x030D196C }, + { 0x9659B78B, 0x6031178A, 0xDB017702, 0x5554FBCE, 0x33525AAC }, + { 0x60B57C8B, 0x58A1BF6B, 0x57EE5D97, 0x6F976042, 0x43948F0A }, + { 0x03220BC9, 0x2D105DCF, 0x1A2F231E, 0x14F15B28, 0x114227CB }, + { 0xB388A264, 0x4A7D76C5, 0x6ABBF6D9, 0x5556254C, 0xEB7DF23E }, + { 0x5DE9B360, 0x0C1008CC, 0xA7020795, 0x3A6C552C, 0x8AE262B6 }, + { 0xEDCA5D89, 0x4D7AB36D, 0x9FA96BC5, 0xFD412746, 0x1D331757 }, + { 0x20C00117, 0x75B2C4BC, 0x7DCBE920, 0x9CE9E0FE, 0xA6F64701 }, + { 0xAFAED554, 0xB54C8CCC, 0xE8B0699F, 0xCDE94601, 0x1FF9AF34 }, + { 0x684CC6F3, 0x90577B5A, 0x28F4656A, 0x90149AD1, 0x7D065F13 }, + { 0xEA63020E, 0x10073485, 0xF7738AF7, 0xECD9DF63, 0xEF52967C }, + { 0xF34D5BDA, 0x676E5492, 0x177B288F, 0x79E936BC, 0x2516EAC9 }, + { 0x9ED015B6, 0xC305A3CA, 0xC5C5C293, 0x3F78BB2D, 0x45738306 }, + { 0x5916345A, 0xDA049129, 0xCEFF739D, 0x4790EFB7, 0x701FCAA4 }, + { 0x48E5557C, 0x3867D1E4, 0xA86F29E8, 0xC5FDC9F4, 0xD2765165 }, + { 0x95EA1BA4, 0x864AA36F, 0x178D6853, 0x4B735820, 0x72C975C0 }, + { 0xF554F1F1, 0x1214B399, 0x45C252C4, 0x3EBA532B, 0xCEC50308 }, + { 0xC5B1C5CE, 0xD3F3540D, 0x63AA0659, 0xB3F95434, 0xF585E134 }, + { 0xEC4D8B91, 0x4BDC50AC, 0x56D11551, 0x2E0C6672, 0x829A3B84 }, + { 0xCF63E412, 0x12B15E91, 0x83051D0F, 0x9CC8BE39, 0x24814888 }, + { 0x96DAB33B, 0x403EE24D, 0xD4448F71, 0x3B6ECC5E, 0xCCBE5361 }, + { 0x05C2385B, 0x6C1EB2AD, 0x44E2D157, 0xAA4F2281, 0x36881398 }, + { 0x25CF1FCB, 0xEB18939F, 0x3420F9B2, 0x31A1A463, 0x4D941996 }, + { 0xE9588C02, 0x7BBCECFF, 0x7AA5B0E7, 0x7E5FE165, 0x9984E1B4 }, + { 0xBC48FB90, 0xDB689948, 0x8610D7E4, 0xC83FB188, 0x933FF050 }, + { 0xD5192898, 0xE4A0029F, 0x62574555, 0x7168276F, 0x77021800 }, + { 0x51D69601, 0x62F5524B, 0x16B5D9BC, 0x624E462D, 0xAE500B1D }, + { 0x17669EA7, 0x1775A41F, 0x09A47393, 0xF0FE3BD7, 0x58178FC1 }, + { 0xEC304D69, 0x4D5E9089, 0x3A899E6C, 0xAE0DA801, 0xB394CF54 }, + { 0x6CA5F72F, 0xDFD83625, 0xC593E31E, 0xE113249B, 0xB7410A7D }, + { 0x2A4099D1, 0x5926389F, 0x85EE807C, 0xBF2B5A2F, 0x3442B2AA }, + { 0x9565CA53, 0xEFB6F5B0, 0x9586A876, 0x3B85C5B5, 0x185F87B0 }, + { 0x89F7CDEA, 0x2FFDECB4, 0xC191EC57, 0x964510D8, 0x23DD8018 }, + { 0x48DC8D1F, 0x593EED5B, 0xA06A9AB7, 0x81AE548A, 0x5261DF4C }, + { 0x0FF22560, 0xD10ADA44, 0x53081653, 0x7E5F18B1, 0xC5C1B2A5 }, + { 0x449AAD97, 0x5215DD71, 0xCB3CAEFE, 0x70C35948, 0x603E83CB }, + { 0x0FCF09D8, 0x502FD0D4, 0x0BD6ABE3, 0x7C2981A7, 0x69BB3DDA }, + { 0x5B7E95C1, 0xDC432A50, 0xDA1830CB, 0xE39D70EF, 0x70553B71 }, + { 0x80B7031D, 0x008ACF8A, 0xD6A52F6D, 0xA32E741A, 0x70F87C14 }, + { 0xAD7EFC8C, 0x048C390D, 0xE2C42E23, 0x7A70EAAC, 0xE4FF9B20 }, + { 0x04F750C8, 0x2EB3118A, 0xC3F13D99, 0xDF48E0AD, 0x28FF9F90 }, + { 0x6BF84DDF, 0x7B3CA371, 0xEB402058, 0x6DA784CF, 0x0654544A }, + { 0x68D02631, 0x2D4D0277, 0x26727DD6, 0x801214CA, 0x5D217B68 }, + { 0x434C131D, 0x68A3DFAE, 0xB017E87F, 0xC7B52C91, 0x42EA8D0F }, + { 0x4E9D3ED1, 0x978E35BB, 0xD5BAC41B, 0x78492E14, 0x2EC940AD }, + { 0xF514F1F1, 0x9214B389, 0x45C252C4, 0x3EBA532B, 0xCEC5032E }, + { 0x5B5F3E79, 0x2ABAB2F5, 0x084C4D5A, 0x31D688D0, 0xD54F32A1 }, + { 0xC9D0422B, 0xF87EFB4B, 0x65C25674, 0xE9924A10, 0xD17731E8 }, + { 0xFF5F83D6, 0xE6C7F0F4, 0x0A96F918, 0x2FBDE919, 0xD25A140C }, + { 0xCBA3FCEB, 0xF3610958, 0x20AB2507, 0xEABFC19C, 0xA4432F0A }, + { 0x3542D3AF, 0x09A7674F, 0xF77389BF, 0xCF5DF542, 0x50815658 }, + { 0x0AF1DF54, 0xF80803CD, 0x3DA3F21D, 0x6084E0FC, 0x1F9EDFFB }, + { 0x6DE2AA90, 0xBFE9A564, 0xD1798BB7, 0x367DEFBD, 0x3716658D }, + { 0xB3CF1AD0, 0x55D872A7, 0x8F853289, 0xF2A83C7A, 0xF18EBF7A }, + { 0x75E81640, 0xCAAC3405, 0xA1DB2536, 0xE6BAE8B9, 0x463ECA30 }, + { 0x14B85EA7, 0xCB24075A, 0x03B2B4C3, 0x84A5CE08, 0xC31CC695 }, + { 0xC55B49C8, 0x249303AE, 0x1CFB0A31, 0x96EF52E4, 0x6A59CC69 }, + { 0x267531D5, 0xAD722E2B, 0xD0601DE1, 0x2B3DFDB4, 0x816EE75B }, + { 0xE415677A, 0xD09449D0, 0x10C8E740, 0xA038FF65, 0x14CBE595 }, + { 0x6A906631, 0x2D4D0275, 0x22727DD6, 0xC01214CA, 0x5D217B68 }, + { 0xB31272C5, 0xC2594005, 0x94B83E46, 0x4D84ED27, 0xF23A67F3 }, + { 0xB2BBF8A5, 0x36004598, 0x971E0455, 0x00BA60CE, 0x93B2D373 }, + { 0xDF37FE5C, 0x6ADBAD69, 0x8A9755B8, 0x3916D1B1, 0x4458FE0E }, + { 0xB4D7FF20, 0xB48820BD, 0x9B77260A, 0x91CA8631, 0xD7D5ACD2 }, + { 0x5D3608F5, 0x436907F3, 0x4CD2CBAD, 0x84C5E362, 0xD1E7240D }, + { 0x34C01C31, 0x60FE5A66, 0x9DFC8172, 0xB528D328, 0xFCD88B0E }, + { 0x409562A7, 0x100957A1, 0x4526AC8B, 0xE7C47A53, 0xBD8B8ED5 }, + { 0xDA4D0FD3, 0x4E28D26C, 0x6A6B8AC1, 0xF82D0118, 0xF315A243 }, + { 0x745A9DB4, 0x9071FBC7, 0x33A67C1D, 0x40063FD4, 0xB8EE6CDA }, + { 0x85A1E2B5, 0x0267BB1F, 0x436DAAF0, 0x65D3EF48, 0xFA1CF504 }, + { 0x247CD0D8, 0x64165577, 0xEE6ACDD4, 0x9C0CBD01, 0x958A76A1 }, + { 0xC5B5C5CE, 0xD373540C, 0x63AA05D9, 0xB3E95434, 0xF185E134 }, + { 0xD17281AB, 0xF5E8C8AE, 0x81F1D98E, 0xF4D8C0CD, 0xD7DF700E }, + { 0x50BAB24E, 0x546D7C13, 0xE047CA14, 0x96E26BC6, 0xF035D936 }, + { 0xD872A27F, 0xEE03F33C, 0x8EBC2CC6, 0x8A82CD68, 0x964A6E4E }, + { 0x629516BC, 0xF182D7B1, 0x4760C49C, 0xF069128A, 0x19CEC52F }, + { 0x9A2CACEB, 0x9C729BAB, 0xBB6024B4, 0x80C26E03, 0xC5C1E2FB }, + { 0xFF5FB1D6, 0xE6C7F0F4, 0x0A96F918, 0x2FBDE919, 0xC218140C }, + { 0xB0A929EE, 0xB189398A, 0xE904C6EE, 0x769ECA8E, 0xC868552F }, + { 0xEF4D67E1, 0x3F7A8BF3, 0x7B4686D9, 0x2E4918B4, 0x88E76F5A }, + { 0xF7202730, 0x6763E65A, 0x01E69E66, 0x213AF56A, 0x07FFDF91 }, + { 0xAD220791, 0xE6A0D905, 0x8855FD21, 0xF65B9F08, 0x0E11BEC6 }, + { 0xCA4FD7C6, 0xD429968D, 0x54EE0F82, 0x5F0B299D, 0xB377A305 }, + { 0xFC749190, 0x84558B4E, 0x16E0249D, 0x1015D925, 0xA045B5DB }, + { 0xAD7430E3, 0x46C2F2B7, 0xFB569EA5, 0xAB7068D2, 0x8BED359A }, + { 0xC2421229, 0xBD61DCB7, 0xDBB300AA, 0x1450F14E, 0xE8B7317D }, + { 0x4E9E2248, 0x4A44E893, 0x95B0CC28, 0x94FE53F8, 0x2997B8AF }, + { 0x18792A74, 0x01C6AC75, 0x51C74F95, 0x5C23C030, 0xC82C03E6 }, + { 0xE6E4579F, 0x4BE76A10, 0x46CE12AC, 0xA4D9E3E7, 0xB01B9244 }, + { 0xB5EC3540, 0x4F5C6BDE, 0xD60F41DB, 0xAAEF0E13, 0x68938D5D }, + { 0xD4544232, 0xAF748A57, 0xB52772F1, 0x126625AF, 0xDB1D5FF1 }, + { 0x1E623791, 0xD9A7B84C, 0x7531F094, 0x3CE5689A, 0x034D096C }, + { 0x55A90C45, 0x2C89BE82, 0x51B53A53, 0xDD30D5DB, 0x950D788D }, + { 0x86A7B294, 0xD7991A0D, 0xA998C3AD, 0xFDBDE0E8, 0x1BDED0B7 }, + { 0x8CCA1BA4, 0x864BA36F, 0x55AD6853, 0x4B734C20, 0xF2C935D0 }, + { 0xCEF9CA2C, 0x3DD0F8C3, 0x5944397A, 0x10B3C86C, 0x766C5640 }, + { 0x9D48FDB5, 0xA9C5D735, 0x63C23A23, 0x1DC99B85, 0x90B1C1D3 }, + { 0xA48DEE95, 0x9782ABB5, 0x422ED881, 0x1F657314, 0x289FA398 }, + { 0x99C33D5E, 0xF1247681, 0x315592C6, 0xF0DF750F, 0xBA5C69FE }, + { 0xC25B509C, 0x2D2C04B6, 0x47866799, 0x58D1953A, 0x33A12CAC }, + { 0x1972680C, 0xC53F300C, 0xCF1C1DE5, 0x1A7ED516, 0xDBC46D76 } +} ; + +#endif // WITH_PPM + +// =================================================================================================================== + +#ifdef __AVR__ + +// encode Parity from Data: Data is 5x 32-bit words = 160 bits, Parity is 1.5x 32-bit word = 48 bits +void LDPC_Encode(const uint32_t *Data, uint32_t *Parity, const uint32_t ParityGen[48][5]) +{ uint8_t ParIdx=0; Parity[ParIdx]=0; uint32_t Mask=1; + for(uint8_t Row=0; Row<48; Row++) + { uint8_t Count=0; + const uint32_t *Gen=ParityGen[Row]; + for(uint8_t Idx=0; Idx<5; Idx++) + { Count+=Count1s(Data[Idx]&pgm_read_dword(Gen+Idx)); } + if(Count&1) Parity[ParIdx]|=Mask; Mask<<=1; + if(Mask==0) { ParIdx++; Parity[ParIdx]=0; Mask=1; } + } +} + +void LDPC_Encode(const uint32_t *Data, uint32_t *Parity) +{ LDPC_Encode(Data, Parity, LDPC_ParityGen); } + +// encode Parity from Data: Data is 20 bytes = 160 bits, Parity is 6 bytes = 48 bits +void LDPC_Encode(const uint8_t *Data, uint8_t *Parity, const uint32_t ParityGen[48][5]) +{ uint8_t ParIdx=0; Parity[ParIdx]=0; uint8_t Mask=1; + for(uint8_t Row=0; Row<48; Row++) + { uint8_t Count=0; + const uint8_t *Gen = (uint8_t *)ParityGen[Row]; + for(uint8_t Idx=0; Idx<20; Idx++) + { Count+=Count1s(Data[Idx]&pgm_read_byte(Gen+Idx)); } + if(Count&1) Parity[ParIdx]|=Mask; Mask<<=1; + if(Mask==0) { ParIdx++; Parity[ParIdx]=0; Mask=1; } + } +} + +void LDPC_Encode(const uint8_t *Data, uint8_t *Parity) +{ LDPC_Encode(Data, Parity, LDPC_ParityGen); } + +void LDPC_Encode(uint8_t *Data) +{ LDPC_Encode(Data, Data+20, LDPC_ParityGen); } + +// check Data against Parity (run 48 parity checks) - return number of failed checks +int8_t LDPC_Check(const uint8_t *Data) // 20 data bytes followed by 6 parity bytes +{ uint8_t Errors=0; + for(uint8_t Row=0; Row<48; Row++) + { uint8_t Count=0; + const uint8_t *Check = (uint8_t *)LDPC_ParityCheck[Row]; + for(uint8_t Idx=0; Idx<26; Idx++) + { Count+=Count1s(Data[Idx] & pgm_read_byte(Check+Idx)); } + if(Count&1) Errors++; } + return Errors; } + +int8_t LDPC_Check(const uint32_t *Packet) +{ return LDPC_Check( (uint8_t *)Packet ); } + +#else // if not 8-bit AVR + +void LDPC_Encode(const uint8_t *Data, uint8_t *Parity, const uint32_t ParityGen[48][5]) +{ uint8_t ParIdx=0; uint8_t ParByte=0; uint8_t Mask=1; + for(uint8_t Row=0; Row<48; Row++) + { uint8_t Count=0; + const uint8_t *Gen = (uint8_t *)(ParityGen[Row]); + for(uint8_t Idx=0; Idx<20; Idx++) + { Count+=Count1s((uint8_t)(Data[Idx]&Gen[Idx])); } + if(Count&1) ParByte|=Mask; Mask<<=1; + if(Mask==0) { Parity[ParIdx++]=ParByte; Mask=1; ParByte=0; } + } + // if(Mask!=1) Parity[ParIdx]=ParByte; +} + +void LDPC_Encode(const uint8_t *Data, uint8_t *Parity) +{ LDPC_Encode(Data, Parity, LDPC_ParityGen_n208k160); } + +void LDPC_Encode(uint8_t *Data) +{ LDPC_Encode(Data, Data+20); } + +// encode Parity from Data: Data is 5x 32-bit words = 160 bits, Parity is 1.5x 32-bit word = 48 bits +static void LDPC_Encode(const uint32_t *Data, uint32_t *Parity, uint8_t DataWords, uint8_t Checks, const uint32_t *ParityGen) +{ // printf("LDPC_Encode: %08X %08X %08X %08X %08X", Data[0], Data[1], Data[2], Data[3], Data[4] ); + uint8_t ParIdx=0; Parity[ParIdx]=0; uint32_t Mask=1; + const uint32_t *Gen=ParityGen; + for(uint8_t Row=0; Row %08X %08X\n", Parity[0], Parity[1] ); +} + +void LDPC_Encode(const uint32_t *Data, uint32_t *Parity) { LDPC_Encode(Data, Parity, 5, 48, (uint32_t *)LDPC_ParityGen_n208k160); } +void LDPC_Encode( uint32_t *Data) { LDPC_Encode(Data, Data+5, 5, 48, (uint32_t *)LDPC_ParityGen_n208k160); } + +#ifdef WITH_PPM +void LDPC_Encode_n354k160(const uint32_t *Data, uint32_t *Parity) { LDPC_Encode(Data, Parity, 5, 194, (uint32_t *)LDPC_ParityGen_n354k160); } +void LDPC_Encode_n354k160( uint32_t *Data) { LDPC_Encode(Data, Data+5, 5, 194, (uint32_t *)LDPC_ParityGen_n354k160); } +#endif + +// check Data against Parity (run 48 parity checks) - return number of failed checks +uint8_t LDPC_Check(const uint32_t *Data, const uint32_t *Parity) // Data and Parity are 32-bit words +{ uint8_t Errors=0; + for(uint8_t Row=0; Row<48; Row++) + { uint8_t Count=0; + const uint32_t *Check=LDPC_ParityCheck_n208k160[Row]; + uint8_t Idx; + for(Idx=0; Idx<5; Idx++) + { Count+=Count1s(Data[Idx]&Check[Idx]); } + Count+=Count1s(Parity[0]&Check[Idx++]); + Count+=Count1s((Parity[1]&Check[Idx++])&0xFFFF); + if(Count&1) Errors++; } + return Errors; } + +uint8_t LDPC_Check(const uint32_t *Data) { return LDPC_Check(Data, Data+5); } + +uint8_t LDPC_Check(const uint8_t *Data) // 20 data bytes followed by 6 parity bytes +{ uint8_t Errors=0; + for(uint8_t Row=0; Row<48; Row++) + { uint8_t Count=0; + const uint8_t *Check = (uint8_t *)LDPC_ParityCheck_n208k160[Row]; + for(uint8_t Idx=0; Idx<26; Idx++) + { uint8_t And = Data[Idx]&Check[Idx]; Count+=Count1s(And); } + if(Count&1) Errors++; } + return Errors; } +#ifdef WITH_PPM +uint8_t LDPC_Check_n354k160(const uint32_t *Data, const uint32_t *Parity) // Data and Parity are 32-bit words +{ uint8_t Errors=0; + for(uint8_t Row=0; Row<194; Row++) + { uint8_t Count=0; + const uint32_t *Check=LDPC_ParityCheck_n354k160[Row]; + uint8_t Idx; + for(Idx=0; Idx<5; Idx++) + { Count+=Count1s(Data[Idx]&Check[Idx]); } + uint8_t ParIdx; + for(ParIdx=0; ParIdx<6; ParIdx++, Idx++) + { Count+=Count1s(Parity[ParIdx]&Check[Idx]); } + Count+=Count1s((Parity[ParIdx]&Check[Idx])&0x0003); + if(Count&1) Errors++; } + return Errors; } + +uint8_t LDPC_Check_n354k160(const uint32_t *Data) { return LDPC_Check_n354k160(Data, Data+5); } +#endif // WITH_PPM + +#endif // __AVR__ + diff --git a/utils/ldpc.h b/utils/ldpc.h new file mode 100644 index 0000000..5541be3 --- /dev/null +++ b/utils/ldpc.h @@ -0,0 +1,425 @@ +#ifndef __LDPC_H__ +#define __LDPC_H__ + +#include +#include +#include + +#include + +#include "bitcount.h" + +#ifndef __AVR__ +// #include +#include +#endif + +#ifdef __AVR__ +#include +#endif + +// extern const uint32_t LDPC_ParityGen_n208k160[48][5]; +// extern const uint32_t LDPC_ParityCheck_n208k160[48][7]; +// extern const uint8_t LDPC_ParityCheckIndex_n208k160[48][24]; +// extern const uint8_t LDPC_BitWeight_n208k160[208]; +#ifdef WITH_PPM +extern const uint32_t LDPC_ParityGen_n354k160[194][5]; +extern const uint32_t LDPC_ParityCheck_n354k160[194][12]; +#endif + +#ifdef __AVR__ + +// encode Parity from Data: Data is 5x 32-bit words = 160 bits, Parity is 1.5x 32-bit word = 48 bits +void LDPC_Encode(const uint32_t *Data, uint32_t *Parity, const uint32_t ParityGen[48][5]); +void LDPC_Encode(const uint32_t *Data, uint32_t *Parity); + +// encode Parity from Data: Data is 20 bytes = 160 bits, Parity is 6 bytes = 48 bits +void LDPC_Encode(const uint8_t *Data, uint8_t *Parity, const uint32_t ParityGen[48][5]); +void LDPC_Encode(const uint8_t *Data, uint8_t *Parity); +void LDPC_Encode( uint8_t *Data); + // check Data against Parity (run 48 parity checks) - return number of failed checks +uint8_t LDPC_Check(const uint8_t *Data); // 20 data bytes followed by 6 parity bytes +uint8_t LDPC_Check(const uint32_t *Packet); + +#else // if not 8-bit AVR + +void LDPC_Encode(const uint8_t *Data, uint8_t *Parity, const uint32_t ParityGen[48][5]); +void LDPC_Encode(const uint8_t *Data, uint8_t *Parity); +void LDPC_Encode( uint8_t *Data); + // encode Parity from Data: Data is 5x 32-bit words = 160 bits, Parity is 1.5x 32-bit word = 48 bits +// void LDPC_Encode(const uint32_t *Data, uint32_t *Parity, const uint32_t ParityGen[48][5]); + +// void LDPC_Encode(const uint32_t *Data, uint32_t *Parity, uint8_t DataWords, uint8_t Checks, const uint32_t *ParityGen); +// inline void LDPC_Encode(const uint32_t *Data, uint32_t *Parity) { LDPC_Encode(Data, Parity, 5, 48, (uint32_t *)LDPC_ParityGen_n208k160); } +// inline void LDPC_Encode( uint32_t *Data) { LDPC_Encode(Data, Data+5, 5, 48, (uint32_t *)LDPC_ParityGen_n208k160); } +// inline void LDPC_Encode_n394k160(const uint32_t *Data, uint32_t *Parity) { LDPC_Encode(Data, Parity, 5, 194, (uint32_t *)LDPC_ParityGen_n354k160); } +// inline void LDPC_Encode_n394k160( uint32_t *Data) { LDPC_Encode(Data, Data+5, 5, 194, (uint32_t *)LDPC_ParityGen_n354k160); } + +void LDPC_Encode(const uint32_t *Data, uint32_t *Parity); +void LDPC_Encode( uint32_t *Data); +#ifdef WITH_PPM +void LDPC_Encode_n354k160(const uint32_t *Data, uint32_t *Parity); +void LDPC_Encode_n354k160( uint32_t *Data); +#endif + // check Data against Parity (run 48 parity checks) - return number of failed checks +uint8_t LDPC_Check(const uint32_t *Data, const uint32_t *Parity); // Data and Parity are 32-bit words +uint8_t LDPC_Check(const uint32_t *Data); +uint8_t LDPC_Check(const uint8_t *Data); // 20 data bytes followed by 6 parity bytes +#ifdef WITH_PPM +uint8_t LDPC_Check_n354k160(const uint32_t *Data, const uint32_t *Parity); // Data and Parity are 32-bit words +uint8_t LDPC_Check_n354k160(const uint32_t *Data); +#endif + +#endif // __AVR__ + +#ifndef __AVR__ + +extern const uint8_t LDPC_ParityCheckIndex_n208k160[48][24]; + +class LDPC_Decoder +{ public: + const static uint8_t UserBits = 160; // 5 32-bit bits = 20 bytes + const static uint8_t UserWords = UserBits/32; + const static uint8_t ParityBits = 48; // 6 bytes (total packet is 26 bytes) + const static uint8_t CodeBits = UserBits+ParityBits; // 160+48 = 208 code bits = 26 bytes + const static uint8_t CodeBytes = (CodeBits+ 7)/ 8; // + const static uint8_t CodeWords = (CodeBits+31)/32; // + const static uint8_t MaxCheckWeight = 24; + // const static uint8_t MaxBitWeight = 8; + + public: + + int16_t InpBit[CodeBits]; // a-priori bits + int16_t ExtBit[CodeBits]; // extrinsic inf. + int16_t OutBit[CodeBits]; // a-posteriori bits + + void Input(const uint8_t *Data, const uint8_t *Err) + { uint8_t Mask=1; uint8_t Idx=0; uint8_t DataByte=0; uint8_t ErrByte=0; + for(uint8_t Bit=0; Bit32767) Inp=32767; else if(Inp<(-32767)) Inp=(-32767); + OutBit[Bit] = InpBit[Bit] = Inp; + ExtBit[Bit]=0; } + } + + void Output(uint32_t Data[CodeWords]) + { uint32_t Mask=1; uint8_t Idx=0; uint32_t Word=0; + for(uint8_t Bit=0; Bit0) Word|=Mask; + Mask<<=1; if(Mask==0) { Data[Idx++]=Word; Word=0; Mask=1; } + } if(Mask>1) Data[Idx++]=Word; + } + + void Output(uint8_t Data[CodeBytes]) + { uint8_t Mask=1; uint8_t Idx=0; uint8_t Byte=0; + for(uint8_t Bit=0; Bit0) Byte|=Mask; + Mask<<=1; if(Mask==0) { Data[Idx++]=Byte; Byte=0; Mask=1; } + } if(Mask>1) Data[Idx++]=Byte; + } + + int8_t ProcessChecks(void) + { for(uint8_t Bit=0; Bit>1); } + return Count; } + + int16_t ProcessCheck(uint8_t Row) + { int16_t MinAmpl=32767; uint8_t MinBit=0; int16_t MinAmpl2=MinAmpl; + uint32_t Word=0; uint32_t Mask=1; + const uint8_t *CheckIndex = LDPC_ParityCheckIndex_n208k160[Row]; + uint8_t CheckWeight = *CheckIndex++; + for(uint8_t Bit=0; Bit0) Word|=Mask; + Mask<<=1; + if(Ampl<0) Ampl=(-Ampl); + if(Ampl + class LDPC_FloatDecoder +{ public: + + const static int MaxCodeBits=512; + const static int MaxParityBits=256; + const static int MaxParityWeight=32; // + int CodeBits; // number of code bits + int ParityBits; // number of parity bits + uint16_t ParityCheckIndex[MaxParityBits][MaxParityWeight]; // list of 1's in the ParityCheck matrix + uint8_t ParityCheckRowWeight[MaxParityBits]; // number of 1's in ParityCheck rows + uint8_t ParityCheckColWeight[MaxCodeBits]; // number of 1's in ParityCheck columns + + Float InpBit[MaxCodeBits]; // a-priori bits + Float ExtBit[MaxCodeBits]; // extrinsic inf. + Float OutBit[MaxCodeBits]; // a-posteriori bits + Float Feedback; + + public: + + LDPC_FloatDecoder() + { CodeBits=0; ParityBits=0; Feedback=0.33; } + + void Clear(void) + { for(int Bit=0; BitMaxCodeBits) return -1; + CodeBits=NewCodeBits; + if(ParityBits>MaxParityBits) return -1; + ParityBits=NewParityBits; + for(int Bit=0; Bit0) Word|=Mask; + Mask<<=1; if(Mask==0) { Data[Idx++]=Word; Word=0; Mask=1; } + } if(Mask>1) Data[Idx++]=Word; + } + + void Output(uint8_t *Data) // format decoded bits as a series of bytes + { uint8_t Mask=1; int Idx=0; uint8_t Byte=0; + for(int Bit=0; Bit0) Byte|=Mask; + Mask<<=1; if(Mask==0) { Data[Idx++]=Byte; Byte=0; Mask=1; } + } if(Mask>1) Data[Idx++]=Byte; + } + + int ProcessChecks(void) + { for(int Bit=0; Bit::max(); int MinBit=0; Float MinAmpl2=MinAmpl; // look for 1st and 2nd smallest LL + uint32_t Word=0; uint32_t Mask=1; + const uint16_t *CheckIndex = ParityCheckIndex[Row]; // indeces of bits in this parity check + int CheckWeight = ParityCheckRowWeight[Row]; // number of bits in this parity check + for(int Bit=0; Bit0) Word|=Mask; // store hard bits in the Word + Mask<<=1; + if(Ampl<0) Ampl=(-Ampl); // strip the LL sign + if(Ampl0; + bool Out=OutBit[Idx]>0; + if(Inp!=Out) Count++; } + return Count; } + +} ; + +#ifdef WITH_PPM +template + class OGN_PPM_Decoder +{ public: + static const int DataBits = 32*5; // 5 words = 160 data bits = OGN packet + static const int ParityBits = 194; // 194 parity bits (Gallager code) + static const int CodeBits = DataBits+ParityBits; // 354 total bits per Gallager code block + static const int BitsPerSymbol = 6; // 6 bits per symbol for PPM modulation + static const int PulsesPerSlot = 1< LDPC_Decoder; // inner LDPC code decoder + + Float InpSymb[CodeSymbols][PulsesPerSlot]; // input from the demodulator + Float ExtSymb[CodeSymbols][PulsesPerSlot]; // output from the LDPC decoder + Float OutSymb[CodeSymbols][PulsesPerSlot]; // input x extrinsic inf. + + public: + OGN_PPM_Decoder() + { LDPC_Decoder.Configure(CodeBits, ParityBits, (uint32_t *)LDPC_ParityCheck_n354k160); + Clear(); } + + void Clear(void) + { for(int Symb=0; Symb=CodeSymbols) || (Symbol>=PulsesPerSlot) ) return; + InpSymb[Slot][Symbol]+=Power; } + + int Process(int Loops=48) + { LDPC_Decoder.Clear(); + + for(int Symb=0; Symb>=1; } + } + } + int CheckErr=0; + for( int Loop=0; Loop %3d\n", Loop, CheckErr); + if(CheckErr==0) break; } + return CheckErr; } + + static uint8_t Gray(uint8_t Binary) { return Binary ^ (Binary>>1); } + + static uint8_t Binary(uint8_t Gray) + { Gray = Gray ^ (Gray >> 4); + Gray = Gray ^ (Gray >> 2); + Gray = Gray ^ (Gray >> 1); + return Gray; } + + void NormExtSymb(Float Norm=1.0) + { for(int Symb=0; Symb +#include + +#include "intmath.h" + +// ============================================================================ + +// https://github.com/mavlink/mavlink/blob/master/message_definitions/v1.0/common.xml + +// System-ID + + +// Component-ID +const uint8_t MAV_COMP_ID_AUTOPILOT1 = 1; // auto-pilot +const uint8_t MAV_COMP_ID_ADSB = 156; // ADS-B receiver +const uint8_t MAV_COMP_ID_GPS = 220; + +// Message-ID +const uint8_t MAV_ID_HEARTBEAT = 0; // + +const uint8_t MAV_ID_SYS_STATUS = 1; // + power data +const uint8_t MAV_ID_SYSTEM_TIME = 2; // + boot and UTC time +const uint8_t MAV_ID_PARAM_VALUE = 22; // + value of an parameter +const uint8_t MAV_ID_GPS_RAW_INT = 24; // + position after the GPS +const uint8_t MAV_ID_RAW_IMU = 27; // + +const uint8_t MAV_ID_SCALED_PRESSURE = 29; // + pressure+temperature +const uint8_t MAV_ID_ATTITUDE = 30; // +const uint8_t MAV_ID_GLOBAL_POSITION_INT = 33; // + combined position estimate +const uint8_t MAV_ID_RC_CHANNELS_RAW = 35; // +const uint8_t MAV_ID_SERVO_OUTPUT_RAW = 36; // +const uint8_t MAV_ID_MISSION_CURRENT = 42; // +const uint8_t MAV_ID_NAV_CONTROLLER_OUTPUT = 62; // +const uint8_t MAV_ID_VFR_HUD = 74; // +const uint8_t MAV_ID_TIMESYNC = 111; +const uint8_t MAV_ID_HIL_GPS = 113; +const uint8_t MAV_ID_SCALED_IMU2 = 116; // +const uint8_t MAV_ID_POWER_STATUS = 125; // 5V and servo power and status +const uint8_t MAV_ID_TERRAIN_REPORT = 136; // +const uint8_t MAV_ID_BATTERY_STATUS = 147; // +const uint8_t MAV_ID_ADSB_VEHICLE = 246; // + traffic information sent by an ADS-B receiver +const uint8_t MAV_ID_COLLISION = 247; // + collision threat detected by auto-pilot +const uint8_t MAV_ID_STATUSTEXT = 253; // + +const uint8_t MAV_ID_DEBUG = 254; + +// -------------------------------------------------------------------------------- + +class MAV_HEARTBEAT // 0 +{ public: + uint32_t custom_mode; + uint8_t type; // MAV-aircraft-type: 1=fixed wing, 2=quadrotor, 7=airship, 8=balloon, D=hexarotor + uint8_t autopilot; // 3=ArduPilotMega, 4=OpenPilot, 13=PX4 + union + { uint8_t base_mode; + struct + { bool custom:1; + bool test:1; + bool autonomous:1; + bool guided:1; + bool stabilize:1; + bool hil_simulation:1; + bool remote_control:1; + bool armed:1; + } ; + } ; + uint8_t system_status; // 1=boot, 2=calib., 3=standby, 4=active, 5=critical, 6=emergency, 7=power-off, 8=terminate + uint8_t mavlink_version; + public: + void Print(void) const + { printf("HEARTBEAT: t%X v%d\n", type, mavlink_version); } +} ; + +class MAV_SYS_STATUS // 1 +{ public: + uint32_t onboard_control_sensors_present; + uint32_t onboard_control_sensors_enabled; + uint32_t onboard_control_sensors_health; + uint16_t load; // [0.1%] + uint16_t battery_voltage; // [mV] + int16_t battery_current; // [10mA] + uint16_t error_comm; + uint16_t error_comm1; + uint16_t error_comm2; + uint16_t error_comm3; + uint16_t error_comm4; + uint8_t battery_remaining; // [%] + void Print(void) const + { printf("SYS_STATUS: %3.1f%% %5.3fV %+5.2fA\n", 0.1*load, 1e-3*battery_voltage, 0.01*battery_current); } +} ; + +class MAV_SYSTEM_TIME // 2 +{ public: + uint64_t time_unix_usec; // [usec] + uint32_t time_boot_ms; // [ms] + public: + void Print(void) const + { printf("SYSTEM_TIME: %14.3f-%8.3f [sec]\n", 1e-6*time_unix_usec, 1e-3*time_boot_ms); } +} ; + +class MAV_PARAM_VALUE +{ public: + float param_value; + uint16_t param_count; + uint16_t param_index; + char param_id[16]; + uint8_t param_type; // (1)2=(u)int8, (3)/4=(u)int16_t, (5)/6=(u)int32_t, (7)/8=(u)int64_t, 9=float, 10=double + public: + void Print(void) const + { printf("PARAM_VALUE: %.16s t%X %d/%d\n", param_id, param_type, param_index, param_count); } +} ; + +class MAV_GPS_RAW_INT // 24 +{ public: + uint64_t time_usec; // [usec] Time-since-boot time or UTC + int32_t lat; // [1e-7deg] Latitude + int32_t lon; // [1e-7deg] Longitude + int32_t alt; // [mm] Altitude AMSL + // int32_t alt_ellipsoid; // [mm] + // uint32_t h_acc; // [mm] + // uint32_t v_acc; // [mm] + // uint32_t vel_acc; // [mm/s] + // uint32_t hdg_acc; // [0.00001deg] + uint16_t eph; // [0.01] HDOP + uint16_t epv; // [0.01] VDOP + uint16_t vel; // [0.01m/s] Valocity + uint16_t cog; // [0.01deg] course-over-Ground + uint8_t fix_type; // [] 0=no GPS, 1=no fix, 2=2D, 3=3D, 4=DGPS + uint8_t satellites_visible; // [] Number of satellites + public: + void Print(void) const + { printf("GPS_RAW_INT: %14.3fsec [%+9.5f, %+10.5f]deg %+5.1fm %3.1fm/s %05.1fdeg %d/%dsat\n", + 1e-6*time_usec, 1e-7*lat, 1e-7*lon, 1e-3*alt, 0.01*vel, 0.01*cog, fix_type, satellites_visible); } +} ; + +class MAV_RAW_IMU +{ public: + uint64_t time_usec; // [usec] Time-since-boot time or UTC + int16_t xacc; // [] Accelerometer + int16_t yacc; + int16_t zacc; + int16_t xgyro; // [] Gyroskop + int16_t ygyro; + int16_t zgyro; + int16_t xmag; // [] Magnetometer + int16_t ymag; + int16_t zmag; +} ; + +class MAV_GLOBAL_POSITION_INT +{ public: + uint32_t time_boot_ms; // [ms] + int32_t lat; // [1e-7deg] + int32_t lon; // [1e-7deg] + int32_t alt; // [mm] AMSL + int32_t relative_alt; // [mm] Above-takeoff ? + int16_t vx; // [cm/s] along latitude: positive => north + int16_t vy; // [cm/s] along longitude: positive => east + int16_t vz; // [cm/s] sink rate: positive => down + int16_t hdg; // [0.01deg] yaw angle + public: + void Print(void) const + { uint16_t Track = IntAtan2(vy, vx); + printf("GLOBAL_POSITION_INT: %10.3fsec [%+9.5f, %+10.5f]deg %5.1f(%+4.1f)m %3.1fm/s %05.1f/%05.1fdeg %+4.1fm/s\n", + 1e-3*time_boot_ms, 1e-7*lat, 1e-7*lon, 1e-3*alt, 1e-3*relative_alt, + 0.01*IntSqrt((int32_t)vx*vx+(int32_t)vy*vy), 360.0/0x10000*Track, 0.01*hdg, + 0.01*vz); } +} ; + +class MAV_SCALED_PRESSURE +{ public: + uint32_t time_boot_ms; // [msec] + float press_abs; // [hPa] + float press_diff; // [hPa] + int16_t temperature; // [0.01degC] + public: + void Print(void) const + { printf("SCALED_PRESSURE: %8.3f [sec] %7.2f %+4.2f [hPa] %+5.2f [degC]\n", + 1e-3*time_boot_ms, press_abs, press_diff, 0.01*temperature); } // with ms5607 there is a bug/feature: pressure is twice as low +} ; + +class MAV_ADSB_VEHICLE // this message is sent by ADS-B or other traffic receiver +{ public: + uint32_t ICAO_address; // ICAO ID (for ADS-B), other ID's for FLARM/OGN/... + int32_t lat; // [1e-7deg] + int32_t lon; // [1e-7deg] + int32_t altitude; // [mm] + uint16_t heading; // [0.01deg] + uint16_t hor_velocity; // [cm/sec] + int16_t ver_velocity; // [cm/sec] + union + { uint16_t flags; // validity: 1=coord. 2=alt. 4=heading 8=velocity 16=callsign 32=squawk 64=simulated + struct + { bool CoordValid:1; // #0 + bool AltValid:1; // #1 + bool HeadingValid:1; // #2 + bool CallsignValid:1; // #3 + bool VelocityValid:1; // #4 + bool SquawkValid:1; // #5 + bool isSimulated:1; // #6 + } ; + } ; + uint16_t squawk; + uint8_t altitude_type; // 0 = pressure/QNH, 1 = GPS + char callsign[9]; // 8+null + uint8_t emiter_type; // 0=no-info, 1=light, 2=small. 3=large, 4=high-vortex, 5=heavy, 6=manuv, 7=rotor, 9=glider, 10=balloon/airship, 11=parachute, 12=ULM, 14=UAV, 15=space, 19=obstacle + uint8_t tslc; // [sec] time since last communication + public: + void Print(void) const + { printf("ADSB_VEHICLE: %.9s %02X:%08lX [%+9.5f, %+10.5f]deg %5.1fm %3.1fm/s %05.1fdeg %+4.1fm/s %dsec\n", + callsign, (int)emiter_type, (long int)ICAO_address, + 1e-7*lat, 1e-7*lon, 1e-3*altitude, 0.01*hor_velocity, 360.0/0x10000*heading, 0.01*ver_velocity, + tslc); } +} ; + +class COLLISION // this message is sent by the autopilot when it detects a collision threat +{ public: + uint32_t id; + float time_to_minimum_delta; + float altitude_minimum_delta; + float horizontal_minimum_delta; + uint8_t src; + uint8_t action; + uint8_t threat_level; +} ; + +// ============================================================================= + +// https://groups.google.com/forum/#!topic/mavlink/-ipDgVeYSiU +static const uint8_t mavlink_message_crcs[256] = {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 78, 196, 0, 0, 15, 3, 0, 0, 0, 0, 0, 153, 183, 51, 59, 118, 148, 21, 0, 243, 124, 0, 0, 38, 20, 158, 152, 143, 0, 0, 0, 106, 49, 22, 143, 140, 5, 150, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 138, 108, 32, 185, 84, 34, 174, 124, 237, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 25, 226, 46, 29, 223, 85, 6, 229, 203, 1, 195, 109, 168, 181, 47, 72, 131, 127, 0, 103, 154, 178, 200, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 34, 71, 15, 0, 0, 0, 0, 0, 0, 0, 163, 105, 0, 35, 0, 0, 0, 0, 0, 0, 0, 90, 104, 85, 95, 130, 184, 0, 8, 204, 49, 170, 44, 83, 46, 0}; + +class MAV_RxMsg // receiver for the MAV messages +{ public: + static const uint8_t MaxBytes = 65; // max. number of bytes + static const uint8_t Sync = 0xFE; // MAV sync byte + + uint16_t Check; + uint8_t Byte[MaxBytes]; + uint8_t Idx; + + public: + void Clear(void) { Idx=0; CheckInit(Check); } + + uint8_t getLen (void) const { return Byte[1]; } // Payload length (not whole packet length) + uint8_t getSeq (void) const { return Byte[2]; } // Sequence (increments with every new message) + uint8_t getSysID (void) const { return Byte[3]; } // System-ID + uint8_t getCompID(void) const { return Byte[4]; } // Component-ID + uint8_t getMsgID (void) const { return Byte[5]; } // Message-ID + void *getPayload(void) const { return (void *)(Byte+6); } // message (pointer to) Payload + + void Print(bool Ext=1) const + { printf("MAV[%2d:%2d] [%02X] %02X:%02X %3d:", Idx, getLen(), getSeq(), getSysID(), getCompID(), getMsgID() ); + if( (getMsgID()==MAV_ID_STATUSTEXT) && isComplete() ) + { printf("(%d) %s\n", Byte[6], Byte+7); } + else + { for(uint8_t i=6; iPrint(); } + else if(getMsgID()==MAV_ID_SYS_STATUS ) { ((const MAV_SYS_STATUS *)getPayload())->Print(); } + else if(getMsgID()==MAV_ID_SYSTEM_TIME ) { ((const MAV_SYSTEM_TIME *)getPayload())->Print(); } + else if(getMsgID()==MAV_ID_SCALED_PRESSURE ) { ((const MAV_SCALED_PRESSURE *)getPayload())->Print(); } + else if(getMsgID()==MAV_ID_GPS_RAW_INT ) { ((const MAV_GPS_RAW_INT *)getPayload())->Print(); } + else if(getMsgID()==MAV_ID_GLOBAL_POSITION_INT ) { ((const MAV_GLOBAL_POSITION_INT *)getPayload())->Print(); } + else if(getMsgID()==MAV_ID_ADSB_VEHICLE ) { ((const MAV_ADSB_VEHICLE *)getPayload())->Print(); } + else if(getMsgID()==MAV_ID_PARAM_VALUE ) { ((const MAV_PARAM_VALUE *)getPayload())->Print(); } + } + } + } + + uint8_t ProcessByte(uint8_t RxByte) // process a single byte: add to the message or reject + { // printf("Process[%2d] 0x%02X\n", Idx, RxByte); + if(Idx==0) // the very first byte: we only accept SYNC + { if(RxByte==Sync) { Byte[Idx++]=RxByte; return 1; } + else { return 0; } + } + if(Idx==1) // second byte: payload length + { Byte[Idx++]=RxByte; CheckPass(Check, RxByte); return 1; } + if(Idx>=MaxBytes) { Clear(); return 0; } // take following bytes + Byte[Idx++]=RxByte; if(Idx<(getLen()+7)) CheckPass(Check, RxByte); + if(Idx==(getLen()+8)) + { CheckPass(Check, mavlink_message_crcs[getMsgID()]); + // printf("[%2d]", Idx); for(uint8_t i=0; i>8)!=Byte[Idx-1]) ) { Clear(); return 0; } + } + return 1; } + + uint8_t isComplete(void) const { return Idx==(getLen()+8); } + + void static CheckInit(uint16_t &Check) { Check=0xFFFF; } + void static CheckPass(uint16_t &Check, uint8_t Byte) + { uint8_t Tmp = Byte ^ (uint8_t)(Check&0xFF); + Tmp ^= (Tmp<<4); + Check = (Check>>8) ^ ((uint16_t)Tmp<<8) ^ ((uint16_t)Tmp<<3) ^ (Tmp>>4); + // printf("CheckPass: 0x%02X => 0x%04X\n", Byte, Check); + } + + static uint8_t Send(uint8_t Len, uint8_t Seq, uint8_t SysID, uint8_t CompID, uint8_t MsgID, const uint8_t *Payload, void (*SendByte)(char) ) + { uint16_t Check; CheckInit(Check); + (*SendByte)(Sync); + (*SendByte)(Len); CheckPass(Check, Len); + (*SendByte)(Seq); CheckPass(Check, Seq); + (*SendByte)(SysID); CheckPass(Check, SysID); + (*SendByte)(CompID); CheckPass(Check, CompID); + (*SendByte)(MsgID); CheckPass(Check, MsgID); + for(uint8_t Idx=0; Idx>8); + return 8+Len; } + + uint8_t Send(void (*SendByte)(char)) const + { return Send(Byte[1], Byte[2], Byte[3], Byte[4], Byte[5], Byte+6, SendByte); } + +} ; + +// ============================================================================ + +#endif // __MAVLINK_H__ diff --git a/utils/nmea.cpp b/utils/nmea.cpp new file mode 100644 index 0000000..239b271 --- /dev/null +++ b/utils/nmea.cpp @@ -0,0 +1,19 @@ +#include "nmea.h" + +uint8_t NMEA_Check(uint8_t *NMEA, uint8_t Len) // NMEA check-sum +{ uint8_t Check=0; // to be calculated over characters between '$' and '*' but _excluding_ those. + for(uint8_t Idx=0; Idx>4; NMEA[Len+1] = Digit<10 ? Digit+'0':Digit+'A'-10; + Digit=Check&0xF; NMEA[Len+2] = Digit<10 ? Digit+'0':Digit+'A'-10; + return 3; } + +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; } diff --git a/utils/nmea.h b/utils/nmea.h new file mode 100644 index 0000000..94c71cd --- /dev/null +++ b/utils/nmea.h @@ -0,0 +1,213 @@ +#ifndef __NMEA_H__ +#define __NMEA_H__ + +#include + +uint8_t NMEA_Check(uint8_t *NMEA, uint8_t Len); +uint8_t NMEA_AppendCheck(uint8_t *NMEA, uint8_t Len); +inline uint8_t NMEA_AppendCheck(char *NMEA, uint8_t Len) { return NMEA_AppendCheck((uint8_t*)NMEA, Len); } +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 +{ public: + static const uint8_t MaxLen=104; // 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 + uint8_t Parms; // number of commas + uint8_t Parm[MaxParms]; // offset to each comma + uint8_t State; // bits: 0:loading, 1:complete, 2:locked, + uint8_t Check; // check sum: should be a XOR of all bytes between '$' and '*' + + public: + void Clear(void) // Clear the frame: discard all data, ready for next message + { State=0; Len=0; Parms=0; } + + void Send(void (*SendByte)(char) ) const + { for(uint8_t Idx=0; Idx=Parms) return 0; + return Data+Parm[Field]; } + + uint8_t ParmLen(uint8_t Field) const + { if(Field>=Parms) return 0; + if(Field==(Parms-1)) return Len-hasCheck()*3-Parm[Field]; + return Parm[Field+1]-Parm[Field]-1; } + + uint8_t isGP(void) const // GPS sentence ? + { if(Data[1]!='G') return 0; + return Data[2]=='P'; } + + uint8_t isGN(void) const + { if(Data[1]!='G') return 0; + return Data[2]=='N'; } + + uint8_t isGx(void) const // GPS or GLONASS sentence ? + { return Data[1]=='G'; } + + uint8_t isGPRMC(void) const // GPS recomended minimum data + { if(!isGP()) return 0; + if(Data[3]!='R') return 0; + if(Data[4]!='M') return 0; + return Data[5]=='C'; } + + uint8_t isGNRMC(void) const // GPS recomended minimum data + { if(!isGN()) return 0; + if(Data[3]!='R') return 0; + if(Data[4]!='M') return 0; + return Data[5]=='C'; } + + uint8_t isGxRMC(void) const // GPS recomended minimum data + { if(!isGx()) return 0; + if(Data[3]!='R') return 0; + if(Data[4]!='M') return 0; + return Data[5]=='C'; } + + uint8_t isGPGGA(void) const // GPS 3-D fix data + { if(!isGP()) return 0; + if(Data[3]!='G') return 0; + if(Data[4]!='G') return 0; + return Data[5]=='A'; } + + uint8_t isGNGGA(void) const // GPS 3-D fix data + { if(!isGN()) return 0; + if(Data[3]!='G') return 0; + if(Data[4]!='G') return 0; + return Data[5]=='A'; } + + uint8_t isGxGGA(void) const // + { if(!isGx()) return 0; + if(Data[3]!='G') return 0; + if(Data[4]!='G') return 0; + return Data[5]=='A'; } + + uint8_t isGPGSA(void) const // + { if(!isGP()) return 0; + if(Data[3]!='G') return 0; + if(Data[4]!='S') return 0; + return Data[5]=='A'; } + + uint8_t isGNGSA(void) const // + { if(!isGN()) return 0; + if(Data[3]!='G') return 0; + if(Data[4]!='S') return 0; + return Data[5]=='A'; } + + uint8_t isGxGSA(void) const // + { 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; + if(Data[3]!='G') return 0; + if(Data[4]!='S') return 0; + return Data[5]=='V'; } + + uint8_t isGPGSV(void) const // GPS satellite data + { if(!isGP()) return 0; + if(Data[3]!='G') return 0; + if(Data[4]!='S') return 0; + return Data[5]=='V'; } + + uint8_t isGNGSV(void) const // GPS satellite data + { if(!isGN()) 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 + { 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 + { return Data[1]=='P'; } + + uint8_t isPOGN(void) const // OGN dedicated NMEA sentence + { if(Data[1]!='P') return 0; + if(Data[2]!='O') return 0; + if(Data[3]!='G') return 0; + return Data[4]=='N'; } + + uint8_t isPOGNB(void) // barometric report from the OGN tracker + { if(!isPOGN()) return 0; + return Data[5]=='B'; } + + uint8_t isPOGNT(void) // other aircraft position (tracking) report from OGN trackers + { if(!isPOGN()) return 0; + return Data[5]=='T'; } + + uint8_t isPOGNS(void) // tracker parameters setup + { if(!isPOGN()) return 0; + return Data[5]=='S'; } + + uint8_t isPOGNL(void) // log file list request + { if(!isPOGN()) return 0; + return Data[5]=='L'; } + +} ; + +#endif // of __NMEA_H__ diff --git a/utils/ogn.h b/utils/ogn.h new file mode 100644 index 0000000..5b44d87 --- /dev/null +++ b/utils/ogn.h @@ -0,0 +1,1601 @@ +#ifndef __OGN_H__ +#define __OGN_H__ + +#include + +#include +#include +#ifndef __AVR__ +#include +#endif + +#include + +#include "intmath.h" + +#include "bitcount.h" +#include "nmea.h" +#include "ubx.h" +#include "mavlink.h" + +#include "ldpc.h" + +#include "format.h" + +#include "ognconv.h" + +#include "ogn1.h" // OGN v1 +#include "ogn2.h" // OGN v2 +#include "fanet.h" +#include "gdl90.h" + +#include "atmosphere.h" + +// --------------------------------------------------------------------------------------------------------------------- + +template + static bool OGN_isSignif(const OGNx_Packet *Packet, const OGNy_Packet *PrevPacket) // is significant: decide whether to store it or not +{ if(PrevPacket==0) return 1; + int8_t TimeDelta = Packet->Position.Time - PrevPacket->Position.Time; + if(TimeDelta<0) TimeDelta+=60; // [sec] time since previous packet + if(TimeDelta>=20) return 1; // [sec] + int16_t Climb = Packet->DecodeClimbRate(); // [0.1m/s] + if(abs(Climb)>=100) return 1; // if climb/decent rate more than 10m/s + int32_t AltDelta=Packet->DecodeAltitude()-PrevPacket->DecodeAltitude(); // [m] altitude change + if(abs(AltDelta)>=20) return 1; // if more than 50m altitude change + int16_t PrevClimb = PrevPacket->DecodeClimbRate(); // [0.1m/s] + int32_t DistDeltaV = (int32_t)(Climb-PrevClimb)*TimeDelta; // [0.1m] + if(abs(DistDeltaV)>=200) return 1; // if climb doistance >= 20m + int16_t Speed = Packet->DecodeSpeed(); // [0.1m/s] + int16_t PrevSpeed = PrevPacket->DecodeSpeed(); // [0.1m/s] + int32_t DistDeltaH = (int32_t)(Speed-PrevSpeed)*TimeDelta; // [0.1m] speed change * time since last recorded packet + if(abs(DistDeltaH)>=200) return 1; // if extrapolation error more than 50m + int16_t Turn = Packet->DecodeTurnRate(); // [0.1deg/s] + int16_t CFaccel = ((int32_t)Turn*Speed*229+0x10000)>>17; // [0.1m/s^2] centrifugal acceleration in turn + if(abs(CFaccel)>=50) return 1; // CFaccel at or above 5m/s^2 (0.5g) + int16_t PrevTurn = PrevPacket->DecodeTurnRate(); // [0.1deg/s] + int16_t PrevCFaccel = ((int32_t)PrevTurn*PrevSpeed*229+0x10000)>>17; // [0.1m/s^2] + int32_t DistDeltaR = abs(CFaccel-PrevCFaccel)*TimeDelta*TimeDelta/2; // [0.1m] + if(abs(DistDeltaR)>=200) return 1; // [0.1m] + return 0; } + + +// --------------------------------------------------------------------------------------------------------------------- + +template + class OGN_TxPacket // OGN packet with FEC code, like for transmission +{ public: + static const int Words = 7; + static const int Bytes = 26; + + OGNx_Packet Packet; // OGN packet + + uint32_t FEC[2]; // Gallager code: 48 check bits for 160 user bits + + public: + + uint8_t Print(char *Out) + { uint8_t Len=0; + Out[Len++]=HexDigit(Packet.Position.AcftType); Out[Len++]=':'; + Out[Len++]='0'+Packet.Header.AddrType; Out[Len++]=':'; + uint32_t Addr = Packet.Header.Address; + Len+=Format_Hex(Out+Len, (uint8_t)(Addr>>16)); + Len+=Format_Hex(Out+Len, (uint16_t)Addr); + Out[Len++]=' '; + Len+=Format_UnsDec(Out+Len, (uint16_t)Packet.Position.Time, 2); + Out[Len++]=' '; + Len+=Format_Latitude(Out+Len, Packet.DecodeLatitude()); + Out[Len++]=' '; + Len+=Format_Longitude(Out+Len, Packet.DecodeLongitude()); + Out[Len++]=' '; + Len+=Format_UnsDec(Out+Len, (uint32_t)Packet.DecodeAltitude()); Out[Len++]='m'; + Out[Len++]=' '; + Len+=Format_UnsDec(Out+Len, Packet.DecodeSpeed(), 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s'; + Out[Len++]=' '; + Len+=Format_SignDec(Out+Len, Packet.DecodeClimbRate(), 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s'; + Out[Len++]='\n'; Out[Len]=0; + return Len; } + + void Dump(void) const + { printf("%08lX: %08lX %08lX %08lX %08lX [%08lX %04lX] (%d)\n", + (long int)Packet.HeaderWord, (long int)Packet.Data[0], (long int)Packet.Data[1], + (long int)Packet.Data[2], (long int)Packet.Data[3], (long int)FEC[0], + (long int)FEC[1], (int)checkFEC() ); } + + void DumpBytes(void) const + { for(uint8_t Idx=0; Idx no errors, all fine) + + uint8_t *Byte(void) const { return (uint8_t *)&Packet.HeaderWord; } // packet as bytes + uint32_t *Word(void) const { return (uint32_t *)&Packet.HeaderWord; } // packet as words + + void recvBytes(const uint8_t *SrcPacket) { memcpy(Byte(), SrcPacket, Bytes); } // load data bytes e.g. from a demodulator +/* + uint8_t calcErrorPattern(uint8_t *ErrPatt, const uint8_t *OtherPacket) const + { uint8_t ByteIdx=0; const uint32_t *WordPtr=Packet.Word(); + for(uint8_t WordIdx=0; WordIdx=Bytes) break; + ErrPatt[ByteIdx]=Packet[ByteIdx]^Word; ByteIdx++; + Word>>=8; } + } + return Bytes; } +*/ +} ; + +// --------------------------------------------------------------------------------------------------------------------- + +template + class OGN_LogPacket // OGN packet in an internal binary log file +{ public: + static const int Words = 6; + static const int Bytes = 24; + + OGNx_Packet Packet; + uint16_t Time; // [16sec] truncated time + union + { uint8_t Flags; + struct + { uint8_t SNR : 6; // [dB] + uint8_t Prot: 1; + uint8_t Rx : 1; // received or (own) transmitted ? + } ; + } ; + uint8_t Check; // simple control sum + + void setTime(uint32_t EstTime) { Time = EstTime>>4; } + uint32_t getTime(uint32_t EstTime) const + { EstTime>>=4; + int16_t Diff = Time-EstTime; + EstTime += Diff; + return (EstTime<<4)+15; } + + uint8_t calcCheck(void) const + { uint8_t Check=0x5A; + uint8_t *Data = (uint8_t*)&Packet; + for(uint8_t Idx=0; Idx<(Bytes-1); Idx++) + { Check+=Data[Idx]; } + return Check^0xA5; } + void setCheck(void) { Check=calcCheck(); } + bool isCorrect(void) const { return calcCheck()==Check; } +} ; + +// --------------------------------------------------------------------------------------------------------------------- + +template + class OGN_RxPacket // OGN packet with FEC code and some reception info +{ public: + static const int Words = 7; + static const int Bytes = 26; + + OGNx_Packet Packet; + + uint32_t FEC[2]; // Gallager code: 48 check bits for 160 user bits + + union + { uint8_t State; // + struct + { bool Saved :1; // has been already saved in internal storage + bool Ready :1; // is ready for transmission + bool Sent :1; // has already been transmitted out + bool Correct :1; // correctly received or corrected by FEC + uint8_t RxErr:4; // number of bit errors corrected upon reception + } ; + } ; + + uint8_t RxChan; // RF channel where the packet was received + uint8_t RxRSSI; // [-0.5dBm] + uint8_t Rank; // rank: low altitude and weak signal => high rank + + public: + + OGN_RxPacket() { Clear(); } + void Clear(void) { Packet.Clear(); State=0; Rank=0; } + + uint8_t *Byte(void) const { return (uint8_t *)&Packet.HeaderWord; } // packet as bytes + uint32_t *Word(void) const { return (uint32_t *)&Packet.HeaderWord; } // packet as words + + void recvBytes(const uint8_t *SrcPacket) { memcpy(Byte(), SrcPacket, Bytes); } // load data bytes e.g. from a demodulator + + uint8_t calcErrorPattern(uint8_t *ErrPatt, const uint8_t *OtherPacket) const + { uint8_t ByteIdx=0; const uint32_t *WordPtr=Packet.Word(); + for(uint8_t WordIdx=0; WordIdx=Bytes) break; + ErrPatt[ByteIdx]=OtherPacket[ByteIdx]^Word; ByteIdx++; + Word>>=8; } + } + return Bytes; } + + // void calcFEC(void) { LDPC_Encode(&Packet.HeaderWord, FEC); } // calculate the 48-bit parity check + // void calcFEC(const uint32_t ParityGen[48][5]) { LDPC_Encode(&PacketHeaderWord, FEC, ParityGen); } + void calcFEC(void) { LDPC_Encode(Packet.Word()); } // calculate the 48-bit parity check + uint8_t checkFEC(void) const { return LDPC_Check(Packet.Word()); } // returns number of parity checks that fail (0 => no errors, all fine) + + int BitErr(OGN_RxPacket &RefPacket) const // return number of different data bits between this Packet and RefPacket + { return Count1s(Packet.HeaderWord^RefPacket.Packet.HeaderWord) + +Count1s(Packet.Data[0]^RefPacket.Packet.Data[0]) + +Count1s(Packet.Data[1]^RefPacket.Packet.Data[1]) + +Count1s(Packet.Data[2]^RefPacket.Packet.Data[2]) + +Count1s(Packet.Data[3]^RefPacket.Packet.Data[3]) + +Count1s(FEC[0]^RefPacket.FEC[0]) + +Count1s((FEC[1]^RefPacket.FEC[1])&0xFFFF); } + + void calcRelayRank(int32_t RxAltitude) // [0.1m] altitude of reception + { if(Packet.Header.Emergency) { Rank=0xFF; return; } // emergency packets always highest rank + Rank=0; + if(Packet.Header.NonPos) return; // only relay position packets + if(Packet.Position.Time>=60) return; // don't relay packets with unknown time - but maybe we should ? + if(Packet.Header.Relay) return; // no rank for relayed packets (only single relay) + if(RxRSSI>128) // [-0.5dB] weaker signal => higher rank + Rank += (RxRSSI-128)>>2; // 1point/2dB less signal + RxAltitude -= 10*Packet.DecodeAltitude(); // [0.1m] lower altitude => higher rank + if(RxAltitude>0) + Rank += RxAltitude>>9; // 2points/100m of altitude below + int16_t ClimbRate = Packet.DecodeClimbRate(); // [0.1m/s] higher sink rate => higher rank + if(ClimbRate<0) + Rank += (-ClimbRate)>>3; // 1point/0.8m/s of sink + } + + uint8_t ReadPOGNT(const char *NMEA) + { uint8_t Len=0; + if(memcmp(NMEA, "$POGNT,", 7)!=0) return -1; + Len+=7; + + if(NMEA[Len+2]!=',') return -1; + int8_t Time=Read_Dec2(NMEA+Len); + if( (Time<0) || (Time>=60) ) return -1; + Packet.Position.Time=Time; + Len+=3; + + if(NMEA[Len+1]!=',') return -1; + int8_t AcftType=Read_Hex1(NMEA[Len]); + if(AcftType<0) return -1; + Packet.Position.AcftType=AcftType; + Len+=2; + + if(NMEA[Len+1]!=',') return -1; + int8_t AddrType=Read_Hex1(NMEA[Len]); + if((AddrType<0) || (AddrType>=4) ) return -1; + Packet.Header.AddrType=AddrType; + Len+=2; + + uint32_t Addr; + int8_t Ret=Read_Hex(Addr, NMEA+Len); if(Ret<=0) return -1; + if(NMEA[Len+Ret]!=',') return -1; + Packet.Header.Address=Addr; + Len+=Ret+1; + + if(NMEA[Len+1]!=',') return -1; + int8_t Relay=Read_Hex1(NMEA[Len]); + if( (Relay<0) || (Relay>1) ) return -1; + Packet.Header.Relay=Relay; + Len+=2; + + if(NMEA[Len+2]!=',') return -1; + int8_t FixQuality=Read_Hex1(NMEA[Len]); + int8_t FixMode=Read_Hex1(NMEA[Len+1]); + if( (FixQuality<0) || (FixQuality>=4) ) return -1; + if( (FixMode<0) || (FixMode>=2) ) return -1; + Packet.Position.FixQuality=FixQuality; + Packet.Position.FixMode=FixMode; + Len+=3; + + int32_t DOP=0; + Ret=Read_Float1(DOP, NMEA+Len); if(Ret<0) return -1; + if(NMEA[Len+Ret]!=',') return -1; + if(DOP<10) DOP=10; + Packet.EncodeDOP(DOP-10); + Len+=Ret+1; + + if(NMEA[Len+10]!=',') return -1; + int8_t Deg=Read_Dec2(NMEA+Len); if(Deg<0) return -1; + int8_t Min=Read_Dec2(NMEA+Len+2); if(Min<0) return -1; + if(NMEA[Len+4]!='.') return -1; + int16_t Frac=Read_Dec4(NMEA+Len+5); if(Frac<0) return -1; + char Sign=NMEA[Len+9]; + int32_t Lat = Deg*600000 + Min*10000 + Frac; + if(Sign=='N') { } else if(Sign=='S') { Lat=(-Lat); } else return -1; + Packet.EncodeLatitude(Lat); + Len+=11; + + if(NMEA[Len+11]!=',') return -1; + Deg=Read_Dec3(NMEA+Len); if(Deg<0) return -1; + Min=Read_Dec2(NMEA+Len+3); if(Min<0) return -1; + if(NMEA[Len+5]!='.') return -1; + Frac=Read_Dec4(NMEA+Len+6); if(Frac<0) return -1; + Sign=NMEA[Len+10]; + int32_t Lon = Deg*600000 + Min*10000 + Frac; + if(Sign=='E') { } else if(Sign=='W') { Lon=(-Lon); } else return -1; + Packet.EncodeLongitude(Lon); + Len+=12; + + int32_t Alt=0; + Ret=Read_SignDec(Alt, NMEA+Len); if(Ret<0) return -1; + Packet.EncodeAltitude(Alt); + if(NMEA[Len+Ret]!=',') return -1; + Len+=Ret+1; + + int32_t AltDiff=0; + Ret=Read_SignDec(AltDiff, NMEA+Len); if(Ret<0) return -1; + // printf("Ret=%d, AltDiff=%d -> %s\n", Ret, AltDiff, NMEA+Len); + if(Ret==0) Packet.clrBaro(); + else Packet.setBaroAltDiff(AltDiff); + if(NMEA[Len+Ret]!=',') return -1; + Len+=Ret+1; + + int32_t Climb=0; + Ret=Read_Float1(Climb, NMEA+Len); if(Ret<0) return -1; + // printf("Ret=%d, Climb=%d -> %s\n", Ret, Climb, NMEA+Len); + Packet.EncodeClimbRate(Climb); + if(NMEA[Len+Ret]!=',') return -1; + Len+=Ret+1; + + int32_t Speed=0; + Ret=Read_Float1(Speed, NMEA+Len); if(Ret<0) return -1; + Packet.EncodeSpeed(Speed); + if(NMEA[Len+Ret]!=',') return -1; + Len+=Ret+1; + + int32_t Heading=0; + Ret=Read_Float1(Heading, NMEA+Len); if(Ret<0) return -1; + Packet.EncodeHeading(Heading); + if(NMEA[Len+Ret]!=',') return -1; + Len+=Ret+1; + + int32_t TurnRate=0; + Ret=Read_Float1(TurnRate, NMEA+Len); if(Ret<0) return -1; + Packet.EncodeTurnRate(TurnRate); + if(NMEA[Len+Ret]!=',') return -1; + Len+=Ret+1; + + int32_t RSSI=0; + Ret=Read_SignDec(RSSI, NMEA+Len); if(Ret<0) return -1; + RxRSSI=(-2*RSSI); + if(NMEA[Len+Ret]!=',') return -1; + Len+=Ret+1; + + int32_t Err=0; + Ret=Read_SignDec(Err, NMEA+Len); if(Ret<0) return -1; + RxErr=Err; + if(NMEA[Len+Ret]!='*') return -1; + Len+=Ret+1; + + return Len; } + + uint8_t WritePOGNT(char *NMEA) + { uint8_t Len=0; + Len+=Format_String(NMEA+Len, "$POGNT,"); // sentence name + if(Packet.Position.Time<60) + Len+=Format_UnsDec(NMEA+Len, (uint16_t)Packet.Position.Time, 2); // [sec] time + NMEA[Len++]=','; + NMEA[Len++]=HexDigit(Packet.Position.AcftType); // [0..F] aircraft-type: 1=glider, 2=tow plane, etc. + NMEA[Len++]=','; + NMEA[Len++]='0'+Packet.Header.AddrType; // [0..3] address-type: 1=ICAO, 2=FLARM, 3=OGN + NMEA[Len++]=','; + uint32_t Addr = Packet.Header.Address; // [24-bit] address + Len+=Format_Hex(NMEA+Len, (uint8_t)(Addr>>16)); + Len+=Format_Hex(NMEA+Len, (uint16_t)Addr); + NMEA[Len++]=','; + NMEA[Len++]='0'+Packet.Header.Relay; // [0..3] counts retransmissions + NMEA[Len++]=','; + NMEA[Len++]='0'+Packet.Position.FixQuality; // [] fix quality + NMEA[Len++]='0'+Packet.Position.FixMode; // [] fix mode + NMEA[Len++]=','; + Len+=Format_UnsDec(NMEA+Len, (uint16_t)(Packet.DecodeDOP()+10),2,1); // [] Dilution of Precision + NMEA[Len++]=','; + Len+=Format_Latitude(NMEA+Len, Packet.DecodeLatitude()); // [] Latitude + NMEA[Len++]=','; + Len+=Format_Longitude(NMEA+Len, Packet.DecodeLongitude()); // [] Longitude + NMEA[Len++]=','; + Len+=Format_UnsDec(NMEA+Len, (uint32_t)Packet.DecodeAltitude()); // [m] Altitude (by GPS) + NMEA[Len++]=','; + if(Packet.hasBaro()) + Len+=Format_SignDec(NMEA+Len, (int32_t)Packet.getBaroAltDiff()); // [m] Standard Pressure Altitude (by Baro) + NMEA[Len++]=','; + Len+=Format_SignDec(NMEA+Len, Packet.DecodeClimbRate(), 2, 1); // [m/s] climb/sink rate (by GPS or pressure sensor) + NMEA[Len++]=','; + Len+=Format_UnsDec(NMEA+Len, Packet.DecodeSpeed(), 2, 1); // [m/s] ground speed (by GPS) + NMEA[Len++]=','; + Len+=Format_UnsDec(NMEA+Len, Packet.DecodeHeading(), 4, 1); // [deg] heading (by GPS) + NMEA[Len++]=','; + Len+=Format_SignDec(NMEA+Len, Packet.DecodeTurnRate(), 2, 1); // [deg/s] turning rate (by GPS) + NMEA[Len++]=','; + Len+=Format_SignDec(NMEA+Len, -(int16_t)RxRSSI/2); // [dBm] received signal level + NMEA[Len++]=','; + Len+=Format_UnsDec(NMEA+Len, (uint16_t)RxErr); // [bits] corrected transmisison errors + Len+=NMEA_AppendCheckCRNL(NMEA, Len); + NMEA[Len]=0; + return Len; } + + // produce PFLAA sentence (relative position) from a reference point [RefLat, RefLon] + uint8_t WritePFLAA(char *NMEA, uint8_t Status, int32_t RefLat, int32_t RefLon, int32_t RefAlt, uint16_t LatCos) + { int32_t LatDist=0, LonDist=0; + if(Packet.calcDistanceVector(LatDist, LonDist, RefLat, RefLon, LatCos)<0) return 0; // return zero, when distance too large + int32_t AltDist = Packet.DecodeAltitude()-RefAlt; + return WritePFLAA(NMEA, Status, LatDist, LonDist, AltDist, Status); } // return number of formatted characters + + uint8_t WritePFLAA(char *NMEA, uint8_t Status, int32_t LatDist, int32_t LonDist, int32_t AltDist) + { uint8_t Len=0; + Len+=Format_String(NMEA+Len, "$PFLAA,"); // sentence name and alarm-level (but no alarms for trackers) + NMEA[Len++]='0'+Status; + NMEA[Len++]=','; + Len+=Format_SignDec(NMEA+Len, LatDist); + NMEA[Len++]=','; + Len+=Format_SignDec(NMEA+Len, LonDist); + NMEA[Len++]=','; + Len+=Format_SignDec(NMEA+Len, AltDist); // [m] relative altitude + NMEA[Len++]=','; + NMEA[Len++]='0'+Packet.Header.AddrType; // address-type (3=OGN) + NMEA[Len++]=','; + uint32_t Addr = Packet.Header.Address; // [24-bit] address + Len+=Format_Hex(NMEA+Len, (uint8_t)(Addr>>16)); // XXXXXX 24-bit address: RND, ICAO, FLARM, OGN + Len+=Format_Hex(NMEA+Len, (uint16_t)Addr); + NMEA[Len++]=','; + Len+=Format_UnsDec(NMEA+Len, Packet.DecodeHeading(), 4, 1); // [deg] heading (by GPS) + NMEA[Len++]=','; + Len+=Format_SignDec(NMEA+Len, Packet.DecodeTurnRate(), 2, 1); // [deg/sec] turn rate + NMEA[Len++]=','; + Len+=Format_UnsDec(NMEA+Len, Packet.DecodeSpeed(), 2, 1); // [approx. m/s] ground speed + NMEA[Len++]=','; + Len+=Format_SignDec(NMEA+Len, Packet.DecodeClimbRate(), 2, 1); // [m/s] climb/sink rate + NMEA[Len++]=','; + NMEA[Len++]=HexDigit(Packet.Position.AcftType); // [0..F] aircraft-type: 1=glider, 2=tow plane, etc. + Len+=NMEA_AppendCheckCRNL(NMEA, Len); + NMEA[Len]=0; + return Len; } // return number of formatted characters + + void Print(void) const + { printf("[%02d/%+6.1fdBm/%2d] ", RxChan, -0.5*RxRSSI, RxErr); + Packet.Print(); } + + uint8_t Print(char *Out) const + { uint8_t Len=0; + Out[Len++]=HexDigit(Packet.Position.AcftType); Out[Len++]=':'; + Out[Len++]='0'+Packet.Header.AddrType; Out[Len++]=':'; + uint32_t Addr = Packet.Header.Address; + Len+=Format_Hex(Out+Len, (uint8_t)(Addr>>16)); + Len+=Format_Hex(Out+Len, (uint16_t)Addr); + Out[Len++]=' '; + Len+=Format_SignDec(Out+Len, -(int16_t)RxRSSI/2); Out[Len++]='d'; Out[Len++]='B'; Out[Len++]='m'; + Out[Len++]=' '; + Len+=Format_UnsDec(Out+Len, (uint16_t)Packet.Position.Time, 2); + Out[Len++]=' '; + Len+=Format_Latitude(Out+Len, Packet.DecodeLatitude()); + Out[Len++]=' '; + Len+=Format_Longitude(Out+Len, Packet.DecodeLongitude()); + Out[Len++]=' '; + Len+=Format_UnsDec(Out+Len, (uint32_t)Packet.DecodeAltitude()); Out[Len++]='m'; + Out[Len++]=' '; + Len+=Format_UnsDec(Out+Len, Packet.DecodeSpeed(), 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s'; + Out[Len++]=' '; + Len+=Format_SignDec(Out+Len, Packet.DecodeClimbRate(), 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s'; + Out[Len++]='\n'; Out[Len]=0; + return Len; } + + void Dump(void) const + { printf("%08lX: %08lX %08lX %08lX %08lX [%08lX %04lX] (%d)\n", + (long int)Packet.HeaderWord, (long int)Packet.Data[0], (long int)Packet.Data[1], + (long int)Packet.Data[2], (long int)Packet.Data[3], + (long int)FEC[0], (long int)FEC[1], (int)checkFEC() ); } + + void DumpBytes(void) const + { for(uint8_t Idx=0; Idx<26; Idx++) + { printf(" %02X", Packet.Byte()[Idx]); } + printf(" (%d)\n", LDPC_Check(Packet.Byte())); } + +} ; + +#ifdef WITH_PPM + +class OGN_PPM_Packet // OGN packet with FEC code and some reception info +{ public: + static const int Words = 12; + + OGN1_Packet Packet; + + uint32_t FEC[7]; // Gallager code: 194 check bits for 160 user bits + + public: + + void calcFEC(void) { LDPC_Encode_n354k160(Packet.Word()); } // calculate the 48-bit parity check + uint8_t checkFEC(void) const { return LDPC_Check_n354k160(Packet.Word()); } // returns number of parity checks that fail (0 => no errors, all fine) + + uint32_t *Word(void) const { return Packet.Word(); } + + void Dump(void) const + { printf("%08lX: %08lX %08lX %08lX %08lX [%08lX %08lX %08lX %08lX %08lX %08lX %01lX] (%d)\n", + (long int)Packet.HeaderWord, (long int)Packet.Data[0], (long int)Packet.Data[1], + (long int)Packet.Data[2], (long int)Packet.Data[3], + (long int)FEC[0], (long int)FEC[1], (long int)FEC[2], (long int)FEC[2], + (long int)FEC[4], (long int)FEC[5], (long int)FEC[6], (int)checkFEC() ); } + + static uint8_t Gray(uint8_t Binary) { return Binary ^ (Binary>>1); } + + static uint8_t Binary(uint8_t Gray) + { Gray = Gray ^ (Gray >> 4); + Gray = Gray ^ (Gray >> 2); + Gray = Gray ^ (Gray >> 1); + return Gray; } + + uint8_t getSymbol(uint16_t Idx) + { if(Idx>=59) return 0xFF; + uint32_t *Word = Packet.Word(); + uint8_t Symbol=0; uint8_t SymbMask=1; + for(uint8_t Bit=0; Bit<6; Bit++, Idx+=59 ) + { uint8_t WordIdx=Idx>>5; uint8_t BitIdx=Idx&31; + uint32_t Mask=1; Mask<<=BitIdx; + if(Word[WordIdx]&Mask) Symbol|=SymbMask; + SymbMask<<=1; } + return Gray(Symbol); } + + void clear(void) + { memset(Packet.Word(), 0, Words*4); } + + void setSymbol(uint16_t Idx, uint8_t Symbol) + { if(Idx>=59) return; + Symbol = Binary(Symbol); + uint32_t *Word = Packet.Word(); + for(uint8_t Bit=0; Bit<6; Bit++, Idx+=59 ) + { if(Symbol&1) + { uint8_t WordIdx=Idx>>5; uint8_t BitIdx=Idx&31; + uint32_t Mask=1; Mask<<=BitIdx; + Word[WordIdx]|=Mask; } + Symbol>>=1; } + } + +} ; + +#endif // WITH_PPM + +// --------------------------------------------------------------------------------------------------------------------- + +template + class OGN_PrioQueue +{ public: + // static const uint8_t Size = 8; // number of packets kept + OGN_RxPacket Packet[Size]; // OGN packets + uint16_t Sum; // sum of all ranks + uint8_t Low, LowIdx; // the lowest rank and the index of it + + public: + void Clear(void) // clear (reset) the queue + { for(uint8_t Idx=0; Idx * operator [](uint8_t Idx) { return Packet+Idx; } + + uint8_t getNew(void) // get (index of) a free or lowest rank packet + { Sum-=Packet[LowIdx].Rank; Packet[LowIdx].Rank=0; Low=0; return LowIdx; } // remove old packet from the rank sum + + OGN_RxPacket *addNew(uint8_t NewIdx) // add the new packet to the queue + { OGN_RxPacket *Prev = 0; + uint32_t AddressAndType = Packet[NewIdx].Packet.getAddressAndType(); // get ID of this packet: ID is address-type and address (2+24 = 26 bits) + for(uint8_t Idx=0; IdxRankIdx) return Idx; } + return Rand%Size; } + + void reCalc(void) // find the lowest rank and calc. the sum of all ranks + { Sum=Low=Packet[0].Rank; LowIdx=0; // take minimum at the first slot + for(uint8_t Idx=1; Idx=60) clean(Idx); + } + } + + void clean(uint8_t Idx) // clean given slot + { Sum-=Packet[Idx].Rank; Packet[Idx].Rank=0; Low=0; LowIdx=Idx; } + + void decrRank(uint8_t Idx, uint8_t Decr=1) // decrement rank of given slot + { uint8_t Rank=Packet[Idx].Rank; if(Rank==0) return; // if zero already: do nothing + if(Decr>Rank) Decr=Rank; // if to decrement by more than the rank already: reduce the decrement + Rank-=Decr; Sum-=Decr; // decrement the rank and the sum of ranks + if(Rank=0) && (Min>=0) && (Sec>=0); } // all data must have been correctly read: negative means not correctly read) + + bool isDateValid(void) const // is the GPS date valid ? + { return (Year>=0) && (Month>=0) && (Day>=0); } + + uint8_t incrTime(void) // increment HH:MM:SS by one second + { Sec++; if(Sec<60) return 0; + Sec=0; + Min++; if(Min<60) return 0; + Min=0; + Hour++; if(Hour<24) return 0; + Hour=0; + return 1; } // return 1 if date needs to be incremented + + uint8_t decrTime(void) // decrement HH:MM:SS by one second + { if(Sec>0) { Sec--; return 0; } + Sec=60; + if(Min>60) { Min--; return 0; } + Min=60; + if(Hour>0) { Hour--; return 0; } + Hour=24; + return 1; } // return 1 if date needs to be decremented + + int32_t calcTimeDiff(GPS_Time &RefTime) const + { int32_t TimeDiff = ((int32_t)Min*6000+(int16_t)Sec*100+FracSec) - ((int32_t)RefTime.Min*6000+(int16_t)RefTime.Sec*100+RefTime.FracSec); + if(TimeDiff<(-180000)) TimeDiff+=360000; // wrap-around 60min + else if(TimeDiff>=180000) TimeDiff-=360000; + return TimeDiff; } // [0.01s] + + uint8_t MonthDays(void) // number of days per month + { const uint16_t Table = 0x0AD5; // 1010 1101 0101 0=30days, 1=31days + // const uint8_t Table[12] = { 31,28,31,30, 31,30,31,31, 30,31,30,31 }; + if( (Month<1) || (Month>12) ) return 0; + if( Month==2) return 28+isLeapYear(); + return 30 + ((Table>>(Month-1))&1); } + + void incrDate(int8_t Days=1) // increment YY:MM:DD + { uint8_t DaysPerMonth = MonthDays(); + Day+=Days; if(Day<=DaysPerMonth) return; + Day-=DaysPerMonth; Month++; if(Month<=12) return; + Month=1; Year++; } + + void decrDate(void) // decrement YY:MM:DD + { if(Day>1) { Day--; return; } + if(Month>1) { Month--; Day=MonthDays(); return; } + Year--; Month=12; Day=MonthDays(); return; } + + void incrTimeDate(void) { if(incrTime()) incrDate(); } + void decrTimeDate(void) { if(decrTime()) decrDate(); } + + void copyTime(GPS_Time &RefTime) // copy HH:MM:SS.SSS from another record + { FracSec = RefTime.FracSec; + Sec = RefTime.Sec; + Min = RefTime.Min; + Hour = RefTime.Hour; } + + void copyDate(GPS_Time &RefTime) // copy YY:MM:DD from another record + { Day = RefTime.Day; + Month = RefTime.Month; + Year = RefTime.Year; } + + void copyTimeDate(GPS_Time &RefTime) { copyTime(RefTime); copyDate(RefTime); } + uint32_t getDayTime(void) const + { return Times60((uint32_t)(Times60((uint16_t)Hour) + Min)) + Sec; } // this appears to save about 100 bytes of code + // return (uint32_t)Hour*SecsPerHour + (uint16_t)Min*SecsPerMin + Sec; } // compared to this line + + uint32_t getUnixTime(void) const // return the Unix timestamp (tested 2000-2037) + { uint16_t Days = DaysSinceYear2000() + DaysSimce1jan(); + return Times60(Times60(Times24((uint32_t)(Days+10957)))) + getDayTime(); } + + uint32_t getFatTime(void) const // return timestamp in FAT format + { uint16_t Date = ((uint16_t)(Year+20)<<9) | ((uint16_t)Month<<5) | Day; + uint16_t Time = ((uint16_t)Hour<<11) | ((uint16_t)Min<<5) | (Sec>>1); + return ((uint32_t)Date<<16) | Time; } + + void setUnixTime(uint32_t Time) // works except for the 1.1.2000 + { uint32_t Days = Time/SecsPerDay; // [day] since 1970 + uint32_t DayTime = Time - Days*SecsPerDay; // [sec] time-of-day + Hour = DayTime/SecsPerHour; DayTime -= (uint32_t)Hour*SecsPerHour; // + Min = DayTime/SecsPerMin; DayTime -= (uint16_t)Min*SecsPerMin; + Sec = DayTime; + FracSec=0; + Days -= 10957+1; // [day] since 2000 minus 1 day + Year = (Days*4)/((365*4)+1); // [year] since 1970 + Days -= 365*Year + (Year/4); + Month = Days/31; + Day = Days-(uint16_t)Month*31+1; Month++; + uint32_t CheckTime = getUnixTime(); + if(CheckTime2) ) Days++; + return Days; } +#else + int16_t DaysSimce1jan(void) const // 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 + { static const uint8_t DaysDiff[12] = { 0, 3, 3, 6, 8, 11, 13, 16, 19, 21, 24, 26 } ; + uint16_t Days = (uint16_t)(Month-1)*28 + DaysDiff[Month-1] + Day - 1; + if(isLeapYear() && (Month>2) ) Days++; + return Days; } +#endif + + uint16_t DaysSinceYear2000(void) const + { uint16_t Days = 365*Year; + if(Year>0) Days += ((Year-1)>>2)+1; + return Days; } + + template + static Type Times60(Type X) { return ((X<<4)-X)<<2; } + + template + static Type Times28(Type X) { X+=(X<<1)+(X<<2); return X<<2; } + + template + static Type Times24(Type X) { X+=(X<<1); return X<<3; } + +} ; + +class GPS_Position: public GPS_Time +{ public: + + union + { uint16_t Flags; // bit #0 = GGA and RMC had same Time + struct + { bool hasGPS :1; // all required GPS information has been supplied (but this is not the GPS lock status) + bool hasBaro :1; // pressure sensor information: pressure, standard pressure altitude, temperature, humidity + // bool hasHum :1; // + bool isReady :1; // is ready for the following treaement + bool Sent :1; // has been transmitted + bool hasTime :1; // Time has been supplied + bool hasRMC :1; // GxRMC has been supplied + bool hasGGA :1; // GxGGA has been supplied + bool hasGSA :1; // GxGSA has been supplied + // bool hasGSV :1; + } ; + } ; + + // uint16_t SatSNRsum; // sum of cSNR from GPGSV + // uint8_t SatSNRcount; // count of satellites from GPGSV + + int8_t FixQuality; // 0 = none, 1 = GPS, 2 = Differential GPS (can be WAAS) + int8_t FixMode; // 0 = not set (from GSA) 1 = none, 2 = 2-D, 3 = 3-D + int8_t Satellites; // number of active satellites + + uint8_t PDOP; // [0.1] dilution of precision + uint8_t HDOP; // [0.1] horizontal dilution of precision + uint8_t VDOP; // [0.1] vertical dilution of precision + + int16_t Speed; // [0.1 m/s] speed-over-ground + int16_t Heading; // [0.1 deg] heading-over-ground + + int16_t ClimbRate; // [0.1 meter/sec) + int16_t TurnRate; // [0.1 deg/sec] + + int32_t Altitude; // [0.1 meter] height above Geoid (sea level) + + int32_t Latitude; // [0.0001/60 deg] about 0.18m accuracy (to convert to u-Blox GPS 1e-7deg units mult by 50/3) + int32_t Longitude; // [0.0001/60 deg] + int16_t GeoidSeparation; // [0.1 meter] difference between Geoid and Ellipsoid + uint16_t LatitudeCosine; // [2^-12] Latitude cosine for distance calculation + + uint32_t Pressure; // [0.25 Pa] from pressure sensor + int32_t StdAltitude; // [0.1 meter] standard pressure altitude (from the pressure sensor and atmosphere calculator) + int16_t Temperature; // [0.1 degC] + int16_t Humidity; // [0.1%] relative humidity + int16_t Accel; // [0.1m/s^2] acceleration along the track + uint16_t Seq; // sequencial number to track GPS positions in a pipe + + public: + + GPS_Position() { Clear(); } + + void Clear(void) + { Flags=0; FixQuality=0; FixMode=0; + PDOP=0; HDOP=0; VDOP=0; + // SatSNRsum=0; SatSNRcount=0; + setDefaultDate(); setDefaultTime(); + Latitude=0; Longitude=0; LatitudeCosine=3000; + Altitude=0; GeoidSeparation=0; + Speed=0; Heading=0; ClimbRate=0; TurnRate=0; + Temperature=0; Pressure=0; StdAltitude=0; Humidity=0; } + + bool isValid(void) const // is GPS data is valid = GPS lock + { if(!isTimeValid()) return 0; // is GPS time valid/present ? + if(!isDateValid()) return 0; // is GPS date valid/present ? + if(FixQuality==0) return 0; // Fix quality must be 1=GPS or 2=DGPS + if(FixMode==1) return 0; // if GSA says "no lock" (when GSA is not there, FixMode=0) + if(Satellites<=0) return 0; // if number of satellites none or invalid + return 1; } +/* + void copyTime(GPS_Position &RefPosition) // copy HH:MM:SS.SSS from another record + { FracSec = RefPosition.FracSec; + Sec = RefPosition.Sec; + Min = RefPosition.Min; + Hour = RefPosition.Hour; } + + void copyDate(GPS_Position &RefPosition) // copy YY:MM:DD from another record + { Day = RefPosition.Day; + Month = RefPosition.Month; + Year = RefPosition.Year; } + + void copyTimeDate(GPS_Position &RefPosition) { copyTime(RefPosition); copyDate(RefPosition); } +*/ +#ifndef __AVR__ // there is not printf() with AVR + void PrintDateTime(void) const { printf("%02d.%02d.%04d %02d:%02d:%05.2f", Day, Month, 2000+Year, Hour, Min, Sec+0.01*FracSec ); } + void PrintTime(void) const { printf("%02d:%02d:%05.2f", Hour, Min, Sec+0.01*FracSec ); } + + int PrintDateTime(char *Out) const { return sprintf(Out, "%02d.%02d.%04d %02d:%02d:%02d.%02d", Day, Month, Year, Hour, Min, Sec, FracSec ); } + int PrintTime(char *Out) const { return sprintf(Out, "%02d:%02d:%02d.%02d", Hour, Min, Sec, FracSec ); } + + void Print(void) const + { printf("Time/Date = "); PrintDateTime(); printf(" "); // printf(" = %10ld.%03dsec\n", (long int)UnixTime, mSec); + printf("FixQuality/Mode=%d/%d: %d satellites DOP/H/V=%3.1f/%3.1f/%3.1f ", FixQuality, FixMode, Satellites, 0.1*PDOP, 0.1*HDOP, 0.1*VDOP); + printf("FixQuality=%d: %d satellites HDOP=%3.1f ", FixQuality, Satellites, 0.1*HDOP); + printf("Lat/Lon/Alt = [%+10.6f,%+10.6f]deg %+3.1f(%+3.1f)m LatCosine=%+6.3f ", 0.0001/60*Latitude, 0.0001/60*Longitude, 0.1*Altitude, 0.1*GeoidSeparation, 1.0/(1<<12)*LatitudeCosine); + printf("Speed/Heading = %3.1fm/s %05.1fdeg ", 0.1*Speed, 0.1*Heading); + printf("Climb = %+5.1fm/s Turn = %+5.1fdeg/sec\n", 0.1*ClimbRate, 0.1*TurnRate); + } + + int Print(char *Out) const + { int Len=0; + Len+=sprintf(Out+Len, "Time/Date = "); Len+=PrintDateTime(Out+Len); printf(" "); // Len+=sprintf(Out+Len, " = %10ld.%02dsec\n", (long int)UnixTime, FracSec); + Len+=sprintf(Out+Len, "FixQuality/Mode=%d/%d: %d satellites DOP/H/V=%3.1f/%3.1f/%3.1f ", FixQuality, FixMode, Satellites, 0.1*PDOP, 0.1*HDOP, 0.1*VDOP); + Len+=sprintf(Out+Len, "Lat/Lon/Alt = [%+10.6f,%+10.6f]deg %+3.1f(%+3.1f)m ", 0.0001/60*Latitude, 0.0001/60*Longitude, 0.1*Altitude, 0.1*GeoidSeparation); + Len+=sprintf(Out+Len, "Speed/Heading = %3.1fm/s %05.1fdeg\n", 0.1*Speed, 0.1*Heading); + return Len; } + + void PrintLine(void) const + { PrintTime(); + printf(" %d/%d/%02d/%4.1f/%4.1f/%4.1f", FixQuality, FixMode, Satellites, 0.1*PDOP, 0.1*HDOP, 0.1*VDOP); + printf(" [%+10.6f,%+10.6f]deg %+3.1f(%+3.1f)m", 0.0001/60*Latitude, 0.0001/60*Longitude, 0.1*Altitude, 0.1*GeoidSeparation); + printf(" %4.1fm/s %05.1fdeg", 0.1*Speed, 0.1*Heading); + printf("\n"); } + + int PrintLine(char *Out) const + { int Len=0; // PrintDateTime(Out); + Out[Len++]=hasTime?'T':'_'; + Out[Len++]=hasGPS ?'G':'_'; + Out[Len++]=hasBaro?'B':'_'; + Out[Len++]=hasRMC ?'R':'_'; + Out[Len++]=hasGGA ?'G':'_'; + Out[Len++]=hasGSA ?'G':'_'; + Out[Len++]=isValid() ?'V':'_'; + Out[Len++]=isTimeValid() ?'T':'_'; + Out[Len++]=isDateValid() ?'D':'_'; + + Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, (uint16_t)Hour, 2); + Out[Len++]=':'; Len+=Format_UnsDec(Out+Len, (uint16_t)Min, 2); + Out[Len++]=':'; Len+=Format_UnsDec(Out+Len, (uint16_t)Sec, 2); + Out[Len++]='.'; Len+=Format_UnsDec(Out+Len, (uint16_t)FracSec, 2); + Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, (uint16_t)FixQuality); + Out[Len++]='/'; Len+=Format_UnsDec(Out+Len, (uint16_t)FixMode); + Out[Len++]='/'; Len+=Format_UnsDec(Out+Len, (uint16_t)Satellites, 2); + Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, PDOP, 2, 1); + Out[Len++]='/'; Len+=Format_UnsDec(Out+Len, HDOP, 2, 1); + Out[Len++]='/'; Len+=Format_UnsDec(Out+Len, VDOP, 2, 1); + Out[Len++]=' '; + Out[Len++]='['; Len+=Format_SignDec(Out+Len, Latitude/6, 7, 5); + Out[Len++]=','; Len+=Format_SignDec(Out+Len, Longitude/6, 8, 5); + Out[Len++]=']'; Out[Len++]='d'; Out[Len++]='e'; Out[Len++]='g'; + Out[Len++]=' '; Len+=Format_SignDec(Out+Len, Altitude, 4, 1); Out[Len++]='m'; + Out[Len++]='/'; Len+=Format_SignDec(Out+Len, GeoidSeparation, 4, 1); Out[Len++]='m'; + Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Speed, 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s'; + Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Heading, 4, 1); Out[Len++]='d'; Out[Len++]='e'; Out[Len++]='g'; + if(hasBaro) + { Out[Len++]=' '; Len+=Format_SignDec(Out+Len, Temperature, 2, 1); Out[Len++]='C'; + Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Pressure/4 ); Out[Len++]='P'; Out[Len++]='a'; + Out[Len++]=' '; Len+=Format_SignDec(Out+Len, StdAltitude, 2, 1); Out[Len++]='m'; } + Out[Len++]='\n'; Out[Len++]=0; return Len; } +#endif // __AVR__ + + int8_t ReadUBX(UBX_RxMsg &RxMsg) + { if(!RxMsg.isNAV()) return 0; + if(RxMsg.isNAV_TIMEUTC()) return ReadUBX_NAV_TIMEUTC(RxMsg); + if(RxMsg.isNAV_POSLLH() ) return ReadUBX_NAV_POSLLH(RxMsg); + if(RxMsg.isNAV_SOL() ) return ReadUBX_NAV_SOL(RxMsg); + return 0; } + + int8_t ReadUBX_NAV_TIMEUTC(UBX_RxMsg &RxMsg) + { UBX_NAV_TIMEUTC *TIMEUTC = (UBX_NAV_TIMEUTC *)(RxMsg.Byte); + Year = TIMEUTC->year-2000; + Month = TIMEUTC->month; + Day = TIMEUTC->day; + Hour = TIMEUTC->hour; + Min = TIMEUTC->min; + Sec = TIMEUTC->sec; + if(TIMEUTC->nano<0) { decrTimeDate(); TIMEUTC->nano+=1000000000; } + FracSec = (TIMEUTC->nano+5000000)/10000000; // [ms] + if(FracSec>=100) { incrTimeDate(); FracSec-=100; } + hasTime = (TIMEUTC->valid&0x02)!=0; + return hasTime; } + + int8_t ReadUBX_NAV_POSLLH(UBX_RxMsg &RxMsg) + { UBX_NAV_POSLLH *POSLLH = (UBX_NAV_POSLLH *)(RxMsg.Byte); + Latitude = 3*(int64_t)POSLLH->lat/50; + Longitude = 3*(int64_t)POSLLH->lon/50; + Altitude = POSLLH->hMSL/100; + GeoidSeparation = (POSLLH->height-POSLLH->hMSL)/100; + hasGPS = 1; + return 1; } + + int8_t ReadUBX_NAV_SOL(UBX_RxMsg &RxMsg) + { UBX_NAV_SOL *SOL = (UBX_NAV_SOL *)(RxMsg.Byte); + FixMode = SOL->gpsFix; + FixQuality = FixMode>=2; + PDOP = SOL->PDOP/10; + Satellites = SOL->numSV; + return 1; } + + int8_t ReadNMEA(NMEA_RxMsg &RxMsg) + { if(RxMsg.isGPGGA()) return ReadGGA(RxMsg); + if(RxMsg.isGNGGA()) return ReadGGA(RxMsg); + if(RxMsg.isGPRMC()) return ReadRMC(RxMsg); + if(RxMsg.isGNRMC()) return ReadRMC(RxMsg); + if(RxMsg.isGPGSA()) return ReadGSA(RxMsg); + if(RxMsg.isGNGSA()) return ReadGSA(RxMsg); + // if(RxMsg.isGxGSV()) return ReadGSV(RxMsg); + return 0; } + + int8_t ReadNMEA(const char *NMEA) + { int Err=0; + Err=ReadGGA(NMEA); if(Err!=(-1)) return Err; + Err=ReadGSA(NMEA); if(Err!=(-1)) return Err; + Err=ReadRMC(NMEA); if(Err!=(-1)) return Err; + // Err=ReadGSV(NMEA); if(Err!=(-1)) return Err; + return 0; } + + int8_t ReadGGA(NMEA_RxMsg &RxMsg) + { if(RxMsg.Parms<14) return -2; // no less than 14 paramaters + hasGPS = ReadTime((const char *)RxMsg.ParmPtr(0))>0; // read time and check if same as the RMC says + FixQuality =Read_Dec1(*RxMsg.ParmPtr(5)); if(FixQuality<0) FixQuality=0; // fix quality: 0=invalid, 1=GPS, 2=DGPS + Satellites=Read_Dec2((const char *)RxMsg.ParmPtr(6)); // number of satellites + if(Satellites<0) Satellites=Read_Dec1(RxMsg.ParmPtr(6)[0]); + if(Satellites<0) Satellites=0; + ReadHDOP((const char *)RxMsg.ParmPtr(7)); // horizontal dilution of precision + ReadLatitude(*RxMsg.ParmPtr(2), (const char *)RxMsg.ParmPtr(1)); // Latitude + ReadLongitude(*RxMsg.ParmPtr(4), (const char *)RxMsg.ParmPtr(3)); // Longitude + ReadAltitude(*RxMsg.ParmPtr(9), (const char *)RxMsg.ParmPtr(8)); // Altitude + ReadGeoidSepar(*RxMsg.ParmPtr(11), (const char *)RxMsg.ParmPtr(10)); // Geoid separation + calcLatitudeCosine(); + return 1; } + + uint8_t WriteGGA(char *GGA) + { uint8_t Len=0; + Len+=Format_String(GGA+Len, "$GPGGA,"); + Len+=Format_UnsDec(GGA+Len, (uint16_t)Hour, 2); + Len+=Format_UnsDec(GGA+Len, (uint16_t)Min, 2); + Len+=Format_UnsDec(GGA+Len, (uint16_t)Sec, 2); + GGA[Len++]='.'; + Len+=Format_UnsDec(GGA+Len, (uint16_t)FracSec, 2); + GGA[Len++]=','; + Len+=Format_Latitude(GGA+Len, Latitude); + GGA[Len]=GGA[Len-1]; GGA[Len-1]=','; Len++; + GGA[Len++]=','; + Len+=Format_Longitude(GGA+Len, Longitude); + GGA[Len]=GGA[Len-1]; GGA[Len-1]=','; Len++; + GGA[Len++]=','; + GGA[Len++]='0'+FixQuality; + GGA[Len++]=','; + Len+=Format_UnsDec(GGA+Len, (uint16_t)Satellites); + GGA[Len++]=','; + Len+=Format_UnsDec(GGA+Len, (uint16_t)HDOP, 2, 1); + GGA[Len++]=','; + Len+=Format_SignDec(GGA+Len, Altitude, 3, 1); + GGA[Len++]=','; + GGA[Len++]='M'; + GGA[Len++]=','; + Len+=Format_SignDec(GGA+Len, GeoidSeparation, 3, 1); + GGA[Len++]=','; + GGA[Len++]='M'; + GGA[Len++]=','; + GGA[Len++]=','; + Len += NMEA_AppendCheckCRNL(GGA, Len); + GGA[Len]=0; + return Len; } + + int8_t ReadGGA(const char *GGA) + { if( (memcmp(GGA, "$GPGGA", 6)!=0) && (memcmp(GGA, "$GNGGA", 6)!=0) ) return -1; // check if the right sequence + uint8_t Index[20]; if(IndexNMEA(Index, GGA)<14) return -2; // index parameters and check the sum + hasGPS = ReadTime(GGA+Index[0])>0; + FixQuality =Read_Dec1(GGA[Index[5]]); if(FixQuality<0) FixQuality=0; // fix quality + Satellites=Read_Dec2(GGA+Index[6]); // number of satellites + if(Satellites<0) Satellites=Read_Dec1(GGA[Index[6]]); + if(Satellites<0) Satellites=0; + ReadHDOP(GGA+Index[7]); // horizontal dilution of precision + ReadLatitude( GGA[Index[2]], GGA+Index[1]); // Latitude + ReadLongitude(GGA[Index[4]], GGA+Index[3]); // Longitude + ReadAltitude(GGA[Index[9]], GGA+Index[8]); // Altitude + ReadGeoidSepar(GGA[Index[11]], GGA+Index[10]); // Geoid separation + calcLatitudeCosine(); + return 1; } + + int8_t ReadGSA(NMEA_RxMsg &RxMsg) + { if(RxMsg.Parms<17) return -1; + FixMode =Read_Dec1(*RxMsg.ParmPtr(1)); if(FixMode<0) FixMode=0; // fix mode + ReadPDOP((const char *)RxMsg.ParmPtr(14)); // total dilution of precision + ReadHDOP((const char *)RxMsg.ParmPtr(15)); // horizontal dilution of precision + ReadVDOP((const char *)RxMsg.ParmPtr(16)); // vertical dilution of precision + return 1; } + + int8_t ReadGSA(const char *GSA) + { if( (memcmp(GSA, "$GPGSA", 6)!=0) && (memcmp(GSA, "$GNGSA", 6)!=0) ) return -1; // check if the right sequence + uint8_t Index[20]; if(IndexNMEA(Index, GSA)<17) return -2; // index parameters and check the sum + FixMode =Read_Dec1(GSA[Index[1]]); if(FixMode<0) FixMode=0; + ReadPDOP(GSA+Index[14]); + ReadHDOP(GSA+Index[15]); + ReadVDOP(GSA+Index[16]); + return 1; } +/* + int8_t ReadGSV(NMEA_RxMsg &RxMsg) + { // + return 1; } + + int8_t ReadGSV(const char *GSV) + { if( (memcmp(GSV, "$GPGSV", 6)!=0) && (memcmp(GSV, "$GNGSV", 6)!=0) ) return -1; // check if the right sequence + uint8_t Index[24]; if(IndexNMEA(Index, GSV)<20) return -2; // index parameters and check the sum + // + return 1; } +*/ + int ReadRMC(NMEA_RxMsg &RxMsg) + { if(RxMsg.Parms<11) return -2; // no less than 12 parameters + hasGPS = ReadTime((const char *)RxMsg.ParmPtr(0))>0; // read time and check if same as the GGA says + if(ReadDate((const char *)RxMsg.ParmPtr(8))<0) setDefaultDate(); // date + ReadLatitude(*RxMsg.ParmPtr(3), (const char *)RxMsg.ParmPtr(2)); // Latitude + ReadLongitude(*RxMsg.ParmPtr(5), (const char *)RxMsg.ParmPtr(4)); // Longitude + ReadSpeed((const char *)RxMsg.ParmPtr(6)); // Speed + ReadHeading((const char *)RxMsg.ParmPtr(7)); // Heading + calcLatitudeCosine(); + return 1; } + + int8_t ReadRMC(const char *RMC) + { if( (memcmp(RMC, "$GPRMC", 6)!=0) && (memcmp(RMC, "$GNRMC", 6)!=0) ) return -1; // check if the right sequence + uint8_t Index[20]; if(IndexNMEA(Index, RMC)<11) return -2; // index parameters and check the sum + hasGPS = ReadTime(RMC+Index[0])>0; + if(ReadDate(RMC+Index[8])<0) setDefaultDate(); + ReadLatitude( RMC[Index[3]], RMC+Index[2]); + ReadLongitude(RMC[Index[5]], RMC+Index[4]); + ReadSpeed(RMC+Index[6]); + ReadHeading(RMC+Index[7]); + calcLatitudeCosine(); + return 1; } +/* + int32_t calcTimeDiff(GPS_Position &RefPos) const + { int32_t TimeDiff = ((int32_t)Min*6000+(int16_t)Sec*100+FracSec) - ((int32_t)RefPos.Min*6000+(int16_t)RefPos.Sec*100+RefPos.FracSec); + if(TimeDiff<(-180000)) TimeDiff+=360000; // wrap-around 60min + else if(TimeDiff>=180000) TimeDiff-=360000; + return TimeDiff; } // [0.01s] +*/ + int16_t calcDifferentials(GPS_Position &RefPos) // calculate climb rate and turn rate with an earlier reference position + { ClimbRate=0; TurnRate=0; + if(RefPos.FixQuality==0) return 0; + int16_t TimeDiff = calcTimeDiff(RefPos); + if(TimeDiff<5) return 0; + TurnRate = Heading-RefPos.Heading; + if(TurnRate>1800) TurnRate-=3600; else if(TurnRate<(-1800)) TurnRate+=3600; + ClimbRate = Altitude-RefPos.Altitude; + if(hasBaro && RefPos.hasBaro && (abs(Altitude-StdAltitude)<2500) ) + { ClimbRate = StdAltitude-RefPos.StdAltitude; } + Accel = Speed-RefPos.Speed; + if(TimeDiff==20) + { ClimbRate*=5; + TurnRate *=5; + Accel *=5; } + else if(TimeDiff==50) + { ClimbRate*=2; + TurnRate *=2; + Accel *=2; } + else if(TimeDiff==100) + { } + else if(TimeDiff==200) + { ClimbRate=(ClimbRate+1)>>1; + TurnRate=( TurnRate+1)>>1; + Accel =( Accel +1)>>1; } + else if(TimeDiff!=0) + { ClimbRate = ((int32_t)ClimbRate*100)/TimeDiff; + TurnRate = ((int32_t) TurnRate*100)/TimeDiff; + Accel = ((int32_t) Accel *100)/TimeDiff; } + return TimeDiff; } // [0.01s] + + void Write(MAV_GPS_RAW_INT *MAV) const + { MAV->time_usec = (int64_t)1000000*getUnixTime()+10000*FracSec; + MAV->lat = ((int64_t)50*Latitude+1)/3; + MAV->lon = ((int64_t)50*Longitude+1)/3; + MAV->alt = 100*Altitude; + MAV->vel = 10*Speed; + MAV->cog = 10*Heading;; + MAV->fix_type = 1+FixQuality; + MAV->eph = 10*HDOP; + MAV->epv = 10*VDOP; + MAV->satellites_visible = Satellites; } + + void Read(const MAV_GPS_RAW_INT *MAV, uint64_t UnixTime_ms=0) + { if(UnixTime_ms) { setUnixTime_ms(UnixTime_ms); hasTime=1; } + Latitude = ((int64_t)MAV->lat*3+25)/50; + Longitude = ((int64_t)MAV->lon*3+25)/50; + Altitude = (MAV->alt+50)/100; // [0.1m] AMSL + Heading = (MAV->cog+5)/10; // [0.1deg] + Speed = (MAV->vel+5)/10; // [0.1m/s] + HDOP = (MAV->eph+5)/10; + VDOP = (MAV->epv+5)/10; + Satellites = MAV->satellites_visible; + FixMode = MAV->fix_type-1; + FixQuality = 1; + hasGPS = 1; } + + void Read(const MAV_GLOBAL_POSITION_INT *MAV, uint64_t UnixTime_ms=0) + { if(UnixTime_ms) { setUnixTime_ms(UnixTime_ms); hasTime=1; } + Latitude = ((int64_t)MAV->lat*3+25)/50; + Longitude = ((int64_t)MAV->lon*3+25)/50; + Altitude = (MAV->alt+50)/100; // [0.1m] AMSL + ClimbRate = -MAV->vz/10; // [0.1m/s] + Heading = (uint32_t)((uint16_t)IntAtan2(MAV->vy, MAV->vx)*(uint32_t)450+0x1000)>>13; // [0.1degC] + Speed = IntSqrt((int32_t)MAV->vx*MAV->vx+(int32_t)MAV->vy*MAV->vy)/10; // [0.1m/s] + FixMode = 3; + FixQuality = 1; + hasGPS = 1; } + + void Read(const MAV_SCALED_PRESSURE *MAV, uint64_t UnixTime_ms=0) + { if(UnixTime_ms) { setUnixTime_ms(UnixTime_ms); hasTime=1; } + Pressure = 100*4*MAV->press_abs; + Temperature = MAV->temperature/10; + hasBaro=1; } + + static int32_t getCordic(int32_t Coord) { return ((int64_t)Coord*83399993+(1<<21))>>22; } // [0.0001/60 deg] => [cordic] + int32_t getCordicLatitude (void) const { return getCordic(Latitude ); } + int32_t getCordicLongitude(void) const { return getCordic(Longitude); } + + // [deg] [0.0001/60deg] [Cordic] [FANET Cordic] + // 180 0x066FF300 0x80000000 0x7FFFBC00 + static int32_t getFANETcordic(int32_t Coord) { return ((int64_t)Coord*83399317+(1<<21))>>22; } // [0.0001/60 deg] => [FANET cordic] + + void EncodeAirPos(FANET_Packet &Packet, uint8_t AcftType=1, bool Track=1) + { int32_t Alt = Altitude; if(Alt<0) Alt=0; else Alt=(Alt+5)/10; + int32_t Lat = getFANETcordic(Latitude); // Latitude: [0.0001/60deg] => [cordic] + int32_t Lon = getFANETcordic(Longitude); // Longitude: [0.0001/60deg] => [cordic] + // other, glider, tow, heli, chute, drop, hang, para, powered, jet, UFO, balloon, air, UAV, ground, static + const uint8_t FNTtype[16] = { 0, 4, 5, 6, 1, 5, 2, 1, 5, 5, 0, 3, 5, 7, 0, 0 } ; // convert aircraft-type from OGN to FANET + Packet.setAirPos(FNTtype[AcftType&0x0F], Track, Lat, Lon, Alt, (((uint16_t)Heading<<4)+112)/225, Speed, ClimbRate, TurnRate); + if(hasBaro) { Packet.setQNE((StdAltitude+5)/10); } + } + + void Encode(GDL90_REPORT &Report) + { Report.setAccuracy(9, 9); + int32_t Lat = getCordicLatitude(); // Latitude: [0.0001/60deg] => [cordic] + int32_t Lon = getCordicLongitude(); // Longitude: [0.0001/60deg] => [cordic] + int32_t Alt = Altitude; // [0.1m] + if(hasBaro) Alt = StdAltitude; + Alt=MetersToFeet(Alt); Alt=(Alt+5)/10; // [feet] + Report.setLatitude(Lat); + Report.setLongitude(Lon); + Report.setAltitude(Alt); + uint16_t HeadAngle = ((int32_t)Heading<<12)/225; // [16-bit cordic] heading angle + int32_t SpeedKts = (3981*(int32_t)Speed+1024)>>11; // [0.1m/s] => [0.1kts] + Report.setHeading((HeadAngle+0x80)>>8); // [8-bit cordic] + Report.setMiscInd(0x2); // + Report.setSpeed((SpeedKts+5)/10); // [knot] + Report.setClimbRate(6*MetersToFeet(ClimbRate)); + } + + template + void Encode(OGNx_Packet &Packet) const + { Packet.Position.FixQuality = FixQuality<3 ? FixQuality:3; // + if((FixQuality>0)&&(FixMode>=2)) Packet.Position.FixMode = FixMode-2; // + else Packet.Position.FixMode = 0; + if(PDOP>0) Packet.EncodeDOP(PDOP-10); // encode PDOP from GSA + else Packet.EncodeDOP(HDOP-10); // or if no GSA: use HDOP + int8_t ShortTime=Sec; // the 6-bit time field in the OGN packet + if(FracSec>=50) { ShortTime+=1; if(ShortTime>=60) ShortTime-=60; } // round to the closest full second + Packet.Position.Time=ShortTime; // Time + Packet.EncodeLatitude(Latitude); // Latitude + Packet.EncodeLongitude(Longitude); // Longitude + Packet.EncodeSpeed(Speed); // Speed + Packet.EncodeHeading(Heading); // Heading = track-over-ground + Packet.EncodeClimbRate(ClimbRate); // Climb rate + Packet.EncodeTurnRate(TurnRate); // Turn rate + Packet.EncodeAltitude((Altitude+5)/10); // Altitude + if(hasBaro) Packet.EncodeStdAltitude((StdAltitude+5)/10); // Pressure altitude + else Packet.clrBaro(); //or no-baro if pressure sensor data not there + } + +/* + template + void EncodeStatus(OGNx_Packet &Packet) const + { Packet.Status.ReportType=0; + int ShortTime=Sec; + if(FracSec>=50) { ShortTime+=1; if(ShortTime>=60) ShortTime-=60; } + Packet.Status.Time=ShortTime; + Packet.Status.FixQuality = FixQuality<3 ? FixQuality:3; + Packet.Status.Satellites = Satellites<15 ? Satellites:15; + Packet.EncodeAltitude((Altitude+5)/10); + if(hasBaro) + { Packet.EncodeTemperature(Temperature); + Packet.Status.Pressure = (Pressure+16)>>5; } + else + { Packet.Status.Pressure = 0; } + Packet.Status.Humidity=0; + } +*/ + template + void EncodeStatus(OGNx_Packet &Packet) const + { Packet.Status.ReportType=0; + int ShortTime=Sec; + if(FracSec>=50) { ShortTime+=1; if(ShortTime>=60) ShortTime-=60; } + Packet.Status.Time=ShortTime; + Packet.Status.FixQuality = FixQuality<3 ? FixQuality:3; + Packet.Status.Satellites = Satellites<15 ? Satellites:15; + Packet.EncodeAltitude((Altitude+5)/10); + if(hasBaro) + { Packet.EncodeTemperature(Temperature); + Packet.Status.Pressure = (Pressure+16)>>5; + Packet.EncodeHumidity(Humidity); } + else + { Packet.Status.Pressure = 0; + Packet.clrHumidity(); } + } + + // uint8_t getFreqPlan(void) const // get the frequency plan from Lat/Lon: 1 = Europe + Africa, 2 = USA/CAnada, 3 = Australia + South America, 4 = New Zeeland + // { if( (Longitude>=(-20*600000)) && (Longitude<=(60*600000)) ) return 1; // between -20 and 60 deg Lat => Europe + Africa: 868MHz band + // if( Latitude<(20*600000) ) // below 20deg latitude + // { if( ( Longitude>(164*600000)) && (Latitude<(-30*600000)) && (Latitude>(-48*600000)) ) return 4; // => New Zeeland + // return 3; } // => Australia + South America: upper half of 915MHz band + // return 2; } // => USA/Canada: full 915MHz band + + template + void Encode(OGNx_Packet &Packet, int16_t dTime) const // Encode position which is extrapolated by the given fraction of a second + { Packet.Position.FixQuality = FixQuality<3 ? FixQuality:3; // + if((FixQuality>0)&&(FixMode>=2)) Packet.Position.FixMode = FixMode-2; // + else Packet.Position.FixMode = 0; + if(PDOP>0) Packet.EncodeDOP(PDOP-10); // encode PDOP from GSA + else Packet.EncodeDOP(HDOP-10); // or if no GSA: use HDOP + int32_t Lat, Lon, Alt; int16_t Head; + calcExtrapolation(Lat, Lon, Alt, Head, dTime); + int16_t ShortTime=Sec; // the 6-bit time field in the OGN packet + dTime += FracSec; + while(dTime>= 50 ) { dTime-=100; ShortTime++; if(ShortTime>=60) ShortTime-=60; } + while(dTime<(-50)) { dTime+=100; ShortTime--; if(ShortTime< 0) ShortTime+=60; } + Packet.Position.Time=ShortTime; // Time + Packet.EncodeLatitude(Lat); // Latitude + Packet.EncodeLongitude(Lon); // Longitude + Packet.EncodeSpeed(Speed); // Speed + Packet.EncodeHeading(Head); // Heading = track-over-ground + Packet.EncodeClimbRate(ClimbRate); // Climb rate + Packet.EncodeTurnRate(TurnRate); // Turn rate + Packet.EncodeAltitude((Alt+5)/10); // Altitude + if(hasBaro) Packet.EncodeStdAltitude((StdAltitude+(Alt-Altitude)+5)/10); // Pressure altitude + else Packet.clrBaro(); //or no-baro if pressure sensor data not there + } + + void Extrapolate(int32_t dTime) // [0.01sec] extrapolate the position by dTime + { int16_t dSpeed = ((int32_t)Accel*dTime)/100; + Speed += dSpeed/2; + int16_t HeadAngle = ((int32_t)Heading<<12)/225; // [cordic] heading angle + int16_t TurnAngle = (((dTime*TurnRate)/25)<<9)/225; // [cordic] + HeadAngle += TurnAngle/2; + int32_t LatSpeed = ((int32_t)Speed*Icos(HeadAngle)+0x800)>>12; // [0.1m/s] + int32_t LonSpeed = ((int32_t)Speed*Isin(HeadAngle)+0x800)>>12; // [0.1m/s] + HeadAngle += TurnAngle-TurnAngle/2; + Speed += dSpeed-dSpeed/2; if(Speed<0) Speed=0; + Latitude += calcLatitudeExtrapolation (dTime, LatSpeed); + Longitude += calcLongitudeExtrapolation(dTime, LonSpeed); + int32_t dAlt = calcAltitudeExtrapolation(dTime); // [0.1m] + Altitude += dAlt; // [0.1m] + if(hasBaro) + { StdAltitude += dAlt; // [0.1m] + Pressure += 4000*dAlt/Atmosphere::PressureLapseRate(Pressure/4, Temperature); } // [0.25Pa] ([Pa], [0.1degC]) + Heading += (dTime*TurnRate)/100; // [0.1deg] + if(Heading<0) Heading+=3600; else if(Heading>=3600) Heading-=3600; // [0.1deg] + int16_t fTime = FracSec+dTime; // [0.01sec] + while(fTime>=100) { incrTimeDate(); fTime-=100; } + while(fTime< 0) { decrTimeDate(); fTime+=100; } + FracSec=fTime; } + + // extrapolate GPS position by a fraction of a second + void calcExtrapolation(int32_t &Lat, int32_t &Lon, int32_t &Alt, int16_t &Head, int32_t dTime) const // [0.01sec] + { int16_t HeadAngle = ((int32_t)Heading<<12)/225; // [] + int16_t TurnAngle = (((dTime*TurnRate)/25)<<9)/225; // [] + HeadAngle += TurnAngle; + int32_t LatSpeed = ((int32_t)Speed*Icos(HeadAngle)+0x800)>>12; // [0.1m/s] + int32_t LonSpeed = ((int32_t)Speed*Isin(HeadAngle)+0x800)>>12; // [0.1m/s] + Lat = Latitude + calcLatitudeExtrapolation (dTime, LatSpeed); + Lon = Longitude + calcLongitudeExtrapolation(dTime, LonSpeed); + Alt = Altitude + calcAltitudeExtrapolation(dTime); + Head = Heading + (dTime*TurnRate)/100; + if(Head<0) Head+=3600; else if(Head>=3600) Head-=3600; } + + int32_t calcAltitudeExtrapolation(int32_t Time) const // [0.01s] + { return Time*ClimbRate/100; } // [0.1m] + + int32_t calcLatitudeExtrapolation(int32_t Time, int32_t LatSpeed) const // [0.01s] [0.1m/s] + { return (Time*LatSpeed*177+0x4000)>>15; } // [0.1m] + + int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed) const // [0.01s] + { int16_t LatCosine = calcLatCosine(calcLatAngle16(Latitude)); + return calcLongitudeExtrapolation(Time, LonSpeed, LatCosine); } + + int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed, int16_t LatCosine) const // [0.01s] + { return ((((int32_t)Time*LonSpeed*177+4)>>3))/LatCosine; } + + // static int32_t calcLatDistance(int32_t Lat1, int32_t Lat2) // [m] distance along latitude + // { return ((int64_t)(Lat2-Lat1)*0x2f684bda+0x80000000)>>32; } + + // static int32_t calcLatAngle32(int32_t Lat) // convert latitude to 32-bit integer angle + // { return ((int64_t)Lat*2668799779u+0x4000000)>>27; } + + static int16_t calcLatAngle16(int32_t Lat) // convert latitude to 16-bit integer angle + { return ((int64_t)Lat*1303125+0x80000000)>>32; } + + // static int32_t calcLatCosine(int32_t LatAngle) // calculate the cosine of the latitude 32-bit integer angle + // { return IntSine((uint32_t)(LatAngle+0x40000000)); } + + // static int32_t calcLatCosine(int16_t LatAngle) // calculate the cosine of the latitude 16-bit integer angle + // { return IntSine((uint16_t)(LatAngle+0x4000)); } + + static int16_t calcLatCosine(int16_t LatAngle) + { return Icos(LatAngle); } + + // int32_t getLatDistance(int32_t RefLatitude) const // [m] distance along latitude + // { return calcLatDistance(RefLatitude, Latitude); } + + // int32_t getLonDistance(int32_t RefLongitude) const // [m] distance along longitude + // { int32_t Dist = calcLatDistance(RefLongitude, Longitude); // + // int16_t LatAngle = calcLatAngle16(Latitude); + // int32_t LatCos = calcLatCosine(LatAngle); + // // printf("Latitude=%+d, LatAngle=%04X LatCos=%08X\n", Latitude, (uint16_t)LatAngle, LatCos); + // return ((int64_t)Dist*LatCos+0x40000000)>>31; } // distance corrected by the latitude cosine + + void calcLatitudeCosine(void) + { int16_t LatAngle = calcLatAngle16(Latitude); + LatitudeCosine = calcLatCosine(LatAngle); } + + int WriteAPRS(char *Out, const char *Call, const char *Icon, uint32_t ID) + { int Len=0; + Len+=Format_String(Out+Len, Call); // Call + Len+=Format_String(Out+Len, ">APRS:/"); + Len+=WriteHHMMSS(Out+Len); // Time + Out[Len++]='h'; + Len+=WriteIGCcoord(Out+Len, Latitude, 2, "NS"); // [DDMM.MM] Latitude + char LatW = Out[Len-2]; + Out[Len-2]=Out[Len-3]; Out[Len-3]=Out[Len-4]; Out[Len-4]='.'; + Out[Len++]=Icon[0]; + Len+=WriteIGCcoord(Out+Len, Longitude, 3, "EW"); // [DDDMM.MM] Longitude + char LonW = Out[Len-2]; + Out[Len-2]=Out[Len-3]; Out[Len-3]=Out[Len-4]; Out[Len-4]='.'; + Out[Len++]=Icon[1]; + Len+=Format_UnsDec(Out+Len, Heading/10, 3); // [deg] Heading + Out[Len++]='/'; + Len+=Format_UnsDec(Out+Len, ((uint32_t)Speed*199+512)>>10, 3); // [kt] speed + Out[Len++] = '/'; Out[Len++] = 'A'; Out[Len++] = '='; Len+=Format_UnsDec(Out+Len, (MetersToFeet(Altitude)+5)/10, 6); // [feet] altitude + Out[Len++]=' '; Out[Len++]='!'; Out[Len++]='W'; Out[Len++]=LatW; Out[Len++]=LonW; Out[Len++]='!'; // more accurate Lat/Lon + Out[Len++]=' '; Out[Len++]='i'; Out[Len++]='d'; Len+=Format_Hex(Out+Len, ID); // ID + + Out[Len++] = ' '; Len+=Format_SignDec(Out+Len, ((int32_t)ClimbRate*10079+256)>>9, 3); Out[Len++] = 'f'; Out[Len++] = 'p'; Out[Len++] = 'm'; // [fpm] + Out[Len++] = ' '; Len+=Format_SignDec(Out+Len, TurnRate/3, 2, 1); Out[Len++] = 'r'; Out[Len++] = 'o'; Out[Len++] = 't'; // [ROT] + + if(hasBaro) + { int32_t Alt=(StdAltitude+5)/10; // [m] standard pressure altitude + if(Alt<0) Alt=0; + Out[Len++] = ' '; Out[Len++] = 'F'; Out[Len++] = 'L'; + Len+=Format_UnsDec(Out+Len, MetersToFeet((uint32_t)Alt), 5, 2); } // [feet] "Flight Level" + + uint16_t DOP=PDOP; if(DOP==0) DOP=HDOP; + uint16_t HorPrec=(DOP*2+5)/10; if(HorPrec>63) HorPrec=63; // [m] + uint16_t VerPrec=(DOP*3+5)/10; if(VerPrec>63) VerPrec=63; // [m] + Out[Len++] = ' '; Out[Len++] = 'g'; Out[Len++] = 'p'; Out[Len++] = 's'; + Len+=Format_UnsDec(Out+Len, HorPrec); Out[Len++] = 'x'; Len+=Format_UnsDec(Out+Len, VerPrec); + + Out[Len]=0; return Len; } + + static int WriteIGCcoord(char *Out, int32_t Coord, uint8_t DegSize, const char *SignChar) + { int Len=0; + bool Neg = Coord<0; if(Neg) Coord=(-Coord); + int32_t Deg = Coord/600000; + Len+=Format_UnsDec(Out+Len, Deg, DegSize); + Coord-=Deg*600000; Coord/=10; + Len+=Format_UnsDec(Out+Len, Coord, 5); + Out[Len++]=SignChar[Neg]; + return Len; } + + int WriteHHMMSS(char *Out) + { Format_UnsDec(Out , Hour, 2); + Format_UnsDec(Out+2, Min , 2); + Format_UnsDec(Out+4, Sec , 2); + return 6; } + + int WriteIGC(char *Out) + { // if(!isValid()) return 0; + int Len=0; + Out[Len++] = 'B'; + if(isTimeValid()) Len+=WriteHHMMSS(Out+Len); + else Len+=Format_String(Out+Len, " "); + if(isValid()) + { Len+=WriteIGCcoord(Out+Len, Latitude, 2, "NS"); + Len+=WriteIGCcoord(Out+Len, Longitude, 3, "EW"); + Out[Len++] = FixMode>2 ? 'A':'V'; } + else Len+=Format_String(Out+Len, " "); + if(hasBaro) + { int32_t Alt = StdAltitude/10; // [m] + if(Alt<0) { Alt = (-Alt); Out[Len++] = '-'; Len+=Format_UnsDec(Out+Len, (uint32_t)Alt, 4); } + else { Len+=Format_UnsDec(Out+Len, (uint32_t)Alt, 5); } + } else Len+=Format_String(Out+Len, " "); + if(isValid()) + { int32_t Alt = (Altitude+GeoidSeparation)/10; // [m] + if(Alt<0) { Alt = (-Alt); Out[Len++] = '-'; Len+=Format_UnsDec(Out+Len, (uint32_t)Alt, 4); } + else { Len+=Format_UnsDec(Out+Len, (uint32_t)Alt, 5); } + } else Len+=Format_String(Out+Len, " "); + Out[Len]=0; return Len; } + + private: + + int8_t ReadLatitude(char Sign, const char *Value) + { int8_t Deg=Read_Dec2(Value); if(Deg<0) return -1; + int8_t Min=Read_Dec2(Value+2); if(Min<0) return -1; + if(Value[4]!='.') return -1; + int16_t FracMin=Read_Dec4(Value+5); if(FracMin<0) return -1; + // printf("Latitude: %c %02d %02d %04d\n", Sign, Deg, Min, FracMin); + Latitude = (int16_t)Deg*60 + Min; + Latitude = Latitude*(int32_t)10000 + FracMin; + // printf("Latitude: %d\n", Latitude); + if(Sign=='S') Latitude=(-Latitude); + else if(Sign!='N') return -1; + // printf("Latitude: %d\n", Latitude); + return 0; } // Latitude units: 0.0001/60 deg + + int8_t ReadLongitude(char Sign, const char *Value) + { int16_t Deg=Read_Dec3(Value); if(Deg<0) return -1; + int8_t Min=Read_Dec2(Value+3); if(Min<0) return -1; + if(Value[5]!='.') return -1; + int16_t FracMin=Read_Dec4(Value+6); if(FracMin<0) return -1; + Longitude = (int16_t)Deg*60 + Min; + Longitude = Longitude*(int32_t)10000 + FracMin; + if(Sign=='W') Longitude=(-Longitude); + else if(Sign!='E') return -1; + return 0; } // Longitude units: 0.0001/60 deg + + int8_t ReadAltitude(char Unit, const char *Value) + { if(Unit!='M') return -1; + return Read_Float1(Altitude, Value); } // Altitude units: 0.1 meter + + int8_t ReadGeoidSepar(char Unit, const char *Value) + { if(Unit!='M') return -1; + return Read_Float1(GeoidSeparation, Value); } // GeoidSepar units: 0.1 meter + + int8_t ReadSpeed(const char *Value) + { int32_t Knots; + if(Read_Float1(Knots, Value)<1) return -1; // Speed: 0.1 knots + Speed=(527*Knots+512)>>10; return 0; } // convert speed to 0.1 meter/sec + + int8_t ReadHeading(const char *Value) + { return Read_Float1(Heading, Value); } // Heading units: 0.1 degree + + int8_t ReadPDOP(const char *Value) + { int16_t DOP; + if(Read_Float1(DOP, Value)<1) return -1; + if(DOP<10) DOP=10; + else if(DOP>255) DOP=255; + PDOP=DOP; return 0; } + + int ReadHDOP(const char *Value) + { int16_t DOP; + if(Read_Float1(DOP, Value)<1) return -1; + if(DOP<10) DOP=10; + else if(DOP>255) DOP=255; + HDOP=DOP; return 0; } + + int ReadVDOP(const char *Value) + { int16_t DOP; + if(Read_Float1(DOP, Value)<1) return -1; + if(DOP<10) DOP=10; + else if(DOP>255) DOP=255; + VDOP=DOP; return 0; } + + int8_t ReadTime(const char *Value) // read the Time field: HHMMSS.ss and check if it is a new one or the same one + { int8_t Prev; int8_t Same=1; + Prev=Hour; + Hour=Read_Dec2(Value); if(Hour<0) return -1; // read hour (two digits), return when invalid + if(Prev!=Hour) Same=0; + Prev=Min; + Min=Read_Dec2(Value+2); if(Min<0) return -1; // read minute (two digits), return when invalid + if(Prev!=Min) Same=0; + Prev=Sec; + Sec=Read_Dec2(Value+4); if(Sec<0) return -1; // read second (two digits), return when invalid + if(Prev!=Sec) Same=0; + Prev=FracSec; + if(Value[6]=='.') // is there a fraction + { FracSec=Read_Dec2(Value+7); if(FracSec<0) return -1; } // read the fraction, return when invalid + if(Prev!=FracSec) Same=0; // return 0 when time is valid but did not change + return Same; } // return 1 when time did not change (both RMC and GGA were for same time) + + int8_t ReadDate(const char *Param) // read the field DDMMYY + { Day=Read_Dec2(Param); if(Day<0) return -1; // read calendar year (two digits - thus need to be extended to four) + Month=Read_Dec2(Param+2); if(Month<0) return -1; // read calendar month + Year=Read_Dec2(Param+4); if(Year<0) return -1; // read calendar day + return 0; } // return 0 when field valid and was read correctly + + public: + + int8_t static IndexNMEA(uint8_t Index[20], const char *Seq) // index parameters and verify the NMEA checksum + { int8_t Ptr=0; + uint8_t Check=0; + if(Seq[Ptr]!='$') return -1; // first chat. must be dollar sign + Ptr++; + for( ; Ptr<=6; Ptr++) // go through the sentence name + { if(Seq[Ptr]==',') break; // stop at comma + Check^=Seq[Ptr]; } // take char. to checksum + if(Seq[Ptr]!=',') return -1; // comma after the sentence name + Check^=Seq[Ptr++]; // take comma to the checksum + Index[0]=Ptr; int8_t Params=1; // first parameter + for( ; ; ) + { char ch=Seq[Ptr++]; if(ch<' ') return -1; // go through the chars + if(ch=='*') break; // break at star (check-sum should follow) + Check^=ch; // get chars to the checksum + if(ch==',') { Index[Params++]=Ptr; } // if comma then counr next parameter + } + if(Seq[Ptr++]!=HexDigit(Check>>4) ) return -2; // verify checksum + if(Seq[Ptr++]!=HexDigit(Check&0x0F)) return -2; + // printf("%s => [%d]\n", Seq, Params); + return Params; } + +} ; + +#endif // of __OGN_H__ + diff --git a/utils/ogn1.h b/utils/ogn1.h new file mode 100644 index 0000000..08ae411 --- /dev/null +++ b/utils/ogn1.h @@ -0,0 +1,846 @@ +#ifndef __OGN1_H__ +#define __OGN1_H__ + +#include + +#include +#include + +#include "ognconv.h" + +#include "intmath.h" + +#include "bitcount.h" +#include "nmea.h" +#include "mavlink.h" + +#include "format.h" + + // the packet description here is how it look on the little-endian CPU before sending it to the RF chip + // nRF905, CC1101, SPIRIT1, RFM69 chips actually reverse the bit order within every byte + // thus on the air the bits appear MSbit first for every byte transmitted + +class OGN1_Packet // Packet structure for the OGN tracker +{ public: + + static const int Words = 5; + static const int Bytes = 20; + + union + { uint32_t HeaderWord; // ECRR PMTT AAAA AAAA AAAA AAAA AAAA AAAA + // E=Emergency, C=enCrypt/Custom, RR=Relay count, P=Parity, M=isMeteo/Other, TT=address Type, AA..=Address:24-bit + // When enCrypt/Custom is set the data (position or whatever) can only be decoded by the owner + // This option is indented to pass any type of custom data not foreseen otheriwse + struct + { unsigned int Address :24; // aircraft address + unsigned int AddrType : 2; // address type: 0 = random, 1 = ICAO, 2 = FLARM, 3 = OGN + unsigned int NonPos : 1; // 0 = position packet, 1 = other information like status + unsigned int Parity : 1; // parity takes into account bits 0..27 thus only the 28 lowest bits + unsigned int Relay : 2; // 0 = direct packet, 1 = relayed once, 2 = relayed twice, ... + unsigned int Encrypted : 1; // packet is encrypted + unsigned int Emergency : 1; // aircraft in emergency (not used for now) + } Header ; + + } ; + + union + { uint32_t Data[4]; // 0: QQTT TTTT LLLL LLLL LLLL LLLL LLLL LLLL QQ=fix Quality:2, TTTTTT=time:6, LL..=Latitude:20 + // 1: MBDD DDDD LLLL LLLL LLLL LLLL LLLL LLLL F=fixMode:1 B=isBaro:1, DDDDDD=DOP:6, LL..=Longitude:20 + // 2: RRRR RRRR SSSS SSSS SSAA AAAA AAAA AAAA RR..=turn Rate:8, SS..=Speed:10, AA..=Alt:14 + // 3: BBBB BBBB YYYY PCCC CCCC CCDD DDDD DDDD BB..=Baro altitude:8, YYYY=AcftType:4, P=Stealth:1, CC..=Climb:9, DD..=Heading:10 + + // meteo/telemetry types: Meteo conditions, Thermal wind/climb, Device status, Precise time, + + // meteo report: Humidity, Barometric pressure, Temperature, wind Speed/Direction + // 2: HHHH HHHH SSSS SSSS SSAA AAAA AAAA AAAA + // 3: TTTT TTTT YYYY BBBB BBBB BBDD DDDD DDDD YYYY = report tYpe (meteo, thermal, water level, other telemetry) + + // Device status: Time, baro pressure+temperature, GPS altitude, supply voltage, TX power, RF noise, software version, software features, hardware features, + // 0: UUUU UUUU UUUU UUUU UUUU UUUU UUUU UUUU UU..=Unix time + // 1: SSSS SSSS SSSS SSSS TTTT TTTT HHHH HHHH SS..=slot time, TT..=temperature, HH..=humidity + // 2: BBBB BBBB BBBB BBBB BBAA AAAA AAAA AAAA Baro pressure[0.5Pa], GPS altitude + // 3: VVVV VVVV YYYY HHHH HHHH XXXX VVVV VVVV VV..=firmware version, YYYY = report type, TT..=Temperatature, XX..=TxPower, VV..=battery voltage + + // Pilot status: + // 0: NNNN NNNN NNNN NNNN NNNN NNNN NNNN NNNN Name: 9 char x 7bit or 10 x 6bit or Huffman encoding ? + // 1: NNNN NNNN NNNN NNNN NNNN NNNN NNNN NNNN + struct + { signed int Latitude:24; // // QQTT TTTT LLLL LLLL LLLL LLLL LLLL LLLL QQ=fix Quality:2, TTTTTT=time:6, LL..=Latitude:24 + unsigned int Time: 6; // [sec] // time, just second thus ambiguity every every minute + unsigned int FixQuality: 2; // // 0 = none, 1 = GPS, 2 = Differential GPS (can be WAAS) + signed int Longitude:24; // // MBDD DDDD LLLL LLLL LLLL LLLL LLLL LLLL F=fixMode:1 B=isBaro:1, DDDDDD=DOP:6, LL..=Longitude:24 + unsigned int DOP: 6; // // GPS Dilution of Precision + unsigned int BaroMSB: 1; // // negated bit #8 of the altitude difference between baro and GPS + unsigned int FixMode: 1; // // 0 = 2-D, 1 = 3-D + unsigned int Altitude:14; // [m] VR // RRRR RRRR SSSS SSSS SSAA AAAA AAAA AAAA RR..=turn Rate:8, SS..=Speed:10, AA..=Alt:14 + unsigned int Speed:10; // [0.1m/s] VR + unsigned int TurnRate: 8; // [0.1deg/s] VR + unsigned int Heading:10; // [360/1024deg] // BBBB BBBB YYYY PCCC CCCC CCDD DDDD DDDD BB..=Baro altitude:8, YYYY=AcftType:4, P=Stealth:1, CC..=Climb:9, DD..=Heading:10 + unsigned int ClimbRate: 9; // [0.1m/s] VR // rate of climb/decent from GPS or from baro sensor + unsigned int Stealth: 1; // // not really used till now + unsigned int AcftType: 4; // [0..15] // type of aircraft: 1 = glider, 2 = towplane, 3 = helicopter, ... + unsigned int BaroAltDiff: 8; // [m] // lower 8 bits of the altitude difference between baro and GPS + } Position; + + struct + { unsigned int Pulse : 8; // [bpm] // pilot: heart pulse rate + unsigned int Oxygen : 7; // [%] // pilot: oxygen level in the blood + unsigned int SatSNR : 5; // [dB] // average SNR of GPS signals + // unsigned int FEScurr : 5; // [A] // FES current + unsigned int RxRate : 4; // [/min] // log2 of received packet rate + unsigned int Time : 6; // [sec] // same as in the position packet + unsigned int FixQuality: 2; + unsigned int AudioNoise: 8; // [dB] // + unsigned int RadioNoise: 8; // [dBm] // noise seen by the RF chip + unsigned int Temperature:8; // [0.1degC] VR // temperature by the baro or RF chip + unsigned int Humidity : 8; // [0.1%] VR // humidity + unsigned int Altitude :14; // [m] VR // same as in the position packet + unsigned int Pressure :14; // [0.08hPa] // barometric pressure + unsigned int Satellites: 4; // [ ] + unsigned int Firmware : 8; // [ ] // firmware version + unsigned int Hardware : 8; // [ ] // hardware version + unsigned int TxPower : 4; // [dBm] // RF trancmitter power (offset = 4) + unsigned int ReportType: 4; // [0] // 0 for the status report + unsigned int Voltage : 8; // [1/64V] VR // supply/battery voltage + } Status; + + union + { uint8_t Byte[16]; + struct + { uint8_t Data[14]; // [16x7bit]packed string of 16-char: 7bit/char + uint8_t DataChars: 4; // [int] number of characters in the packed string + uint8_t ReportType: 4; // [1] // 1 for the Info packets + uint8_t Check; // CRC check + } ; + } Info; + + struct + { signed int Latitude:24; // // Latitude of the measurement + unsigned int Time: 6; // [sec] // time, just second thus ambiguity every every minute + unsigned int : 2; // // spare + signed int Longitude:24; // // Longitude of the measurement + unsigned int : 6; // // spare + unsigned int BaroMSB: 1; // // negated bit #8 of the altitude difference between baro and GPS + unsigned int : 1; // // spare + unsigned int Altitude:14; // [m] VR // Altitude of the measurement + unsigned int Speed:10; // [0.1m/s] VR // Horizontal wind speed + unsigned int : 8; // // spare + unsigned int Heading:10; // // Wind direction + unsigned int ClimbRate: 9; // [0.1m/s] VR // Vertical wind speed + unsigned int : 1; // // spare + unsigned int ReportType: 4; // // 2 for wind/thermal report + unsigned int BaroAltDiff: 8; // [m] // lower 8 bits of the altitude difference between baro and GPS + } Wind; + + } ; + + uint8_t *Byte(void) const { return (uint8_t *)&HeaderWord; } // packet as bytes + uint32_t *Word(void) const { return (uint32_t *)&HeaderWord; } // packet as words + + // 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 char *InfoParmName(uint8_t Idx) { static const char *Name[InfoParmNum] = + { "Pilot", "Manuf", "Model", "Type", "SN", "Reg", "ID", "Class", + "Task" , "Base" , "ICE" , "PilotID", "Hard", "Soft" } ; + return Idx>6, 3, 2); Out[Len++]='V'; + Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Status.TxPower+4); + Out[Len++]='/'; Out[Len++]='-'; Len+=Format_UnsDec(Out+Len, 5*Status.RadioNoise, 2, 1); Out[Len++]='d'; Out[Len++]='B'; Out[Len++]='m'; + Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, (1<>16)); + Len+=Format_Hex(JSON+Len, (uint16_t)(Header.Address)); + JSON[Len++]='\"'; + JSON[Len++]=','; + Len+=Format_String(JSON+Len, "\"addr_type\":"); + JSON[Len++] = HexDigit(Header.AddrType); + if(!Header.Encrypted && !Header.NonPos) // if non-encrypted position + { Len+=Format_String(JSON+Len, ",\"acft_type\":\""); + JSON[Len++] = HexDigit(Position.AcftType); + JSON[Len++]='\"'; + Len+=Format_String(JSON+Len, ",\"acft_cat\":\""); // GDL90 aircraft category + // no-info, glider, tow, heli, parachute, drop-plane, hang-glider, para-glider, powered, jet, UFO, balloon, Zeppelin, UAV, ground vehicle, static } ; + const uint8_t AcftCat[16] = { 0, 9, 1, 7, 11, 1, 12, 12, 1, 2, 0, 10, 10, 14, 18, 19 } ; + Len+=Format_Hex(JSON+Len, AcftCat[Position.AcftType]); + JSON[Len++]='\"'; + Len+=Format_String(JSON+Len, ",\"stealth\":"); + JSON[Len++] = '0'+Position.Stealth; + Len+=Format_String(JSON+Len, ",\"lat_deg\":"); + Len+=Format_SignDec(JSON+Len, (int32_t)(((int64_t)50*DecodeLatitude()+1)/3), 8, 7, 1); + Len+=Format_String(JSON+Len, ",\"lon_deg\":"); + Len+=Format_SignDec(JSON+Len, (int32_t)(((int64_t)50*DecodeLongitude()+1)/3), 8, 7, 1); + int32_t Altitude=DecodeAltitude(); + Len+=Format_String(JSON+Len, ",\"alt_msl_m\":"); + Len+=Format_UnsDec(JSON+Len, (uint32_t)Altitude); + if(hasBaro()) + { Altitude+=getBaroAltDiff(); + Len+=Format_String(JSON+Len, ",\"alt_std_m\":"); + Len+=Format_SignDec(JSON+Len, Altitude, 1, 0, 1); } + Len+=Format_String(JSON+Len, ",\"track_deg\":"); + 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); + 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 + { } + if(Status.ReportType==1) // info + { char Value[16]; + uint8_t InfoType; + uint8_t Idx=0; + for( ; ; ) + { uint8_t Chars = readInfo(Value, InfoType, Idx); + if(Chars==0) break; + if(InfoTypeICAO_address = HeaderWord&0x03FFFFFF; + MAV->lat = ((int64_t)50*DecodeLatitude()+1)/3; + MAV->lon = ((int64_t)50*DecodeLongitude()+1)/3; + MAV->altitude = 1000*DecodeAltitude(); + MAV->heading = 10*DecodeHeading(); + MAV->hor_velocity = 10*DecodeSpeed(); + MAV->ver_velocity = 10*DecodeClimbRate(); + MAV->flags = 0x17; + MAV->altitude_type = 1; + MAV->callsign[0] = 0; + MAV->tslc = 0; + MAV->emiter_type = 0; } +*/ + void Encode(MAV_ADSB_VEHICLE *MAV) + { MAV->ICAO_address = Header.Address; + MAV->lat = ((int64_t)50*DecodeLatitude()+1)/3; // convert coordinates to [1e-7deg] + MAV->lon = ((int64_t)50*DecodeLongitude()+1)/3; + MAV->altitude = 1000*DecodeAltitude(); // convert to [mm[ + MAV->heading = 10*DecodeHeading(); // [cdeg/s] + MAV->hor_velocity = 10*DecodeSpeed(); // [cm/s] + MAV->ver_velocity = 10*DecodeClimbRate(); // [cm/s] + MAV->flags = 0x1F; // all valid except for Squawk, not simulated + MAV->altitude_type = 1; // GPS altitude + const static char Prefix[4] = { 'R', 'I', 'F', 'O' }; // prefix for Random, ICAO, Flarm and OGN address-types + MAV->callsign[0] = Prefix[Header.AddrType]; // create a call-sign from address-type and address + Format_Hex((char *)MAV->callsign+1, ( uint8_t)(Header.Address>>16)); // highest byte + Format_Hex((char *)MAV->callsign+3, (uint16_t)(Header.Address&0xFFFF)); // two lower bytes + MAV->callsign[7] = 0; // end-of-string for call-sign + MAV->squawk = 0; // what shall we put there for OGN ? + MAV->tslc = 1; // 1sec for now but should be more precise + const static uint8_t EmitterType[16] = // conversion table from OGN aircraft-type + { 0, 9, 2, 7, // unknown, glider, towplane, helicopter + 11, 3, 9, 11, // parachute, drop plane, hang-glider, para-glider + 2, 3, 15, 10, // powered aircraft, jet aircraft, UFO, balloon + 10, 14, 2, 19 }; // airship, UAV, ground vehiele, fixed object + MAV->emiter_type = EmitterType[Position.AcftType]; // convert from the OGN + } + + static const char *getAprsIcon(uint8_t AcftType) + { static const char *AprsIcon[16] = // Icons for various FLARM acftType's + { "/z", // 0 = ? + "/'", // 1 = (moto-)glider (most frequent) + "/'", // 2 = tow plane (often) + "/X", // 3 = helicopter (often) + "/g" , // 4 = parachute (rare but seen - often mixed with drop plane) + "\\^", // 5 = drop plane (seen) + "/g" , // 6 = hang-glider (rare but seen) + "/g" , // 7 = para-glider (rare but seen) + "\\^", // 8 = powered aircraft (often) + "/^", // 9 = jet aircraft (rare but seen) + "/z", // A = UFO (people set for fun) + "/O", // B = balloon (seen once) + "/O", // C = airship (seen once) + "/'", // D = UAV (drones, can become very common) + "/z", // E = ground support (ground vehicles at airfields) + "\\n" // F = static object (ground relay ?) + } ; + return AcftType<16 ? AprsIcon[AcftType]:0; + } + + int8_t 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 + Data++; + const char *Dest = strchr(Msg, '>'); if(Dest==0) return -1; // where the destination call is + Dest++; + const char *Comma = strchr(Dest, ','); // the first comma after the destination call + + Position.AcftType=0xF; + + uint8_t AddrType; + uint32_t Address; + if(memcmp(Msg, "RND", 3)==0) AddrType=0; + else if(memcmp(Msg, "ICA", 3)==0) AddrType=1; + else if(memcmp(Msg, "FLR", 3)==0) AddrType=2; + else if(memcmp(Msg, "OGN", 3)==0) AddrType=3; + else AddrType=4; + if(AddrType<4) + { if(Read_Hex(Address, Msg+3)==6) Header.Address=Address; + Header.AddrType=AddrType; } + + if(Comma) + { if(memcmp(Comma+1, "RELAY*" , 6)==0) Header.Relay=1; + else if(Comma[10]=='*') Header.Relay=1; + } + + if(Data[0]!='/') return -1; + int8_t Time; + if(Data[7]=='h') // HHMMSS UTC time + { Time=Read_Dec2(Data+5); if(Time<0) return -1; } + else if(Data[7]=='z') // DDHHMM UTC time + { Time=0; } + else return -1; + + Position.Time=Time; + Data+=8; + + Position.FixMode=1; + Position.FixQuality=1; + EncodeDOP(0xFF); + + int8_t LatDeg = Read_Dec2(Data); if(LatDeg<0) return -1; + int8_t LatMin = Read_Dec2(Data+2); if(LatMin<0) return -1; + if(Data[4]!='.') return -1; + int8_t LatFrac = Read_Dec2(Data+5); if(LatFrac<0) return -1; + int32_t Latitude = (int32_t)LatDeg*600000 + (int32_t)LatMin*10000 + (int32_t)LatFrac*100; + char LatSign = Data[7]; + Data+=8+1; + + int16_t LonDeg = Read_Dec3(Data); if(LonDeg<0) return -1; + int8_t LonMin = Read_Dec2(Data+3); if(LonMin<0) return -1; + if(Data[5]!='.') return -1; + int8_t LonFrac = Read_Dec2(Data+6); if(LonFrac<0) return -1; + int32_t Longitude = (int32_t)LonDeg*600000 + (int32_t)LonMin*10000 + (int32_t)LonFrac*100; + char LonSign = Data[8]; + Data+=9+1; + + int16_t Speed=0; + int16_t Heading=0; + if(Data[3]=='/') + { Heading=Read_Dec3(Data); + Speed=Read_Dec3(Data+4); + Data+=7; } + EncodeHeading(Heading*10); + EncodeSpeed(((int32_t)Speed*337146+0x8000)>>16); + + uint32_t Altitude=0; + if( (Data[0]=='/') && (Data[1]=='A') && (Data[2]=='=') && (Read_UnsDec(Altitude, Data+3)==6) ) + { Data+=9; } + EncodeAltitude(FeetToMeters(Altitude)); + + for( ; ; ) + { if(Data[0]!=' ') break; + Data++; + + if( (Data[0]=='!') && (Data[1]=='W') && (Data[4]=='!') ) + { Latitude += (Data[2]-'0')*10; + Longitude += (Data[3]-'0')*10; + Data+=5; continue; } + + if( (Data[0]=='i') && (Data[1]=='d') ) + { uint32_t ID; Read_Hex(ID, Data+2); + Header.Address = ID&0x00FFFFFF; + Header.AddrType = (ID>>24)&0x03; + Position.AcftType = (ID>>26)&0x0F; + Position.Stealth = ID>>31; + Data+=10; continue; } + + if( (Data[0]=='F') && (Data[1]=='L') && (Data[5]=='.') ) + { int16_t FLdec=Read_Dec3(Data+2); + int16_t FLfrac=Read_Dec2(Data+6); + if( (FLdec>=0) && (FLfrac>=0) ) + { uint32_t StdAlt = FLdec*100+FLfrac; + EncodeStdAltitude(FeetToMeters(StdAlt)); } + Data+=8; continue; } + + if( (Data[0]=='+') || (Data[0]=='-') ) + { int32_t Value; int8_t Len=Read_Float1(Value, Data); + if(Len>0) + { Data+=Len; + if(memcmp(Data, "fpm", 3)==0) { EncodeClimbRate((333*Value+0x8000)>>16); Data+=3; continue; } + if(memcmp(Data, "rot", 3)==0) { EncodeTurnRate(3*Value); Data+=3; continue; } + } + } + + if( (Data[0]=='g') && (Data[1]=='p') && (Data[2]=='s') ) + { int16_t HorPrec=Read_Dec2(Data+3); + if(HorPrec<0) HorPrec=Read_Dec1(Data[3]); + if(HorPrec>=0) + { uint16_t DOP=HorPrec*5; if(DOP<10) DOP=10; else if(DOP>230) DOP=230; + EncodeDOP(DOP-10); Data+=5; } + } + while(Data[0]>' ') Data++; + } + + if(LatSign=='S') Latitude=(-Latitude); else if(LatSign!='N') return -1; + EncodeLatitude(Latitude); + if(LonSign=='W') Longitude=(-Longitude); else if(LonSign!='E') return -1; + EncodeLongitude(Longitude); + + return 0; } + + uint8_t WriteAPRS(char *Msg, uint32_t Time, const char *ProtName="APRS") // write an APRS position message + { uint8_t Len=0; + static const char *AddrTypeName[4] = { "RND", "ICA", "FLR", "OGN" } ; + memcpy(Msg+Len, AddrTypeName[Header.AddrType], 3); Len+=3; + Len+=Format_Hex(Msg+Len, (uint8_t)(Header.Address>>16)); + Len+=Format_Hex(Msg+Len, (uint16_t)(Header.Address)); + Msg[Len++] = '>'; + uint8_t ProtLen = strlen(ProtName); + memcpy(Msg+Len, ProtName, ProtLen); Len+=ProtLen; + if(Header.Relay) + { 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(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?'>':'/'; + Len+=Format_HHMMSS(Msg+Len, Time); + Msg[Len++] = 'h'; + + if(Header.NonPos) { Len+=WriteStatus(Msg+Len); Msg[Len++]='\n'; Msg[Len]=0; return Len; } + + const char *Icon = getAprsIcon(Position.AcftType); + + int32_t Lat = DecodeLatitude(); + bool NegLat = Lat<0; if(NegLat) Lat=(-Lat); + uint32_t LatDeg = Lat/600000; + Len+=Format_UnsDec(Msg+Len, LatDeg, 2); + Lat -= LatDeg*600000; + uint32_t LatMin = Lat/100; + Len+=Format_UnsDec(Msg+Len, LatMin, 4, 2); + Lat -= LatMin*100; + Msg[Len++] = NegLat ? 'S':'N'; + Msg[Len++] = Icon[0]; + + int32_t Lon = DecodeLongitude(); + bool NegLon = Lon<0; if(NegLon) Lon=(-Lon); + uint32_t LonDeg = Lon/600000; + Len+=Format_UnsDec(Msg+Len, LonDeg, 3); + Lon -= LonDeg*600000; + uint32_t LonMin = Lon/100; + Len+=Format_UnsDec(Msg+Len, LonMin, 4, 2); + Lon -= LonMin*100; + Msg[Len++] = NegLon ? 'W':'E'; + Msg[Len++] = Icon[1]; + + Len+=Format_UnsDec(Msg+Len, (DecodeHeading()+5)/10, 3); + Msg[Len++] = '/'; + Len+=Format_UnsDec(Msg+Len, ((uint32_t)DecodeSpeed()*199+512)>>10, 3); + Msg[Len++] = '/'; Msg[Len++] = 'A'; Msg[Len++] = '='; Len+=Format_UnsDec(Msg+Len, MetersToFeet(DecodeAltitude()), 6); + + Msg[Len++] = ' '; + Msg[Len++] = '!'; + Msg[Len++] = 'W'; + Msg[Len++] = '0'+Lat/10; + Msg[Len++] = '0'+Lon/10; + Msg[Len++] = '!'; + + 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(hasBaro()) + { int32_t Alt = DecodeStdAltitude(); + if(Alt<0) Alt=0; + Msg[Len++] = ' '; Msg[Len++] = 'F'; Msg[Len++] = 'L'; + Len+=Format_UnsDec(Msg+Len, MetersToFeet((uint32_t)Alt), 5, 2); } + + uint16_t DOP=10+DecodeDOP(); + uint16_t HorPrec=(DOP*2+5)/10; if(HorPrec>63) HorPrec=63; + uint16_t VerPrec=(DOP*3+5)/10; if(VerPrec>63) VerPrec=63; + 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]=0; + return Len; } + +#endif // __AVR__ + + // calculate distance vector [LatDist, LonDist] from a given reference [RefLat, Reflon] + int calcDistanceVector(int32_t &LatDist, int32_t &LonDist, int32_t RefLat, int32_t RefLon, uint16_t LatCos=3000, int32_t MaxDist=0x7FFF) + { LatDist = ((DecodeLatitude()-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 = ((DecodeLongitude()-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 setDistanceVector(int32_t LatDist, int32_t LonDist, int32_t RefLat, int32_t RefLon, uint16_t LatCos=3000) + { EncodeLatitude(RefLat+(LatDist*27)/5); + LonDist = (LonDist<<12)/LatCos; // LonDist/=cosine(Latitude) + EncodeLongitude(RefLon+(LonDist*27)/5); } + + // Centripetal acceleration + static int16_t calcCPaccel(int16_t Speed, int16_t TurnRate) { return ((int32_t)TurnRate*Speed*229+0x10000)>>17; } // [0.1m/s^2] + int16_t calcCPaccel(void) { return calcCPaccel(DecodeSpeed(), DecodeTurnRate()); } + + // Turn radius + static int16_t calcTurnRadius(int16_t Speed, int16_t TurnRate, int16_t MaxRadius=0x7FFF) // [m] ([0.1m/s], [], [m]) + { if(TurnRate==0) return 0; + int32_t Radius = 14675*Speed; + Radius /= TurnRate; Radius = (Radius+128)>>8; + 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++]=':'; + 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++]=' '; + // 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); + Out[Len++]=' '; + 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++]='\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; } + + uint32_t getAddressAndType(void) const { return HeaderWord&0x03FFFFFF; } // Address with address-type: 26-bit + void setAddressAndType(uint32_t AddrAndType) { HeaderWord = (HeaderWord&0xFC000000) | (AddrAndType&0x03FFFFFF); } + + bool goodAddrParity(void) const { return ((Count1s(HeaderWord&0x0FFFFFFF)&1)==0); } // Address parity should be EVEN + void calcAddrParity(void) { if(!goodAddrParity()) HeaderWord ^= 0x08000000; } // if not correct parity, flip the parity bit + + void EncodeLatitude(int32_t Latitude) // encode Latitude: units are 0.0001/60 degrees + { Position.Latitude = Latitude>>3; } + + int32_t DecodeLatitude(void) const + { int32_t Latitude = Position.Latitude; + // if(Latitude&0x00800000) Latitude|=0xFF000000; + Latitude = (Latitude<<3)+4; return Latitude; } + + void EncodeLongitude(int32_t Longitude) // encode Longitude: units are 0.0001/60 degrees + { Position.Longitude = Longitude>>=4; } + + int32_t DecodeLongitude(void) const + { int32_t Longitude = Position.Longitude; + Longitude = (Longitude<<4)+8; return Longitude; } + + bool hasBaro(void) const { return Position.BaroMSB || Position.BaroAltDiff; } + void clrBaro(void) { Position.BaroMSB=0; Position.BaroAltDiff=0; } + int16_t getBaroAltDiff(void) const { int16_t AltDiff=Position.BaroAltDiff; if(Position.BaroMSB==0) AltDiff|=0xFF00; return AltDiff; } + void setBaroAltDiff(int32_t AltDiff) + { if(AltDiff<(-255)) AltDiff=(-255); else if(AltDiff>255) AltDiff=255; + Position.BaroMSB = (AltDiff&0xFF00)==0; Position.BaroAltDiff=AltDiff&0xFF; } + void EncodeStdAltitude(int32_t StdAlt) { setBaroAltDiff((StdAlt-DecodeAltitude())); } + int32_t DecodeStdAltitude(void) const { return (DecodeAltitude()+getBaroAltDiff()); } + + void EncodeAltitude(int32_t Altitude) // encode altitude in meters + { if(Altitude<0) Altitude=0; + Position.Altitude = UnsVRencode((uint16_t)Altitude); } +// Position.Altitude = EncodeUR2V12((uint16_t)Altitude); } + + int32_t DecodeAltitude(void) const // return Altitude in meters + { return UnsVRdecode(Position.Altitude); } +// { return DecodeUR2V12(Position.Altitude); } + + void EncodeDOP(uint8_t DOP) + { Position.DOP = UnsVRencode(DOP); } +// { Position.DOP = EncodeUR2V4(DOP); } + + uint8_t DecodeDOP(void) const + { return UnsVRdecode(Position.DOP); } +// { return DecodeUR2V4(Position.DOP); } + + void EncodeSpeed(int16_t Speed) // speed in 0.2 knots (or 0.1m/s) + { if(Speed<0) Speed=0; + else Speed = UnsVRencode(Speed); // EncodeUR2V8(Speed); + Position.Speed = Speed; } + + int16_t DecodeSpeed(void) const // return speed in 0.2 knots or 0.1m/s units + { return UnsVRdecode(Position.Speed); } +// { return DecodeUR2V8(Position.Speed); } // => max. speed: 3832*0.2 = 766 knots + + int16_t DecodeHeading(void) const // return Heading in 0.1 degree units 0..359.9 deg + { int32_t Heading = Position.Heading; + return (Heading*3600+512)>>10; } + + void EncodeHeading(int16_t Heading) + { Position.Heading = (((int32_t)Heading<<10)+180)/3600; } + + void setHeadingAngle(uint16_t HeadingAngle) + { Position.Heading = (((HeadingAngle+32)>>6)); } + + 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 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 EncodeClimbRate(int16_t Climb) + { Position.ClimbRate = EncodeSR2V6(Climb); } + + int16_t DecodeClimbRate(void) const + { return DecodeSR2V6(Position.ClimbRate); } + +// -------------------------------------------------------------------------------------------------------------- +// Status fields + + void clrTemperature(void) { Status.Temperature=0x80; } + bool hasTemperature(void) const { return Status.Temperature!=0x80; } + void EncodeTemperature(int16_t Temp) { Status.Temperature=EncodeSR2V5(Temp-200); } // [0.1degC] + int16_t DecodeTemperature(void) const { return 200+DecodeSR2V5(Status.Temperature); } + + void EncodeVoltage(uint16_t Voltage) { Status.Voltage=EncodeUR2V6(Voltage); } // [1/64V] + uint16_t DecodeVoltage(void) const { return DecodeUR2V6(Status.Voltage); } + + void clrHumidity(void) { Status.Humidity=0x80; } + bool hasHumidity(void) const { return Status.Humidity!=0x80; } + void EncodeHumidity(uint16_t Hum) { Status.Humidity=EncodeSR2V5((int16_t)(Hum-520)); } // [0.1%] + uint16_t DecodeHumidity(void) const { return 520+DecodeSR2V5(Status.Humidity); } + +// -------------------------------------------------------------------------------------------------------------- +// Info fields: pack and unpack 7-bit char into the Info packets + + void setInfoChar(uint8_t Char, uint8_t Idx) // put 7-bit Char onto give position + { if(Idx>=16) return; // Idx = 0..15 + Char&=0x7F; + uint8_t BitIdx = Idx*7; // [bits] bit index to the target field + Idx = BitIdx>>3; // [bytes] index of the first byte to change + uint8_t Ofs = BitIdx&0x07; + if(Ofs==0) { Info.Data[Idx] = (Info.Data[Idx]&0x80) | Char ; return; } + if(Ofs==1) { Info.Data[Idx] = (Info.Data[Idx]&0x01) | (Char<<1) ; return; } + uint8_t Len1 = 8-Ofs; + uint8_t Len2 = Ofs-1; + uint8_t Msk1 = 0xFF; Msk1<<=Ofs; + uint8_t Msk2 = 0x01; Msk2 = (Msk2<>Len1); } + + uint8_t getInfoChar(uint8_t Idx) const // get 7-bit Char from given position + { if(Idx>=16) return 0; // Idx = 0..15 + uint8_t BitIdx = Idx*7; // [bits] bit index to the target field + Idx = BitIdx>>3; // [bytes] index of the first byte to change + uint8_t Ofs = BitIdx&0x07; + if(Ofs==0) return Info.Data[Idx]&0x7F; + if(Ofs==1) return Info.Data[Idx]>>1; + uint8_t Len = 8-Ofs; + return (Info.Data[Idx]>>Ofs) | ((Info.Data[Idx+1]<=15) return 0; + uint8_t Len=0; + for( ; ; ) + { uint8_t Char = Value[Len]; if(Char==0) break; + if(Idx>=15) return 0; + setInfoChar(Char, Idx++); + Len++; } + setInfoChar(InfoType, Idx); // terminating character + Info.DataChars=Idx; // update number of characters + return Len+1; } // return number of added Value characters + + uint8_t readInfo(char *Value, uint8_t &InfoType, uint8_t ValueIdx=0) const + { uint8_t Len=0; // count characters in the info-string + uint8_t Chars = Info.DataChars; // total number of characters in the record + char Char=0; + for( ; ; ) // loop over characters + { if((ValueIdx+Len)>Chars) return 0; // return failure if overrun the data + Char = getInfoChar(ValueIdx+Len); // get the character + if(Char<0x20) break; // if less than 0x20 (space) then this is the terminator + Value[Len++]=Char; } + Value[Len]=0; // null-terminate the infor string + InfoType=Char; // get the info-type: Pilot, Type, etc. + return Len+1; } // return number of character taken thus info length + terminator + + uint8_t InfoCheck(void) const + { uint8_t Check=0; + for( uint8_t Idx=0; Idx<15; Idx++) + { Check ^= Info.Byte[Idx]; } + // printf("Check = %02X\n", Check); + return Check; } + + void setInfoCheck(void) + { Info.Check = InfoCheck(); + // printf("Check = %02X\n", Info.Check); + } + + uint8_t goodInfoCheck(void) const + { return Info.Check == InfoCheck(); } + +// -------------------------------------------------------------------------------------------------------------- + + void Encrypt (const uint32_t Key[4]) { XXTEA_Encrypt(Data, 4, Key, 8); } // encrypt with given Key + void Decrypt (const uint32_t Key[4]) { XXTEA_Decrypt(Data, 4, Key, 8); } // decrypt with given Key + + void Whiten (void) { TEA_Encrypt_Key0(Data, 8); TEA_Encrypt_Key0(Data+2, 8); } // whiten the position + void Dewhiten(void) { TEA_Decrypt_Key0(Data, 8); TEA_Decrypt_Key0(Data+2, 8); } // de-whiten the position + + uint8_t getTxSlot(uint8_t Idx) const // Idx=0..15 + { const uint32_t *DataPtr = Data; + uint32_t Mask=1; Mask<<=Idx; + uint8_t Slot=0; + for(uint8_t Bit=0; Bit<6; Bit++) + { Slot>>=1; + if(DataPtr[Bit]&Mask) Slot|=0x20; + Mask<<=1; Slot>>=1; } + return EncodeGray(Slot); } + +} ; + +#endif // of __OGN1_H__ + diff --git a/utils/ogn2.h b/utils/ogn2.h new file mode 100644 index 0000000..dfa8512 --- /dev/null +++ b/utils/ogn2.h @@ -0,0 +1,434 @@ +#ifndef __OGN2_H__ +#define __OGN2_H__ + +#include + +#include +#include + +#include "intmath.h" + +#include "ognconv.h" + +#include "bitcount.h" +#include "nmea.h" +#include "mavlink.h" + +#include "format.h" + +// uint32_t OGN2_SYNC = 0xF56D3738; + + // the packet description here is how it look on the little-endian CPU before sending it to the RF chip + // nRF905, CC1101, SPIRIT1, RFM69 chips actually reverse the bit order within every byte + // thus on the air the bits appear MSbit first for every byte transmitted + +class OGN2_Packet // Packet structure for the OGN tracker +{ public: + + static const int Words = 5; + static const int Bytes = 20; + + union + { uint32_t HeaderWord; // ECNR POTT AAAA AAAA AAAA AAAA AAAA AAAA + // E=Emergency, C=enCrypt/Custom, R=Relay, P=Parity, O=Other, TT=address Type, AA..=Address:24-bit + // When enCrypt/Custom is set the data (position or whatever) can only be decoded by the owner + // This option is indented to pass any type of custom data not foreseen otheriwse + + struct + { unsigned int Address :24; // aircraft address + unsigned int AddrType : 2; // address type: 0 = random, 1 = ICAO, 2 = FLARM, 3 = OGN + unsigned int Relay : 1; // 0 = direct packet, 1 = relayed packet + unsigned int Parity : 1; // parity takes into account bits 0..27 thus only the 28 lowest bits + unsigned int NonPos : 1; // 0 = position packet, 1 = other information like status + // unsigned int Auth : 2; // Authentication: 00 = no auth. 01 = auth. this packet, 10/11 = auth response + // Auth:NonPos: 000 = position, 010 = position, to be followed by crypto-response, + // 101 = response #0, 111 = response #1 + // 001 = non-position: status, info, etc. + // 100 = ???, 110 = ???, 011 = ??? + unsigned int NonOGN : 1; // 0 = OGN packet, 1 = other systems, like MAVlink + unsigned int Encrypted : 1; // packet is encrypted + unsigned int Emergency : 1; // aircraft in emergency (not used for now) + } Header ; + + } ; + + union + { uint32_t Data[4]; + + struct + { unsigned int AcftType: 4; // [0..15] // type of aircraft: 1 = glider, 2 = towplane, 3 = helicopter, ... + unsigned int Heading:10; // [0.35deg] // Ground track in cordic units + unsigned int ClimbRate: 9; // [0.1m/s] VR + unsigned int BaroAltDiff: 9; // [m] // lower 8 bits of the altitude difference between baro and GPS + + unsigned int Altitude:14; // [m] VR // Altitude + unsigned int Speed:10; // [0.1m/s] VR // Ground speed + unsigned int TurnRate: 8; // [0.1deg/s] VR // Ground heading (track) rate + + unsigned int Latitude:24; // [1.2m] // Latitude in cordic units + unsigned int Time: 6; // [sec] // time, just second thus ambiguity every every minute + unsigned int FixQuality: 2; // // 0 = no fix, 1 = GPS, 2 = diff. GPS, 3 = other + + unsigned int Longitude:25; // [1.2m] // Longitude in cordic units + unsigned int DOP: 6; // // GPS Dilution of Precision + unsigned int FixMode: 1; // [2-D/3-D] + } Position; + + + struct + { + unsigned int ReportType: 4; // [0] // 0 for the status report + unsigned int TxPower : 4; // [dBm] // RF trancmitter power + unsigned int Firmware : 8; // [ ] // firmware version + unsigned int Hardware : 8; // [ ] // hardware version + unsigned int Voltage : 8; // [1/64V] VR // supply voltager + + unsigned int Altitude :14; // [m] VR // same as in the position packet + unsigned int Pressure :14; // [0.08hPa] // barometric pressure + unsigned int Satellites: 4; // [ ] + + unsigned int Pulse : 8; // [bpm] // pilot: heart pulse rate + unsigned int Oxygen : 7; // [%] // pilot: oxygen level in the blood + // unsigned int FEScurr : 5; // [A] // + unsigned int SatSNR : 5; // [dB] // average SNR of GPS signals + unsigned int RxRate : 4; // [/min] // log2 of received packet rate + unsigned int Time : 6; // [sec] // same as in the position packet + unsigned int FixQuality: 2; + + unsigned int AudioNoise: 8; // [dB] // + unsigned int RadioNoise: 8; // [dBm] // noise seen by the RF chip + unsigned int Temperature:8; // [0.1degC] VR // temperature by the baro or RF chip + unsigned int Humidity : 8; // [%] // humidity + } Status ; + + union + { uint8_t Byte[16]; + struct + { uint8_t ReportType: 4; // [1] // 1 for the Info packets + uint8_t DataChars: 4; // [int] number of characters in the packed string + uint8_t Data[14]; // [16x7bit]packed string of 16-char: 7bit/char + uint8_t Check; // CRC check + } ; + } Info; + + } ; + + uint8_t *Byte(void) const { return (uint8_t *)&HeaderWord; } // packet as bytes + uint32_t *Word(void) const { return (uint32_t *)&HeaderWord; } // packet as words + + static const uint8_t InfoParmNum = 14; // [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" } ; + return Idx>13; // convert from 1/600000deg to meters (40000000m = 360deg) => x 5/27 = 151$ + if(abs(LatDist)>MaxDist) return -1; + LonDist = ((DecodeLongitude()-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 setDistanceVector(int32_t LatDist, int32_t LonDist, int32_t RefLat, int32_t RefLon, uint16_t LatCos=3000) + { EncodeLatitude(RefLat+(LatDist*27)/5); + LonDist = (LonDist<<12)/LatCos; // LonDist/=cosine(Latitude) + EncodeLongitude(RefLon+(LonDist*27)/5); } + + // Centripetal acceleration + static int16_t calcCPaccel(int16_t Speed, int16_t TurnRate) { return ((int32_t)TurnRate*Speed*229+0x10000)>>17; } // [0.1m/s^2] + int16_t calcCPaccel(void) { return calcCPaccel(DecodeSpeed(), DecodeTurnRate()); } + + // Turn radius + static int16_t calcTurnRadius(int16_t Speed, int16_t TurnRate, int16_t MaxRadius=0x7FFF) // [m] ([0.1m/s], [], [m]) + { if(TurnRate==0) return 0; + int32_t Radius = 14675*Speed; + Radius /= TurnRate; Radius = (Radius+128)>>8; + if(abs(Radius)>MaxRadius) return 0; + return Radius; } + int16_t calcTurnRadius(int16_t MaxRadius=0x7FFF) { return calcTurnRadius(DecodeSpeed(), DecodeTurnRate(), MaxRadius); } + + void Clear(void) { HeaderWord=0; Data[0]=0; Data[1]=0; Data[2]=0; Data[3]=0; } + + uint32_t getAddressAndType(void) const { return HeaderWord&0x03FFFFFF; } // Address with address-type: 26-bit + void setAddressAndType(uint32_t AddrAndType) { HeaderWord = (HeaderWord&0xFC000000) | (AddrAndType&0x03FFFFFF); } + + bool goodAddrParity(void) const { return ((Count1s(HeaderWord&0x0FFFFFFF)&1)==0); } // Address parity should be EVEN + void calcAddrParity(void) { if(!goodAddrParity()) HeaderWord ^= 0x08000000; } // if not correct parity, flip the parity bit + + void clrBaro(void) { Position.BaroAltDiff=EncodeGray((uint16_t)0x100); } + bool hasBaro(void) const { return DecodeGray((uint16_t)Position.BaroAltDiff)!=0x100; } + + void setBaroAltDiff(int32_t AltDiff) + { if(AltDiff<(-255)) AltDiff=(-255); + else if(AltDiff>255) AltDiff=255; + Position.BaroAltDiff=EncodeGray((uint16_t)(AltDiff&0x1FF)); } + int16_t getBaroAltDiff(void) const + { int16_t AltDiff=DecodeGray((uint16_t)Position.BaroAltDiff); + if(AltDiff & 0x100) AltDiff |= 0xFE00; + return AltDiff; } + + void EncodeStdAltitude(int32_t StdAlt) { setBaroAltDiff((StdAlt-DecodeAltitude())); } + int32_t DecodeStdAltitude(void) const { return (DecodeAltitude()+getBaroAltDiff()); } + + void EncodeAltitude(int32_t Altitude) // encode altitude in meters + { Altitude += 1024; + if(Altitude<0) Altitude=0; + Position.Altitude = EncodeGray((uint16_t)UnsVRencode((uint16_t)Altitude)); } + + int32_t DecodeAltitude(void) const // return Altitude in meters + { return (int32_t)UnsVRdecode(DecodeGray((uint16_t)Position.Altitude))-1024; } + + static const int32_t FullAngle = 360*600000; // Latitude and Longitude are encoded in units of 1/10000 arcmin + static const int32_t HalfAngle = FullAngle/2; + + // void EncodeLatitude(int32_t Latitude) + // { Position.Latitude = ((int64_t)Latitude*83399993+(1<<28))>>29; } + + // int32_t DecodeLatitude(void) const + // { int32_t Latitude = Position.Latitude; + // return ((int64_t)Latitude*HalfAngle+(1<<23))>>24; } + + void EncodeLatitude(int32_t Latitude) + { Latitude = ((int64_t)Latitude*83399993+(1<<28))>>29; // convert to cordic units + Latitude &= 0x00FFFFFF; + Position.Latitude = EncodeGray((uint32_t)Latitude); } + + int32_t DecodeLatitude(void) const + { int32_t Latitude = DecodeGray((uint32_t)Position.Latitude); + if(Latitude&0x00800000) Latitude |= 0xFF000000; + return ((int64_t)Latitude*HalfAngle+(1<<23))>>24; } // convert from cordic units to 1/10000 arcmin + + void EncodeLongitude(int32_t Longitude) + { Longitude = ((int64_t)Longitude*83399993+(1<<28))>>29; + Longitude &= 0x01FFFFFF; + Position.Longitude = EncodeGray((uint32_t)Longitude); } + + int32_t DecodeLongitude(void) const + { int32_t Longitude = DecodeGray((uint32_t)Position.Longitude); + if(Longitude&0x01000000) Longitude |= 0xFE000000; + Longitude = ((int64_t)Longitude*HalfAngle+(1<<23))>>24; + return Longitude; } + + void EncodeDOP(uint8_t DOP) + { Position.DOP = EncodeGray(UnsVRencode(DOP)); } + + uint8_t DecodeDOP(void) const + { return UnsVRdecode(DecodeGray((uint8_t)Position.DOP)); } + + void EncodeSpeed(int16_t Speed) // speed in 0.2 knots (or 0.1m/s) + { if(Speed<0) Speed=0; + else Speed=UnsVRencode(Speed); + Position.Speed = EncodeGray((uint16_t)Speed); } + + int16_t DecodeSpeed(void) const // return speed in 0.2 knots or 0.1m/s units + { return UnsVRdecode(DecodeGray((uint16_t)Position.Speed)); } // => max. speed: 3832*0.2 = 766 knots + + void EncodeHeading(int16_t Heading) + { if(Heading<0) Heading+=3600; + Heading = ((int32_t)Heading*1165+(1<<11))>>12; + Position.Heading = EncodeGray((uint16_t)Heading); } + + int16_t DecodeHeading(void) const // return Heading in 0.1 degree units 0..359.9 deg + { int32_t Heading = DecodeGray((uint16_t)Position.Heading); + return (Heading*3600+512)>>10; } + + void setHeadingAngle(uint16_t HeadingAngle) + { Position.Heading = EncodeGray((uint16_t)((HeadingAngle+32)>>6)); } + + uint16_t getHeadingAngle(void) const + { return (uint16_t)DecodeGray((uint16_t)Position.Heading)<<6; } + + void clrTurnRate(void) { Position.TurnRate=0x80; } + bool hasTurnRate(void) const { return Position.TurnRate==0x80; } + + void EncodeTurnRate(int16_t Turn) // [0.1 deg/sec] + { Position.TurnRate = SignVRencode(Turn); } + + int16_t DecodeTurnRate(void) const + { return SignVRdecode(Position.TurnRate); } + + void clrClimbRate(void) { Position.ClimbRate=0x100; } + bool hasClimbRate(void) const { return Position.ClimbRate==0x100; } + + void EncodeClimbRate(int16_t Climb) // [0.1m/s] + { Position.ClimbRate = SignVRencode(Climb); } + + int16_t DecodeClimbRate(void) const + { return SignVRdecode(Position.ClimbRate); } + +// -------------------------------------------------------------------------------------------------------------- +// Status fields + + void clrTemperature(void) { Status.Temperature=EncodeGray((uint8_t)0x80); } + bool hasTemperature(void) const { return DecodeGray((uint8_t)Status.Temperature)!=0x80; } + void EncodeTemperature(int16_t Temp) { Status.Temperature=EncodeGray(EncodeSR2V5(Temp-200)); } // [0.1degC] + int16_t DecodeTemperature(void) const { return 200+DecodeSR2V5(DecodeGray((uint8_t)Status.Temperature)); } + + void clrHumidity(void) { Status.Humidity=EncodeGray((uint8_t)0x80); } + bool hasHumidity(void) const { return DecodeGray((uint8_t)Status.Humidity)!=0x80; } + void EncodeHumidity(uint16_t Hum) { Status.Humidity=EncodeGray(EncodeSR2V5((int16_t)(Hum-525))); } // [0.1%] + uint16_t DecodeHumidity(void) const { return 525+DecodeSR2V5(DecodeGray((uint8_t)Status.Humidity)); } + + void EncodeVoltage_mV(uint16_t Voltage) { EncodeVoltage((Voltage*8+63)/125); } + void EncodeVoltage(uint16_t Voltage) // [1/64V] + { if(Voltage<80) Voltage = 0; + else Voltage-=80; + Status.Voltage=EncodeGray(EncodeUR2V6(Voltage)); } + uint16_t DecodeVoltage(void) const { return 80+DecodeUR2V6(DecodeGray((uint8_t)Status.Voltage)); } + +// -------------------------------------------------------------------------------------------------------------- +// Info fields: pack and unpack 7-bit char into the Info packets + + void setInfoChar(uint8_t Char, uint8_t Idx) // put 7-bit Char onto give position + { if(Idx>=16) return; // Idx = 0..15 + Char&=0x7F; + uint8_t BitIdx = Idx*7; // [bits] bit index to the target field + Idx = BitIdx>>3; // [bytes] index of the first byte to change + uint8_t Ofs = BitIdx&0x07; + if(Ofs==0) { Info.Data[Idx] = (Info.Data[Idx]&0x80) | Char ; return; } + if(Ofs==1) { Info.Data[Idx] = (Info.Data[Idx]&0x01) | (Char<<1) ; return; } + uint8_t Len1 = 8-Ofs; + uint8_t Len2 = Ofs-1; + uint8_t Msk1 = 0xFF; Msk1<<=Ofs; + uint8_t Msk2 = 0x01; Msk2 = (Msk2<>Len1); } + + uint8_t getInfoChar(uint8_t Idx) const // get 7-bit Char from given position + { if(Idx>=16) return 0; // Idx = 0..15 + uint8_t BitIdx = Idx*7; // [bits] bit index to the target field + Idx = BitIdx>>3; // [bytes] index of the first byte to change + uint8_t Ofs = BitIdx&0x07; + if(Ofs==0) return Info.Data[Idx]&0x7F; + if(Ofs==1) return Info.Data[Idx]>>1; + uint8_t Len = 8-Ofs; + return (Info.Data[Idx]>>Ofs) | ((Info.Data[Idx+1]<=15) return 0; + uint8_t Len=0; + for( ; ; ) + { uint8_t Char = Value[Len]; if(Char==0) break; + if(Idx>=15) return 0; + setInfoChar(Char, Idx++); + Len++; } + setInfoChar(InfoType, Idx); // terminating character + Info.DataChars=Idx; // update number of characters + return Len+1; } // return number of added Value characters + + uint8_t readInfo(char *Value, uint8_t &InfoType, uint8_t ValueIdx=0) const + { uint8_t Len=0; // count characters in the info-string + uint8_t Chars = Info.DataChars; // total number of characters in the record + char Char=0; + for( ; ; ) // loop over characters + { if((ValueIdx+Len)>Chars) return 0; // return failure if overrun the data + Char = getInfoChar(ValueIdx+Len); // get the character + if(Char<0x20) break; // if less than 0x20 (space) then this is the terminator + Value[Len++]=Char; } + Value[Len]=0; // null-terminate the infor string + InfoType=Char; // get the info-type: Pilot, Type, etc. + return Len+1; } // return number of character taken thus info length + terminator + + uint8_t InfoCheck(void) const + { uint8_t Check=0; + for( uint8_t Idx=0; Idx<15; Idx++) + { Check ^= Info.Byte[Idx]; } + // printf("Check = %02X\n", Check); + return Check; } + + void setInfoCheck(void) + { Info.Check = InfoCheck(); + // printf("Check = %02X\n", Info.Check); + } + + uint8_t goodInfoCheck(void) const + { return Info.Check == InfoCheck(); } + +// -------------------------------------------------------------------------------------------------------------- + + void Whiten (uint8_t Prefix=0x05) + { uint32_t Mask = ((uint32_t)Prefix<<26) | (HeaderWord & 0x03FFFFFF); + for( uint8_t Idx=0; Idx<4; Idx++) + { XorShift32(Mask); Mask += 0x01234567; + Data[Idx] ^= Mask; } + } + + void Dewhiten(void) { Whiten(); } + +} ; + +#endif // __OGN2_H__ + diff --git a/utils/ognconv.cpp b/utils/ognconv.cpp new file mode 100644 index 0000000..5ca9959 --- /dev/null +++ b/utils/ognconv.cpp @@ -0,0 +1,271 @@ +#include + +#include "ognconv.h" + +// ============================================================================================== + +uint32_t FeetToMeters(uint32_t Altitude) { return (Altitude*312+512)>>10; } // [feet] => [m] +uint32_t MetersToFeet(uint32_t Altitude) { return (Altitude*3360+512)>>10; } // [m] => [feet] + +// ============================================================================================== + +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); + else if(Value<0x700) Value = 0x200 | ((Value-0x300)>>2); + else if(Value<0xF00) Value = 0x300 | ((Value-0x700)>>3); + else Value = 0x3FF; + return Value; } + +uint16_t DecodeUR2V8(uint16_t Value) // Decode 10bit 0..0x3FF +{ uint16_t Range = Value>>8; + Value &= 0x0FF; + if(Range==0) return Value; // 000..0FF + if(Range==1) return 0x101+(Value<<1); // 100..2FE + if(Range==2) return 0x302+(Value<<2); // 300..6FC + return 0x704+(Value<<3); } // 700..EF8 // in 12bit (0..3832) + + +uint8_t EncodeUR2V5(uint16_t Value) // Encode unsigned 9bit (0..472) as 7bit +{ if(Value<0x020) { } + else if(Value<0x060) Value = 0x020 | ((Value-0x020)>>1); + else if(Value<0x0E0) Value = 0x040 | ((Value-0x060)>>2); + else if(Value<0x1E0) Value = 0x060 | ((Value-0x0E0)>>3); + else Value = 0x07F; + return Value; } + +uint16_t DecodeUR2V5(uint16_t Value) // Decode 7bit as unsigned 9bit (0..472) +{ uint8_t Range = (Value>>5)&0x03; + Value &= 0x1F; + if(Range==0) { } // 000..01F + else if(Range==1) { Value = 0x021+(Value<<1); } // 020..05E + else if(Range==2) { Value = 0x062+(Value<<2); } // 060..0DC + else { Value = 0x0E4+(Value<<3); } // 0E0..1D8 => max. Value = 472 + return Value; } + +uint8_t EncodeSR2V5(int16_t Value) // Encode signed 10bit (-472..+472) as 8bit +{ uint8_t Sign=0; if(Value<0) { Value=(-Value); Sign=0x80; } + Value = EncodeUR2V5(Value); + return Value | Sign; } + +int16_t DecodeSR2V5( int16_t Value) // Decode +{ int16_t Sign = Value&0x80; + Value = DecodeUR2V5(Value&0x7F); + return Sign ? -Value: Value; } + +uint16_t EncodeUR2V6(uint16_t Value) // Encode unsigned 10bit (0..952) as 8 bit +{ if(Value<0x040) { } + else if(Value<0x0C0) Value = 0x040 | ((Value-0x040)>>1); + else if(Value<0x1C0) Value = 0x080 | ((Value-0x0C0)>>2); + else if(Value<0x3C0) Value = 0x0C0 | ((Value-0x1C0)>>3); + else Value = 0x0FF; + return Value; } + +uint16_t DecodeUR2V6(uint16_t Value) // Decode 8bit as unsigned 10bit (0..952) +{ uint16_t Range = (Value>>6)&0x03; + Value &= 0x3F; + if(Range==0) { } // 000..03F + else if(Range==1) { Value = 0x041+(Value<<1); } // 040..0BE + else if(Range==2) { Value = 0x0C2+(Value<<2); } // 0C0..1BC + else { Value = 0x1C4+(Value<<3); } // 1C0..3B8 => max. Value = 952 + return Value; } + +uint16_t EncodeSR2V6(int16_t Value) // Encode signed 11bit (-952..+952) as 9bit +{ uint16_t Sign=0; if(Value<0) { Value=(-Value); Sign=0x100; } + Value = EncodeUR2V6(Value); + return Value | Sign; } + + int16_t DecodeSR2V6( int16_t Value) // Decode 9bit as signed 11bit (-952..+952) +{ int16_t Sign = Value&0x100; + Value = DecodeUR2V6(Value&0x00FF); + return Sign ? -Value: Value; } + +uint8_t EncodeUR2V4(uint8_t DOP) +{ if(DOP<0x10) { } + else if(DOP<0x30) DOP = 0x10 | ((DOP-0x10)>>1); + else if(DOP<0x70) DOP = 0x20 | ((DOP-0x30)>>2); + else if(DOP<0xF0) DOP = 0x30 | ((DOP-0x70)>>3); + else DOP = 0x3F; + return DOP; } + +uint8_t DecodeUR2V4(uint8_t DOP) +{ uint8_t Range = DOP>>4; + DOP &= 0x0F; + if(Range==0) return DOP; // 00..0F + if(Range==1) return 0x11+(DOP<<1); // 10..2E + if(Range==2) return 0x32+(DOP<<2); // 30..6C + return 0x74+(DOP<<3); } // 70..E8 => max. DOP = 232*0.1=23.2 + +uint16_t EncodeUR2V12(uint16_t Value) // encode unsigned 16-bit (0..61432) as 14-bit +{ if(Value<0x1000) { } + else if(Value<0x3000) Value = 0x1000 | ((Value-0x1000)>>1); + else if(Value<0x7000) Value = 0x2000 | ((Value-0x3000)>>2); + else if(Value<0xF000) Value = 0x3000 | ((Value-0x7000)>>3); + else Value = 0x3FFF; + return Value; } + +uint16_t DecodeUR2V12(uint16_t Value) +{ uint16_t Range = Value>>12; + Value &=0x0FFF; + if(Range==0) return Value; // 0000..0FFF + if(Range==1) return 0x1001+(Value<<1); // 1000..2FFE + if(Range==2) return 0x3002+(Value<<2); // 3000..6FFC + return 0x7004+(Value<<3); } // 7000..EFF8 => max: 61432 + +// ============================================================================================== + +uint8_t EncodeGray(uint8_t Binary) +{ return Binary ^ (Binary>>1); } + +uint8_t DecodeGray(uint8_t Gray) +{ Gray ^= (Gray >> 4); + Gray ^= (Gray >> 2); + Gray ^= (Gray >> 1); + return Gray; } + +uint16_t EncodeGray(uint16_t Binary) +{ return Binary ^ (Binary>>1); } + +uint16_t DecodeGray(uint16_t Gray) +{ Gray ^= (Gray >> 8); + Gray ^= (Gray >> 4); + Gray ^= (Gray >> 2); + Gray ^= (Gray >> 1); + return Gray; } + +uint32_t EncodeGray(uint32_t Binary) +{ return Binary ^ (Binary>>1); } + +uint32_t DecodeGray(uint32_t Gray) +{ Gray ^= (Gray >>16); + Gray ^= (Gray >> 8); + Gray ^= (Gray >> 4); + Gray ^= (Gray >> 2); + Gray ^= (Gray >> 1); + return Gray; } + +// ============================================================================================== +// TEA encryption/decryption +// Data is 2 x 32-bit word +// Key is 4 x 32-bit word + +void TEA_Encrypt (uint32_t* Data, const uint32_t *Key, int Loops) +{ uint32_t v0=Data[0], v1=Data[1]; // set up + const uint32_t delta=0x9e3779b9; uint32_t sum=0; // a key schedule constant + uint32_t k0=Key[0], k1=Key[1], k2=Key[2], k3=Key[3]; // cache key + for (int i=0; i < Loops; i++) // basic cycle start + { sum += delta; + v0 += ((v1<<4) + k0) ^ (v1 + sum) ^ ((v1>>5) + k1); + v1 += ((v0<<4) + k2) ^ (v0 + sum) ^ ((v0>>5) + k3); } // end cycle + Data[0]=v0; Data[1]=v1; +} + +void TEA_Decrypt (uint32_t* Data, const uint32_t *Key, int Loops) +{ uint32_t v0=Data[0], v1=Data[1]; // set up + const uint32_t delta=0x9e3779b9; uint32_t sum=delta*Loops; // a key schedule constant + uint32_t k0=Key[0], k1=Key[1], k2=Key[2], k3=Key[3]; // cache key + for (int i=0; i < Loops; i++) // basic cycle start */ + { v1 -= ((v0<<4) + k2) ^ (v0 + sum) ^ ((v0>>5) + k3); + v0 -= ((v1<<4) + k0) ^ (v1 + sum) ^ ((v1>>5) + k1); + sum -= delta; } // end cycle + Data[0]=v0; Data[1]=v1; +} + +void TEA_Encrypt_Key0 (uint32_t* Data, int Loops) +{ uint32_t v0=Data[0], v1=Data[1]; // set up + const uint32_t delta=0x9e3779b9; uint32_t sum=0; // a key schedule constant + for (int i=0; i < Loops; i++) // basic cycle start + { sum += delta; + v0 += (v1<<4) ^ (v1 + sum) ^ (v1>>5); + v1 += (v0<<4) ^ (v0 + sum) ^ (v0>>5); } // end cycle + Data[0]=v0; Data[1]=v1; +} + +void TEA_Decrypt_Key0 (uint32_t* Data, int Loops) +{ uint32_t v0=Data[0], v1=Data[1]; // set up + const uint32_t delta=0x9e3779b9; uint32_t sum=delta*Loops; // a key schedule constant + for (int i=0; i < Loops; i++) // basic cycle start + { v1 -= (v0<<4) ^ (v0 + sum) ^ (v0>>5); + v0 -= (v1<<4) ^ (v1 + sum) ^ (v1>>5); + sum -= delta; } // end cycle + Data[0]=v0; Data[1]=v1; +} + +// ============================================================================================== +// XXTEA encryption/decryption + +static uint32_t XXTEA_MX(uint8_t E, uint32_t Y, uint32_t Z, uint8_t P, uint32_t Sum, const uint32_t Key[4]) +{ return ((((Z>>5) ^ (Y<<2)) + ((Y>>3) ^ (Z<<4))) ^ ((Sum^Y) + (Key[(P&3)^E] ^ Z))); } + +void XXTEA_Encrypt(uint32_t *Data, uint8_t Words, const uint32_t Key[4], 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; + uint8_t E = (Sum>>2)&3; + for (uint8_t P=0; P<(Words-1); P++) + { Y = Data[P+1]; + Z = Data[P] += XXTEA_MX(E, Y, Z, P, Sum, Key); } + Y = Data[0]; + Z = Data[Words-1] += XXTEA_MX(E, Y, Z, Words-1, Sum, Key); + } +} + +void XXTEA_Decrypt(uint32_t *Data, uint8_t Words, const uint32_t Key[4], uint8_t Loops) +{ const uint32_t Delta = 0x9e3779b9; + uint32_t Sum = Loops*Delta; + uint32_t Y = Data[0]; uint32_t Z; + for( ; Loops; Loops--) + { uint8_t E = (Sum>>2)&3; + for (uint8_t P=Words-1; P; P--) + { Z = Data[P-1]; + Y = Data[P] -= XXTEA_MX(E, Y, Z, P, Sum, Key); } + Z = Data[Words-1]; + Y = Data[0] -= XXTEA_MX(E, Y, Z, 0, Sum, Key); + Sum -= Delta; + } +} + +// ============================================================================================== + +void XorShift32(uint32_t &Seed) // simple random number generator +{ Seed ^= Seed << 13; + Seed ^= Seed >> 17; + Seed ^= Seed << 5; } + +void xorshift64(uint64_t &Seed) +{ Seed ^= Seed >> 12; + Seed ^= Seed << 25; + Seed ^= Seed >> 27; } + +// ============================================================================================== + +const static unsigned char MapAscii85[86] = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz!#$%&()*+-;<=>?@^_`{|}~"; + +const static uint8_t UnmapAscii85[128] = +{ 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, + 85, 62, 85, 63, 64, 65, 66, 85, 67, 68, 69, 70, 85, 71, 85, 85, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 85, 72, 73, 74, 75, 76, + 77, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 85, 85, 85, 78, 79, + 80, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 81, 82, 83, 84, 85 }; + +uint8_t EncodeAscii85(char *Ascii, uint32_t Word) +{ for( uint8_t Idx=5; Idx; ) + { uint32_t Div = Word/85; + Idx--; + Ascii[Idx]=MapAscii85[Word-Div*85]; + Word=Div; } + Ascii[5]=0; + return 5; } + +uint8_t DecodeAscii85(uint32_t &Word, const char *Ascii) +{ Word=0; + for( uint8_t Idx=0; Idx<5; Idx++) + { char Char = Ascii[Idx]; if(Char<=0) return 0; + uint8_t Dig = UnmapAscii85[(uint8_t)Char]; + if(Dig>=85) return 0; + Word = Word*85+Dig; } + return 5; } + +// ============================================================================================== + diff --git a/utils/ognconv.h b/utils/ognconv.h new file mode 100644 index 0000000..7b931ef --- /dev/null +++ b/utils/ognconv.h @@ -0,0 +1,85 @@ +#ifndef __OGNCONV_H__ +#define __OGNCONV_H__ + +#include + +uint32_t FeetToMeters(uint32_t Altitude); // +uint32_t MetersToFeet(uint32_t Altitude); // + +uint16_t EncodeUR2V8(uint16_t Value); // Encode unsigned 12bit (0..3832) as 10bit +uint16_t DecodeUR2V8(uint16_t Value); // Decode 10bit 0..0x3FF + +uint8_t EncodeUR2V5(uint16_t Value); // Encode unsigned 9bit (0..472) as 7bit +uint16_t DecodeUR2V5(uint16_t Value); // Decode 7bit as unsigned 9bit (0..472) + +uint8_t EncodeSR2V5(int16_t Value); // Encode signed 10bit (-472..+472) as 8bit +int16_t DecodeSR2V5( int16_t Value); // Decode + +uint16_t EncodeUR2V6(uint16_t Value); // Encode unsigned 10bit (0..952) as 8 bit +uint16_t DecodeUR2V6(uint16_t Value); // Decode 8bit as unsigned 10bit (0..952) + +uint16_t EncodeSR2V6(int16_t Value); // Encode signed 11bit (-952..+952) as 9bit + int16_t DecodeSR2V6( int16_t Value); // Decode 9bit as signed 11bit (-952..+952) + +// uint16_t EncodeUR2V12(uint16_t Value); // encode unsigned 16-bit (0..61432) as 14-bit +// uint16_t DecodeUR2V12(uint16_t Value); + +// uint8_t EncodeUR2V4(uint8_t DOP); +// uint8_t DecodeUR2V4(uint8_t DOP); + +template + Type UnsVRdecode(Type Value) +{ const Type Thres = 1<>Bits; + Value &= Thres-1; + if(Range==0) return Value; + if(Range==1) return Thres+1+(Value<<1); + if(Range==2) return 3*Thres+2+(Value<<2); + return 7*Thres+4+(Value<<3); } + +template + Type UnsVRencode(Type Value) +{ const Type Thres = 1<>1); + if(Value< 7*Thres) return 2*Thres | ((Value-3*Thres)>>2); + if(Value<15*Thres) return 3*Thres | ((Value-7*Thres)>>3); + return 4*Thres-1; } + +template + Type SignVRencode(Type Value) +{ const Type SignMask = 1<<(Bits+2); + Type Sign=0; if(Value<0) { Value=(-Value); Sign=SignMask; } + Value = UnsVRencode(Value); + return Value | Sign; } + +template + Type SignVRdecode(Type Value) +{ const Type SignMask = 1<<(Bits+2); + Type Sign = Value&SignMask; + Value = UnsVRdecode(Value&(SignMask-1)); + return Sign ? -Value: Value; } + +uint8_t EncodeGray(uint8_t Binary); +uint8_t DecodeGray(uint8_t Gray); +uint16_t EncodeGray(uint16_t Binary); +uint16_t DecodeGray(uint16_t Gray); +uint32_t EncodeGray(uint32_t Binary); +uint32_t DecodeGray(uint32_t Gray); + +void TEA_Encrypt (uint32_t* Data, const uint32_t *Key, int Loops); +void TEA_Decrypt (uint32_t* Data, const uint32_t *Key, int Loops); + +void TEA_Encrypt_Key0 (uint32_t* Data, int Loops); +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 XorShift32(uint32_t &Seed); // simple random number generator +void xorshift64(uint64_t &Seed); + +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 + +#endif // __OGNCONV_H__ diff --git a/utils/read_log.cc b/utils/read_log.cc new file mode 100644 index 0000000..00a34fd --- /dev/null +++ b/utils/read_log.cc @@ -0,0 +1,132 @@ +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "serial.h" +#include "ogn.h" + +static double getTime(void) // read the system time at this very moment +{ struct timespec now; clock_gettime(CLOCK_REALTIME, &now); return now.tv_sec + 1e-9*now.tv_nsec; } + +static const char *PortName = "/dev/ttyUSB0"; // default serial port name +static int BaudRate = 115200; // default baud rate on the serial port +static int ListOnly = 0; +static uint32_t SelectFile = 0; +static int Help = 0; + +static SerialPort Port; + +const int MaxLineLen = 512; +static char Line[MaxLineLen]; + +static std::vector LogFileList; // list of log file start times + +static int List(void) +{ + sleep(1); + Port.Write(12); // send Ctrl-L to list the log files + + uint8_t Index[32]; + int LineIdx=0; + double Start=getTime(); // sart counting the time + for( ; ; ) + { char Byte; + if(Port.Read(Byte)<=0) // get a byte from the serial port + { double Now=getTime(); // if non, then check time + if((Now-Start)>=10.0) break; // if idle for more than 4 sec then stop recording the log files + usleep(1000); continue; } // if no new bytes on the serial port sleep a little + // printf("%3d: %02X %c\n", LineIdx, Byte, Byte<=' '?' ':Byte); + if(Byte<' ') // if a control (non-printable) character + { Line[LineIdx]=0; // terminate the line and process it + if(LineIdx<7) { LineIdx=0; continue; } + // printf("%s\n", Line); + if(memcmp(Line, "$POGNL,", 7)!=0) { LineIdx=0; continue; } // only take POGNL sentences + int8_t Err=GPS_Position::IndexNMEA(Index, Line); // index the parameters + if(Err<2) { LineIdx=0; continue; } // if less than two parameters, then skip this message + if((Index[1]-Index[0])!=13) { LineIdx=0; continue; } // filename must be 13 characters long + uint32_t UnixTime=0; + if(Read_Hex(UnixTime, Line+Index[0])!=8) { LineIdx=0; continue; } // get the starting time: 8 hex characters + time_t Time = UnixTime; + struct tm *TM = gmtime(&Time); + printf("%s (%d,%d) %02d:%02d:%02d %02d.%02d.%04d\n", + Line, Err, Index[1]-Index[0], TM->tm_hour, TM->tm_min, TM->tm_sec, TM->tm_mday, TM->tm_mon+1, TM->tm_year+1900); + Start=getTime(); // set new start time counter + LogFileList.push_back(Time); + LineIdx=0; continue; } + if(LineIdx=8.0) break; + usleep(1000); continue; } // if no new bytes on the serial port sleep a little + if(Byte<' ') // if a control (non-printable) character + { Line[LineIdx]=0; + if(Line[0]=='$') { LineIdx=0; continue; } + if(LineIdx<12) { LineIdx=0; continue; } + printf("%s\n", Line); + Start=getTime(); + Records++; + LineIdx=0; continue; } + if(LineIdx +#include +#include +#include +#include + +class SerialPort +{ public: + + int DeviceHandle; + + public: + + SerialPort() + { DeviceHandle=(-1); } + + ~SerialPort() + { Close(); } + + int isOpen(void) const { return DeviceHandle>=0; } + + int Open(const char *DeviceName="/dev/ttyS0", int BaudRate=9600) + { Close(); + + DeviceHandle=open(DeviceName, O_RDWR | O_NOCTTY | O_NDELAY ); + if(DeviceHandle<0) return -1; + + int Speed; // speed_t Speed; + if(BaudRate==4800) Speed=B4800; + else if(BaudRate==9600) Speed=B9600; + else if(BaudRate==19200) Speed=B19200; + else if(BaudRate==38400) Speed=B38400; + else if(BaudRate==57600) Speed=B57600; + else if(BaudRate==115200) Speed=B115200; + else if(BaudRate==230400) Speed=B230400; + // else if(BaudRate==128000) Speed=B128000; + // else if(BaudRate==256000) Speed=B256000; + else return -2; + + struct termios Options; + tcgetattr(DeviceHandle, &Options); + + if(cfsetispeed(&Options, Speed)<0) return -2; + if(cfsetospeed(&Options, Speed)<0) return -2; + // cfsetspeed(&Options, Speed); + + Options.c_cflag |= (CLOCAL | CREAD); + + Options.c_cflag &= ~PARENB; // 8-bits, no parity + Options.c_cflag &= ~CSTOPB; + Options.c_cflag &= ~CSIZE; + Options.c_cflag |= CS8; + + Options.c_cc[VTIME] = 0; + Options.c_cc[VMIN] = 1; + + tcsetattr(DeviceHandle, TCSANOW, &Options); +/* + int Bytes=sizeof(Options); + uint8_t *Ptr = (uint8_t *)(&Options); + printf("Options[%d] =", Bytes); + for( ; Bytes; Bytes--, Ptr++) + { printf(" %02X", *Ptr); } + printf("\n"); +*/ + return 0; } + + int OpenFileForRead(const char *FileName) + { Close(); + DeviceHandle=open(FileName, O_RDONLY); + if(DeviceHandle<0) return -1; + return 0; } + + int Close(void) + { if(DeviceHandle>=0) + { close(DeviceHandle); DeviceHandle=(-1); } + return 0; } + + int Read(char *Buffer, size_t MaxChars) + { int Len=read(DeviceHandle, Buffer, MaxChars); + return Len<=0 ? 0:Len; } + + int Read(char &Char) + { return Read(&Char, 1); } + + int Write(const char *Buffer, size_t Chars) + { int Len=write(DeviceHandle, Buffer, Chars); + return Len<=0 ? 0:Len; } + + int Write(const char *String) + { return Write(String, strlen(String)); } + + int Write(char Char) + { return Write(&Char,1); } + +} ; + diff --git a/utils/ubx.h b/utils/ubx.h new file mode 100644 index 0000000..0f8f8ff --- /dev/null +++ b/utils/ubx.h @@ -0,0 +1,316 @@ +#ifndef __UBX_H__ +#define __UBX_H__ + +class UBX_NAV_POSLLH // 0x01 0x02 +{ public: + uint32_t iTOW; // [ms] Time-of-Week + int32_t lon; // [1e-7 deg] Longitude + int32_t lat; // [1e-7 deg] Latitude + int32_t height; // [mm] height above elipsoid (GPS altitude) + int32_t hMSL; // [mm] height above Mean Sea Level + uint32_t hAcc; // [mm] horizontal accuracy + uint32_t vAcc; // [mm] vertical accuracy +} ; + +class UBX_NAV_STATUS // 0x01 0x03 +{ public: + uint32_t iTOW; // [ms] Time-of-Week + uint8_t gpsFix; // Fix type: 0:none, 1=dead reckoning, 2:2-D, 3:3-D, 4:GPS+dead reckoning, 5:time-only + uint8_t flags; // xxxxTWDF => T:Time-of-Week is valid, W:Week-Number is valid, D:Diff. GPS is used, F:Fix valid + uint8_t diffStat; // DD => 00:none, 01:PR+PRR corr., 10: PR+PRR+CP corr., 11: high accuracy PR+PRR+CP + uint8_t res; // reserved PR=Pseudo-Range, PRR=Pseudo-Range Rate + uint32_t ttff; // [ms] Time To First Fix + uint32_t msss; // [ms] Since Startup +} ; + +class UBX_NAV_DOP // 0x01 0x04 +{ public: + uint32_t iTOW; // [ms] Time-of-Week + uint16_t gDOP; // [1/100] geometrical + uint16_t pDOP; // [1/100] position + uint16_t tDOP; // [1/100] time + uint16_t vDOP; // [1/100] vertical + uint16_t hDOP; // [1/100] horizontal + uint16_t nDOP; // [1/100] north-south + uint16_t eDOP; // [1/100] east-west + uint16_t padding; // padding for round size +} ; + +class UBX_NAV_SOL // 0x01 0x06 +{ public: + uint32_t iTOW; // [ms] Time-of-Week + int32_t fTOW; // [ns] Time-of-Week + int16_t Week; // [week] + uint8_t gpsFix; // 0=none, 1=DR, 2=2D, 3=3D, 4=DR+GPS, 5=time-only + uint8_t flags; + int32_t ecefX; // [cm] ECEF position + int32_t ecefY; // [cm] + int32_t ecefZ; // [cm] + uint32_t pAcc; // [cm] position accuracy + int32_t ecefVX; // [cm/s] ECEF velocity + int32_t ecefVY; // [cm/s] + int32_t ecefVZ; // [cm/s] + uint32_t sAcc; // [cm/s] speed accuracy + uint16_t PDOP; // [1/100] + uint8_t reserved1; // + uint8_t numSV; // [satellites] + uint8_t reserved2[4]; // +} ; + +class UBX_NAV_VELNED // 0x01 0x12 +{ public: + uint32_t iTOW; // [ms] Time-of-Week + int32_t velN; // [cm/s] velocity North + int32_t velE; // [cm/s] velocity East + int32_t velD; // [cm/s] velocity Down + uint32_t Speed; // [cm/s] velocity + uint32_t gSpeed; // [cm/s] ground speed (horizontal velocity) + int32_t heading; // [1e-5 deg] ground heading + uint32_t sAcc; // [cm/s] speed accuracy + uint32_t cAcc; // [1e-5 deg] heading accuracy +} ; + +class UBX_NAV_TIMEGPS // 0x01 0x020 +{ public: + uint32_t iTOW; // [ms] Time-of-Week + int32_t fTOW; // [ns] reminder of Time-of-Week + uint16_t week; + uint8_t leapS; + uint8_t valid; // bits: 0:ToW, 1:week, 2:leapS + uint32_t tAcc; // [ns] + + public: + static const uint32_t SecsPerWeek = 7*24*60*60; + uint8_t Valid(void) const + { return (valid&0x03)==0x03; } + uint32_t UnixTime() const + { return (iTOW+10)/1000 + week*SecsPerWeek + 315964785; } // http://www.andrews.edu/~tzs/timeconv/timedisplay.php +} ; + +class UBX_NAV_TIMEUTC // 0x01 0x21 +{ public: + uint32_t iTOW; // [ms] Time-of-Week + uint32_t tAcc; // [ns] accurary estimate + int32_t nano; // [ns] + uint16_t year; // 1999..2099 + uint8_t month; // 1..12 + uint8_t day; // 1..31 + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t valid; // bits: 0:ToW, 1:WN, 2:UTC +} ; + +class UBX_RXM_PMREQ // 0x02 0x41 +{ public: + uint32_t duration; // [ms] + uint32_t flags; // bit #1 = enter backup mode +} ; + +class UBX_CFG_CFG +{ public: + uint32_t clearMask; + uint32_t saveMask; + uint32_t loadMask; + uint8_t deviceMask; +} ; + +class UBX_CFG_PRT // 0x06 0x00 +{ public: + uint8_t portID; // 0 = I2C, 1 = UART1, 2 = UART2, 3 = USB, 4 = SPI + uint8_t reserved1; + uint16_t txReady; // 0 = disable this feature + int32_t mode; // 00 10x x 11 x 1 xxxx => 0x08D0 + uint32_t baudRate; // [bps] + int16_t inProtoMask; // bit 0:UBX, bit 1:NMEA + int16_t outProtoMask; // bit 0:UBX, bit 1:NMEA + uint16_t flags; // bit 1:extendedTxTimeout + uint16_t reserved2; +} ; + +class UBX_CFG_MSG // 0x06 0x01 +{ public: + uint8_t msgClass; // 0xF0:00=GGA, 0xF0:02=GSA, 0xF0:04=RMC, 0xF0:41=TXT + uint8_t msgID; + uint8_t rate; // message send rate +} ; + +/* +class UBX_CFG_MSG // 0x06 0x01 +{ public: + uint8_t msgClass; // 0xF0:00=GGA, 0xF0:02=GSA, 0xF0:04=RMC, 0xF0:41=TXT + uint8_t msgID; + uint8_t rate[6]; // message send rate +} ; +*/ + +class UBX_CFG_RATE // 0x06 0x08 +{ public: + uint16_t measRate; // [ms] measurement rate + uint16_t navRate; // [cycles] = 1 + uint16_t timeRef; // 0=UTC, 1=GPS +} ; + +class UBX_CFG_NAV5 // 0x06 0x24 +{ public: + uint16_t mask; // bit #0 = apply dynamic mode settings, #1 = apply min. elev. settings, #2 = apply fix mode settings + uint8_t dynModel; // 6 = airborne 1g, 7 = 2g, 8 = 4g + uint8_t fixMode; // 1=2D only, 2=3D only, 3=auto 2/3D + int32_t fixAlt; // [0.01m] + uint32_t fixAltVar; // [0.001m] + int8_t minElev; // [deg] minimum satelite elevation + uint8_t drLimit; // [sec] Dead Reconning time limit + uint16_t pDop; + uint16_t tDop; + uint16_t pAcc; // [m] + uint16_t tAcc; // [m] + uint8_t staticHoldThres; // [cm/s] + uint8_t dgpsTimeout; // [s] + uint8_t cnoThreshNumSVs; // + uint8_t cnoThresh; // [dBHz] + uint8_t reserved2[2]; + uint32_t reserved3; + uint32_t reserved4; +} ; + +// UBX Class packet numbers +const uint8_t UBX_NAV = 0x01; // navigation +const uint8_t UBX_ACK = 0x05; // acknoledgement of configuration +const uint8_t UBX_CFG = 0x06; // configuration + +class UBX_RxMsg // receiver for the UBX sentences +{ public: + // most information in the UBX packets is already aligned to 32-bit boundary + // thus it makes sense to have the packet so aligned when receiving it. + static const uint8_t MaxWords=32; // 10; // maximum number of 32-bit words (excl. head and tail) + static const uint8_t MaxBytes=4*MaxWords; // max. number of bytes + static const uint8_t SyncL=0xB5; // UBX sync bytes + static const uint8_t SyncH=0x62; + + union + { uint32_t Word[MaxWords]; // here we store the UBX packet (excl. head and tail) + uint8_t Byte[MaxBytes]; } ; + uint8_t Class; // Class (01=NAV) + uint8_t ID; // ID + uint8_t Bytes; // number of bytes in the packet (excl. head and tail) + + private: + uint8_t Padding; // just to make the structure size be a multiple of 4-bytes + uint8_t State; // bits: 0:loading, 1:complete, 2:locked, + uint8_t Idx; // loading index + + uint8_t CheckA; // UBX check sum (two bytes) + uint8_t CheckB; + void CheckInit(void) { CheckA=0; CheckB=0; } // initialize the checksum + void CheckPass(uint8_t Byte) { CheckA+=Byte; CheckB+=CheckA; } // pass a byte through the checksum + + public: + void RecalcCheck(void) + { CheckInit(); + CheckPass(Class); CheckPass(ID); CheckPass(Bytes); CheckPass(0x00); + for(uint8_t Idx=0; IdxMaxBytes) { Clear(); return; } + break; + case 5: // MSB of packet length (expect zero) + CheckPass(RxByte); if(RxByte!=0) { Clear(); return; } + break; + default: // past the header, now load the packet content + uint8_t ByteIdx=Idx-6; + if(ByteIdx