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

Correct FANET position encoding

pull/20/head
Pawel Jalocha 2020-06-09 22:12:30 +01:00
rodzic f7ee06670a
commit 3ce49daa8d
3 zmienionych plików z 35 dodań i 21 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

26
main/fanet.h
Wyświetl plik

@ -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

2
main/gps.h
Wyświetl plik

@ -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
Wyświetl plik

@ -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);