diff --git a/main/fanet.h b/main/fanet.h index 5ea93a2..43009d6 100644 --- a/main/fanet.h +++ b/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; IdxLen; 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)>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 diff --git a/main/gps.h b/main/gps.h index d031835..86bd25f 100644 --- a/main/gps.h +++ b/main/gps.h @@ -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 diff --git a/main/ogn.h b/main/ogn.h index 0e2bada..d6f06d3 100644 --- a/main/ogn.h +++ b/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);