#ifndef __RELPOS_H__ #define __RELPOS_H__ #include #include #include #include #include "intmath.h" #include "gdl90.h" #include "ogn.h" #include "adsl.h" // ======================================================================================================= class Acft_RelPos // 3-D relative position with speed and turn rate { public: int16_t T; // [0.5sec] int16_t X,Y,Z; // [0.5m] uint16_t Speed; // [0.5m/s] uint16_t Heading; // [360/0x10000 deg] 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] 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.1fs: [%+7.1f,%+7.1f,%+7.1f]m %5.1fm/s %05.1fdeg", 0.5*T, 0.5*X, 0.5*Y, 0.5*Z, 0.5*Speed, (360.0/0x10000)*Heading); if(hasClimb) printf(" %+5.1fm/s", 0.5*Climb); else printf(" ---.-m/s"); if(hasTurn) printf(" %+5.1fdeg/s", (360.0/0x10000)*Turn); else printf(" ---.-deg/s"); printf(" [%3.1fm]", 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); printf("\n"); } void Clear(void) { T =0; X =0; Y =0; Z =0; Speed=0; Heading=0; Climb=0; Turn=0; Error=4; } uint32_t SqrDistance(Acft_RelPos &Target) { int32_t dX = Target.X-X; int32_t dY = Target.Y-Y; int32_t dZ = Target.Z-Z; return dX*dX+dY*dY+dZ*dZ; } // [0.25m^2] static uint32_t SqrDistance(int16_t dX, int16_t dY, int16_t dZ) { return (int32_t)dX*dX + (int32_t)dY*dY + (int32_t)dZ*dZ; } static uint32_t SqrDistance(int16_t dX, int16_t dY) { return (int32_t)dX*dX + (int32_t)dY*dY; } uint32_t FastDistance(Acft_RelPos &Target) { int16_t dX = Target.X-X; int16_t dY = Target.Y-Y; int16_t dZ = Target.Z-Z; return FastDistance(dX, dY, dZ); } // [0.5m] static uint16_t FastDistance(int16_t dX, int16_t dY) { dX = abs(dX); dY = abs(dY); if(dX>dY) return dX+dY/2; else return dY+dX/2; } static uint16_t FastDistance(int16_t dX, int16_t dY, int16_t dZ) { return FastDistance((int16_t)FastDistance(dX, dY), dZ); } // predict self and target until MinSepar is reached but no longer than MaxTime int16_t StepTillMinSepar(Acft_RelPos &Target, uint16_t MinSepar, int16_t MaxTime=40) // [0.5m] [0.5s] { int16_t PredTime=0; // count time by which we predict uint16_t MaxTurn=abs(Turn); // the max. turn rate uint16_t Turn2=abs(Target.Turn); if(Turn2>MaxTurn) MaxTurn=Turn2; int16_t MaxStepTime = 32; // [0.5s] max. allowed stpping time period if(MaxTurn>=0x100) MaxStepTime=0x2000/MaxTurn; // [0.5s] maximup step time (for sharp turns) if(MaxStepTime<2) MaxStepTime=2; // [0.5s] but don't do smaller steps than 1sec uint16_t TotSpeed = FastDistance(Speed, Climb) + FastDistance(Target.Speed, Target.Climb); // [0.5m/s] "total" speed, thus the sum of the two speeds magnitudes #ifdef DEBUG_PRINT printf("StepTillMinSepar( , MinSepar=%3.1fm, MaxTime=%3.1fs)\n", 0.5*MinSepar, 0.5*MaxTime); Print(); Target.Print(); #endif for( ; ; ) { if(MaxTime==0) return PredTime; // if max. prediction time reached: stop int32_t DistMargin = FastDistance(Target); // [0.5m] Distance margin to the target if(DistMargin<=MinSepar) return PredTime; // If distance margin already below minimum int16_t dT = Target.T-T; // [0.5sec] Target may not be exact same time int32_t dS = (dT*TotSpeed)>>1; // [0.5m] thus some extra distance margin DistMargin -= abs(dS); // [0.5m] subtract margin due to time difference if(DistMargin<=MinSepar) return PredTime; // [0.5m] if distance margin below minimum DistMargin -= MinSepar; // [0.5m] subtract minimum separation from the distance margin if((TotSpeed*MaxTime) < (2*DistMargin) ) // If plenty enough margin given the speed { uint16_t TimeMargin=MaxTime+2; if((2*DistMargin)<(TotSpeed*TimeMargin)) TimeMargin=2*DistMargin/TotSpeed+1; TimeMargin+=PredTime; // if(TimeMargin>240) TimeMargin=240; return TimeMargin; } int16_t StepTime = (2*DistMargin)/TotSpeed+1; // [0.5s] Time margin given distance margin and speed #ifdef DEBUG_PRINT printf("DistMargin=%3.1fm => StepTime=%+4.1fs\n", 0.5*DistMargin, 0.5*StepTime); #endif // if(StepTime==0) return PredTime; // if(StepTime<1) StepTime=1; // minimum step time for prediction if(StepTime>MaxStepTime) StepTime=MaxStepTime; // [0.5s] maximum step time for prediction if(StepTime>MaxTime) StepTime=MaxTime; int16_t NextTime = T+StepTime; // [0.5s] time StepFwd(StepTime); int16_t TgtStepTime = NextTime-Target.T; if(TgtStepTime>0) Target.StepFwd(TgtStepTime); #ifdef DEBUG_PRINT Print(); Target.Print(); #endif PredTime+=StepTime; // [0.5s] count time by which we predict MaxTime-=StepTime; } // [0.5s] decrement the max. time we should predict return MaxTime+1; } // return estimated time margin before the separation could fall below minimum // predict the minimum distance: does not take turn into account thus must not be used on significant distances int16_t MissTime(Acft_RelPos &Target, int16_t MaxTime=40) // [0.5s] { int32_t dX = Target.X; // [0.5m] Target distance vector int32_t dY = Target.Y; int32_t dZ = Target.Z; int32_t tVx, tVy; Target.getSpeedVector(tVx, tVy); // [0.5m/s] Target speed vector int32_t tVz = Target.Climb; int16_t dT = Target.T - T; // [0.5sec] time difference betwen target and me if(dT) { dX -= (dT*tVx)>>1; // adjust target position for the time diff. dY -= (dT*tVy)>>1; dZ -= (dT*tVz)>>1; } dX -= X; // [0.5m] Target relative position dY -= Y; dZ -= Z; int32_t dVx, dVy; getSpeedVector(dVx, dVy); // [0.5m/s] Target relative speed dVx = tVx-dVx; dVy = tVy-dVy; int32_t dVz = tVz - Climb; int32_t DistSqrRate = dVx*dX + dVy*dY + dVz*dZ; // [0.25m2/s] 3-D scalar product: rel. distance x rel. speed // if(DistSqrRate==0) return MaxTime; // if target neither moving away nor closing // if(DistSqrRate>0) return MaxTime; // if target moving away from me int32_t RelVelSqr = dVx*dVx + dVy*dVy + dVz*dVz; // [0.25m2/s2] 3-D relative velocity square // printf("MissTime() DistSqrRate = %+4.1fm^2/s RelVelSqr = %3.1f(m/s)^2\n", 0.25*DistSqrRate, 0.5*RelVelSqr); if( ( RelVelSqr*MaxTime) <= (-2*DistSqrRate) ) return MaxTime; // if min. approach time is longer than maximum if( (-RelVelSqr*MaxTime) >= (-2*DistSqrRate) ) return -MaxTime; // if min. approach time is longer than maximum return (-2*DistSqrRate+(RelVelSqr/2))/RelVelSqr; } // [0.5sec] time of the closest approach void calcDir(void) // calculate the direction unity vector { Dx = Icos(Heading); // DirX = cos(Heading) Dy = Isin(Heading); } // DirY = sin(Heading) // void calcAccel(void) // { int32_t A = ((int32_t)Turn*Speed*201+0x8000)>>16; // [1/64m/s^2] // Ax = (-A*Dy+0x2000)>>14; // [1/16m/s^2] // Ay = (+A*Dx+0x2000)>>14; } // void calcTurn(int16_t MaxRadius=10000) // calculate the turning radius and turning circle center // { R=getTurnRadius(MaxRadius); if(R==0) return; // Ox = X - ((Dy*R)>>12); // Oy = Y + ((Dx*R)>>12); } int16_t getTurnDev(int16_t dT) // approx. horizontal deviation from straight line due to the turn { int32_t A = ((int32_t)Turn*Speed*201+0x8000)>>16; // [1/64m/s^2] acceleration // printf("getTurnDev() A=%6.3fm/s^2\n", A/64.0); return (A*dT*dT+0x80)>>8; } // [0.5m] int16_t getTurnRadius(int32_t MaxRadius=0x7FFF) const { int32_t Radius = (int32_t)Speed*10436; if(Turn==0) return 0; // return zero for radius larger than maximum defined Radius/=Turn; if(abs(Radius)>MaxRadius) return 0; return Radius; } // return turning radius [0.5m] = turning circle diameter [m] void getSpeedVector(int32_t &Vx, int32_t &Vy) // [0.5m/s] [0.5m/s] { Vx = ((int32_t)Dx*Speed+0x800)>>12; // Vx = DirX * Speed Vy = ((int32_t)Dy*Speed+0x800)>>12; } // Vy = DirY * Speed void getSpeedVector(int16_t &Vx, int16_t &Vy) // [0.5m/s] [0.5m/s] { Vx = ((int32_t)Dx*Speed+0x800)>>12; // Vx = DirX * Speed Vy = ((int32_t)Dy*Speed+0x800)>>12; } // Vy = DirY * Speed void StepFwd(int16_t Time) // [0.5s] predict the position into the future { int16_t Vx, Vy; int16_t Time1 = Time>>1; // [s] int16_t Time2 = Time-Time1; // [0.5s] getSpeedVector(Vx, Vy); int16_t dX = Time1*Vx; int16_t dY = Time1*Vy; if(hasTurn) Heading += (Time*Turn)>>1; calcDir(); // calcAccel(); getSpeedVector(Vx, Vy); dX += Time2*Vx; dY += Time2*Vy; X += dX/2; Y += dY/2; if(hasClimb) Z += (Time*Climb)>>1; T += Time; } void StepFwdSecs(int16_t Secs) // [sec] predict the position Secs into the future { int16_t Vx, Vy; int16_t Secs1=Secs>>1; int16_t Secs2=Secs-Secs1; getSpeedVector(Vx, Vy); X += Secs1*Vx; Y += Secs1*Vy; if(hasTurn) Heading += Secs*Turn; calcDir(); // calcAccel(); getSpeedVector(Vx, Vy); X += Secs2*Vx; Y += Secs2*Vy; if(hasClimb) Z += Secs*Climb; T += 2*Secs; } void StepFwd4secs(void) // predict the position four second into the future { int16_t Vx, Vy; getSpeedVector(Vx, Vy); X += 2*Vx; Y += 2*Vy; if(hasTurn) Heading += 4*Turn; calcDir(); // calcAccel(); getSpeedVector(Vx, Vy); X += 2*Vx; Y += 2*Vy; if(hasClimb) Z += 4*Climb; T += 8; } void StepFwd2secs(void) // predict the position two seconds into the future { int16_t Vx, Vy; getSpeedVector(Vx, Vy); // get hor. speed vector from speed and dir. vector X += Vx; Y += Vy; // incr. the hor. coordinates by half the speed if(hasTurn) Heading += 2*Turn; // increment the heading by the turning rate calcDir(); // calcAccel(); // recalc. direction and acceleration getSpeedVector(Vx, Vy); // recalc. the speed vector X += Vx; Y += Vy; // incr. horizotal coord. by half the speed vector if(hasClimb) Z += 2*Climb; // increment the rel. altitude by the climb rate T += 4; } // increment time by 2sec void StepFwd1sec(void) // predict the position one second into the future { int16_t Vx, Vy; getSpeedVector(Vx, Vy); X += Vx/2; Y += Vy/2; if(hasTurn) Heading += Turn; calcDir(); // calcAccel(); getSpeedVector(Vx, Vy); X += Vx/2; Y += Vy/2; if(hasClimb) 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; } int32_t Read(ADSL_Packet &Packet, uint32_t RxTime, uint32_t RefTime, int32_t RefLat, int32_t RefLon, int32_t RefAlt, uint16_t LatCos=3000, int16_t GeoidSepar=40, int32_t MaxDist=15000) { Flags=0; uint16_t msTime=0; uint32_t PosTime=Packet.getTime(msTime, RxTime); if(PosTime) T = PosTime-RefTime; // [sec] else T = RxTime-RefTime; T<<=1; if(msTime>=500) T++; // [0.5sec] int32_t LatDist, LonDist; // [1/60000deg] if(Packet.calcDistanceVectorOGN(LatDist, LonDist, RefLat, RefLon, LatCos, MaxDist)<0) return -1; // [m, m, , , , m] X = LatDist*2; // [m] => [0.5m] relative along latitude Y = LonDist*2; // [m] => [0.5m] relative along longitude Z = Packet.getAlt()-RefAlt-GeoidSepar; Z*=2; Speed = Packet.getSpeed()/2; Heading = Packet.getTrack()<<7; if(Packet.hasClimb()) { Climb=Packet.getClimb()/4; hasClimb=1; } calcDir(); Error=Packet.getHorAccur(); return 0; } void Write(ADSL_Packet &Packet, uint8_t RefTime, int32_t RefLat, int32_t RefLon, int32_t RefAlt, uint16_t LatCos=3000, int16_t GeoidSepar=40) { int16_t Time=RefTime+(T>>1); Packet.TimeStamp = ((Time%15)<<2) | ((T&1)<<1); Packet.setDistanceVectorOGN(X>>1, Y>>1, RefLat, RefLon, LatCos); Packet.setAlt(RefAlt+(Z>>1)+GeoidSepar); // Packet.setSpeed(Speed*2); // [0.5m/s] => [0.25m/s] Packet.setTrack(Heading>>7); // [cordic] Packet.setClimb(Climb*4); // [0.5m/s] => [0.125m/s] Packet.setHorAccur(Error); Packet.setVerAccur(Error+Error/2); } template // read position from an OGN packet, use provided reference 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=15000) { 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*2; // [m] => [0.5m] relative along latitude Y = LonDist*2; // [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] 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(); // calcTurn(); // printf("Read: "); Packet.Print(); // Print(); return 1; } template // write position into the OGN packet, using given reference void Write(OGNx_Packet &Packet, uint8_t RefTime, int32_t RefLat, int32_t RefLon, int32_t RefAlt, uint16_t LatCos=3000) { int16_t Time=RefTime+(T>>1); // if(Time<0) Time+=60; else if(Time>=60) Time-=60; Packet.Position.Time = Time%60; Packet.setDistanceVector(X>>1, Y>>1, RefLat, RefLon, LatCos); Packet.EncodeAltitude(RefAlt+(Z>>1)); // 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.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__