#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 Idx0) { Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, (((uint32_t)Status.Pressure<<3)+5)/10, 2, 1); Out[Len++]='h'; Out[Len++]='P'; Out[Len++]='a'; } if(hasTemperature()) { Out[Len++]=' '; Len+=Format_SignDec(Out+Len, DecodeTemperature(), 2, 1); Out[Len++]='d'; Out[Len++]='e'; Out[Len++]='g'; Out[Len++]='C'; } if(hasHumidity()) { Out[Len++]=' '; Len+=Format_SignDec(Out+Len, DecodeHumidity(), 2, 1); Out[Len++]='%'; } Out[Len++]=' '; Len+=Format_SignDec(Out+Len, ((uint32_t)DecodeVoltage()*100+32)>>6, 3, 2); Out[Len++]='V'; Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Status.TxPower+4); Out[Len++]='/'; Out[Len++]='-'; Len+=Format_UnsDec(Out+Len, 5*Status.RadioNoise, 2, 1); Out[Len++]='d'; Out[Len++]='B'; Out[Len++]='m'; Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, (1<>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__