#ifndef __LOKOUT_H__ #define __LOKOUT_H__ #include #include #include #include #include "intmath.h" // #define DEBUG_PRINT #include "relpos.h" // ======================================================================================================= class LookOut_Target { 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 { uint8_t Flags; // flags struct { bool isMoving :1; // is a moving target 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 } ; } ; int16_t dX; // [0.5m] relative position of target int16_t dY; // [0.5m] int16_t dZ; // [0.5m] int16_t Vx; // [0.5m/s] relative speed of target int16_t Vy; // [0.5m/s] int16_t Vz; // [0.5m/s] // 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 int16_t MissTime; // [0.5s] estimated closest approach time uint16_t MissDist; // [0.5m] estimated closest approach distance public: void Clear(void) { Pred=0; Flags=0; HorDist=0; MissDist=0; Rank=0xFFFF; } // uint16_t HorRelSpeed(void) const { } uint32_t DistSqr(void) const { return (int32_t)dX*dX + (int32_t)dY*dY + (int32_t)dZ*dZ; } // [0.25m^2] Distance-square to the Target uint32_t VelSqr (void) const { return (int32_t)Vx*Vx + (int32_t)Vy*Vy + (int32_t)Vz*Vz; } // [0.5m/s^2] Relative velocity square of the Target void calcVel(Acft_RelPos &RefPos) // calc. relative target velocity against a reference position { Pos.getSpeedVector(Vx, Vy); // get horizontal speed vector from Speed and Heading int16_t rVx, rVy; RefPos.getSpeedVector(rVx, rVy); // same for the reference position Vx -= rVx; Vy -=rVy; // difference Vz = Pos.Climb-RefPos.Climb; } // vertical different int16_t getBearing (void) const { return IntAtan2(dY, dX); } // bearing to the target uint16_t getHorDist (void) const { return Acft_RelPos::FastDistance(dX, dY); } // relative horizontal distance to the target uint16_t getRelHorSpeed(void) const { return Acft_RelPos::FastDistance(Vx, Vy); } // relative horiz. speed of the target void Print(void) const { printf("%08lX/%+5.1fs/%7.1fm/%7.1fm/%5.1fs/%5.1fm/%+5.1fs/w%d", (long int)ID, 0.5*Pred, 0.5*DistMargin, 0.5*HorDist, 0.5*TimeMargin, 0.5*MissDist, 0.5*MissTime, WarnLevel); // printf(" [%+7.1f,%+7.1f,%+7.1f]m [%+5.1f,%+5.1f,%+5.1f]m/s", 0.5*dX, 0.5*dY, 0.5*dZ, 0.5*Vx, 0.5*Vy, 0.5*Vz); // printf(" [%+5.2f,%+5.2f]m/s^-2", 0.0625*Ax, 0.0625*Ay); Pos.Print(); } uint8_t Print(char *output) { uint8_t Len=0; return Len; } uint8_t WritePFLAA(char *NMEA) { uint8_t Len=0; Len+=Format_String(NMEA+Len, "$PFLAA,"); // sentence name and alarm-level (but no alarms for trackers) NMEA[Len++]='0'+WarnLevel; NMEA[Len++]=','; Len+=Format_SignDec(NMEA+Len, dX/2); NMEA[Len++]=','; Len+=Format_SignDec(NMEA+Len, dY/2); NMEA[Len++]=','; Len+=Format_SignDec(NMEA+Len, dZ/2); // [m] relative altitude NMEA[Len++]=','; uint8_t AddrType = (ID>>24)&0x03; #ifdef WITH_SKYDEMON // SkyDemon hack which accepts only 1 or 2 if(AddrType!=1) AddrType=2; #endif NMEA[Len++]='0'+AddrType; // address-type (3=OGN) NMEA[Len++]=','; uint32_t Addr = 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++]=','; Len+=Format_UnsDec(NMEA+Len, (225*Pos.Heading+0x800)>>12, 4, 1); // [deg] heading (by GPS) NMEA[Len++]=','; Len+=Format_SignDec(NMEA+Len, (225*Pos.Turn+0x800)>>12, 2, 1); // [deg/sec] turn rate NMEA[Len++]=','; Len+=Format_UnsDec(NMEA+Len, 5*Pos.Speed, 2, 1); // [approx. m/s] ground speed NMEA[Len++]=','; Len+=Format_SignDec(NMEA+Len, 5*Pos.Climb, 2, 1); // [m/s] climb/sink rate NMEA[Len++]=','; NMEA[Len++]=HexDigit(ID>>26); // [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 } ; // ======================================================================================================= 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 union { uint8_t Flags; struct { uint8_t GpsPrec :6; // GPS position precision bool isMoving :1; // own position moving bool hasPosition :1; // own position is valid } ; } ; uint8_t WarnLevel; // highest warning level of all the targets uint8_t WeakestIdx; // index for the weakest target (or a not allocated target) uint32_t WeakestRank; // rank of the weakest target uint8_t Targets; // [aircrafts] actual number of targets monitored 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 const static int32_t DistRange = 7000; // [m] drop immediately anything beyond this distance const static int16_t MinHorizSepar = 30; // [m] minimum horizontal separation 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. char Line[80]; // for printing public: void Clear(void) { Flags=0; ID=0; Pos.Clear(); Pred=0; Targets=0; WeakestIdx=0; WeakestRank=0xFFFFFFFF; WorstTgtIdx=0; WorstTgtTime=0xFF; for(uint8_t Idx=0; IdxgetBearing()-Pos.Heading; } uint8_t WritePOGNA(char *NMEA, const LookOut_Target *Tgt) // Alert NMEA centence { uint8_t Len=0; Len+=Format_String(NMEA+Len, "$POGNA,"); Len+=NMEA_AppendCheckCRNL(NMEA, Len); NMEA[Len]=0; return Len; } void PrintPFLA(void) // print (for debug) $PFLAU and PFLAA { WritePFLAU(Line); printf("%s", Line); for(uint8_t Idx=0; Idx0) Tgt = Target + WorstTgtIdx; uint8_t Len=0; Len+=Format_String(NMEA+Len, "$PFLAU,"); Len+=Format_UnsDec(NMEA+Len, Targets, 1); // number of targets received NMEA[Len++]=','; NMEA[Len++]='0'+hasPosition; // TX status NMEA[Len++]=','; NMEA[Len++]='0'+hasPosition; // GPS status: 0=no fix, 1=on the ground, 2=airborne NMEA[Len++]=','; NMEA[Len++]='1'; // power status: one could monitor the supply NMEA[Len++]=','; NMEA[Len++]='0'+WarnLevel; // Warning level: 0..3 NMEA[Len++]=','; if(Tgt) // [deg] relative bearing: -180..+180 { Len+=Format_SignDec(NMEA+Len, ((int32_t)getRelBearing(Tgt)*45+0x1000)>>13, 1); } NMEA[Len++]=','; NMEA[Len++]='0'+((WarnLevel>0)<<1); // alarm-type: 0=none, 2=aircraft, 3=obstacle/zone/terrain NMEA[Len++]=','; if(Tgt) // [m] relative vertical distance { Len+=Format_SignDec(NMEA+Len, (Tgt->dZ)>>1, 1); } NMEA[Len++]=','; if(Tgt) // [m] relative horizontal distance { Len+=Format_UnsDec(NMEA+Len, (Tgt->HorDist)>>1, 1); } NMEA[Len++]=','; if(Tgt) // ID #ifdef WITH_SKYDEMON { Len+=Format_Hex(NMEA+Len, Tgt->ID & 0x00FFFFFF); } // maybe just 6 digits should be produced ? #else { Len+=Format_Hex(NMEA+Len, Tgt->ID); } #endif Len+=NMEA_AppendCheckCRNL(NMEA, Len); NMEA[Len]=0; return Len; } 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); 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; IdxAlloc) Tgt->Print(); } } template int32_t Start(OGNx_Packet &Me) { Clear(); ID = Me.getAddressAndType() | ((uint32_t)Me.Position.AcftType<<26) ; RefTime = Me.Position.Time; RefLat = Me.DecodeLatitude(); RefLon = Me.DecodeLongitude(); RefAlt = Me.DecodeAltitude(); LatCos = Icos(GPS_Position::calcLatAngle16(RefLat)); Pred=0; return Pos.Read(Me, RefTime, RefLat, RefLon, RefAlt, LatCos, DistRange); } template const LookOut_Target *ProcessOwn(OGNx_Packet &Me) // process my own position { // printf("ProcessOwn() ... entry\n"); 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 } else { hasPosition = Start(Me)>=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. WarnLevel=0; Targets=0; WorstTgtIdx=0; WorstTgtTime=0xFF; for(uint8_t Idx=0; IdxAlloc) continue; // skip empty slots if(Tgt->DistMargin==0) // those with no safety margin { while(Tgt->Pos.T<=(Pos.T-4)) // bring closer in time to my (new) position { Tgt->Pos.StepFwd2secs(); Tgt->Pred+=4; } } uint8_t Warn=calcTarget(Tgt); // (re)calculate the target if(Warn) { if(Warn>WarnLevel) WarnLevel=Warn; if(Tgt->TimeMarginTimeMargin; WorstTgtIdx=Idx; } } Targets++; } // printf("ProcessOwn() ... exit\n"); // if(Targets==0) return 0; // return NULL if no targets are tracked LookOut_Target *Tgt = Target+WorstTgtIdx; if( (!Tgt->Alloc) || (Tgt->DistMargin>0) ) return 0; // return NULL if target is not a thread return Tgt; } // return the pointer to the most dangerous target template const LookOut_Target *ProcessTarget(OGNx_Packet &Packet) // 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 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; for(OldIdx=0; OldIdxTarget[WeakestIdx].Pos.T) return Target+OldIdx; // if position is not really older 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 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; } uint8_t Warn=calcTarget(New); // calculate the safety margin for the target if(Warn>WarnLevel) WarnLevel=Warn; // printf("ProcessTarget() ... calc()\n"); uint8_t MaxIdx=WeakestIdx; uint16_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>=Max) { Max=Tgt.Rank; MaxIdx=Idx; } // if weaker rank found: note it } WeakestIdx=MaxIdx; // tqke the weakest slot for the nest time return New; } uint8_t calcTarget(LookOut_Target *Tgt) // calculate the savety margin for the (new) target { Tgt->TimeMargin=0xFF; // initially set inf. time margin Tgt->WarnLevel=0; // warning level=0 Tgt->MissTime=0; Tgt->MissDist=0; uint16_t Margin = calcVertMargin(Tgt); // [0.5m] calc. vertical margin if(Margin==0) Margin = calcHorizMargin(Tgt); // [0.5m] if vertical margin iz zero then get horizontal margin Tgt->DistMargin = Margin; // [0.5m] if(Margin>0) // if there is still safety margin, no more calc. (dealloc. ?) { // Tgt->TimeMargin = Margin/; return 0; } // return warning level = 0 // at this point the distance is possibly small enough so we may hit the target #ifdef DEBUG_PRINT printf("calcTarget(0x%08X) ... no abs. safety margin\n", Tgt->ID); #endif Tgt->calcVel(Pos); // calculate relative velocity // uint32_t RelVelSqr = Tgt->VelSqr(); // [0.25(m/s)^2] velocity square int16_t dT = Pos.T - Tgt->Pos.T; // [0.5s] we need to recalc. the distance if the target time is not same as mine if(dT) // [0.5s] if time difference is non-zero { int16_t Vx,Vy,Vz; // [0.5m/s] Target speed vector Tgt->Pos.getSpeedVector(Vx, Vy); Vz=Tgt->Pos.Climb; // [0.5m/s] Tgt->dX += (dT*Vx)>>1; // update Target relative distance Tgt->dY += (dT*Vy)>>1; Tgt->dZ += (dT*Vz)>>1; Tgt->HorDist = Acft_RelPos::FastDistance(Tgt->dX, Tgt->dY); } // update Target horizontal distance // uint32_t RelDistSqr = Tgt->DistSqr(); // [0.25m^2] distance square // uint32_t WarnTimeSqr = (uint32_t)WarnTime*WarnTime; // [s] warning time square // printf("calcTarget(0x%08X) ...\n", Tgt->ID); // printf("RelDistSqr = %3.1fm^ RelVelSqr=%3.1f(m/s)^2 Time=%ds\n", 0.25*RelDistSqr, 0.25*RelVelSqr, RelVelSqr ? IntSqrt(RelDistSqr/RelVelSqr):0xFFFF); // if((RelVelSqr*WarnTimeSqr)<=RelDistSqr) return 0; uint16_t RelVel = Acft_RelPos::FastDistance(Tgt->Vx, Tgt->Vy, Tgt->Vz); // [0.5m/s] uint16_t MinMissDist = 4*RelVel+Pos.Error + Tgt->Pos.Error + 2*MinHorizSepar; // [0.5m] #ifdef DEBUG_PRINT printf("Target: [%+4.1f, %+4.1f, %+4.1f] = %3.1fm/s MinMissDist=%3.1fm\n", 0.5*Tgt->Vx, 0.5*Tgt->Vy, 0.5*Tgt->Vz, 0.5*RelVel, 0.5*MinMissDist); #endif PredMe=Pos; PredTgt=Tgt->Pos; // copy my position and the target to temporary variables int16_t TimeMargin = PredMe.StepTillMinSepar(PredTgt, MinMissDist, 2*(WarnTime+2)); // predict when minimum separation is reached #ifdef DEBUG_PRINT printf("StepTillMinSepar(0x%08X, %3.1fm, %1ds) => %+4.1fs\n", Tgt->ID, 0.5*MinMissDist, WarnTime+2, 0.5*TimeMargin); #endif Tgt->TimeMargin=TimeMargin; // store the time margin till minimum separation Tgt->MissTime=TimeMargin; if(TimeMargin>(2*WarnTime)) return 0; // if time-to-margin longer than warning time then return no warning Tgt->WarnLevel=1; // otherwise set already the first warning level if(TimeMargin>WarnTime) return Tgt->WarnLevel; // is time-to-margin longer than half the warning time, then stop calculations here, return 1st warning level #ifdef DEBUG_PRINT printf("Me :"); PredMe.Print(); printf("Tgt:"); PredTgt.Print(); #endif for(uint8_t Count=3; Count; Count--) { int16_t MissTime = PredMe.MissTime(PredTgt, WarnTime); #ifdef DEBUG_PRINT printf("%d: MissTime = %+4.1fs\n", Count, 0.5*MissTime); #endif if(abs(MissTime)<=1) break; PredMe.StepFwd(MissTime); PredTgt.StepFwd(PredMe.T-PredTgt.T); } #ifdef DEBUG_PRINT printf("Me :"); PredMe.Print(); printf("Tgt:"); PredTgt.Print(); #endif Tgt->MissDist = PredMe.FastDistance(PredTgt); Tgt->MissTime = PredMe.T-Pos.T; #ifdef DEBUG_PRINT printf("MissTime = %+4.1f, MissDist = %4.1f\n", 0.5*Tgt->MissTime, 0.5*Tgt->MissDist); #endif if( (Tgt->MissTime<0) || (Tgt->MissTime>(2*WarnTime)) || (Tgt->MissDist>MinMissDist) ) Tgt->WarnLevel=0; else if(Tgt->MissDist<(2*MinHorizSepar)) { Tgt->WarnLevel=2; if(Tgt->MissTime<(2*WarnTime/3)) Tgt->WarnLevel=3; } #ifdef DEBUG_PRINT printf("calcTarget(%08X) V=[%+5.1f, %+5.1f, %+5.1f]m/s D=[%+7.1f, %+7.1f, %+7.1f]m MissTime=%5.1fsec MissDist=%6.1fm\n", Tgt->ID, 0.5*Tgt->Vx, 0.5*Tgt->Vy, 0.5*Tgt->Vz, 0.5*Tgt->dX, 0.5*Tgt->dY, 0.5*Tgt->dZ, 0.5*Tgt->MissTime, 0.5*Tgt->MissDist); #endif return Tgt->WarnLevel; } uint16_t calcVertMargin(LookOut_Target *Tgt) // calculate vertical savety margin { Tgt->dZ = Tgt->Pos.Z - Pos.Z; // [0.5m] relative vertical distance Tgt->Vz = Tgt->Pos.Climb - Pos.Climb; // [0.5ms/s] relative vertical speed int16_t VertError = Pos.Error+Tgt->Pos.Error; VertError+=VertError/2; // [0.5m] est. total vertical error VertError += 2*MinVertSepar; // [0.5m] if(abs(Tgt->dZ)<=VertError) return 0; // if vertical distance less than margin required: return zero margin if(Tgt->dZ>0) { if(Tgt->Vz>=0) return Tgt->dZ-VertError; } // if target is higher and is climbing return relative vertical distance else { if(Tgt->Vz<=0) return -Tgt->dZ-VertError; } // if target is lower and is falling, return like above int16_t dT = Tgt->Pos.T - Pos.T; // [0.5sec] time diff. in measured positions int32_t MaxAlt = ( (int32_t)Tgt->Vz * (2*(WarnTime+4)+abs(dT)) )>>1; // [0.5m] max. altitude change within the warning time MaxAlt = abs(MaxAlt) + VertError; // [0.5m] // printf("calcVertMargin() dAlt=%+4.1f dT=%+4.1f Climb=%+4.1f MaxAlt=%+4.1f\n", 0.5*Tgt->dZ, 0.5*dT, 0.5*Tgt->Vz, 0.5*MaxAlt); if(Tgt->dZ>0) // if target is above { if(Tgt->dZdZ-MaxAlt); } // [0.5m] else // if target is below { if((-Tgt->dZ)dZ-MaxAlt); } // [0.5m] } // return the vertical margin: if positive: we are safe, if zero: we are too close uint16_t calcHorizMargin(LookOut_Target *Tgt) { Tgt->dX = Tgt->Pos.X - Pos.X; // [0.5m] relative distance Tgt->dY = Tgt->Pos.Y - Pos.Y; // [0.5m] Tgt->HorDist = Acft_RelPos::FastDistance(Tgt->dX, Tgt->dY); // [0.5m] estimate horizontal distance int16_t HorError = Pos.Error+Tgt->Pos.Error; // [0.5m] sum GPS error from me and the target HorError += 2*MinHorizSepar; // [0.5m] add the min. separation required int16_t dT = abs(Tgt->Pos.T - Pos.T); // [0.5m] time difference between my data and target data int32_t MaxDistT = ((int32_t)Tgt->Pos.Speed * (2*(WarnTime+4)+dT))>>1; // [0.5m] possible distance covered by the target int32_t MaxDistM = ((int32_t) Pos.Speed * (2*(WarnTime+4)+dT))>>1; // [0.5m] possible distance covered by me int32_t MaxDist = MaxDistT + MaxDistM + HorError; // [0.5m] add horizontal separation 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 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" 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; int32_t LatDist, LonDist; if(Me.calcDistanceVector(LatDist, LonDist, RefLat, RefLon, LatCos, DistRange)<0) { return; } RefLat = Me.DecodeLatitude(); RefLon = Me.DecodeLongitude(); LatDist*=2; LonDist*=2; Pos.X -= LatDist; Pos.Y -= LonDist; for(uint8_t Idx=0; Idx