kopia lustrzana https://github.com/pjalocha/esp32-ogn-tracker
Updates from other projects
Pawel Jalocha
rodzic
e7c33be541
commit
05ac9aee9b
236
main/lookout.h
236
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<MaxTargets; Idx++)
|
||||
{ if(!Target[Idx].Alloc) continue;
|
||||
|
|
@ -196,11 +210,11 @@ class LookOut
|
|||
}
|
||||
}
|
||||
|
||||
void WritePFLA(void (*Output)(char)) // produce $PFLAU and PFLAA on the console output
|
||||
void WritePFLA(void (*Output)(char)) // produce $PFLAU and PFLAA on the console output
|
||||
{ WritePFLAU(Line); Format_String(Output, Line);
|
||||
for(uint8_t Idx=0; Idx<MaxTargets; Idx++)
|
||||
{ if(!Target[Idx].Alloc) continue;
|
||||
if( Target[Idx].DistMargin) continue;
|
||||
{ if(!Target[Idx].Alloc) continue; // skip empty slots
|
||||
if( Target[Idx].DistMargin) continue; // skip slots with distance margin remaining
|
||||
Target[Idx].WritePFLAA(Line);
|
||||
Format_String(Output, Line);
|
||||
}
|
||||
|
|
@ -242,9 +256,82 @@ class LookOut
|
|||
NMEA[Len]=0;
|
||||
return Len; }
|
||||
|
||||
int32_t Latitude(int32_t LatDist) const { return RefLat + (LatDist*27)/10; } // relative distance [0.5m] => 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<MaxTargets; Idx++)
|
||||
{ const LookOut_Target *Tgt = Target+Idx;
|
||||
|
|
@ -253,37 +340,38 @@ class LookOut
|
|||
}
|
||||
|
||||
template <class OGNx_Packet>
|
||||
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 <class OGNx_Packet>
|
||||
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; Idx<MaxTargets; Idx++) // go over targets
|
||||
{ LookOut_Target *Tgt = Target+Idx;
|
||||
|
|
@ -294,8 +382,8 @@ class LookOut
|
|||
}
|
||||
uint8_t Warn=calcTarget(Tgt); // (re)calculate the target
|
||||
if(Warn)
|
||||
{ if(Warn>WarnLevel) WarnLevel=Warn;
|
||||
if(Tgt->TimeMargin<WorstTgtTime) { WorstTgtTime=Tgt->TimeMargin; WorstTgtIdx=Idx; }
|
||||
{ if(Warn>WarnLevel) WarnLevel=Warn; // register highest warning level
|
||||
if(Tgt->TimeMargin<WorstTgtTime) { WorstTgtTime=Tgt->TimeMargin; 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 <class OGNx_Packet>
|
||||
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; OldIdx<MaxTargets; OldIdx++) // scan targets already on the list
|
||||
{ if(Target[OldIdx].Alloc==0) continue;
|
||||
if(OldIdx==WeakestIdx) continue;
|
||||
{ if(Target[OldIdx].Alloc==0) continue; // skip not allocated
|
||||
if(OldIdx==WeakestIdx) continue; // skip the new position
|
||||
if(Target[OldIdx].ID==ID) break; } // to find previous position for the target
|
||||
if(OldIdx<MaxTargets) // if found
|
||||
{ if((Target[OldIdx].Pos.T-Target[OldIdx].Pred)>Target[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(MaxDist<Tgt->HorDist) 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<MaxTargets; Idx++) // go over the targets
|
||||
{ LookOut_Target &Tgt = Target[Idx]; if(!Tgt.Alloc) continue; // skip unallocated
|
||||
Tgt.Pos.T-=2*TimeDelta; // shift the relative time
|
||||
|
|
@ -472,7 +562,7 @@ class LookOut
|
|||
}
|
||||
|
||||
void AdjustRefAlt(void) // shift the vertical reference point when we get too far off
|
||||
{ if(fabs(Pos.Z)<(2*200)) return; // don't shift if less than 200m from the reference point
|
||||
{ if(abs(Pos.Z)<(2*200)) return; // don't shift if less than 200m from the reference point
|
||||
int16_t AltDelta=Pos.Z/2;
|
||||
RefAlt+=AltDelta;
|
||||
Pos.Z-=2*AltDelta;
|
||||
|
|
@ -481,17 +571,17 @@ class LookOut
|
|||
}
|
||||
|
||||
template <class OGNx_Packet>
|
||||
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<MaxTargets; Idx++)
|
||||
for(uint8_t Idx=0; Idx<MaxTargets; Idx++) // shift the relative position of every target
|
||||
{ if(Target[Idx].Alloc) { Target[Idx].Pos.X-=LatDist; Target[Idx].Pos.Y-=LonDist; } }
|
||||
LatCos = Icos(GPS_Position::calcLatAngle16(RefLat));
|
||||
}
|
||||
|
|
|
|||
109
main/ogn.h
109
main/ogn.h
|
|
@ -52,10 +52,10 @@ template <class OGNx_Packet, class OGNy_Packet>
|
|||
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 <class OGNx_Packet=OGN1_Packet>
|
|||
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 <class OGNx_Packet=OGN1_Packet>
|
|||
|
||||
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 <class OGNx_Packet>
|
||||
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)
|
||||
|
|
|
|||
70
main/ogn1.h
70
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
|
||||
|
|
|
|||
|
|
@ -378,7 +378,7 @@ static void ProcessRxPacket(OGN_RxPacket<OGN_Packet> *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<OGN_Packet> *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);
|
||||
|
|
|
|||
|
|
@ -1,3 +1,6 @@
|
|||
#ifndef __RELPOS_H__
|
||||
#define __RELPOS_H__
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
#include <stdint.h>
|
||||
|
|
@ -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 <class OGNx_Packet> // 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__
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue