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

FANET: QNE encoding and packet reception

pull/20/head
Pawel Jalocha 2020-05-02 20:37:15 +01:00
rodzic 1d83ab73b3
commit e23f222c55
4 zmienionych plików z 100 dodań i 27 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

62
main/fanet.h
Wyświetl plik

@ -14,8 +14,14 @@
class FANET_Packet
{ public:
uint8_t Len;
static const int MaxBytes = 32;
union
{ uint8_t Flags;
struct
{ uint8_t CR:3; // Coding rate
} ;
} ;
uint8_t Len; // [bytes] packet length
static const int MaxBytes = 40;
uint8_t Byte[MaxBytes+2];
public:
@ -100,9 +106,14 @@ class FANET_Packet
{ uint16_t Press = Byte[1]; Press<<=8; Press|=Byte[0]; return Press+4300; }
static int16_t getQNE(uint8_t Byte) // [m] difference between pressure altitude and GPS altitude
{ int16_t QNE = Byte&0x3F; if(Byte&0x40) QNE|=0xFFC0;
if(Byte&0x80) return QNE*2;
{ 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;
@ -115,7 +126,7 @@ class FANET_Packet
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)
{ setHeader(1);
uint8_t Ofs=MsgOfs();
Len=15;
Len=Ofs+11;
setLat(Byte+Ofs, Lat); // [cordic]
setLon(Byte+Ofs+3, Lon); // [cordic]
Byte[Ofs+7]=(AcftType<<4) | (Track<<7);
@ -126,6 +137,12 @@ class FANET_Packet
setClimb(Byte+Ofs+9, Climb); // [0.1m/s]
Byte[Ofs+10] = Dir; } // [cordic]
void setQNE(int32_t StdAltitude) // [m] only for air-position
{ uint8_t Ofs=MsgOfs();
int32_t Alt=getAltitude(Byte+Ofs+6);
if(Len<(Ofs+12)) Len=Ofs+12;
setQNE(Byte+Ofs+11, StdAltitude-Alt); }
// [0..15] [0..1] [cordic] [cordic]
void setGndPos(uint8_t Status, uint8_t Track, int32_t Lat, int32_t Lon)
{ setHeader(7);
@ -135,6 +152,26 @@ class FANET_Packet
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; }
void Print(char *Name=0) const
{ if(Name) printf("%s ", Name);
printf("[%2d:%d:%2d] FNT%06X", Len, Type(), MsgLen(), getAddr());
@ -176,6 +213,9 @@ class FANET_Packet
printf(" [%+9.5f,%+10.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 QNE = getQNE(Byte[Idx+11]);
printf(" %+dm", QNE); }
printf("\n"); return; }
if(Type()==7) // ground position
{ uint8_t Idx=MsgOfs(); uint8_t Status=Byte[Idx+6];
@ -242,14 +282,18 @@ class FANET_Packet
class FANET_RxPacket: public FANET_Packet
{ public:
double Time; // reception time
uint32_t sTime; // [ s] reception time
uint16_t msTime; // [ms]
int16_t FreqOfs; // [ 10Hz]
uint8_t SNR; // [0.25dB]
int8_t SNR; // [0.25dB]
int8_t RSSI; // [dBm]
uint8_t BitErr; // number of bit errors
uint8_t CodeErr; // number of block errors
public:
uint32_t SlotTime(void) const { return floor(Time-0.3); }
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<=300) Slot--; return Slot; }
void Print(char *Name=0) const
{ char HHMMSS[8];
@ -419,7 +463,7 @@ class FANET_RxPacket: public FANET_Packet
class FANET_Name
{ public:
static const int MaxSize = 40;
static const int MaxSize = 32;
uint32_t Time;
// uint8_t Type;
char Name[MaxSize];

14
main/ogn.h
Wyświetl plik

@ -1102,13 +1102,15 @@ class GPS_Position
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; }
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]
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]
// 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); }
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);
if(hasBaro) { Packet.setQNE((StdAltitude+5)/10); }
}
void Encode(GDL90_REPORT &Report)
{ Report.setAccuracy(9, 9);

2
main/rf.cpp
Wyświetl plik

@ -339,7 +339,7 @@ extern "C"
TxChan = RF_FreqPlan.getChannel(RF_SlotTime, 1, 1); // transmit channel
RX_Channel = TxChan;
#ifdef WITH_FANET
#if defined(WITH_FANET) && defined(WITH_RFM95)
const FANET_Packet *FNTpkt = FNT_TxFIFO.getRead(0); // read the packet from the FANET transmitt queue
if(FNTpkt) // was there any ?
{ TRX.SetLoRa(); // switch TRX to LoRa

49
main/rfm.h
Wyświetl plik

@ -7,8 +7,9 @@
// #include "config.h"
#include "ogn.h"
#include "fanet.h"
class RFM_RxPktData // packet received by the RF chip
class RFM_RxPktData // OGN packet received by the RF chip
{ public:
static const uint8_t Bytes=26; // [bytes] number of bytes in the packet
uint32_t Time; // [sec] Time slot
@ -255,6 +256,11 @@ class RFM_TRX
Buff[3] = 0;
Block_Write(Buff, 3, Addr); }
uint32_t ReadFreq(uint8_t Addr=REG_FRFMSB)
{ uint8_t *Data = Block_Read(3, Addr);
uint32_t Freq=Data[0]; Freq<<=8; Freq|=Data[1]; Freq<<=8; Freq|=Data[2];
return Freq; }
void WriteFIFO(const uint8_t *Data, uint8_t Len)
{ Block_Write(Data, Len, REG_FIFO); }
@ -503,8 +509,7 @@ class RFM_TRX
return 0; }
int ConfigureFNT(uint8_t CR=1) // configure for FANET/LoRa
{ // WriteMode(RF_OPMODE_LORA_STANDBY);
WriteTxPower(0);
{ WriteTxPower(0);
WriteByte(0x00, REG_LORA_HOPPING_PERIOD); // disable fast-hopping
WriteByte(0xF1, REG_LORA_SYNC); // SYNC for FANET
WriteWord(0x0005, REG_LORA_PREAMBLE_MSB); // [symbols] minimal preamble
@ -513,10 +518,12 @@ class RFM_TRX
WriteByte(0xC3, REG_LORA_DETECT_OPTIMIZE);
WriteByte(0x0A, REG_LORA_DETECT_THRESHOLD);
WriteByte(0x04, REG_LORA_MODEM_CONFIG3); // LNA auto-gain ?
WriteByte(0x64, REG_LORA_SYMBOL_TIMEOUT); // 0x64 = default or more ?
WriteByte( 40, REG_LORA_PACKET_MAXLEN); // [bytes] enough ?
WriteByte(0xFF, REG_LORA_SYMBOL_TIMEOUT); // 0x64 = default or more ?
WriteByte(FANET_Packet::MaxBytes, REG_LORA_PACKET_MAXLEN); // [bytes] enough ?
WriteByte(0x00, REG_LORA_RX_ADDR);
setChannel(0); // operating channel
WriteWord(0x0000, REG_DIOMAPPING1); // 001122334455___D signals: 00=DIO0 11=DIO1 22=DIO2 33=DIO3 44=DIO4 55=DIO5 D=MapPreambleDetect
// DIO0: 00=RxDone, 01=TxDone, 10=CadDone
return 0; }
int SendPacketFNT(const uint8_t *Data, uint8_t Len)
@ -529,17 +536,37 @@ class RFM_TRX
WriteMode(RF_OPMODE_LORA_TX); // enter transmission mode
return 0; } // afterwards just wait for TX mode to stop
int ReceivePacketFNT(FANET_RxPacket &Packet)
{ uint8_t Stat = ReadByte(REG_LORA_MODEM_STATUS); // coding rate in three top bits
Packet.SNR = ReadByte(REG_LORA_PACKET_SNR); // [0.25dB] read SNR
Packet.RSSI = -157+ReadByte(REG_LORA_PACKET_RSSI); // [dBm] read RSSI
int32_t FreqOfs = ReadFreq(REG_LORA_FREQ_ERR_MSB); // (FreqOfs*1718+0x8000)>>16 [10Hz]
Packet.FreqOfs = (FreqOfs*1718+0x8000)>>16; // [10Hz]
if(FreqOfs&0x00080000) FreqOfs|=0xFFF00000; // extend the sign bit
else FreqOfs&=0x000FFFFF;
Packet.BitErr=0;
Packet.CodeErr=0;
int Len=ReceivePacketFNT(Packet.Byte, Packet.MaxBytes);
// printf("ReceivePacketFNT() => %d %02X %3.1fdB %+ddBm 0x%08X=%+6.3fkHz, %02X%02X%02X%02X\n",
// Packet.Len, Stat, 0.25*Packet.SNR, Packet.RSSI, FreqOfs, 0.5*0x1000000/32e9*FreqOfs,
// Packet.Byte[0], Packet.Byte[1], Packet.Byte[2], Packet.Byte[3]);
Packet.Len=Len; return Len; }
int ReceivePacketFNT(uint8_t *Data, uint8_t MaxLen)
{ uint8_t Len=ReadByte(REG_LORA_PACKET_BYTES); // packet length
uint8_t Ptr=ReadByte(REG_LORA_PACKET_ADDR); // packet address in FIFO
WriteByte(Ptr, REG_LORA_FIFO_ADDR); // ask to read FIFO from this address
uint8_t Stat = ReadByte(REG_LORA_MODEM_STATUS); //
int8_t SNR = ReadByte(REG_LORA_PACKET_SNR); // [0.25dB] read SNR
int8_t RSSI = ReadByte(REG_LORA_PACKET_RSSI); // [dBm] read RSSI
// uint8_t Stat = ReadByte(REG_LORA_MODEM_STATUS); //
// int8_t SNR = ReadByte(REG_LORA_PACKET_SNR); // [0.25dB] read SNR
// int8_t RSSI = ReadByte(REG_LORA_PACKET_RSSI); // [dBm] read RSSI
// int32_t FreqOfs = ReadFreq(REG_LORA_FREQ_ERR_MSB); // (FreqOfs*1718+0x8000)>>16 [10Hz]
// if(FreqOfs&0x00080000) FreqOfs|=0xFFF00000; // extend the sign bit
// else FreqOfs&=0x000FFFFF;
uint8_t *ReadData = ReadFIFO(Len); // read data from FIFO
// printf("ReceivePacketFNT( , %d) => %d [%02X] %02X %3.1fdB %+ddBm %02X%02X%02X%02X\n",
// MaxLen, Len, Ptr, Stat, 0.25*SNR, -157+RSSI,
// ReadData[0], ReadData[1], ReadData[2], ReadData[3]);
memcpy(Data, ReadData, Len);
// printf("ReceivePacketFNT( , %d) => %d [%02X] %02X %3.1fdB %+ddBm 0x%08X=%+6.3fkHz, %02X%02X%02X%02X\n",
// MaxLen, Len, Ptr, Stat, 0.25*SNR, -157+RSSI, FreqOfs, 0.5*0x1000000/32e9*FreqOfs,
// ReadData[0], ReadData[1], ReadData[2], ReadData[3]);
return Len; }
int ConfigureOGN(int16_t Channel, const uint8_t *Sync, bool PW=0)