mirror
/
esp32-ogn-tracker
kopia lustrzana https://github.com/pjalocha/esp32-ogn-tracker
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność
esp32-ogn-tracker/main/relpos.h

358 wiersze
18 KiB
C++
Czysty Bezpośredni odnośnik Wina Historia

#ifndef __RELPOS_H__
#define __RELPOS_H__
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#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 <class OGNx_Packet> // 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 <class OGNx_Packet> // 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 <class OGNx_Packet> // 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__
Reference in New Issue View Git Blame Kopiuj bezpośredni odnośnik