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/fanet.h

730 wiersze
34 KiB
C++
Czysty Bezpośredni odnośnik Wina Historia

#ifndef __FANET_H__
#define __FANET_H__
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <map>
#include "format.h"
#include "intmath.h"
// ===============================================================================================
class FANET_Packet
{ public:
union
{ uint8_t Flags;
struct
{ uint8_t CR:3; // Coding rate used (RX) or to be used (TX)
bool hasCRC:1; // CRC was there (RX)
bool badCRC:1; // CRC was bad (RX)
bool Done:1;
} ;
} ;
uint8_t Len; // [bytes] packet length
static const int MaxBytes = 40;
uint8_t Byte[MaxBytes+2];
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
uint32_t getAddr(void) const { uint32_t Addr=Byte[1]; Addr<<=8; Addr|=Byte[3]; Addr<<=8; Addr|=Byte[2]; return Addr; } // 24-bit source address
uint8_t getAddrPref(void) const { return Byte[1]; }
uint8_t getAddrType(void) const // address-type based on prefix
{ uint8_t Pref=getAddrPref();
if(Pref==0x08 || Pref==0x11 || Pref==0x20 || Pref==0xDD || Pref==0xDE || Pref==0xDF) return 2;
return 3; }
void setAddress(uint32_t Addr) { setAddrPref(Addr>>16); setAddrLow(Addr); } // full 24-bit address
void setAddrPref(uint8_t Prefix) { Byte[1]=Prefix; } // address prefix
void setAddrLow(uint16_t Addr ) { Byte[2]=Addr; Byte[3]=Addr>>8; } // lower 16-bits of the address
void setHeader(uint8_t Type) { Byte[0] = 0x40 | (Type&0x3F); }
void setType(uint8_t Type) { Byte[0] = (Byte[0]&0xC0) | (Type&0x3F); } // packet-type: 1=air-position
uint8_t ExtHeaderLen(void) const // length ot the extended header (zero in most cases)
{ if(!ExtHeader()) return 0;
uint8_t Len=1;
if(Byte[4]&0x20) Len+=3; // if Unicast
if(Byte[4]&0x10) Len+=4; // if Signature
return Len; }
uint8_t MsgOfs(void) const { return 4+ExtHeaderLen(); } // offset to the actual message (past the header and ext. header)
uint8_t MsgLen(void) const { return Len-4-ExtHeaderLen(); } // length of the actual message
const uint8_t *Msg(void) const { return Byte+MsgOfs(); } // pointer to the message, past the header
uint8_t *Msg(void) { return Byte+MsgOfs(); }
void setName(const char *Name)
{ setHeader(2);
uint8_t Ofs;
for(Ofs=MsgOfs(); Ofs<MaxBytes; Ofs++)
{ char ch = *Name++; if(ch==0) break;
Byte[Ofs]=ch; }
// xif(Ofs<MaxBytes) Byte[Ofs++]=0;
Len=Ofs; }
static float FloatCoord(int32_t Coord) // convert from FANET cordic units to float degrees
{ // const float Conv = 90.0/0x40000000; // conversion factor (exact cordic)
const float Conv = 90.0007295677/0x40000000; // FANET cordic conversion factor (not exactly cordic)
return Conv*Coord; }
static int32_t getLat(const uint8_t *Byte) // FANET cordic units
{ int32_t Latitude=Byte[2]; Latitude<<=8; Latitude|=Byte[1]; Latitude<<=8; Latitude|=Byte[0]; Latitude<<=7; return Latitude; }
static void setLat(uint8_t *Byte, int32_t Lat)
{ Lat = (Lat+0x40)>>7; Byte[0]=Lat; Byte[1]=Lat>>8; Byte[2]=Lat>>16; }
static int32_t getLon(const uint8_t *Byte) // FANET cordic units
{ int32_t Longitude=Byte[2]; Longitude<<=8; Longitude|=Byte[1]; Longitude<<=8; Longitude|=Byte[0]; Longitude<<=8; return Longitude; }
static void setLon(uint8_t *Byte, int32_t Lon)
{ Lon = (Lon+0x80)>>8; Byte[0]=Lon; Byte[1]=Lon>>8; Byte[2]=Lon>>16; }
static uint16_t getSpeed(uint8_t Byte) // [0.5km/h] for Type=1
{ if(Byte<0x80) return Byte;
return (uint16_t)5*(Byte-0x80); }
static void setSpeed(uint8_t *Byte, uint16_t Speed)
{ if(Speed>=128) { Speed=(Speed+2)/5; if(Speed>=0x80) Speed=0x7F; Speed|=0x80; }
Byte[0]=Speed; }
static int16_t getClimb(uint8_t Byte) // [0.1m/s]
{ int16_t Climb = Byte&0x3F; if(Byte&0x40) Climb|=0xFFC0;
if(Byte&0x80) return Climb*5;
return Climb; }
static void setClimb(uint8_t *Byte, int16_t Climb) // [0.1m/s]
{ if(Climb>63)
{ Climb = (Climb+2)/5; if(Climb>63) Climb=63;
Byte[0] = 0x80 | Climb; }
else if(Climb<(-63))
{ Climb = (Climb-2)/5; if(Climb<(-63)) Climb=(-63);
Byte[0] = 0x80 | Climb; }
else
{ Byte[0] = Climb&0x7F; }
}
static uint16_t getDir(uint8_t Byte) // [deg]
{ uint16_t Dir = ((uint16_t)45*Byte+0x10)>>5; return Dir; }
static uint16_t getPressure(const uint8_t *Byte) // [0.1hPa]
{ uint16_t Press = Byte[1]; Press<<=8; Press|=Byte[0]; return Press+4300; }
static int16_t getTurnRate(uint8_t Byte) // [0.25deg/s]
{ int16_t Rate = Byte&0x7F; if(Byte&0x40) Rate|=0xFF80;
if(Byte&0x80) return Rate<<2;
return Rate; }
static void setTurnRate(uint8_t *Byte, int16_t Rate)
{ if(Rate>= 64 ) { if(Rate> 254 ) Rate= 254 ; Byte[0] = 0x80 | ((Rate+2)>>2); return; }
if(Rate<(-64)) { if(Rate<(-254)) Rate=(-254); Byte[0] = 0x80 | (((Rate+2)>>2)&0x7F); return; }
Byte[0] = Rate&0x7F; }
static int16_t getQNE(uint8_t Byte) // [m] difference between pressure altitude and GPS altitude
{ int16_t QNE = Byte&0x7F; if(Byte&0x40) QNE|=0xFF80;
if(Byte&0x80) return QNE<<2;
return QNE; }
static void setQNE(uint8_t *Byte, int16_t QNE)
{ if(QNE>= 64 ) { if(QNE> 254 ) QNE= 254 ; Byte[0] = 0x80 | ((QNE+2)>>2); return; }
if(QNE<(-64)) { if(QNE<(-254)) QNE=(-254); Byte[0] = 0x80 | (((QNE+2)>>2)&0x7F); return; }
Byte[0] = QNE&0x7F; }
static uint16_t getAltitude(const uint8_t *Byte) // [m]
{ uint16_t Alt = Byte[1]; Alt<<=8; Alt|=Byte[0]; Alt&=0x0FFF;
if(Alt<0x800) return Alt;
return (Alt-0x800)<<2; }
void setAltitude(uint8_t *Byte, uint16_t Alt) // [m]
{ if(Alt>=0x800) { Alt>>=2; if(Alt>0x800) Alt=0x800; Alt|=0x800; }
Byte[0]=Alt; Byte[1] = (Byte[1]&0xF0) | (Alt>>8); }
// [0..7] [0..1] [FANET cordic] [FANET cordic] [m] [cordic] [0.1m/s] [0.1m/s] [0.1deg/s]
void setAirPos(uint8_t AcftType, uint8_t Track, int32_t Lat, int32_t Lon, int16_t Alt, uint8_t Dir, uint16_t Speed, int16_t Climb, int16_t Turn)
{ setHeader(1);
uint8_t Ofs=MsgOfs();
Len=Ofs+12;
setLat(Byte+Ofs, Lat); // [cordic]
setLon(Byte+Ofs+3, Lon); // [cordic]
Byte[Ofs+7]=(AcftType<<4) | (Track<<7);
if(Alt<0) Alt=0;
setAltitude(Byte+Ofs+6, Alt); // [m]
Speed = (Speed*23+16)>>5; // [0.1m/s] => [0.5km/h]
setSpeed(Byte+Ofs+8, Speed); // [0.5km/h]
setClimb(Byte+Ofs+9, Climb); // [0.1m/s]
Byte[Ofs+10] = Dir; // [cordic]
setTurnRate(Byte+Ofs+11, Turn*2/5); } // [0.25deg/s]
void setQNE(int32_t StdAltitude) // [m] only for air-position
{ uint8_t Ofs=MsgOfs();
int32_t Alt=getAltitude(Byte+Ofs+6);
if(Len<(Ofs+13)) Len=Ofs+13;
setQNE(Byte+Ofs+12, StdAltitude-Alt); }
// [0..15] [0..1] [FANET cordic] [FANET cordic]
void setGndPos(uint8_t Status, uint8_t Track, int32_t Lat, int32_t Lon)
{ setHeader(7);
uint8_t Ofs=MsgOfs();
Len=11;
setLat(Byte+Ofs, Lat);
setLon(Byte+Ofs+3, Lon);
Byte[Ofs+6] = (Status<<4) | Track; }
/*
* $FNNGB,manufacturer(hex),id(hex),name(up to 32bytes),type/status,latitude,longitude,altitude,climb,speed,heading*checksum
* manufacturer: 1-2 chars hex
* id: 1-4 chars hex
* name: string up to 32 chars
* type/status: while airborne: aircraft type: 0-7 (3D tracking), else: status: 0-15 (2D tracking) +10 -> 10-25
* latitude: %.5f in degree
* longitude: %.5f in degree
* altitude: %.f in meter, -1000 for ground
* climb: %.1f in m/s
* speed: %.1f in km/h
* heading: %.f in degree
*
* for the types please see: https://github.com/3s1d/fanet-stm32/blob/master/Src/fanet/radio/protocol.txt
*
*/
uint8_t WriteFNNGB(char *Out)
{ return 0; }
int DecodePosition(float &Lat, float &Lon, int &Alt)
{ uint8_t Idx=MsgOfs();
if(Type()==1)
{ Lat = FloatCoord(getLat(Byte+Idx));
Lon = FloatCoord(getLon(Byte+Idx+3));
Alt = getAltitude(Byte+Idx+6);
return 3; }
if(Type()==7)
{ Lat = FloatCoord(getLat(Byte+Idx));
Lon = FloatCoord(getLon(Byte+Idx+3));
Alt = 0;
return 2; }
return 0; }
void Print(const char *Name=0) const
{ if(Name) printf("%s ", Name);
printf("[%2d:%d:%2d] FNT%06X", Len, Type(), MsgLen(), getAddr());
if(Type()==2) // Name
{ printf(" ");
for(uint8_t Idx=MsgOfs(); Idx<Len; Idx++)
printf("%c", Byte[Idx]);
printf("\n"); return; }
if(Type()==3) // Message
{ uint8_t Idx=MsgOfs();
printf(" Msg%02X: ", Byte[Idx++]);
for( ; Idx<Len; Idx++)
printf("%c", Byte[Idx]);
printf("\n"); return; }
if(Type()==4) // Service, mostly meteo
{ uint8_t Idx=MsgOfs(); uint8_t Service=Byte[Idx++];
int32_t Lat=getLat(Byte+Idx); Idx+=3; // [FANET cordic]
int32_t Lon=getLon(Byte+Idx); Idx+=3; // [FANET cordic]
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
{ uint16_t Dir = Byte[Idx++]; // [cordic]
uint16_t Wind = getSpeed(Byte[Idx++]); // [0.2km/h]
uint16_t Gust = getSpeed(Byte[Idx++]); // [0.2km/h]
printf(" %03.0fdeg %3.1f/%3.1fkm/h", (180.0/128)*Dir, 0.2*Wind, 0.2*Gust); }
if(Service&0x10) printf(" %3.1f%%", 0.4*Byte[Idx++]); // humidity
if(Service&0x08) // pressure
{ printf(" %3.1fhPa", 0.1*getPressure(Byte+Idx)); Idx+=2; }
if(Service&0x02) printf(" %1.0f%%", (100.0/15)*Byte[Idx++]); // charge state
printf("\n"); return; }
if(Type()==1) // airborne position
{ uint8_t Idx=MsgOfs(); uint8_t AcftType=Byte[Idx+7]>>4;
int32_t Lat=getLat(Byte+Idx); // [FANET cordic]
int32_t Lon=getLon(Byte+Idx+3); // [FANET cordic]
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(" [%+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)
{ int16_t Rate = getTurnRate(Byte[Idx+11]);
printf(" %+3.1fdeg/s", 0.25*Rate); }
if((Idx+12)<Len)
{ int16_t QNE = getQNE(Byte[Idx+12]);
printf(" %+dm", QNE); }
printf("\n"); return; }
if(Type()==7) // ground position
{ uint8_t Idx=MsgOfs(); uint8_t Status=Byte[Idx+6];
int32_t Lat=getLat(Byte+Idx); // [FANET cordic]
int32_t Lon=getLon(Byte+Idx+3); // [FANET cordic]
printf(" [%+09.5f,%+010.5f] s%02X", FloatCoord(Lat), FloatCoord(Lon), Status);
printf("\n"); return; }
if(Type()==8) // Hardware/Software
{ uint8_t Idx=MsgOfs();
uint8_t Hw = Byte[Idx];
uint16_t Fw = Byte[Idx+2]; Fw<<=8; Fw|=Byte[Idx+1];
printf(" Hw%02X Fw%02d.%02d.%04d%c", Hw, Fw&0x1F, (Fw>>5)&0x0F, 2019+((Fw>>9)&0x3F), Fw&0x8000?'d':'r');
printf("\n"); return; }
printf("\n");
}
uint8_t Read(const char *Inp) // read packet from a hex dump
{ for( Len=0; Len<MaxBytes; Len++) // read as many hex bytes as you can
{ int8_t Upp = Read_Hex1(Inp[0]); if(Upp<0) break; // 1st digit
int8_t Low = Read_Hex1(Inp[1]); if(Low<0) break; // 2nd digit
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 1e-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 = CoordUBX(Lat); // convert from FANET cordic to UBX 1e-7 deg
char Sign;
if(Lat>0) { Sign='N'; }
else { Sign='S'; Lat=(-Lat); }
int32_t Dig;
Dig=Lat/100000000; (*Str++)='0'+Dig; Lat-=Dig*100000000;
Dig=Lat/10000000; (*Str++)='0'+Dig; Lat-=Dig*10000000;
Lat*=60;
Dig=Lat/100000000; (*Str++)='0'+Dig; Lat-=Dig*100000000;
Dig=Lat/10000000; (*Str++)='0'+Dig; Lat-=Dig*10000000;
(*Str++)='.';
Dig=Lat/1000000; (*Str++)='0'+Dig; Lat-=Dig*1000000;
Dig=Lat/100000; (*Str++)='0'+Dig; Lat-=Dig*100000;
Dig=Lat/10000; HighRes ='0'+Dig; Lat-=Dig*10000;
(*Str++)=Sign;
return 8; }
static int Format_Lon(char *Str, int32_t Lon, char &HighRes) // format longitude after APRS
{ Lon = CoordUBX(Lon); // convert from FANET cordic to UBX 1e-7 deg
char Sign;
if(Lon>0) { Sign='E'; }
else { Sign='W'; Lon=(-Lon); }
int32_t Dig;
Dig=Lon/1000000000; (*Str++)='0'+Dig; Lon-=Dig*1000000000;
Dig=Lon/100000000; (*Str++)='0'+Dig; Lon-=Dig*100000000;
Dig=Lon/10000000; (*Str++)='0'+Dig; Lon-=Dig*10000000;
Lon*=60;
Dig=Lon/100000000; (*Str++)='0'+Dig; Lon-=Dig*100000000;
Dig=Lon/10000000; (*Str++)='0'+Dig; Lon-=Dig*10000000;
(*Str++)='.';
Dig=Lon/1000000; (*Str++)='0'+Dig; Lon-=Dig*1000000;
Dig=Lon/100000; (*Str++)='0'+Dig; Lon-=Dig*100000;
Dig=Lon/10000; HighRes ='0'+Dig; Lon-=Dig*10000;
(*Str++)=Sign;
return 9; }
} ;
class FANET_RxPacket: public FANET_Packet
{ public:
uint32_t sTime; // [ s] reception time
uint16_t msTime; // [ms]
int16_t FreqOfs; // [ 10Hz]
int8_t SNR; // [0.25dB]
int8_t RSSI; // [dBm]
uint8_t BitErr; // number of bit errors
uint8_t CodeErr; // number of block errors
uint8_t Sync; // sync symbols used: 0xF1 for sx127x but 0x12 for sx1262 ?
public:
void setTime(double RxTime) { sTime=floor(RxTime); msTime=floor(1000.0*(RxTime-sTime)); }
double getTime(void) const { return (double)sTime+0.001*msTime; }
uint32_t SlotTime(void) const { uint32_t Slot=sTime; if(msTime<100) Slot--; return Slot; }
void Print(char *Name=0) const
{ char HHMMSS[8];
Format_HHMMSS(HHMMSS, SlotTime()); HHMMSS[6]='h'; HHMMSS[7]=0;
printf("%s CR%c%c%c %3.1fdB/%de %+3.1fkHz ", HHMMSS, '0'+CR, hasCRC?'c':'_', badCRC?'-':'+', 0.25*SNR, BitErr, 1e-2*FreqOfs);
FANET_Packet::Print(Name); }
int PrintJSON(char *JSON, uint8_t AddrType=0) const
{ const uint8_t *Msg = this->Msg();
uint8_t MsgLen = this->MsgLen();
uint8_t Type = this->Type();
if(Type!=1 && Type!=7) { JSON[0]=0; return 0; }
int Len=0;
JSON[Len++]='{';
uint32_t Address = getAddr();
if(AddrType==0) AddrType = getAddrType();
Len+=Format_String(JSON+Len, "\"Address\":\"");
Len+=Format_Hex(JSON+Len, Byte[1]);
Len+=Format_Hex(JSON+Len, Byte[3]);
Len+=Format_Hex(JSON+Len, Byte[2]);
Len+=Format_String(JSON+Len, "\", \"AddrType\":");
JSON[Len++] = '0'+AddrType;
Len+=Format_String(JSON+Len, "\", \"ID\":\"");
Len+=Format_Hex(JSON+Len, Address | ((uint32_t)AddrType<<24));
uint32_t Time = SlotTime(); // sTime; if(msTime<100) Time--;
Len+=Format_String(JSON+Len, "\", \"Time\":");
Len+=Format_UnsDec(JSON+Len, Time);
if(Type==1)
{ const uint8_t OGNtype[8] = { 0, 7, 6, 0xB, 1, 8, 3, 0xD } ; // OGN aircraft types
uint8_t AcftType=Msg[7]>>4; // get the aircraft-type and online-track flag
Len+=Format_String(JSON+Len, ", \"AcftType\":");
Len+=Format_UnsDec(JSON+Len, OGNtype[AcftType&0x7]);
uint32_t Alt=getAltitude(Msg+6); // [m] decode the altitude
uint32_t Speed=getSpeed(Msg[8]); // [0.5km/h] ground speed
Speed = (Speed*355+0x80)>>8; // [0.5km/h] => [0.1m/s] convert
int32_t Climb=getClimb(Msg[9]); // [0.1m/s] climb rate
uint16_t Dir=getDir(Msg[10]); // [deg]
Len+=Format_String(JSON+Len, ", \"Alt\":");
Len+=Format_UnsDec(JSON+Len, Alt);
Len+=Format_String(JSON+Len, ", \"Track\":");
Len+=Format_UnsDec(JSON+Len, Dir);
Len+=Format_String(JSON+Len, ", \"Speed\":");
Len+=Format_UnsDec(JSON+Len, Speed, 2, 1);
Len+=Format_String(JSON+Len, ", \"Climb\":");
Len+=Format_SignDec(JSON+Len, Climb, 2, 1, 1);
if(MsgLen>11)
{ int16_t Turn=getTurnRate(Msg[11]);
Len+=Format_String(JSON+Len, ", \"Turn\":");
Len+=Format_SignDec(JSON+Len, Turn*10/4, 2, 1, 1); }
if(MsgLen>12)
{ int32_t AltStd=Alt; Alt+=getQNE(Msg[12]);
Len+=Format_String(JSON+Len, ", \"StdAlt\":");
Len+=Format_SignDec(JSON+Len, AltStd, 1, 0, 1); }
}
if(Type==1 || Type==7)
{ int32_t Lat = getLat(Msg); // [cordic] decode the latitude
int32_t Lon = getLon(Msg+3); // [cordic] decode the longitude
Len+=Format_String(JSON+Len, ", \"Lat\":");
Len+=Format_SignDec(JSON+Len, CoordUBX(Lat), 8, 7, 1);
Len+=Format_String(JSON+Len, ", \"Lon\":");
Len+=Format_SignDec(JSON+Len, CoordUBX(Lon), 8, 7, 1); }
Len+=Format_String(JSON+Len, ", \"RxProt\":\"FNT\"");
if(SNR>0)
{ Len+=Format_String(JSON+Len, ", \"RxSNR\":");
Len+=Format_SignDec(JSON+Len, ((int16_t)SNR*10+2-843)>>2, 2, 1, 1); }
Len+=Format_String(JSON+Len, ", \"RxErr\":");
Len+=Format_UnsDec(JSON+Len, BitErr);
Len+=Format_String(JSON+Len, ", \"RxFreqOfs\":");
Len+=Format_SignDec(JSON+Len, FreqOfs/10, 1, 1);
JSON[Len++]=' '; JSON[Len++]='}';
JSON[Len]=0; return Len; }
int WriteStxJSON(char *JSON, uint8_t AddrType=0) const
{ int Len=0;
Len+=Format_String(JSON+Len, "\"addr\":\"");
Len+=Format_Hex(JSON+Len, Byte[1]);
Len+=Format_Hex(JSON+Len, Byte[3]);
Len+=Format_Hex(JSON+Len, Byte[2]);
JSON[Len++]='\"';
JSON[Len++]=',';
Len+=Format_String(JSON+Len, "\"addr_type\":");
if(AddrType==0) AddrType = getAddrType();
JSON[Len++] = '0'+AddrType;
const uint8_t *Msg = this->Msg();
uint8_t MsgLen = this->MsgLen();
uint8_t Type = this->Type();
uint32_t Time = SlotTime(); // sTime; if(msTime<100) Time--;
Len+=Format_String(JSON+Len, ",\"time\":");
Len+=Format_UnsDec(JSON+Len, Time);
int64_t RxTime=(int64_t)sTime-Time; RxTime*=1000; RxTime+=msTime;
Len+=Format_String(JSON+Len, ",\"rx_time\":");
Len+=Format_SignDec(JSON+Len, RxTime, 4, 3, 1);
if(Type==2)
{ Len+=Format_String(JSON+Len, ",\"Name\":\"");
for(int Idx=0; Idx<MsgLen; Idx++)
{ uint8_t Byte=Msg[Idx]; if(Byte==0) break;
JSON[Len++]=Byte; }
JSON[Len++]='\"'; }
if(Type==1) // for airborne position
{ uint8_t AcftType=Msg[7]>>4; // get the aircraft-type and online-track flag
Len+=Format_String(JSON+Len, ",\"acft_type\":\"");
JSON[Len++] = HexDigit(AcftType&0x7);
JSON[Len++]='\"';
Len+=Format_String(JSON+Len, ",\"acft_cat\":\""); // GDL90 aircraft category
// no-info, para-glider, hang-glider, balloon, glider, powered, heli, UAV
const uint8_t AcftCat[8] = { 0, 12, 12, 10, 9, 1, 7, 14 } ;
Len+=Format_Hex(JSON+Len, AcftCat[AcftType&0x07]);
JSON[Len++]='\"';
Len+=Format_String(JSON+Len, ",\"no_track\":");
JSON[Len++]='0' + (AcftType>>3); }
if(Type==4) // for service/weather
{ uint8_t Service=Msg[0]; //
Len+=Format_String(JSON+Len, ",\"service\":\"");
if(Service&0x80) JSON[Len++]='I';
if(Service&0x40) JSON[Len++]='T';
if(Service&0x20) JSON[Len++]='W';
if(Service&0x10) JSON[Len++]='H';
if(Service&0x08) JSON[Len++]='B';
// if(Service&0x04) JSON[Len++]='R';
if(Service&0x02) JSON[Len++]='C';
JSON[Len++]='\"';
int32_t Lat = getLat(Msg+1); // [cordic] decode the latitude
int32_t Lon = getLon(Msg+4); // [cordic] decode the longitude
Len+=Format_String(JSON+Len, ",\"lat_deg\":");
Len+=Format_SignDec(JSON+Len, CoordUBX(Lat), 8, 7, 1);
Len+=Format_String(JSON+Len, ",\"lon_deg\":");
Len+=Format_SignDec(JSON+Len, CoordUBX(Lon), 8, 7, 1);
int Idx=7;
if(Service&0x40)
{ Len+=Format_String(JSON+Len, ",\"temp_deg\":");
Len+=Format_SignDec(JSON+Len, (int16_t)5*((int8_t)Msg[Idx++]), 2, 1, 1); }
if(Service&0x20)
{ uint16_t Dir = Msg[Idx++]; // [cordic]
Len+=Format_String(JSON+Len, ",\"wind_deg\":");
Len+=Format_UnsDec(JSON+Len, (45*Dir+16)>>5, 2, 1);
uint16_t Wind = getSpeed(Msg[Idx++]); // [0.2km/h]
Len+=Format_String(JSON+Len, ",\"wind_kmh\":");
Len+=Format_UnsDec(JSON+Len, 2*Wind, 2, 1);
uint16_t Gust = getSpeed(Msg[Idx++]);
Len+=Format_String(JSON+Len, ",\"gust_kmh\":");
Len+=Format_UnsDec(JSON+Len, 2*Gust, 2, 1); }
if(Service&0x10)
{ Len+=Format_String(JSON+Len, ",\"hum_perc\":");
Len+=Format_UnsDec(JSON+Len, (uint16_t)4*Msg[Idx++], 2, 1); }
if(Service&0x08)
{ Len+=Format_String(JSON+Len, ",\"press_hpa\":");
Len+=Format_UnsDec(JSON+Len, getPressure(Msg+Idx), 2, 1);
Idx+=2; }
}
if(Type==1 || Type==7) // airborne or ground position
{ int32_t Lat = getLat(Msg); // [cordic] decode the latitude
int32_t Lon = getLon(Msg+3); // [cordic] decode the longitude
Len+=Format_String(JSON+Len, ",\"lat_deg\":");
Len+=Format_SignDec(JSON+Len, CoordUBX(Lat), 8, 7, 1);
Len+=Format_String(JSON+Len, ",\"lon_deg\":");
Len+=Format_SignDec(JSON+Len, CoordUBX(Lon), 8, 7, 1); }
if(Type==1) // for airpborne position
{ uint32_t Alt=getAltitude(Msg+6); // [m] decode the altitude
uint32_t Speed=getSpeed(Msg[8]); // [0.5km/h] ground speed
Speed = (Speed*355+0x80)>>8; // [0.5km/h] => [0.1m/s] convert
int32_t Climb=getClimb(Msg[9]); // [0.1m/s] climb rate
uint16_t Dir=getDir(Msg[10]); // [deg]
Len+=Format_String(JSON+Len, ",\"alt_msl_m\":");
Len+=Format_UnsDec(JSON+Len, Alt);
Len+=Format_String(JSON+Len, ",\"track_deg\":");
Len+=Format_UnsDec(JSON+Len, Dir);
Len+=Format_String(JSON+Len, ",\"speed_mps\":");
Len+=Format_UnsDec(JSON+Len, Speed, 2, 1);
Len+=Format_String(JSON+Len, ",\"climb_mps\":");
Len+=Format_SignDec(JSON+Len, Climb, 2, 1, 1);
if(MsgLen>11)
{ int16_t Turn=getTurnRate(Msg[11]);
Len+=Format_String(JSON+Len, ",\"turn_dps\":");
Len+=Format_SignDec(JSON+Len, Turn*10/4, 2, 1, 1); }
if(MsgLen>12)
{ int32_t AltStd=Alt; Alt+=getQNE(Msg[12]);
Len+=Format_String(JSON+Len, ",\"alt_std_m\":");
Len+=Format_SignDec(JSON+Len, AltStd, 1, 0, 1); }
Len+=Format_String(JSON+Len, ",\"on_ground\":0"); }
if(Type==7) // for ground position
{ uint8_t Status = Msg[6];
Len+=Format_String(JSON+Len, ",\"no_track\":1");
JSON[Len++]='0' + (Status&1);
Len+=Format_String(JSON+Len, ",\"on_ground\":1"); }
return Len; }
int WriteAPRS(char *Out, uint8_t AddrType=0)
{ bool Report=0;
if(AddrType==0) AddrType = getAddrType(); // 2 (FLARM) or 3 (OGN)
int Len=0;
bool isPosition = Type()==1 || Type()==4 || Type()==7;
Len+=Format_String(Out+Len, "FNT");
Len+=Format_Hex(Out+Len, Byte[1]);
Len+=Format_Hex(Out+Len, Byte[3]);
Len+=Format_Hex(Out+Len, Byte[2]);
Len+=Format_String(Out+Len, ">OGNFNT,qOR:");
Out[Len++]=isPosition?'/':'>';
Len+=Format_HHMMSS(Out+Len, SlotTime());
Out[Len++]='h';
const uint8_t *Msg = this->Msg();
uint8_t MsgLen = this->MsgLen();
switch(Type())
{ case 2: // Name: pilot or weather station
{ Len+=Format_String(Out+Len, " Name=\"");
for(int Idx=0; Idx<MsgLen; Idx++)
{ uint8_t Byte=Msg[Idx]; if(Byte==0) break;
Out[Len++]=Byte; }
Out[Len++]='\"';
Report=Byte[1]!=0x06; break; } // do not report names of the Burnair weather stations
case 4: // Service: mostly meteo
{ uint8_t Service=Msg[0];
int32_t Lat = getLat(Msg+1); // [cordic]
int32_t Lon = getLon(Msg+4); // [cordic]
int Idx=7;
int8_t Temp; // [0.5degC]
if(Service&0x40) Temp=Msg[Idx++]; // if temperature
uint16_t Dir; uint16_t Speed; uint16_t Gust;
if(Service&0x20) // [if wind data]
{ Dir = getDir(Msg[Idx++]); // [deg]
Speed = getSpeed(Msg[Idx++]); // [0.2km/h]
Gust = getSpeed(Msg[Idx++]); } // [0.2km/h]
uint16_t Hum; // [0.4%]
if(Service&0x10) Hum=Msg[Idx++]; // if humidity
uint16_t Press; // [0.1hPa]
if(Service&0x08) Press=getPressure(Msg+Idx); // if pressure
Idx+=2;
char hLat, hLon;
Len+=Format_Lat(Out+Len, Lat, hLat);
Out[Len++]='/';
Len+=Format_Lon(Out+Len, Lon, hLon);
Out[Len++]='_';
if(Service&0x20)
{ Len+=Format_UnsDec(Out+Len, Dir, 3); // [deg]
Out[Len++]='/';
Len+=Format_UnsDec(Out+Len, (Speed+4)>>3, 3); // [0.2km/h -> mph]
Out[Len++]='g';
Len+=Format_UnsDec(Out+Len, (Gust+4)>>3, 3); // [0.2km/h -> mph]
} else Len+=Format_String(Out+Len, ".../...g...");
Out[Len++]='t';
if(Service&0x40)
{ // int16_t Fahr=Temp; Fahr+=4*Temp/5; Fahr/=2; Fahr+=32; //
int16_t Fahr = (((int16_t)Temp*115+64)>>7) + 32; // [0.5degC] => [degF]
if(Fahr>=0) Len+=Format_UnsDec(Out+Len, Fahr, 3);
else Len+=Format_SignDec(Out+Len, Fahr, 2);
} else Len+=Format_String(Out+Len, "...");
if(Service&0x10)
{ Out[Len++]='h';
Hum = (Hum*4+5)/10; if(Hum>=100) Hum=00;
Len+=Format_UnsDec(Out+Len, Hum, 2); }
if(Service&0x08)
{ Out[Len++]='b';
Len+=Format_UnsDec(Out+Len, Press, 5); }
Report=Byte[1]!=0x06; break; } // don't report Burnair weather reports
case 1: // airborne position
{ const char *AcftIcon[8] = { "/z", "/g", "/g", "/O", "/'", "\\^", "/X", "/'" } ; // APRS icons for aircraft types
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
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
ID<<=24; ID |= getAddr(); // address
int32_t Lat = getLat(Msg); // [cordic]
int32_t Lon = getLon(Msg+3); // [cordic]
uint32_t Alt=getAltitude(Msg+6); // [m]
uint32_t Feet = ((int32_t)3360*Alt+512)>>10; // [feet]
uint32_t Speed=getSpeed(Msg[8]); // [0.5km/h]
uint32_t Knots=(Speed*553+1024)>>11; // knots
int32_t Climb=getClimb(Msg[9]); // [0.1m/s]
int32_t ClimbFeet = ((int32_t)1968*Climb+50)/100; // [fpm]
uint16_t Dir=getDir(Msg[10]); // [deg]
int16_t Turn=getQNE(Msg[11]); // [0.25deg/s]
int16_t QNE=getQNE(Msg[12]); // [m]
int32_t StdAlt=Alt+QNE; if(StdAlt<0) StdAlt=0; // [m]
uint32_t StdFeet = ((int32_t)3360*StdAlt+512)>>10; // [feet]
char hLat, hLon;
Len+=Format_Lat(Out+Len, Lat, hLat);
Out[Len++]=Icon[0];
Len+=Format_Lon(Out+Len, Lon, hLon);
Out[Len++]=Icon[1];
Len+=Format_UnsDec(Out+Len, Dir, 3);
Out[Len++]='/';
Len+=Format_UnsDec(Out+Len, Knots, 3);
Len+=Format_String(Out+Len, "/A=");
Len+=Format_UnsDec(Out+Len, Feet, 6);
Len+=Format_String(Out+Len, " !W");
Out[Len++]=hLat;
Out[Len++]=hLon;
Out[Len++]='!';
Len+=Format_String(Out+Len, " id");
Len+=Format_Hex(Out+Len, ID);
Out[Len++]=' ';
Len+=Format_SignDec(Out+Len, ClimbFeet);
Len+=Format_String(Out+Len, "fpm");
if(MsgLen>11)
{ Out[Len++]=' ';
Len+=Format_SignDec(Out+Len, Turn*5/6, 2, 1);
Len+=Format_String(Out+Len, "rot"); }
if(MsgLen>12)
{ Len+=Format_String(Out+Len, " FL");
Len+=Format_UnsDec(Out+Len, StdFeet, 5, 2); }
Len+=Format_String(Out+Len, " FNT1"); Out[Len++]='0'+(AcftType&7);
Report=1; break; }
case 7: // ground position
{ // const char *StatusMsg[16] = { 0, "Walking", "Vehicle", "Bike", "Boot", 0, 0, 0,
// "Need-ride", "Landed-well", 0, 0, "Need-technical",
// "Need-medical", "Distress(man)", "Distress(auto)" } ;
uint8_t Status = Msg[6];
bool Track = Status&1;
Status>>=4;
const char *Icon = "\\n"; // static object
if(Status>=13) Icon = "\\!"; // Emergency
// const char *StatMsg = StatusMsg[Status];
uint8_t AcftType = 15; //
uint32_t ID = (AcftType<<2) | AddrType; // acft-type and addr-type
if(!Track) ID|=0x80; // stealth flag
ID<<=24; ID |= getAddr(); // address
int32_t Lat = getLat(Msg); // [cordic]
int32_t Lon = getLon(Msg+3); // [cordic]
char hLat, hLon;
Len+=Format_Lat(Out+Len, Lat, hLat);
Out[Len++]=Icon[0];
Len+=Format_Lon(Out+Len, Lon, hLon);
Out[Len++]=Icon[1];
Len+=Format_String(Out+Len, " !W");
Out[Len++]=hLat;
Out[Len++]=hLon;
Out[Len++]='!';
Len+=Format_String(Out+Len, " id");
Len+=Format_Hex(Out+Len, ID);
// if(StatMsg)
// { Out[Len++]=' '; Len+=Format_String(Out+Len, StatMsg); }
Len+=Format_String(Out+Len, " FNT7"); Out[Len++]=HexDigit(Status);
Report=1; break; }
}
Out[Len++]=' '; Out[Len++]='s';
Len+=Format_Hex(Out+Len, Sync);
if(SNR>0)
{ Out[Len++]=' ';
Len+=Format_SignDec(Out+Len, ((int16_t)SNR*10+2-843)>>2, 2, 1, 1);
Out[Len++]='d'; Out[Len++]='B'; }
Out[Len++]=' ';
Len+=Format_SignDec(Out+Len, FreqOfs/10, 2, 1);
Len+=Format_String(Out+Len,"kHz");
if(BitErr)
{ Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, BitErr); Out[Len++]='e'; }
if(!Report) Len=0; // if not to be reported
Out[Len]=0; return Len; }
} ;
// =========================================================================================
#ifndef ARDUINO
class FANET_Name
{ public:
static const int MaxSize = 32;
uint32_t Time;
// uint8_t Type;
char Name[MaxSize];
public:
FANET_Name() { Time=0; Name[0]=0; }
int Copy(const uint8_t *Src, int Size)
{ if(Size>=MaxSize) Size=MaxSize-1;
memcpy(Name, Src, Size); Name[Size]=0;
return Size; }
} ;
#include <map>
class FANET_NameList
{ public:
std::map<uint32_t, FANET_Name> List;
public:
int Update(FANET_RxPacket &Packet)
{ if(Packet.Type()!=2) return 0;
uint32_t Addr = Packet.getAddr();
FANET_Name &Name = List[Addr];
Name.Time = Packet.SlotTime();
Name.Copy(Packet.Msg(), Packet.MsgLen());
return 1; }
} ;
#endif
// ===============================================================================================
#endif // __FANET_H__
Reference in New Issue View Git Blame Kopiuj bezpośredni odnośnik