From e23f222c55269c279b9d6e5802782919d37da4cd Mon Sep 17 00:00:00 2001 From: Pawel Jalocha Date: Sat, 2 May 2020 20:37:15 +0100 Subject: [PATCH] FANET: QNE encoding and packet reception --- main/fanet.h | 62 ++++++++++++++++++++++++++++++++++++++++++++-------- main/ogn.h | 14 +++++++----- main/rf.cpp | 2 +- main/rfm.h | 49 +++++++++++++++++++++++++++++++---------- 4 files changed, 100 insertions(+), 27 deletions(-) diff --git a/main/fanet.h b/main/fanet.h index 4525f65..411c361 100644 --- a/main/fanet.h +++ b/main/fanet.h @@ -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)>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); diff --git a/main/rf.cpp b/main/rf.cpp index 5a13ee0..5fd5c90 100644 --- a/main/rf.cpp +++ b/main/rf.cpp @@ -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 diff --git a/main/rfm.h b/main/rfm.h index 48743bb..988eebb 100644 --- a/main/rfm.h +++ b/main/rfm.h @@ -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)