diff --git a/main/ogn1.h b/main/ogn1.h index 34ef0cf..73db93e 100644 --- a/main/ogn1.h +++ b/main/ogn1.h @@ -568,8 +568,11 @@ class OGN1_Packet // Packet structure for the OGN tracker Msg[Len++] = ' '; Msg[Len++] = 'i'; Msg[Len++] = 'd'; Len+=Format_Hex(Msg+Len, ((uint32_t)Position.AcftType<<26) | ((uint32_t)Header.AddrType<<24) | Header.Address); - Msg[Len++] = ' '; Len+=Format_SignDec(Msg+Len, ((int32_t)DecodeClimbRate()*10079+256)>>9, 3); Msg[Len++] = 'f'; Msg[Len++] = 'p'; Msg[Len++] = 'm'; - Msg[Len++] = ' '; Len+=Format_SignDec(Msg+Len, DecodeTurnRate()/3, 2, 1); Msg[Len++] = 'r'; Msg[Len++] = 'o'; Msg[Len++] = 't'; + if(hasClimbRate()) + { Msg[Len++] = ' '; Len+=Format_SignDec(Msg+Len, ((int32_t)DecodeClimbRate()*10079+256)>>9, 3); Msg[Len++] = 'f'; Msg[Len++] = 'p'; Msg[Len++] = 'm'; } + + if(hasTurnRate()) + { Msg[Len++] = ' '; Len+=Format_SignDec(Msg+Len, DecodeTurnRate()/3, 2, 1); Msg[Len++] = 'r'; Msg[Len++] = 'o'; Msg[Len++] = 't'; } if(hasBaro()) { int32_t Alt = DecodeStdAltitude(); @@ -714,16 +717,16 @@ class OGN1_Packet // Packet structure for the OGN tracker uint16_t getHeadingAngle(void) const { return (uint16_t)Position.Heading<<6; } - void clrTurnRate(void) { Position.TurnRate=0x80; } + void clrTurnRate(void) { Position.TurnRate=0x80; } // mark turn-rate as absent bool hasTurnRate(void) const { return Position.TurnRate==0x80; } - void EncodeTurnRate(int16_t Turn) // [0.1 deg/sec] + void EncodeTurnRate(int16_t Turn) // [0.1 deg/sec] { Position.TurnRate = EncodeSR2V5(Turn); } int16_t DecodeTurnRate(void) const { return DecodeSR2V5(Position.TurnRate); } - void clrClimbRate(void) { Position.ClimbRate=0x100; } + void clrClimbRate(void) { Position.ClimbRate=0x100; } // mark climb rate as absent bool hasClimbRate(void) const { return Position.ClimbRate==0x100; } void EncodeClimbRate(int16_t Climb) diff --git a/main/relpos.h b/main/relpos.h index 122c532..bc1ce7c 100644 --- a/main/relpos.h +++ b/main/relpos.h @@ -236,6 +236,16 @@ class Acft_RelPos // 3-D relative position with speed and turn rate Z += Climb; T += 2; } + template // zero the position, read differentials from an OGN packet + void Start(OGNx_Packet &Packet) + { T=0; X=0; Y=0; Z=0; + Speed = (Packet.DecodeSpeed()+2)/5; // [0.1m/s] => [0.5m/s] + Heading = Packet.getHeadingAngle(); // [360/0x10000deg] + Climb = Packet.DecodeClimbRate()/5; // [0.1m/s] => [0.5m/s] + Turn = ((int32_t)Packet.DecodeTurnRate()*1165+32)>>6; // [0.1deg/s] => [360/0x10000deg/s] + calcDir(); + Error = (2*Packet.DecodeDOP()+22)/5; } + template // read position from an OGN packet, use provided reference int32_t Read(OGNx_Packet &Packet, uint8_t RefTime, int32_t RefLat, int32_t RefLon, int32_t RefAlt, uint16_t LatCos=3000, int32_t MaxDist=10000) { T = (int16_t)Packet.Position.Time-(int16_t)RefTime;