kopia lustrzana https://github.com/pjalocha/esp32-ogn-tracker
Correct FANET position encoding
rodzic
f7ee06670a
commit
3ce49daa8d
26
main/fanet.h
26
main/fanet.h
|
@ -29,6 +29,13 @@ class FANET_Packet
|
|||
|
||||
public:
|
||||
FANET_Packet() { Len=0; }
|
||||
|
||||
uint8_t Dump(char *Out)
|
||||
{ uint8_t Len=0;
|
||||
for(int Idx=0; Idx<this->Len; Idx++)
|
||||
{ Len+=Format_Hex(Out+Len, Byte[Idx]); }
|
||||
return Len; }
|
||||
|
||||
bool ExtHeader(void) const { return Byte[0]&0x80; } // is there extended header ?
|
||||
bool Forward(void) const { return Byte[0]&0x40; } // forward flag
|
||||
uint8_t Type(void) const { return Byte[0]&0x3F; } // message type
|
||||
|
@ -193,7 +200,7 @@ class FANET_Packet
|
|||
{ uint8_t Idx=MsgOfs(); uint8_t Service=Byte[Idx++];
|
||||
int32_t Lat=getLat(Byte+Idx); Idx+=3;
|
||||
int32_t Lon=getLon(Byte+Idx); Idx+=3;
|
||||
printf(" [%+9.5f,%+10.5f] s%02X", FloatCoord(Lat), FloatCoord(Lon), Service);
|
||||
printf(" [%+09.5f,%+010.5f] s%02X", FloatCoord(Lat), FloatCoord(Lon), Service);
|
||||
if(Service&0x80) printf(" Igw");
|
||||
if(Service&0x40) printf(" %+3.1fC", 0.5*(int8_t)Byte[Idx++]); // temperature
|
||||
if(Service&0x20) // wind direction and speed
|
||||
|
@ -213,7 +220,7 @@ class FANET_Packet
|
|||
uint16_t Alt=getAltitude(Byte+Idx+6); // [m]
|
||||
uint16_t Speed=getSpeed(Byte[Idx+8]); // [0.5km/h]
|
||||
int16_t Climb=getClimb(Byte[Idx+9]); // [0.1m/s]
|
||||
printf(" [%+9.5f,%+10.5f] %dm %3.1fkm/h %03.0fdeg %+4.1fm/s a%X%c",
|
||||
printf(" [%+09.5f,%+010.5f] %dm %3.1fkm/h %03.0fdeg %+4.1fm/s a%X%c",
|
||||
FloatCoord(Lat), FloatCoord(Lon), Alt, 0.5*Speed, (180.0/128)*Byte[Idx+10], 0.1*Climb,
|
||||
AcftType&0x07, AcftType&0x08?'T':'H');
|
||||
if((Idx+11)<Len)
|
||||
|
@ -224,7 +231,7 @@ class FANET_Packet
|
|||
{ uint8_t Idx=MsgOfs(); uint8_t Status=Byte[Idx+6];
|
||||
int32_t Lat=getLat(Byte+Idx);
|
||||
int32_t Lon=getLon(Byte+Idx+3);
|
||||
printf(" [%+9.5f,%+10.5f] s%02X", FloatCoord(Lat), FloatCoord(Lon), Status);
|
||||
printf(" [%+09.5f,%+010.5f] s%02X", FloatCoord(Lat), FloatCoord(Lon), Status);
|
||||
printf("\n"); return; }
|
||||
if(Type()==8) // Hardware/Software
|
||||
{ uint8_t Idx=MsgOfs();
|
||||
|
@ -242,9 +249,11 @@ class FANET_Packet
|
|||
Byte[Len] = (Upp<<4) | Low; Inp+=2; } // new byte, count input
|
||||
return Len; } // return number of bytes read = packet length
|
||||
|
||||
static int32_t CoordUBX(int32_t Coord) { return ((int64_t)900007296*Coord+0x20000000)>>30; } // convert FANET-cordic to UBX 10e-7deg units
|
||||
// ((int64_t)900000000*Coord+0x20000000)>>30; // this is the exact formula, but FANET is not exact here
|
||||
|
||||
static int Format_Lat(char *Str, int32_t Lat, char &HighRes) // format latitude after APRS
|
||||
{ // Lat = ((int64_t)900000000*Lat+0x20000000)>>30; // convert from cordic to UBX 1e-7 deg
|
||||
Lat = ((int64_t)900007296*Lat+0x20000000)>>30; // convert from FANET cordic to UBX 1e-7 deg
|
||||
{ Lat = CoordUBX(Lat); // convert from FANET cordic to UBX 1e-7 deg
|
||||
char Sign;
|
||||
if(Lat>0) { Sign='N'; }
|
||||
else { Sign='S'; Lat=(-Lat); }
|
||||
|
@ -262,8 +271,7 @@ class FANET_Packet
|
|||
return 8; }
|
||||
|
||||
static int Format_Lon(char *Str, int32_t Lon, char &HighRes) // format longitude after APRS
|
||||
{ // Lon = ((int64_t)900000000*Lon+0x20000000)>>30;
|
||||
Lon = ((int64_t)900007296*Lon+0x20000000)>>30;
|
||||
{ Lon = CoordUBX(Lon); // convert from FANET cordic to UBX 1e-7 deg
|
||||
char Sign;
|
||||
if(Lon>0) { Sign='E'; }
|
||||
else { Sign='W'; Lon=(-Lon); }
|
||||
|
@ -374,7 +382,7 @@ class FANET_RxPacket: public FANET_Packet
|
|||
const uint8_t OGNtype[8] = { 0, 7, 6, 0xB, 1, 8, 3, 0xD } ; // OGN aircraft types
|
||||
uint8_t AcftType=Msg[7]>>4; // aircraft-type and online-tracking flag
|
||||
const char *Icon = AcftIcon[AcftType&7]; // APRS icon
|
||||
uint8_t AddrType = 2; // 2 (FLARM) or 3 (OGN) ?
|
||||
uint8_t AddrType = getAddrType(); // 2 (FLARM) or 3 (OGN)
|
||||
uint32_t ID = (OGNtype[AcftType&7]<<2) | AddrType; // acft-type and addr-type
|
||||
bool Track = AcftType&0x08; // online tracking flag
|
||||
if(!Track) ID|=0x80; // if no online tracking the set as stealth flag
|
||||
|
@ -425,7 +433,7 @@ class FANET_RxPacket: public FANET_Packet
|
|||
const char *Icon = "\\n"; // static object
|
||||
if(Status>=13) Icon = "\\!"; // Emergency
|
||||
// const char *StatMsg = StatusMsg[Status];
|
||||
uint8_t AddrType = 2; //
|
||||
uint8_t AddrType = getAddrType(); //
|
||||
uint8_t AcftType = 15; //
|
||||
uint32_t ID = (AcftType<<2) | AddrType; // acft-type and addr-type
|
||||
if(!Track) ID|=0x80; // stealth flag
|
||||
|
|
|
@ -8,7 +8,7 @@
|
|||
#include "lowpass2.h"
|
||||
|
||||
#ifdef WITH_ESP32
|
||||
const uint8_t GPS_PosPipeSize = 32; // number of GPS positions held in a pipe
|
||||
const uint8_t GPS_PosPipeSize = 8; // number of GPS positions held in a pipe
|
||||
#else
|
||||
const uint8_t GPS_PosPipeSize = 4; // number of GPS positions held in a pipe
|
||||
#endif
|
||||
|
|
28
main/ogn.h
28
main/ogn.h
|
@ -651,7 +651,7 @@ class GPS_Position
|
|||
{ uint8_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; // barometric information has beed supplied
|
||||
bool hasBaro :1; // pressure sensor information: pressure, standard pressure altitude, temperature, humidity
|
||||
// bool hasHum :1; //
|
||||
bool isReady :1; // is ready for the following treaement
|
||||
bool Sent :1; // has been transmitted
|
||||
|
@ -841,9 +841,10 @@ class GPS_Position
|
|||
Out[Len++]='/'; Len+=Format_SignDec(Out+Len, GeoidSeparation, 4, 1); Out[Len++]='m';
|
||||
Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Speed, 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s';
|
||||
Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Heading, 4, 1); Out[Len++]='d'; Out[Len++]='e'; Out[Len++]='g';
|
||||
Out[Len++]=' '; Len+=Format_SignDec(Out+Len, Temperature, 2, 1); Out[Len++]='C';
|
||||
Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Pressure/4 ); Out[Len++]='P'; Out[Len++]='a';
|
||||
Out[Len++]=' '; Len+=Format_SignDec(Out+Len, StdAltitude, 2, 1); Out[Len++]='m';
|
||||
if(hasBaro)
|
||||
{ Out[Len++]=' '; Len+=Format_SignDec(Out+Len, Temperature, 2, 1); Out[Len++]='C';
|
||||
Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Pressure/4 ); Out[Len++]='P'; Out[Len++]='a';
|
||||
Out[Len++]=' '; Len+=Format_SignDec(Out+Len, StdAltitude, 2, 1); Out[Len++]='m'; }
|
||||
Out[Len++]='\n'; Out[Len++]=0; return Len; }
|
||||
#endif // __AVR__
|
||||
|
||||
|
@ -904,7 +905,7 @@ class GPS_Position
|
|||
return 0; }
|
||||
|
||||
int8_t ReadGGA(NMEA_RxMsg &RxMsg)
|
||||
{ if(RxMsg.Parms<14) return -1; // no less than 14 paramaters
|
||||
{ if(RxMsg.Parms<14) return -2; // no less than 14 paramaters
|
||||
hasGPS = ReadTime((const char *)RxMsg.ParmPtr(0))>0; // read time and check if same as the RMC says
|
||||
FixQuality =Read_Dec1(*RxMsg.ParmPtr(5)); if(FixQuality<0) FixQuality=0; // fix quality: 0=invalid, 1=GPS, 2=DGPS
|
||||
Satellites=Read_Dec2((const char *)RxMsg.ParmPtr(6)); // number of satellites
|
||||
|
@ -996,7 +997,7 @@ class GPS_Position
|
|||
return 1; }
|
||||
*/
|
||||
int ReadRMC(NMEA_RxMsg &RxMsg)
|
||||
{ if(RxMsg.Parms<12) return -1; // no less than 12 parameters
|
||||
{ if(RxMsg.Parms<11) return -2; // no less than 12 parameters
|
||||
hasGPS = ReadTime((const char *)RxMsg.ParmPtr(0))>0; // read time and check if same as the GGA says
|
||||
if(ReadDate((const char *)RxMsg.ParmPtr(8))<0) setDefaultDate(); // date
|
||||
ReadLatitude(*RxMsg.ParmPtr(3), (const char *)RxMsg.ParmPtr(2)); // Latitude
|
||||
|
@ -1008,7 +1009,7 @@ class GPS_Position
|
|||
|
||||
int8_t ReadRMC(const char *RMC)
|
||||
{ if( (memcmp(RMC, "$GPRMC", 6)!=0) && (memcmp(RMC, "$GNRMC", 6)!=0) ) return -1; // check if the right sequence
|
||||
uint8_t Index[20]; if(IndexNMEA(Index, RMC)<12) return -2; // index parameters and check the sum
|
||||
uint8_t Index[20]; if(IndexNMEA(Index, RMC)<11) return -2; // index parameters and check the sum
|
||||
hasGPS = ReadTime(RMC+Index[0])>0;
|
||||
if(ReadDate(RMC+Index[8])<0) setDefaultDate();
|
||||
ReadLatitude( RMC[Index[3]], RMC+Index[2]);
|
||||
|
@ -1099,13 +1100,18 @@ class GPS_Position
|
|||
Temperature = MAV->temperature/10;
|
||||
hasBaro=1; }
|
||||
|
||||
int32_t getCordicLatitude (void) const { return ((int64_t)Latitude *83399993+(1<<21))>>22; }
|
||||
int32_t getCordicLongitude(void) const { return ((int64_t)Longitude*83399993+(1<<21))>>22; }
|
||||
static int32_t getCordic(int32_t Coord) { return ((int64_t)Coord*83399993+(1<<21))>>22; } // [0.0001/60 deg] => [cordic]
|
||||
int32_t getCordicLatitude (void) const { return getCordic(Latitude ); }
|
||||
int32_t getCordicLongitude(void) const { return getCordic(Longitude); }
|
||||
|
||||
// [deg] [0.0001/60deg] [Cordic] [FANET Cordic]
|
||||
// 180 0x066FF300 0x80000000 0x7FFFBC00
|
||||
static int32_t getFANETcordic(int32_t Coord) { return ((int64_t)Coord*83399317+(1<<21))>>22; } // [0.0001/60 deg] => [FANET cordic]
|
||||
|
||||
void EncodeAirPos(FANET_Packet &Packet, uint8_t AcftType=1, bool Track=1)
|
||||
{ int32_t Alt = Altitude; if(Alt<0) Alt=0; else Alt=(Alt+5)/10;
|
||||
int32_t Lat = getCordicLatitude(); // Latitude: [0.0001/60deg] => [cordic]
|
||||
int32_t Lon = getCordicLongitude(); // Longitude: [0.0001/60deg] => [cordic]
|
||||
int32_t Lat = getFANETcordic(Latitude); // Latitude: [0.0001/60deg] => [cordic]
|
||||
int32_t Lon = getFANETcordic(Longitude); // Longitude: [0.0001/60deg] => [cordic]
|
||||
// other, glider, tow, heli, chute, drop, hang, para, powered, jet, UFO, balloon, air, UAV, ground, static
|
||||
const uint8_t FNTtype[16] = { 0, 4, 5, 6, 1, 5, 2, 1, 5, 5, 0, 3, 5, 7, 0, 0 } ; // convert aircraft-type from OGN to FANET
|
||||
Packet.setAirPos(FNTtype[AcftType&0x0F], Track, Lat, Lon, Alt, (((uint16_t)Heading<<4)+112)/225, Speed, ClimbRate);
|
||||
|
|
Ładowanie…
Reference in New Issue