From 05ac9aee9bde0c4369e1448ba507b1ccc10c609d Mon Sep 17 00:00:00 2001 From: Pawel Jalocha Date: Sun, 7 Aug 2022 06:12:15 +0100 Subject: [PATCH] Updates from other projects --- main/lookout.h | 236 ++++++++++++++++++++++++++++++++++--------------- main/ogn.h | 109 ++++++++++++++++------- main/ogn1.h | 70 +++++++++++---- main/proc.cpp | 6 +- main/relpos.h | 56 ++++++++---- 5 files changed, 336 insertions(+), 141 deletions(-) diff --git a/main/lookout.h b/main/lookout.h index d38d6f0..053df59 100644 --- a/main/lookout.h +++ b/main/lookout.h @@ -11,34 +11,46 @@ // #define DEBUG_PRINT +#include "gdl90.h" #include "relpos.h" // ======================================================================================================= -class LookOut_Target +class LookOut_Target // describes a flying aircrafts { public: - uint32_t ID; // ID of the target = aircraft ID - Acft_RelPos Pos; // Position relative to the reference Lat/Lon/Alt - int8_t Pred; // [0.5sec] amount of time by which this position has been predicted/extrapolated - uint8_t GpsPrec; // GPS position error including prediction + union + { uint32_t ID; // ID of the target = aircraft ID + struct + { uint32_t Address:24; // + uint8_t AddrType: 2; // + uint8_t AcftType: 4; // + } ; + } ; + Acft_RelPos Pos; // Position relative to the reference Lat/Lon/Alt + int8_t Pred; // [0.5sec] amount of time by which own position has been predicted/extrapolated + uint8_t GpsPrec; // GPS position error (includes prediction errors) union { uint8_t Flags; // flags struct - { bool isMoving :1; // is a moving target + { // bool isMoving :1; // is a moving target + bool isTracked :1; // is being tracked or only stored + // bool isHidden :1; // hidden track, should normally not be revealed + // bool hasStdAlt :1; // has pressure StdAlt + bool Reported :1; // this target has already been reported with $PFLAA or GDL90 + uint8_t Sys :3; // 0=FLR, 1=OGN, 2=PilotAware, 3=FANET, 4=ADS-B bool Alloc :1; // is allocated or not (a free slot, where a new target can go into) - // bool Reported :1; // this target has already been reported with $PFLAA } ; } ; - union - { uint32_t Rank; // rank: lowest means shorter time margin, shorter distance margin thus bigger thread - struct - { uint16_t DistMargin; // [0.5m] remaining safety margin: if positive, then considered no thread at all - uint8_t TimeMargin; // [0.5s] time to target (if no distance margin left) - uint8_t WarnLevel; // assigned warning level: 0, 1, 2 or 3 - } ; - } ; + union + { uint32_t Rank; // rank: lowest means shorter time margin, shorter distance margin thus bigger thread + struct + { uint16_t DistMargin; // [0.5m] remaining safety margin: if positive, then considered no thread at all + uint8_t TimeMargin; // [0.5s] time to target (if no distance margin left) + uint8_t WarnLevel; // assigned warning level: 0, 1, 2 or 3 + } ; + } ; int16_t dX; // [0.5m] relative position of target int16_t dY; // [0.5m] @@ -48,10 +60,11 @@ class LookOut_Target int16_t Vy; // [0.5m/s] int16_t Vz; // [0.5m/s] + // int16_t dStdAlt; // [0.5m] // int16_t Ax; // [1/16m/s^2] relative acceleration of target // int16_t Ay; // [1/16m/s^2] - uint16_t HorDist; // [0.5m] relltive hor. distance to target + uint16_t HorDist; // [0.5m] relative hor. distance to target int16_t MissTime; // [0.5s] estimated closest approach time uint16_t MissDist; // [0.5m] estimated closest approach distance @@ -126,8 +139,16 @@ class LookOut_Target class LookOut { public: uint32_t ID; // ID of me (own aircraft) - Acft_RelPos Pos; // Position relative to the reference Lat/Lon/Alt - int8_t Pred; // [0.5sec] amount of time by which position has been predicted/extrapolated + Acft_RelPos Pos; // Own position relative to the reference Lat/Lon/Alt + + uint32_t RefTime; // [sec] ref. T for the local T,X,Y,Z coord. system + // ref. X,Y,Z for the local T,X,Y,Z coord. system + int32_t RefLat; // [1/60000deg] + int32_t RefLon; // [1/60000deg] + int32_t RefAlt; // [m] + int16_t LatCos; // [2^-12] + + int8_t Pred; // [0.5sec] amount of time by which position has been predicted/extrapolated union { uint8_t Flags; @@ -146,13 +167,6 @@ class LookOut uint8_t WorstTgtIdx; // [] most dangereous target uint8_t WorstTgtTime; // [0.5s] time to closest approach - uint8_t RefTime; // [sec] ref. T for the local T,X,Y,Z coord. system - int16_t LatCos; // [2^-12] - // ref. X,Y,Z for the local T,X,Y,Z coord. system - int32_t RefLat; // [1/60000deg] - int32_t RefLon; // [1/60000deg] - int32_t RefAlt; // [m] - const static uint8_t MaxTargets = 32; // maximum number of targets LookOut_Target Target[MaxTargets]; // array of Targets @@ -161,9 +175,9 @@ class LookOut const static int16_t MinVertSepar = 20; // [m] minimum vertical separation const static int16_t WarnTime = 20; // [sec] target warning prior to impact - Acft_RelPos PredMe, PredTgt; // for temporary storage for predictions. + Acft_RelPos PredMe, PredTgt; // for temporary storage of predictions. - char Line[80]; // for printing + char Line[120]; // for printing public: @@ -186,7 +200,7 @@ class LookOut NMEA[Len]=0; return Len; } - void PrintPFLA(void) // print (for debug) $PFLAU and PFLAA + void PrintPFLA(void) // print (for debug) $PFLAU and PFLAA { WritePFLAU(Line); printf("%s", Line); for(uint8_t Idx=0; Idx Latitude [1/60000 deg] + int32_t Longitude(int32_t LonDist) const { LonDist = (LonDist<<12)/LatCos; return RefLon + (LonDist*27)/10; } + + // static int32_t CordicCoord(int32_t Coord) { return ((int64_t)Coord*83399993+(1<<21))>>22; } // [1/60000deg] => [cordic] + + void Write(GDL90_REPORT &Report) const // Own GDL90 position report + { Report.Clear(); + Report.setAddress(ID&0xFFFFFF); + Report.setAddrType(((ID>>24)&0x03)!=1); // ICAO or non-ICAO + Report.setAcftType(ID>>26); + Report.setAcftCall(ID); + int32_t Alt = RefAlt+(Pos.Z>>1)+(Pos.dStdAlt>>1); // [m] + Report.setAltitude(MetersToFeet(Alt)); + Report.setHeading(Pos.Heading>>8); + Report.setSpeed(Pos.Speed); // [0.5m/s] => [knots] + Report.setClimbRate((int32_t)99*Pos.Climb); // [0.5m/s] => [fpm] + int32_t Lat = Latitude (Pos.X); Report.setLatOGN(Lat); + int32_t Lon = Longitude(Pos.Y); Report.setLonOGN(Lon); + Report.setMiscInd(0x2); + Report.setAccuracy(9, 9); } + + void Write(GDL90_REPORT &Report, const LookOut_Target *Tgt) const // Target GDL90 position report + { Report.Clear(); + Report.setAlertStatus(Tgt->WarnLevel>0); + Report.setAddress(Tgt->ID&0xFFFFFF); + Report.setAddrType(((Tgt->ID>>24)&0x03)!=1); + Report.setAcftType(Tgt->ID>>26); + Report.setAcftCall(Tgt->ID); + int32_t Alt = RefAlt+(Tgt->Pos.Z>>1)+(Tgt->Pos.dStdAlt>>1); // [m] + Report.setAltitude(MetersToFeet(Alt)); + Report.setHeading(Tgt->Pos.Heading>>8); + Report.setSpeed(Tgt->Pos.Speed); // [0.5m/s] => [knots] + Report.setClimbRate((int32_t)99*Tgt->Pos.Climb); // [0.5m/s] => [fpm] + int32_t Lat = Latitude (Tgt->Pos.X); Report.setLatOGN(Lat); + int32_t Lon = Longitude(Tgt->Pos.Y); Report.setLonOGN(Lon); + Report.setMiscInd(0x2); + Report.setAccuracy(9, 9); } + + uint8_t WritePGAV5(char *NMEA, const LookOut_Target *Tgt) const + { uint8_t Len=0; + Len+=Format_String(NMEA+Len, "$PGVA5,"); // sentence name } + // age of position + NMEA[Len++]=','; + uint32_t Addr = Tgt->ID & 0xFFFFFF; // [24-bit] address + Len+=Format_Hex(NMEA+Len, (uint8_t)(Addr>>16)); // 24-bit address: RND, ICAO, FLARM, OGN + Len+=Format_Hex(NMEA+Len, (uint16_t)Addr); + NMEA[Len++]=','; + // [deg] Lattitude + NMEA[Len++]=','; + // [deg] Longitude + NMEA[Len++]=','; + // [feet] GPS altitude (HAE = MSL+GeoidSepar) + NMEA[Len++]=','; + // [feet] standard pressure altitude + NMEA[Len++]=','; + // FlightID (or call ?) + NMEA[Len++]=','; + // [deg] ground track + NMEA[Len++]=','; + // [kts] ground speed + NMEA[Len++]=','; + // [fpm] vertical speed + NMEA[Len++]=','; + // [] signal strength + NMEA[Len++]=','; + // [hex] aircraft category + Len+=NMEA_AppendCheckCRNL(NMEA, Len); + NMEA[Len]=0; + return Len; } // return number of formatted characters + void Print(void) const { if(!hasPosition) return; - printf("Ref: %02d: [%+10.6f, %+11.6f]deg %ldm\n", RefTime, 0.0001/60*RefLat, 0.0001/60*RefLon, (long int)RefAlt); + uint32_t Sec = RefTime%60; + uint32_t Min = ((RefTime-Sec)/60)%60; + uint32_t Hour = ((RefTime-Sec-Min*60)/3600)%24; + printf("%08lX Ref: %02d:%02d:%02d: [%+10.6f, %+11.6f]deg %ldm\n", (long int)ID, Hour, Min, Sec, 0.0001/60*RefLat, 0.0001/60*RefLon, (long int)RefAlt); printf("%08lX/%+5.1fs/ Margin/ HorDist/Margin/ Miss/ Miss/w%d", (long int)ID, 0.5*Pred, WarnLevel); Pos.Print(); for(uint8_t Idx=0; Idx - int32_t Start(OGNx_Packet &Me) + int32_t Start(OGNx_Packet &OwnPos, uint32_t RxTime) { Clear(); - ID = Me.getAddressAndType() | ((uint32_t)Me.Position.AcftType<<26) ; - RefTime = Me.Position.Time; - RefLat = Me.DecodeLatitude(); - RefLon = Me.DecodeLongitude(); - RefAlt = Me.DecodeAltitude(); + ID = OwnPos.getAddressAndType() | ((uint32_t)OwnPos.Position.AcftType<<26) ; + RefTime = OwnPos.getTime(RxTime); // set reference time + RefLat = OwnPos.DecodeLatitude(); // and reference positon + RefLon = OwnPos.DecodeLongitude(); + RefAlt = OwnPos.DecodeAltitude(); // and reference altitude LatCos = Icos(GPS_Position::calcLatAngle16(RefLat)); Pred=0; - return Pos.Read(Me, RefTime, RefLat, RefLon, RefAlt, LatCos, DistRange); } + return Pos.Read(OwnPos, RxTime, RefTime, RefLat, RefLon, RefAlt, LatCos, DistRange); } template - const LookOut_Target *ProcessOwn(OGNx_Packet &Me) // process my own position + const LookOut_Target *ProcessOwn(OGNx_Packet &OwnPos, uint32_t RxTime) // process own position { // printf("ProcessOwn() ... entry\n"); - if(hasPosition) // in my position is valid + if(hasPosition) // in my position is valid { Pred=0; - if(Pos.Read(Me, RefTime, RefLat, RefLon, RefAlt, LatCos, DistRange)<0) // read the new position - { hasPosition = Start(Me)>=0; } // if this fails, attempt to start from the new position + if(Pos.Read(OwnPos, RxTime, RefTime, RefLat, RefLon, RefAlt, LatCos, DistRange)<0) // read the new position + { hasPosition = Start(OwnPos, RxTime)>=0; } // if this fails, attempt to start from the new position + // if(!Pos.hasStdAlt) // if no StdAlt + // { } // get it from targets } else - { hasPosition = Start(Me)>=0; - } + { hasPosition = Start(OwnPos, RxTime)>=0; } if(hasPosition) // if already started { AdjustRefTime(Pos.T); // adjust time ref. point if needed AdjustRefAlt(); // adjust vertical ref. altitude if needed - AdjustRefLatLon(Me); } // adjuest horizontal Lat/Lon position if needed. + AdjustRefLatLon(OwnPos); } // adjust horizontal Lat/Lon position if needed. WarnLevel=0; Targets=0; - WorstTgtIdx=0; + WorstTgtIdx=0; // get ready to search the most dangerous aircraft WorstTgtTime=0xFF; for(uint8_t Idx=0; IdxWarnLevel) WarnLevel=Warn; - if(Tgt->TimeMarginTimeMargin; WorstTgtIdx=Idx; } + { if(Warn>WarnLevel) WarnLevel=Warn; // register highest warning level + if(Tgt->TimeMarginTimeMargin; WorstTgtIdx=Idx; } // and shortest time margin } Targets++; } // printf("ProcessOwn() ... exit\n"); @@ -305,21 +393,23 @@ class LookOut return Tgt; } // return the pointer to the most dangerous target template - const LookOut_Target *ProcessTarget(OGNx_Packet &Packet) // process positions of other aircrafts + const LookOut_Target *ProcessTarget(OGNx_Packet &Packet, uint32_t RxTime) // process positions of other aircrafts { // printf("ProcessTarget(%d) ... entry\n", WeakestIdx); LookOut_Target *New = Target+WeakestIdx; // get a free or lowest rank slot - New->Clear(); - if(New->Pos.Read(Packet, RefTime, RefLat, RefLon, RefAlt, LatCos, DistRange)<0) return 0; // calculate the position against the reference position + New->Clear(); // put the new position there + if(New->Pos.Read(Packet, RxTime, RefTime, RefLat, RefLon, RefAlt, LatCos, DistRange)<0) return 0; // calculate the position against the reference position + if(!New->Pos.hasStdAlt) // if no baro altitude + { if(Pos.hasStdAlt) { New->Pos.dStdAlt=Pos.dStdAlt; New->Pos.hasStdAlt=1;} } // take it from own uint32_t ID = Packet.getAddressAndType() | ((uint32_t)Packet.Position.AcftType<<26) ; // get ID New->ID = ID; // set ID of this position // printf("ProcessTarget() ... %08X\n", ID); - uint8_t OldIdx; + uint8_t OldIdx; // possible previous index to the same ID for(OldIdx=0; OldIdxTarget[WeakestIdx].Pos.T) return Target+OldIdx; // if position is not really older than stop processing this (not new) position + { if((Target[OldIdx].Pos.T-Target[OldIdx].Pred)>Target[WeakestIdx].Pos.T) return Target+OldIdx; // if position is not really newer than stop processing this (not new) position Target[OldIdx].Alloc=0; } // mark old position as "not allocated" New->Alloc=1; // mark this position as allocated @@ -327,20 +417,20 @@ class LookOut AdjustRefTime(New->Pos.T); // possibly adjust the time reference after this new position time // printf("ProcessTarget() ... AdjustRefTime()\n"); - if(Pos.T<=(New->Pos.T-4)) // bring my position closer in time - { Pos.StepFwd2secs(); Pred+=4; } + if(Pos.T<=(New->Pos.T-4)) // if new position more than 2sec away from own + { Pos.StepFwd2secs(); Pred+=4; } // bring own position closer in time uint8_t Warn=calcTarget(New); // calculate the safety margin for the target - if(Warn>WarnLevel) WarnLevel=Warn; + if(Warn>WarnLevel) WarnLevel=Warn; // record higest warnign level // printf("ProcessTarget() ... calc()\n"); - uint8_t MaxIdx=WeakestIdx; uint16_t Max=New->Rank; // look for the lowest rank position on the list + uint8_t MaxIdx=WeakestIdx; uint32_t Max=New->Rank; // look for the lowest rank position on the list for( uint8_t Idx=MaxIdx; ; ) // go over targets { Idx++; if(Idx>=MaxTargets) Idx=0; if(Idx==WeakestIdx) break; // end the loop when back at New LookOut_Target &Tgt = Target[Idx]; if(!Tgt.Alloc) { MaxIdx=Idx; break; } // if unallocated target found: stop the search - if(Tgt.Rank==0xFFFF) { MaxIdx=Idx; break; } // if abs. weakest target found: stop the search + // if(Tgt.Rank==0xFFFFFFFF) { MaxIdx=Idx; break; } // if abs. weakest target found: stop the search if(Tgt.Rank>=Max) { Max=Tgt.Rank; MaxIdx=Idx; } // if weaker rank found: note it } WeakestIdx=MaxIdx; // tqke the weakest slot for the nest time @@ -458,12 +548,12 @@ class LookOut if(MaxDistHorDist) return Tgt->HorDist-MaxDist; // [0.5m] return the (positive) difference: we are safe return 0; } // zero-margin => bad ! - void AdjustRefTime(int16_t TimeDelta) // adjust the time reference point - { if(TimeDelta<(2*12)) return; // in more than 10sec into the future from the current reference - TimeDelta/=2; - RefTime+=TimeDelta; if(RefTime>=60) RefTime-=60; // shift time reference + void AdjustRefTime(int16_t TimeDelta) // [0.5s] adjust the time reference point + { if(TimeDelta<(2*12)) return; // if less than 12sec into the future than skip it + TimeDelta/=2; // [sec] + RefTime+=TimeDelta; // shift time reference Pos.T-=2*TimeDelta; // shift the relative time on my own position - if(Pos.T<(-2*60)) hasPosition=0; // if older than 60sec declare "no position" + if(Pos.T<(-2*30)) hasPosition=0; // if older than 30sec declare "no position" for(uint8_t Idx=0; Idx - void AdjustRefLatLon(OGNx_Packet &Me) // shift the horizontal reference point when we get too far off - { if( (fabs(Pos.X)<(2*1000)) && (fabs(Pos.Y)<(2*500)) ) return; + void AdjustRefLatLon(OGNx_Packet &Me) // shift the horizontal reference point when we get too far off + { if( (abs(Pos.X)<(2*1000)) && (abs(Pos.Y)<(2*1000)) ) return; // when we are off by 1km horizontal int32_t LatDist, LonDist; - if(Me.calcDistanceVector(LatDist, LonDist, RefLat, RefLon, LatCos, DistRange)<0) { return; } - RefLat = Me.DecodeLatitude(); + if(Me.calcDistanceVector(LatDist, LonDist, RefLat, RefLon, LatCos, DistRange)<0) { return; } // distance from the current reference + RefLat = Me.DecodeLatitude(); // take the ref. Lat/Lon from the packet RefLon = Me.DecodeLongitude(); - LatDist*=2; + LatDist*=2; // [m/s] => [0.5m/s] LonDist*=2; Pos.X -= LatDist; Pos.Y -= LonDist; - for(uint8_t Idx=0; Idx if(abs(DistDeltaH)>=200) return 1; // if extrapolation error more than 50m int16_t Turn = Packet->DecodeTurnRate(); // [0.1deg/s] int16_t CFaccel = ((int32_t)Turn*Speed*229+0x10000)>>17; // [0.1m/s^2] centrifugal acceleration in turn - if(abs(CFaccel)>=50) return 1; // CFaccel at or above 5m/s^2 (0.5g) + if(abs(CFaccel)>=25) return 1; // CFaccel at or above 5m/s^2 (0.5g) int16_t PrevTurn = PrevPacket->DecodeTurnRate(); // [0.1deg/s] int16_t PrevCFaccel = ((int32_t)PrevTurn*PrevSpeed*229+0x10000)>>17; // [0.1m/s^2] - int32_t DistDeltaR = abs(CFaccel-PrevCFaccel)*TimeDelta*TimeDelta/2; // [0.1m] + int32_t DistDeltaR = abs(CFaccel-PrevCFaccel)*(int32_t)TimeDelta*TimeDelta/2; // [0.1m] if(abs(DistDeltaR)>=200) return 1; // [0.1m] return 0; } @@ -182,17 +182,22 @@ template union { uint8_t State; // struct - { bool Saved :1; // has been already saved in internal storage - bool Warn :1; // there is a warning associated with this packet - bool Spare :1; + { // 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 Warn :2; // LookOut warning level } ; } ; uint8_t RxChan; // RF channel where the packet was received uint8_t RxRSSI; // [-0.5dBm] - uint8_t Rank; // relay rank: low altitude and weak signal => high rank + uint8_t Rank; // rank: low altitude and weak signal => high rank + + int16_t LatDist; // [m] + int16_t LonDist; // [m] + // int16_t AltDist; // [m] public: @@ -201,7 +206,16 @@ template 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 - +/* + int Print(char *Out) const + { int Len = sprintf(Out, "%c%X:%c:%06lX R%c%c", + Packet.Position.Stealth ?'s':' ', (int)Packet.Position.AcftType, '0'+Packet.Header.AddrType, (long int)Packet.Header.Address, + '0'+Packet.Header.Relay, Packet.Header.Emergency?'E':' '); + Len+= sprintf(Out+Len, " %d/%dD/%4.1f", (int)Position.FixQuality, (int)Position.FixMode+2, 0.1*(10+DecodeDOP()) ); + if(Position.Time<60) Len+=sprintf(Out+Len, " %02ds:", (int)Position.Time); + else Len+=sprintf(Out+Len, " ---:");; + return Len; } +*/ 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 @@ -885,19 +899,24 @@ class GPS_Position: public GPS_Time { public: union - { uint16_t Flags; // bit #0 = GGA and RMC had same Time + { uint32_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 hasTime :1; // Time has been supplied - bool hasDate :1; // Time has been supplied + bool hasDate :1; // Date 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; bool isReady :1; // is ready for the following treaement bool Sent :1; // has been transmitted + bool hasBaro :1; // pressure sensor information: pressure, standard pressure altitude, temperature + bool hasHum :1; // has humidity (not all baro have humiditiy) + bool hasClimb :1; // has climb-rate computed or measured + bool hasTurn :1; // + bool hasAccel :1; // + bool hasIAS :1; // has Indicated Air Speed calculated/measured + bool hasAHRS :1; // has Attitude Heading Reference System data bool InFlight :1; // take-off and landing detection } ; } ; @@ -908,14 +927,19 @@ class GPS_Position: public GPS_Time 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 + uint16_t AirSpeed; // [0.1m/s] Indicated Air Speed + + int16_t Pitch; // [] AHRS Attitude + int16_t Roll; // [] AHRS Inclination + int16_t ClimbRate; // [0.1 meter/sec) int16_t TurnRate; // [0.1 deg/sec] @@ -930,7 +954,7 @@ class GPS_Position: public GPS_Time 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 + int16_t LongAccel; // [0.1m/s^2] acceleration along the track uint16_t Seq; // sequencial number to track GPS positions in a pipe public: @@ -1234,7 +1258,9 @@ class GPS_Position: public GPS_Time return TimeDiff; } // [0.01s] */ int16_t calcDifferentials(GPS_Position &RefPos, bool useBaro=1) // calculate climb rate and turn rate with an earlier reference position - { ClimbRate=0; TurnRate=0; + { // ClimbRate=0; hasClimb=0; + // TurnRate=0; hasTurn=0; + // LongAccel=0; hasAccel=0; if(RefPos.FixQuality==0) return 0; // give up if no fix on the reference position int16_t TimeDiff = calcTimeDiff(RefPos); // [ms] time difference between positions if(TimeDiff<10) return 0; // [ms] give up if smaller than 10ms (as well when negative) @@ -1243,30 +1269,37 @@ class GPS_Position: public GPS_Time ClimbRate = Altitude-RefPos.Altitude; // [0.1m/s] climb rate as altitude difference if(useBaro && hasBaro && RefPos.hasBaro && (abs(Altitude-StdAltitude)<2500) ) // if there is baro data then { ClimbRate += StdAltitude-RefPos.StdAltitude; // [0.1m/s] on pressure altitude - ClimbRate = (ClimbRate+1)>>1; } - Accel = Speed-RefPos.Speed; // longitual acceleration + ClimbRate = (ClimbRate+1)>>1; } // take average of the GPS and baro climb rate + LongAccel = Speed-RefPos.Speed; // longitual acceleration if(TimeDiff==100) // [ms] if 0.1sec difference - { ClimbRate*=10; - TurnRate *=10; - Accel *=10; } + { ClimbRate *=10; + TurnRate *=10; + LongAccel *=10; } if(TimeDiff==200) // [ms] if 0.2sec difference - { ClimbRate*=5; - TurnRate *=5; - Accel *=5; } + { ClimbRate *=5; + TurnRate *=5; + LongAccel *=5; } + if(TimeDiff==250) // [ms] if 0.25sec difference + { ClimbRate *=4; + TurnRate *=4; + LongAccel *=4; } else if(TimeDiff==500) - { ClimbRate*=2; - TurnRate *=2; - Accel *=2; } + { ClimbRate *=2; + TurnRate *=2; + LongAccel *=2; } else if(TimeDiff==1000) { } else if(TimeDiff==2000) - { ClimbRate=(ClimbRate+1)>>1; - TurnRate=( TurnRate+1)>>1; - Accel =( Accel +1)>>1; } + { ClimbRate = (ClimbRate+1)>>1; + TurnRate = ( TurnRate+1)>>1; + LongAccel = (LongAccel+1)>>1; } else if(TimeDiff!=0) { ClimbRate = ((int32_t)ClimbRate*1000)/TimeDiff; - TurnRate = ((int32_t) TurnRate*1000)/TimeDiff; - Accel = ((int32_t) Accel *1000)/TimeDiff; } + TurnRate = ((int32_t) TurnRate*1000)/TimeDiff; + LongAccel = ((int32_t)LongAccel*1000)/TimeDiff; } + // printf("calcDifferences( , %d) %02d.%03ds hasBaro:%d:%d %4dms %3.1f/%3.1f m %+4.1f m/s\n", + // useBaro, Sec, mSec, hasBaro, RefPos.hasBaro, TimeDiff, 0.1*Altitude, 0.1*StdAltitude, 0.1*ClimbRate); + hasClimb=0; hasTurn=0; hasAccel=0; return TimeDiff; } // [ms] void Write(MAV_GPS_RAW_INT *MAV) const @@ -1349,6 +1382,12 @@ class GPS_Position: public GPS_Time Report.setClimbRate(6*MetersToFeet(ClimbRate)); } + int EncodeIAS(OGN1_Packet &Packet) + { if(!hasIAS) return 0; + Packet.Position.Time = 61; // "current" packet with airspeed + Packet.EncodeSpeed(AirSpeed); + return 1; } + template void Encode(OGNx_Packet &Packet) const { Packet.Position.FixQuality = FixQuality<3 ? FixQuality:3; // @@ -1403,7 +1442,7 @@ class GPS_Position: public GPS_Time if(hasHum) Packet.EncodeHumidity(Humidity); else Packet.clrHumidity(); } else - { Packet.Status.Pressure = 0; + { Packet.Status.Pressure=0; Packet.clrTemperature(); Packet.clrHumidity(); } } @@ -1461,7 +1500,7 @@ class GPS_Position: public GPS_Time } void Extrapolate(int32_t dTime) // [ms] extrapolate the position by dTime - { int16_t dSpeed = ((int32_t)Accel*dTime)/1000; + { int16_t dSpeed = ((int32_t)LongAccel*dTime)/1000; Speed += dSpeed/2; int16_t HeadAngle = ((int32_t)Heading<<12)/225; // [cordic] heading angle int16_t TurnAngle = (((dTime*TurnRate)/250)<<9)/225; // [cordic] @@ -1532,7 +1571,9 @@ class GPS_Position: public GPS_Time // { return IntSine((uint16_t)(LatAngle+0x4000)); } static int16_t calcLatCosine(int16_t LatAngle) - { return Icos(LatAngle); } + { int16_t LatCos=Icos(LatAngle); + if(LatCos<=0) LatCos=1; // protect against zero as it is used for division + return LatCos; } // int32_t getLatDistance(int32_t RefLatitude) const // [m] distance along latitude // { return calcLatDistance(RefLatitude, Latitude); } @@ -1612,7 +1653,7 @@ class GPS_Position: public GPS_Time { Len+=WriteIGCcoord(Out+Len, Latitude, 2, "NS"); // DDMM.MMM latitude Len+=WriteIGCcoord(Out+Len, Longitude, 3, "EW"); // DDDMM.MMM longitude Out[Len++] = FixMode>2 ? 'A':'V'; } // fix mode - else Len+=Format_String(Out+Len, " "); // is position not valid then leave empty + else Len+=Format_String(Out+Len, " "); // is position not valid then leave empty if(hasBaro) // if pressure data is there { int32_t Alt = StdAltitude/10; // [m] pressure altitude if(Alt<0) { Alt = (-Alt); Out[Len++] = '-'; Len+=Format_UnsDec(Out+Len, (uint32_t)Alt, 4); } // -AAAA (when negative) diff --git a/main/ogn1.h b/main/ogn1.h index 5592ff8..a4d2772 100644 --- a/main/ogn1.h +++ b/main/ogn1.h @@ -133,6 +133,13 @@ class OGN1_Packet // Packet structure for the OGN tracker unsigned int BaroAltDiff: 8; // [m] // lower 8 bits of the altitude difference between baro and GPS } Wind; + struct + { uint8_t Data[14]; // up to 14 bytes od specific data + unsigned int DataLen : 4; // 0..14 number of bytes in the message + unsigned int ReportType: 4; // 15 for the manufacturer specific mesage + unsigned int ManufID : 8; // Manufacturer identification: 0 for Cube-Board + } ManufMsg; // manufacturer-specific message + } ; uint8_t *Byte(void) const { return (uint8_t *)&HeaderWord; } // packet as bytes @@ -148,6 +155,14 @@ class OGN1_Packet // Packet structure for the OGN tracker #ifndef __AVR__ + uint32_t getTime(uint32_t RefTime, int FwdMargin=5) const + { if(Position.Time>=60) return 0; + int Sec=RefTime%60; + int DiffSec=Position.Time-Sec; + if(DiffSec>FwdMargin) DiffSec-=60; // difference should always be zero or negative, but can be small positive for predicted positions + else if(DiffSec<=(-60+FwdMargin)) DiffSec+=60; + return RefTime+DiffSec; } // get out the correct position time + void Dump(void) const { printf("%08lX: %08lX %08lX %08lX %08lX\n", (long int)HeaderWord, (long int)Data[0], (long int)Data[1], @@ -237,10 +252,18 @@ class OGN1_Packet // Packet structure for the OGN tracker Idx+=Chars; } Out[Len]=0; return Len; } */ + void setStatus (void) { Status.ReportType=0; } + void setInfo (void) { Status.ReportType=1; } + void setManufMsg(void) { Status.ReportType=15; } + + bool isStatus (void) const { return Status.ReportType==0; } + bool isInfo (void) const { return Status.ReportType==1; } + bool isManufMsg(void) const { return Status.ReportType==15; } + void Print(void) const { if(!Header.NonPos) { PrintPosition(); return; } - if(Status.ReportType==0) { PrintDeviceStatus(); return; } - if(Status.ReportType==1) { PrintDeviceInfo(); return; } + if(isStatus()) { PrintDeviceStatus(); return; } + if(isInfo ()) { PrintDeviceInfo(); return; } } void PrintDeviceInfo(void) const @@ -286,6 +309,13 @@ class OGN1_Packet // Packet structure for the OGN tracker printf("\n"); } + int DecodePosition(float &Lat, float &Lon, int &Alt) + { if(Header.NonPos) return 0; + Lat = (0.0001f/60)*DecodeLatitude(); + Lon = (0.0001f/60)*DecodeLongitude(); + Alt = DecodeAltitude(); + return 3; } + int WriteStxJSON(char *JSON) const // Stratux JSON message { int Len=0; Len+=Format_String(JSON+Len, "\"addr\":\""); @@ -329,10 +359,10 @@ class OGN1_Packet // Packet structure for the OGN tracker Len+=Format_SignDec(JSON+Len, DecodeTurnRate(), 2, 1, 1); } Len+=Format_String(JSON+Len, ",\"DOP\":"); Len+=Format_UnsDec(JSON+Len, 10+DecodeDOP(), 2, 1); } - if(!Header.Encrypted && Header.NonPos) // non-encrypted status and info - { if(Status.ReportType==0) // status + else if(!Header.Encrypted && Header.NonPos) // non-encrypted status and info + { if(isStatus()) // status { } - if(Status.ReportType==1) // info + else if(isInfo()) // info { char Value[16]; uint8_t InfoType; uint8_t Idx=0; @@ -407,7 +437,7 @@ class OGN1_Packet // Packet structure for the OGN tracker return AcftType<16 ? AprsIcon[AcftType]:0; } - int8_t ReadAPRS(const char *Msg) // read an APRS position message + int ReadAPRS(const char *Msg) // read an APRS position message { Clear(); const char *Data = strchr(Msg, ':'); if(Data==0) return -1; // where the time/position data starts @@ -435,14 +465,21 @@ class OGN1_Packet // Packet structure for the OGN tracker } if(Data[0]!='/') return -1; - int8_t Time; + int Sec, Min, Hour; if(Data[7]=='h') // HHMMSS UTC time - { Time=Read_Dec2(Data+5); if(Time<0) return -1; } + { Sec =Read_Dec2(Data+5); if(Sec<0) return -1; + Min =Read_Dec2(Data+3); if(Min<0) return -1; + Hour=Read_Dec2(Data+1); if(Hour<0) return -1; + } else if(Data[7]=='z') // DDHHMM UTC time - { Time=0; } + { Sec =0; + Min =Read_Dec2(Data+5); if(Min<0) return -1; + Hour=Read_Dec2(Data+3); if(Hour<0) return -1; + } else return -1; - Position.Time=Time; + int Time = Sec + Min*60 + Hour*3600; + Position.Time=Sec; Data+=8; Position.FixMode=1; @@ -528,7 +565,7 @@ class OGN1_Packet // Packet structure for the OGN tracker if(LonSign=='W') Longitude=(-Longitude); else if(LonSign!='E') return -1; EncodeLongitude(Longitude); - return 0; } + return Time; } // [sec] return Time-of-Day uint8_t WriteAPRS(char *Msg, uint32_t Time, const char *ProtName="APRS") // write an APRS position message { uint8_t Len=0; @@ -546,16 +583,17 @@ class OGN1_Packet // Packet structure for the OGN tracker if(Header.NonPos && Status.ReportType>1) { Msg[Len]=0; return 0; } // give up if neither position nor status nor info if(Position.Time<60) - { uint32_t DayTime=Time%86400; int Sec=DayTime%60; // second of the time the packet was recevied - int DiffSec=Position.Time-Sec; if(DiffSec>4) DiffSec-=60; // difference should always be zero or negative, but can be small positive for predicted positions - Time+=DiffSec; } // get out the correct position time + // { uint32_t DayTime=Time%86400; int Sec=DayTime%60; // second of the time the packet was recevied + // int DiffSec=Position.Time-Sec; if(DiffSec>4) DiffSec-=60; // difference should always be zero or negative, but can be small positive for predicted positions + // Time+=DiffSec; } // get out the correct position time + Time = getTime(Time, 5); Msg[Len++] = Header.NonPos || Header.Encrypted?'>':'/'; Len+=Format_HHMMSS(Msg+Len, Time); Msg[Len++] = 'h'; if(Header.NonPos) // status and info packets - { if(Status.ReportType==0) Len+=WriteStatus(Msg+Len); - else Len+=WriteDeviceInfo(Msg+Len); + { if(isStatus()) Len+=WriteStatus(Msg+Len); + else if(isInfo() ) Len+=WriteDeviceInfo(Msg+Len); /* Msg[Len++]='\n'; */ Msg[Len]=0; return Len; } if(Header.Encrypted) // encrypted packets diff --git a/main/proc.cpp b/main/proc.cpp index 6033b94..dbe9fd2 100644 --- a/main/proc.cpp +++ b/main/proc.cpp @@ -378,7 +378,7 @@ static void ProcessRxPacket(OGN_RxPacket *RxPacket, uint8_t RxPacket // Format_String(CONS_UART_Write, Line, 0, Len); // xSemaphoreGive(CONS_Mutex); #ifdef WITH_LOOKOUT - const LookOut_Target *Tgt=Look.ProcessTarget(RxPacket->Packet); // process the received target postion + const LookOut_Target *Tgt=Look.ProcessTarget(RxPacket->Packet, RxTime); // process the received target postion if(Tgt) Warn=Tgt->WarnLevel; // remember warning level of this target RxPacket->Warn = Warn>0; #ifdef WITH_GDL90 @@ -396,7 +396,7 @@ static void ProcessRxPacket(OGN_RxPacket *RxPacket, uint8_t RxPacket if(KNOB_Tick>12) Play(Play_Vol_1 | Play_Oct_2 | 7, 3); // if Knob>12 => make a beep for every received packet #endif #endif // WITH_LOOKOUT - bool Signif = PrevRxPacket!=0; + bool Signif = PrevRxPacket==0; if(!Signif) Signif=OGN_isSignif(&(RxPacket->Packet), &(PrevRxPacket->Packet)); // compare against previous packet of same ID from the relay queue #ifdef WITH_APRS if(Signif) APRSrx_FIFO.Write(*RxPacket); // APRS queue for received packets @@ -659,7 +659,7 @@ void vTaskPROC(void* pvParameters) FNTbackOff = 8+(RX_Random&0x1); } // every 9 or 10sec #endif // WITH_FANET #ifdef WITH_LOOKOUT - const LookOut_Target *Tgt=Look.ProcessOwn(PosPacket.Packet); // process own position, get the most dangerous target + const LookOut_Target *Tgt=Look.ProcessOwn(PosPacket.Packet, PosTime); // process own position, get the most dangerous target #ifdef WITH_PFLAA if(Parameters.Verbose) { xSemaphoreTake(CONS_Mutex, portMAX_DELAY); diff --git a/main/relpos.h b/main/relpos.h index bc1ce7c..1e57349 100644 --- a/main/relpos.h +++ b/main/relpos.h @@ -1,3 +1,6 @@ +#ifndef __RELPOS_H__ +#define __RELPOS_H__ + #include #include @@ -6,6 +9,7 @@ #include "intmath.h" +#include "gdl90.h" #include "ogn.h" // ======================================================================================================= @@ -19,15 +23,23 @@ class Acft_RelPos // 3-D relative position with speed and turn rate int16_t Climb; // [0.5m/s] int16_t Turn; // [360/0x10000 deg/s] [2*PI/0x10000 rad/sec] int16_t Dx,Dy; // [2^-12] directon vector - calc. on Heading + int16_t dStdAlt; // [0.5m] uint8_t Error; // [0.5m] - uint8_t Spare; + union + { uint8_t Flags; + struct + { bool hasStdAlt:1; + bool hasClimb :1; + bool hasTurn :1; + } ; + } ; // int16_t Ax,Ay; // [1/16m/s^2] acceleration vactor // int16_t R; // [0.5m] (signed) turning radius - calc. from Turn and Speed // int16_t Ox,Oy; // [0.5m] turning circle center - only valid when R!=0 public: void Print(void) const - { printf("%+7.1f: [%+7.1f,%+7.1f,%+7.1f]m %5.1fm/s %05.1fdeg %+5.1fm/s %+5.1fdeg/sec [%3.1fm]", + { printf("%+7.1fs: [%+7.1f,%+7.1f,%+7.1f]m %5.1fm/s %05.1fdeg %+5.1fm/s %+5.1fdeg/s [%3.1fm]", 0.5*T, 0.5*X, 0.5*Y, 0.5*Z, 0.5*Speed, (360.0/0x10000)*Heading, 0.5*Climb, (360.0/0x10000)*Turn, 0.5*Error); // printf(" [%+6.2f,%+6.2f]m/s^2", 0.0625*Ax, 0.0625*Ay); // if(R) printf(" R:%+8.1fm [%+7.1f, %+7.1f]", 0.5*R, 0.5*Ox, 0.5*Oy); @@ -247,19 +259,26 @@ class Acft_RelPos // 3-D relative position with speed and turn rate 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; - if(T<=(-30)) T+=60; else if(T>30) T-=60; - T<<=1; - int32_t LatDist, LonDist; - if(Packet.calcDistanceVector(LatDist, LonDist, RefLat, RefLon, LatCos, MaxDist)<0) return -1; - X = LatDist<<1; // [m] => [0.5m] - Y = LonDist<<1; // [m] => [0.5m] - Z = (Packet.DecodeAltitude()-RefAlt)<<1; // [m] => [0.5m] + int32_t Read(OGNx_Packet &Packet, uint32_t RxTime, uint32_t RefTime, int32_t RefLat, int32_t RefLon, int32_t RefAlt, uint16_t LatCos=3000, int32_t MaxDist=10000) + { Flags=0; + uint32_t PosTime=Packet.getTime(RxTime); + if(PosTime) T = PosTime-RefTime; // [sec] + else T = RxTime-RefTime; + T<<=1; // [0.5sec] + int32_t LatDist, LonDist; // [1/60000deg] + if(Packet.calcDistanceVector(LatDist, LonDist, RefLat, RefLon, LatCos, MaxDist)<0) return -1; // [m, m, , , , m] + X = LatDist<<1; // [m] => [0.5m] relative along latitude + Y = LonDist<<1; // [m] => [0.5m] relative along longitude + Z = (Packet.DecodeAltitude()-RefAlt)<<1; // [m] => [0.5m] relative vertical + if(Packet.hasBaro()) { dStdAlt=Packet.getBaroAltDiff()<<1; hasStdAlt=1; } 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] + if(Packet.hasClimbRate()) + { Climb = Packet.DecodeClimbRate()/5; // [0.1m/s] => [0.5m/s] + hasClimb = 1; } + if(Packet.hasTurnRate()) + { Turn = ((int32_t)Packet.DecodeTurnRate()*1165+32)>>6; // [0.1deg/s] => [360/0x10000deg/s] + hasTurn = 1; } calcDir(); Error = (2*Packet.DecodeDOP()+22)/5; // calcAccel(); @@ -275,15 +294,22 @@ class Acft_RelPos // 3-D relative position with speed and turn rate Packet.Position.Time = Time%60; Packet.setDistanceVector(X>>1, Y>>1, RefLat, RefLon, LatCos); Packet.EncodeAltitude(RefAlt+(Z>>1)); // - Packet.clrBaro(); // don't know the standard pressure altitude + if(hasStdAlt) Packet.setBaroAltDiff(dStdAlt>>1); // set std. altitude differemce + else Packet.clrBaro(); // don't know the standard pressure altitude Packet.EncodeSpeed(Speed*5); // [0.5m/s] => [0.1m/s] Packet.setHeadingAngle(Heading); // Packet.EncodeClimbRate(Climb*5); // [0.5m/s] => [0.1m/s] Packet.EncodeTurnRate((Turn*7+64)>>7); // [360/0x10000deg/s] => [0.1deg/s] - Packet.EncodeDOP((5*Error)/2-10); + Packet.EncodeDOP((5*Error)/2-10); + Packet.Position.FixMode=1; + Packet.Position.FixQuality=1; } + + void Write(GDL90_REPORT &Report, uint8_t RefTime, int32_t RefLat, int32_t RefLon, int32_t RefAlt, uint16_t LatCos=3000) + { } } ; // ======================================================================================================= +#endif // __RELPOS_H__