diff --git a/main/config.h b/main/config.h index beba1ff..43c2ad5 100644 --- a/main/config.h +++ b/main/config.h @@ -1,24 +1,24 @@ -#define DEFAULT_AcftType 8 // [0..15] default aircraft-type: Powered Aircraft +#define DEFAULT_AcftType 1 // [0..15] default aircraft-type: glider #define DEFAULT_GeoidSepar 40 // [m] #define DEFAULT_CONbaud 115200 #define DEFAULT_PPSdelay 100 #define DEFAULT_FreqPlan 0 -#define DEFAULT_DispPage 3 // Fab501 Page to Display After Boot or Reset -#define WIFI_ADDRESS_IP1 192 // 192.168.1.1 for IP Address -#define WIFI_ADDRESS_IP2 168 -#define WIFI_ADDRESS_IP3 1 -#define WIFI_ADDRESS_IP4 1 -#define WIFI_ADDRESS_GW1 0 // 0.0.0.0 for Gateway -#define WIFI_ADDRESS_GW2 0 -#define WIFI_ADDRESS_GW3 0 -#define WIFI_ADDRESS_GW4 0 -#define WIFI_ADDRESS_MK1 255 // 255.255.255.0 for Mask -#define WIFI_ADDRESS_MK2 255 -#define WIFI_ADDRESS_MK3 255 -#define WIFI_ADDRESS_MK4 0 +#define DEFAULT_DispPage 3 // Fab501 Page to Display After Boot or Reset + +#define WIFI_ADDRESS_IP1 192 // 192.168.1.1 for IP Address +#define WIFI_ADDRESS_IP2 168 +#define WIFI_ADDRESS_IP3 1 +#define WIFI_ADDRESS_IP4 1 +#define WIFI_ADDRESS_GW1 0 // 0.0.0.0 for Gateway +#define WIFI_ADDRESS_GW2 0 +#define WIFI_ADDRESS_GW3 0 +#define WIFI_ADDRESS_GW4 0 +#define WIFI_ADDRESS_MK1 255 // 255.255.255.0 for Mask +#define WIFI_ADDRESS_MK2 255 +#define WIFI_ADDRESS_MK3 255 +#define WIFI_ADDRESS_MK4 0 // #define WITH_HELTEC // HELTEC module: PCB LED on GPI025 -// #define WITH_HELTEC_V2 // HELTEC module v2 // #define WITH_TTGO // TTGO module: PCB LED on GPIO2, GPIO25 free to use as DAC2 output // #define WITH_TBEAM // T-Beam module #define WITH_TBEAM_V10 // T-Beam module @@ -27,15 +27,13 @@ // #define WITH_ILI9341 // 320x240 M5stack // #define WITH_ST7789 // IPS 240x240 ST7789 -// #define WITH_TFT_LCD // TFT LCD +// #define WITH_TFT_LCD // TFT LCD (incomplete) // #define WITH_OLED // OLED display on the I2C: some TTGO modules are without OLED display // #define WITH_OLED2 // 2nd OLED display, I2C address next higher -#define WITH_U8G2_OLED // I2C OLED through the U8g2 library -#define WITH_U8G2_SH1106 // correct controller for the bigger OLED -// #define WITH_U8G2_FLIP // flip the OLED screen (rotate by 180deg) +// #define WITH_U8G2_OLED // I2C OLED through the U8g2 library #define WITH_RFM95 // RF chip selection: both HELTEC and TTGO use sx1276 which is same as RFM95 -//#define WITH_SX1262 // SX1262 Support +// #define WITH_SX1262 // RF chip selection: both HELTEC and TTGO use sx1276 which is same as RFM95 // #define WITH_SLEEP // with software sleep mode controlled by the long-press on the button @@ -54,48 +52,47 @@ // #define WITH_GPS_SRF // #define WITH_MAVLINK -#define WITH_GPS_UBX_PASS // to pass directly UBX packets to/from GPS -#define WITH_GPS_NMEA_PASS // to pass directly NMEA to/from GPS +// #define WITH_GPS_UBX_PASS // to pass directly UBX packets to/from GPS +// #define WITH_GPS_NMEA_PASS // to pass directly NMEA to/from GPS // #define WITH_BMP180 // BMP180 pressure sensor // #define WITH_BMP280 // BMP280 pressure sensor #define WITH_BME280 // BMP280 with humidity (but still works with BMP280) // #define WITH_MS5607 // MS5607 pressure sensor -// #define WITH_MS5611 // MS5611 pressure sensor - -// #define WITH_BMX055 // BMX055 magnetic and IMU sensor - -#define WITH_LORAWAN // LoRaWAN connectivity -#define WITH_FANET // FANET transmission and reception -#define WITH_PAW // Add PAW transmission #define WITH_PFLAA // PFLAU and PFLAA for compatibility with XCsoar and LK8000 // #define WITH_POGNT -// #define WITH_GDL90 -// #define WITH_PGAV5 #define WITH_LOOKOUT -#define WITH_SKYDEMON //Adapt NMEA Output for SKYDEMON - #define WITH_CONFIG // interpret the console input: $POGNS to change parameters -#define WITH_BEEPER // with digital buzzer +// #define WITH_BEEPER // with digital buzzer // #define WITH_SOUND // with analog sound produced by DAC on pin 25 // #define WITH_KNOB // #define WITH_VARIO -// #define WITH_SD // use the SD card in SPI mode and FAT file system -#define WITH_SPIFFS // use SPIFFS file system in Flash -// #define WITH_SPIFFS_FAT -#define WITH_LOG // log own positions and other received to SPIFFS -// #define WITH_SDLOG // log own position and other data to uSD card +#define WITH_ADSL +// #define WITH_PAW +// #define WITH_FANET +#define WITH_LORAWAN -//#define WITH_STRATUX -#define WITH_BT_SPP // Bluetooth serial port for smartphone/tablet link -#define WITH_WIFI // attempt to connect to the wifi router for uploading the log files -#define WITH_AP // Open Access Point MOde -#define WITH_HTTP // Open Web Interface +#define WITH_AP // WiFi Access Point: can work together with BT_SPP +// #define WITH_AP_BUTTON // only starts when button pressed at sartup +// #define WITH_BT_SPP // Bluetooth serial port for smartphone/tablet link: can work together with WiFi Access point +// #define WITH_STRATUX // beta-code: connect to Stratux WiFi and serve as GPS and OGN transmitter/receiver +// #define WITH_APRS // alpha-code: attempt to connect to the wifi router for uploading the log files to APRS + +#define WITH_HTTP // HTTP server, works with AP dna should work with Stratux as well + +// #define WITH_SD // use the SD card in SPI mode and FAT file system +#define WITH_SPIFFS_FAT +#define WITH_SPIFFS // use SPIFFS file system in Flash +#define WITH_LOG // log own positions and other received to SPIFFS and possibly to uSD // #define WITH_ENCRYPT // Encrypt (optionally) the position +#if defined(WITH_STRATUX) || defined(WITH_APRS) || defined(WITH_AP) +#define WITH_WIFI +#endif + diff --git a/main/disp_oled.cpp b/main/disp_oled.cpp index 7704d69..12a0e0a 100644 --- a/main/disp_oled.cpp +++ b/main/disp_oled.cpp @@ -351,7 +351,7 @@ void OLED_DrawRelay(u8g2_t *OLED, GPS_Position *GPS) u8g2_DrawStr(OLED, 0, 24, Line); uint8_t LineIdx=1; for( uint8_t Idx=0; Idx *Packet = RelayQueue.Packet+Idx; if(Packet->Rank==0) continue; + { OGN_RxPacket *Packet = RelayQueue.Packet+Idx; if(!Packet->Alloc) continue; uint8_t Len=0; Line[Len++]='0'+Packet->Packet.Header.AddrType; Line[Len++]=':'; diff --git a/main/fanet.h b/main/fanet.h index c79a8f2..e334839 100644 --- a/main/fanet.h +++ b/main/fanet.h @@ -193,6 +193,20 @@ class FANET_Packet 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()); @@ -263,7 +277,7 @@ 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 + 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 @@ -314,11 +328,12 @@ class FANET_RxPacket: public FANET_Packet 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<=300) Slot--; return Slot; } + uint32_t SlotTime(void) const { uint32_t Slot=sTime; if(msTime<100) Slot--; return Slot; } void Print(char *Name=0) const { char HHMMSS[8]; @@ -326,7 +341,72 @@ class FANET_RxPacket: public FANET_Packet 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 WriteStxJSON(char *JSON) const + 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]); @@ -335,11 +415,12 @@ class FANET_RxPacket: public FANET_Packet JSON[Len++]='\"'; JSON[Len++]=','; Len+=Format_String(JSON+Len, "\"addr_type\":"); - JSON[Len++] = HexDigit(getAddrType()); + 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=sTime; if(msTime<300) Time--; + 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; @@ -439,8 +520,9 @@ class FANET_RxPacket: public FANET_Packet Len+=Format_String(JSON+Len, ",\"on_ground\":1"); } return Len; } - int WriteAPRS(char *Out) + 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"); @@ -510,7 +592,6 @@ 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 = 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 @@ -566,7 +647,6 @@ 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 = getAddrType(); // uint8_t AcftType = 15; // uint32_t ID = (AcftType<<2) | AddrType; // acft-type and addr-type if(!Track) ID|=0x80; // stealth flag @@ -589,9 +669,11 @@ class FANET_RxPacket: public FANET_Packet 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)/4, 2, 1, 1); + 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); @@ -605,6 +687,8 @@ class FANET_RxPacket: public FANET_Packet // ========================================================================================= +#ifndef ARDUINO + class FANET_Name { public: static const int MaxSize = 32; @@ -622,6 +706,8 @@ class FANET_Name } ; +#include + class FANET_NameList { public: std::map List; @@ -636,6 +722,7 @@ class FANET_NameList return 1; } } ; +#endif // =============================================================================================== diff --git a/main/ogn.h b/main/ogn.h index cd7579a..2991c5f 100644 --- a/main/ogn.h +++ b/main/ogn.h @@ -26,6 +26,7 @@ #include "ogn1.h" // OGN v1 #include "ogn2.h" // OGN v2 +#include "adsl.h" // ADS-L #include "fanet.h" #include "gdl90.h" @@ -68,7 +69,7 @@ template static const int Words = 7; static const int Bytes = 26; - OGNx_Packet Packet; // OGN packet + OGNx_Packet Packet; // OGN packet uint32_t FEC[2]; // Gallager code: 48 check bits for 160 user bits @@ -180,11 +181,12 @@ template uint32_t FEC[2]; // Gallager code: 48 check bits for 160 user bits union - { uint8_t State; // + { uint8_t State; // state bits and small values struct { // bool Saved :1; // has been already saved in internal storage // bool Ready :1; // is ready for transmission // bool Sent :1; // has already been transmitted out + bool Alloc :1; // allocated in a queue or list bool Correct :1; // correctly received or corrected by FEC uint8_t RxErr:4; // number of bit errors corrected upon reception uint8_t Warn :2; // LookOut warning level @@ -195,8 +197,8 @@ template uint8_t RxRSSI; // [-0.5dBm] uint8_t Rank; // rank: low altitude and weak signal => high rank - int16_t LatDist; // [m] - int16_t LonDist; // [m] + int16_t LatDist; // [m] distance along the latitude + int16_t LonDist; // [m] distance along the longitude // int16_t AltDist; // [m] public: @@ -206,16 +208,7 @@ template uint8_t *Byte(void) const { return (uint8_t *)&Packet.HeaderWord; } // packet as bytes uint32_t *Word(void) const { return (uint32_t *)&Packet.HeaderWord; } // packet as words -/* - int Print(char *Out) const - { int Len = sprintf(Out, "%c%X:%c:%06lX R%c%c", - Packet.Position.Stealth ?'s':' ', (int)Packet.Position.AcftType, '0'+Packet.Header.AddrType, (long int)Packet.Header.Address, - '0'+Packet.Header.Relay, Packet.Header.Emergency?'E':' '); - Len+= sprintf(Out+Len, " %d/%dD/%4.1f", (int)Position.FixQuality, (int)Position.FixMode+2, 0.1*(10+DecodeDOP()) ); - if(Position.Time<60) Len+=sprintf(Out+Len, " %02ds:", (int)Position.Time); - else Len+=sprintf(Out+Len, " ---:");; - return Len; } -*/ + void recvBytes(const uint8_t *SrcPacket) { memcpy(Byte(), SrcPacket, Bytes); } // load data bytes e.g. from a demodulator uint8_t calcErrorPattern(uint8_t *ErrPatt, const uint8_t *OtherPacket) const @@ -252,8 +245,8 @@ template if(RxRSSI>128) // [-0.5dB] weaker signal => higher rank Rank += (RxRSSI-128)>>2; // 1point/2dB less signal if(Packet.Header.Encrypted) return; // for exncrypted packets we only take signal strength - RxAltitude -= Packet.DecodeAltitude(); // [m] lower altitude => higher rank - if(RxAltitude>0) + RxAltitude -= Packet.DecodeAltitude(); // [m] receiver altitude - target altitude + if(RxAltitude>0) // Rank += RxAltitude>>6; // 1points/64m of altitude below int16_t ClimbRate = Packet.DecodeClimbRate(); // [0.1m/s] higher sink rate => higher rank if(ClimbRate<0) @@ -599,19 +592,20 @@ template uint8_t getNew(void) // get (index of) a free or lowest rank packet { Sum-=Packet[LowIdx].Rank; Packet[LowIdx].Rank=0; Low=0; return LowIdx; } // remove old packet from the rank sum - uint8_t size(void) + uint8_t size(void) // count all slots with Alloc flag set { uint8_t Count=0; for(uint8_t Idx=0; Idx *addNew(uint8_t NewIdx) // add the new packet to the queue + OGN_RxPacket *addNew(uint8_t NewIdx) // add the new packet to the queue { OGN_RxPacket *Prev = 0; + Packet[NewIdx].Alloc=1; // mark this clot as allocated uint32_t AddressAndType = Packet[NewIdx].Packet.getAddressAndType(); // get ID of this packet: ID is address-type and address (2+24 = 26 bits) for(uint8_t Idx=0; Idx uint16_t RankIdx = Rand%Sum; uint8_t Idx; uint16_t RankSum=0; for(Idx=0; IdxRankIdx) return Idx; } return Rand%Size; } void reCalc(void) // find the lowest rank and calc. the sum of all ranks { Sum=Low=Packet[0].Rank; LowIdx=0; // take minimum at the first slot for(uint8_t Idx=1; Idx void cleanTime(uint8_t Time) // clean up slots of given Time { for(int Idx=0; Idx=60) clean(Idx); } } - void clean(uint8_t Idx) // clean given slot - { Sum-=Packet[Idx].Rank; Packet[Idx].Rank=0; Low=0; LowIdx=Idx; } + void clean(uint8_t Idx) // clean given slot, remove it from the sum + { Sum-=Packet[Idx].Rank; Packet[Idx].Rank=0; Packet[Idx].Alloc=0; Low=0; LowIdx=Idx; } void decrRank(uint8_t Idx, uint8_t Decr=1) // decrement rank of given slot { uint8_t Rank=Packet[Idx].Rank; if(Rank==0) return; // if zero already: do nothing @@ -660,12 +656,13 @@ template uint8_t Print(char *Out) { uint8_t Len=0; for(uint8_t Idx=0; Idx [cordic] int32_t Lon = getCordicLongitude(); // Longitude: [0.0001/60deg] => [cordic] @@ -1382,14 +1383,25 @@ class GPS_Position: public GPS_Time Report.setClimbRate(6*MetersToFeet(ClimbRate)); } - int EncodeIAS(OGN1_Packet &Packet) - { if(!hasIAS) return 0; - Packet.Position.Time = 61; // "current" packet with airspeed - Packet.EncodeSpeed(AirSpeed); - return 1; } + void Encode(ADSL_Packet &Packet) const + { Packet.setAlt((Altitude+GeoidSeparation+5)/10); + Packet.setLat(Packet.OGNtoFNT(Latitude)); + Packet.setLon(Packet.OGNtoFNT(Longitude)); + Packet.TimeStamp = (Sec*4+mSec/250)&0x3F; + Packet.setSpeed(((uint32_t)Speed*4+5)/10); + Packet.setClimb(((int32_t)ClimbRate*8+5)/10); + // if(hasClimb) Packet.setClimb(((int32_t)ClimbRate*8+5)/10); + // else Packet.clrClimb(); + Packet.setTrack(((uint32_t)Heading*32+112)/225); + Packet.Integrity[0]=0; Packet.Integrity[1]=0; + if((FixQuality>0)&&(FixMode>=2)) + { Packet.setHorPrec((HDOP*2+5)/10); + Packet.setVerPrec((VDOP*3+5)/10); } + } - template - void Encode(OGNx_Packet &Packet) const + // template + // void Encode(OGNx_Packet &Packet) const + void Encode(OGN1_Packet &Packet) const { Packet.Position.FixQuality = FixQuality<3 ? FixQuality:3; // if((FixQuality>0)&&(FixMode>=2)) Packet.Position.FixMode = FixMode-2; // else Packet.Position.FixMode = 0; diff --git a/main/ogn1.h b/main/ogn1.h index a4d2772..a180083 100644 --- a/main/ogn1.h +++ b/main/ogn1.h @@ -191,8 +191,46 @@ class OGN1_Packet // Packet structure for the OGN tracker { printf(" %02X", Byte()[Idx]); } printf("\n"); } - uint8_t WriteStatus(char *Out) - { uint8_t Len=0; + int Print(char *Out) const + { int Len=0; + Out[Len++]='0'+Header.AddrType; Out[Len++]=':'; + uint32_t Addr = Header.Address; + Len+=Format_Hex(Out+Len, (uint8_t)(Addr>>16)); + Len+=Format_Hex(Out+Len, (uint16_t)Addr); + Out[Len++]=' '; + Out[Len++]='R'; + Out[Len++]='0'+Header.Relay; + if(!Header.NonPos) return Len+PrintPosition(Out+Len); + if(isStatus()) return Len+PrintDeviceStatus(Out+Len); + if(isInfo ()) return Len+PrintDeviceInfo(Out+Len); + Out[Len]=0; return Len; } + + int PrintPosition(char *Out) const + { int Len=0; + Out[Len++]=' '; + // Out[Len++]=HexDigit(Position.AcftType); Out[Len++]=':'; + // Len+=Format_SignDec(Out+Len, -(int16_t)RxRSSI/2); Out[Len++]='d'; Out[Len++]='B'; Out[Len++]='m'; + // Out[Len++]=' '; + Len+=Format_UnsDec(Out+Len, (uint16_t)Position.Time, 2); + Len+=Format_String(Out+Len, "s ["); + Len+=Format_SignDec(Out+Len, DecodeLatitude()/6, 7, 5); + Out[Len++]=','; + Len+=Format_SignDec(Out+Len, DecodeLongitude()/6, 8, 5); + Len+=Format_String(Out+Len, "]deg "); + // Len+=Format_Latitude(Out+Len, DecodeLatitude()); + // Out[Len++]=' '; + // Len+=Format_Longitude(Out+Len, DecodeLongitude()); + // Out[Len++]=' '; + Len+=Format_UnsDec(Out+Len, (uint32_t)DecodeAltitude()); Out[Len++]='m'; + Out[Len++]=' '; + Len+=Format_UnsDec(Out+Len, DecodeSpeed(), 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s'; + Out[Len++]=' '; + Len+=Format_SignDec(Out+Len, DecodeClimbRate(), 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s'; + Out[Len]=0; + return Len; } + + int PrintDeviceStatus(char *Out) const + { int Len=0; Out[Len++]=' '; Out[Len++]='h'; Len+=Format_Hex(Out+Len, (uint8_t)Status.Hardware); Out[Len++]=' '; Out[Len++]='v'; Len+=Format_Hex(Out+Len, (uint8_t)Status.Firmware); Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Status.Satellites); Out[Len++]='s'; Out[Len++]='a'; Out[Len++]='t'; @@ -210,15 +248,15 @@ class OGN1_Packet // Packet structure for the OGN tracker Out[Len++]='/'; Out[Len++]='-'; Len+=Format_UnsDec(Out+Len, 5*Status.RadioNoise, 2, 1); Out[Len++]='d'; Out[Len++]='B'; Out[Len++]='m'; Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, (1<MaxRadius) return 0; return Radius; } int16_t calcTurnRadius(int16_t MaxRadius=0x7FFF) { return calcTurnRadius(DecodeSpeed(), DecodeTurnRate(), MaxRadius); } - +/* uint8_t Print(char *Out) const { uint8_t Len=0; Out[Len++]=HexDigit(Position.AcftType); Out[Len++]=':'; @@ -716,7 +753,7 @@ class OGN1_Packet // Packet structure for the OGN tracker Len+=Format_SignDec(Out+Len, DecodeClimbRate(), 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s'; Out[Len++]='\n'; Out[Len]=0; return Len; } - +*/ // OGN1_Packet() { Clear(); } void Clear(void) { HeaderWord=0; Data[0]=0; Data[1]=0; Data[2]=0; Data[3]=0; } diff --git a/main/ognconv.cpp b/main/ognconv.cpp index 5393e8a..d76ba03 100644 --- a/main/ognconv.cpp +++ b/main/ognconv.cpp @@ -4,6 +4,8 @@ #include "format.h" #include "ognconv.h" +// #pragma GCC optimize("O2") // XXTEA does not run at default -0s optimization - reason not understood + // ============================================================================================== int32_t FeetToMeters(int32_t Altitude) { return (Altitude*312+512)>>10; } // [feet] => [m] @@ -159,8 +161,7 @@ void TEA_Encrypt (uint32_t* Data, const uint32_t *Key, int Loops) { sum += delta; v0 += ((v1<<4) + k0) ^ (v1 + sum) ^ ((v1>>5) + k1); v1 += ((v0<<4) + k2) ^ (v0 + sum) ^ ((v0>>5) + k3); } // end cycle - Data[0]=v0; Data[1]=v1; -} + Data[0]=v0; Data[1]=v1; } void TEA_Decrypt (uint32_t* Data, const uint32_t *Key, int Loops) { uint32_t v0=Data[0], v1=Data[1]; // set up @@ -170,8 +171,7 @@ void TEA_Decrypt (uint32_t* Data, const uint32_t *Key, int Loops) { v1 -= ((v0<<4) + k2) ^ (v0 + sum) ^ ((v0>>5) + k3); v0 -= ((v1<<4) + k0) ^ (v1 + sum) ^ ((v1>>5) + k1); sum -= delta; } // end cycle - Data[0]=v0; Data[1]=v1; -} + Data[0]=v0; Data[1]=v1; } void TEA_Encrypt_Key0 (uint32_t* Data, int Loops) { uint32_t v0=Data[0], v1=Data[1]; // set up @@ -180,8 +180,7 @@ void TEA_Encrypt_Key0 (uint32_t* Data, int Loops) { sum += delta; v0 += (v1<<4) ^ (v1 + sum) ^ (v1>>5); v1 += (v0<<4) ^ (v0 + sum) ^ (v0>>5); } // end cycle - Data[0]=v0; Data[1]=v1; -} + Data[0]=v0; Data[1]=v1; } void TEA_Decrypt_Key0 (uint32_t* Data, int Loops) { uint32_t v0=Data[0], v1=Data[1]; // set up @@ -190,23 +189,22 @@ void TEA_Decrypt_Key0 (uint32_t* Data, int Loops) { v1 -= (v0<<4) ^ (v0 + sum) ^ (v0>>5); v0 -= (v1<<4) ^ (v1 + sum) ^ (v1>>5); sum -= delta; } // end cycle - Data[0]=v0; Data[1]=v1; -} + Data[0]=v0; Data[1]=v1; } // ============================================================================================== // XXTEA encryption/decryption -static uint32_t XXTEA_MX(uint8_t E, uint32_t Y, uint32_t Z, uint8_t P, uint32_t Sum, const uint32_t Key[4]) -{ return ((((Z>>5) ^ (Y<<2)) + ((Y>>3) ^ (Z<<4))) ^ ((Sum^Y) + (Key[(P&3)^E] ^ Z))); } +static uint32_t XXTEA_MX(uint32_t E, uint32_t Y, uint32_t Z, uint32_t P, uint32_t Sum, const uint32_t Key[4]) +{ return (((Z>>5) ^ (Y<<2)) + ((Y>>3) ^ (Z<<4))) ^ ((Sum^Y) + (Key[(P&3)^E] ^ Z)); } -void XXTEA_Encrypt(uint32_t *Data, uint8_t Words, const uint32_t Key[4], uint8_t Loops) +void XXTEA_Encrypt(uint32_t *Data, uint32_t Words, const uint32_t Key[4], uint32_t Loops) { const uint32_t Delta = 0x9e3779b9; uint32_t Sum = 0; uint32_t Z = Data[Words-1]; uint32_t Y; for( ; Loops; Loops--) { Sum += Delta; - uint8_t E = (Sum>>2)&3; - for (uint8_t P=0; P<(Words-1); P++) + uint32_t E = (Sum>>2)&3; + for (uint32_t P=0; P<(Words-1); P++) { Y = Data[P+1]; Z = Data[P] += XXTEA_MX(E, Y, Z, P, Sum, Key); } Y = Data[0]; @@ -214,13 +212,13 @@ void XXTEA_Encrypt(uint32_t *Data, uint8_t Words, const uint32_t Key[4], uint8_t } } -void XXTEA_Decrypt(uint32_t *Data, uint8_t Words, const uint32_t Key[4], uint8_t Loops) +void XXTEA_Decrypt(uint32_t *Data, uint32_t Words, const uint32_t Key[4], uint32_t Loops) { const uint32_t Delta = 0x9e3779b9; uint32_t Sum = Loops*Delta; uint32_t Y = Data[0]; uint32_t Z; for( ; Loops; Loops--) - { uint8_t E = (Sum>>2)&3; - for (uint8_t P=Words-1; P; P--) + { uint32_t E = (Sum>>2)&3; + for (uint32_t P=Words-1; P; P--) { Z = Data[P-1]; Y = Data[P] -= XXTEA_MX(E, Y, Z, P, Sum, Key); } Z = Data[Words-1]; @@ -229,6 +227,35 @@ void XXTEA_Decrypt(uint32_t *Data, uint8_t Words, const uint32_t Key[4], uint8_t } } +static uint32_t XXTEA_MX_KEY0(uint32_t Y, uint32_t Z, uint32_t Sum) +{ return (((Z>>5) ^ (Y<<2)) + ((Y>>3) ^ (Z<<4))) ^ ((Sum^Y) + Z); } + +void XXTEA_Encrypt_Key0(uint32_t *Data, uint32_t Words, uint32_t Loops) +{ const uint32_t Delta = 0x9e3779b9; + uint32_t Sum = 0; + uint32_t Z = Data[Words-1]; uint32_t Y; + for( ; Loops; Loops--) + { Sum += Delta; + for (uint32_t P=0; P<(Words-1); P++) + { Y = Data[P+1]; + Z = Data[P] += XXTEA_MX_KEY0(Y, Z, Sum); } + Y = Data[0]; + Z = Data[Words-1] += XXTEA_MX_KEY0(Y, Z, Sum); } +} + +void XXTEA_Decrypt_Key0(uint32_t *Data, uint32_t Words, uint32_t Loops) +{ const uint32_t Delta = 0x9e3779b9; + uint32_t Sum = Loops*Delta; + uint32_t Y = Data[0]; uint32_t Z; + for( ; Loops; Loops--) + { for (uint32_t P=Words-1; P; P--) + { Z = Data[P-1]; + Y = Data[P] -= XXTEA_MX_KEY0(Y, Z, Sum); } + Z = Data[Words-1]; + Y = Data[0] -= XXTEA_MX_KEY0(Y, Z, Sum); + Sum -= Delta; } +} + // ============================================================================================== void XorShift32(uint32_t &Seed) // simple random number generator @@ -236,7 +263,7 @@ void XorShift32(uint32_t &Seed) // simple random number generator Seed ^= Seed >> 17; Seed ^= Seed << 5; } -void xorshift64(uint64_t &Seed) +void XorShift64(uint64_t &Seed) { Seed ^= Seed >> 12; Seed ^= Seed << 25; Seed ^= Seed >> 27; } diff --git a/main/ognconv.h b/main/ognconv.h index 5854e96..f1fda3c 100644 --- a/main/ognconv.h +++ b/main/ognconv.h @@ -73,11 +73,16 @@ void TEA_Decrypt (uint32_t* Data, const uint32_t *Key, int Loops); void TEA_Encrypt_Key0 (uint32_t* Data, int Loops); void TEA_Decrypt_Key0 (uint32_t* Data, int Loops); -void XXTEA_Encrypt(uint32_t *Data, uint8_t Words, const uint32_t Key[4], uint8_t Loops); -void XXTEA_Decrypt(uint32_t *Data, uint8_t Words, const uint32_t Key[4], uint8_t Loops); +void XXTEA_Encrypt(uint32_t *Data, uint32_t Words, const uint32_t Key[4], uint32_t Loops); +void XXTEA_Decrypt(uint32_t *Data, uint32_t Words, const uint32_t Key[4], uint32_t Loops); + +void XXTEA_Encrypt_Key0(uint32_t *Data, uint32_t Words, uint32_t Loops); +void XXTEA_Decrypt_Key0(uint32_t *Data, uint32_t Words, uint32_t Loops); void XorShift32(uint32_t &Seed); // simple random number generator -void xorshift64(uint64_t &Seed); +void XorShift64(uint64_t &Seed); +uint64_t inline XorShift64star(uint64_t &Seed) +{ XorShift64(Seed); return Seed*UINT64_C(0x2545f4914f6cdd1d); } uint8_t EncodeAscii85( char *Ascii, uint32_t Word ); // Encode 32-bit Word into 5-char Ascii-85 string uint8_t DecodeAscii85(uint32_t &Word, const char *Ascii); // Decode 5-char Ascii-85 to 32-bit Word diff --git a/main/proc.cpp b/main/proc.cpp index dbe9fd2..cc2c5af 100644 --- a/main/proc.cpp +++ b/main/proc.cpp @@ -357,7 +357,9 @@ static void ProcessRxPacket(OGN_RxPacket *RxPacket, uint8_t RxPacket return; } bool DistOK = RxPacket->Packet.calcDistanceVector(LatDist, LonDist, GPS_Latitude, GPS_Longitude, GPS_LatCosine)>=0; if(DistOK) - { RxPacket->calcRelayRank(GPS_Altitude/10); // calculate the relay-rank (priority for relay) + { RxPacket->LatDist=LatDist; + RxPacket->LonDist=LonDist; + RxPacket->calcRelayRank(GPS_Altitude/10); // calculate the relay-rank (priority for relay) OGN_RxPacket *PrevRxPacket = RelayQueue.addNew(RxPacketIdx); // add to the relay queue and get the previous packet of same ID #ifdef WITH_POGNT { uint8_t Len=RxPacket->WritePOGNT(Line); // print on the console as $POGNT @@ -642,6 +644,18 @@ void vTaskPROC(void* pvParameters) if(TxBackOff) TxBackOff--; else { RF_TxFIFO.Write(); // complete the write into the TxFIFO +#ifdef WITH_ADSL + ADSL_Packet *Packet = ADSL_TxFIFO.getWrite(); + Packet->Init(); + Packet->setAddress (Parameters.Address); + Packet->setAddrType(Parameters.AddrType); + Packet->setRelay(0); + Packet->setAcftType(Parameters.AcftType); + Position->Encode(*Packet); + Packet->Scramble(); // this call hangs when -Os is used to compile + Packet->setCRC(); + ADSL_TxFIFO.Write(); +#endif TxBackOff = 0; if(AverSpeed<10 && Parameters.AcftType!=3 && Parameters.AcftType!=0xD) TxBackOff += 3+(RX_Random&0x1); if(TX_Credit<=0) TxBackOff+=1; } @@ -651,9 +665,9 @@ void vTaskPROC(void* pvParameters) if(FNTbackOff) FNTbackOff--; // if( (SlotTime&0x07)==(RX_Random&0x07) ) // every 8sec else - { FANET_Packet *FNTpkt = FNT_TxFIFO.getWrite(); - FNTpkt->setAddress(Parameters.Address); - Position->EncodeAirPos(*FNTpkt, Parameters.AcftType, !Parameters.Stealth); + { FANET_Packet *Packet = FNT_TxFIFO.getWrite(); + Packet->setAddress(Parameters.Address); + Position->EncodeAirPos(*Packet, Parameters.AcftType, !Parameters.Stealth); XorShift32(RX_Random); FNT_TxFIFO.Write(); FNTbackOff = 8+(RX_Random&0x1); } // every 9 or 10sec diff --git a/main/rf.cpp b/main/rf.cpp index 9e4e8ca..bf13700 100644 --- a/main/rf.cpp +++ b/main/rf.cpp @@ -11,11 +11,15 @@ // =============================================================================================== // OGNv1 SYNC: 0x0AF3656C encoded in Manchester -static const uint8_t OGN1_SYNC[8] = { 0xAA, 0x66, 0x55, 0xA5, 0x96, 0x99, 0x96, 0x5A }; +static const uint8_t OGN1_SYNC[10] = { 0xAA, 0x66, 0x55, 0xA5, 0x96, 0x99, 0x96, 0x5A, 0x00, 0x00 }; // OGNv2 SYNC: 0xF56D3738 encoded in Machester -static const uint8_t OGN2_SYNC[8] = { 0x55, 0x99, 0x96, 0x59, 0xA5, 0x95, 0xA5, 0x6A }; +static const uint8_t OGN2_SYNC[10] = { 0x55, 0x99, 0x96, 0x59, 0xA5, 0x95, 0xA5, 0x6A, 0x00, 0x00 }; -static const uint8_t PAW_SYNC [8] = { 0xB4, 0x2B, 0x00, 0x00, 0x00, 0x00, 0x18, 0x71 }; +// ADS-L SYNC: 0xF5724B18 encoded in Manchester (fixed packet length 0x18 is included) +static const uint8_t ADSL_SYNC[10] = { 0x55, 0x99, 0x95, 0xA6, 0x9A, 0x65, 0xA9, 0x6A, 0x00, 0x00 }; + +// PilotAWare SYNC including the packet size and CRC seed +static const uint8_t PAW_SYNC [10] = { 0xB4, 0x2B, 0x00, 0x00, 0x00, 0x00, 0x18, 0x71, 0x00, 0x00 }; #ifdef WITH_OGN1 static const uint8_t *OGN_SYNC = OGN1_SYNC; @@ -36,6 +40,10 @@ static uint32_t RF_SlotTime; // [sec] UTC time which belongs to t FIFO RF_RxFIFO; // buffer for received packets FIFO, 4> RF_TxFIFO; // buffer for transmitted packets +#ifdef WITH_ADSL + FIFO ADSL_TxFIFO; +#endif + #ifdef WITH_FANET FIFO FNT_RxFIFO; FIFO FNT_TxFIFO; @@ -51,17 +59,12 @@ static Delay RX_OGN_CountDelay; uint32_t RX_Random=0x12345678; // Random number from LSB of RSSI readouts -// void XorShift32(uint32_t &Seed) // simple random number generator -// { Seed ^= Seed << 13; -// Seed ^= Seed >> 17; -// Seed ^= Seed << 5; } - static uint8_t RX_Channel=0; // (hopping) channel currently being received -static void SetTxChannel(uint8_t TxChan=RX_Channel) // default channel to transmit is same as the receive channel +static void SetTxChannel(uint8_t TxChan=RX_Channel, const uint8_t *SYNC=OGN_SYNC) // default channel to transmit is same as the receive channel { #ifdef WITH_SX1262 - TRX.OGN_Configure(TxChan&0x7F, OGN_SYNC); + TRX.OGN_Configure(TxChan&0x7F, SYNC); TRX.WaitWhileBusy_ms(2); TRX.WriteTxPower(Parameters.TxPower); TRX.WaitWhileBusy_ms(2); @@ -73,24 +76,38 @@ static void SetTxChannel(uint8_t TxChan=RX_Channel) // default channel t TRX.WriteTxPower(Parameters.TxPower); // set TX for transmission #endif TRX.setChannel(TxChan&0x7F); - TRX.FSK_WriteSYNC(8, 7, OGN_SYNC); // Full SYNC for TX + TRX.FSK_WriteSYNC(8, 7, SYNC); // Full SYNC for TX #endif // WITH_SX1262 } -static void SetRxChannel(uint8_t RxChan=RX_Channel) +static void SetRxChannel(uint8_t RxChan=RX_Channel, const uint8_t *SYNC=OGN_SYNC) { #ifdef WITH_SX1262 + // TRX.OGN_Configure(RxChan&0x7F, SYNC); TRX.setChannel(RxChan&0x7F); - TRX.FSK_WriteSYNC(7, 7, OGN_SYNC); // Shorter SYNC for RX + TRX.FSK_WriteSYNC(7, 7, SYNC); // Shorter SYNC for RX #else TRX.WriteTxPowerMin(); // setup for RX TRX.setChannel(RxChan&0x7F); - TRX.FSK_WriteSYNC(7, 7, OGN_SYNC); // Shorter SYNC for RX + TRX.FSK_WriteSYNC(7, 7, SYNC); // Shorter SYNC for RX #endif } static uint8_t ReceivePacket(void) // see if a packet has arrived -{ if(!TRX.DIO0_isOn()) return 0; // DIO0 line HIGH signals a new packet has arrived +{ +#ifdef DEBUG_PRINT + uint16_t Mask=TRX.ReadIrqFlags(); + // static uint16_t BackOff=0; + if(Mask /* || BackOff==0 */ ) + { xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + Format_String(CONS_UART_Write, "IRQ:0x"); + Format_Hex(CONS_UART_Write, Mask); + Format_String(CONS_UART_Write, "\n"); + xSemaphoreGive(CONS_Mutex); + TRX.ClearIrqFlags(0xFFFF); /* BackOff=100; */ } + // else BackOff--; +#endif + if(!TRX.DIO0_isOn()) return 0; // DIO0 line HIGH signals a new packet has arrived #ifdef WITH_LED_RX LED_RX_Flash(20); #endif @@ -131,10 +148,57 @@ static uint32_t ReceiveUntil(TickType_t End) // static uint32_t ReceiveFor(TickType_t Ticks) // keep receiving packets for given period of time // { return ReceiveUntil(xTaskGetTickCount()+Ticks); } +#ifdef WITH_ADSL +static uint8_t Transmit(uint8_t TxChan, const ADSL_Packet *Packet, uint8_t Thresh, uint8_t MaxWait=7) +{ + if(Packet==0) return 0; // if no packet to send: simply return + + if(MaxWait) + { for( ; MaxWait; MaxWait--) // wait for a given maximum time for a free radio channel + { +#ifdef WITH_RFM69 + TRX.TriggerRSSI(); +#endif + vTaskDelay(1); + uint8_t RxRSSI=TRX.ReadRSSI(); + RX_Random = (RX_Random<<1) | (RxRSSI&1); + if(RxRSSI>=Thresh) break; } + if(MaxWait==0) return 0; } + +#ifdef WITH_LED_TX + LED_TX_Flash(20); +#endif + TRX.setModeStandby(); // switch to standby + vTaskDelay(1); + TRX.FSK_Configure(TxChan&0x7F, ADSL_SYNC, ADSL_Packet::TxBytes-3); + SetTxChannel(TxChan, ADSL_SYNC); + +#ifdef WITH_SX1262 +#else // not WITH_SX1262 + TRX.ClearIrqFlags(); + TRX.FSK_WritePacket(&(Packet->Version), ADSL_Packet::TxBytes-3); // write packet into FIFO (apply Manchester conversion) + TRX.setModeTX(); // transmit + vTaskDelay(5); // wait 5ms (about the OGN packet time) + uint8_t Break=0; + for(uint16_t Wait=400; Wait; Wait--) // wait for transmission to end + { uint16_t Flags=TRX.ReadIrqFlags(); + if(Flags&RF_IRQ_PacketSent) Break++; + if(Break>=2) break; } + TRX.setModeStandby(); // switch to standy + // vTaskPrioritySet(0, tskIDLE_PRIORITY+2); +#endif + + TRX.FSK_Configure(RX_Channel&0x7F, OGN_SYNC, OGN_TxPacket::Bytes); + SetRxChannel(); + TRX.setModeRX(); // back to receive mode + TRX.ClearIrqFlags(); + + return 1; } +#endif + static uint8_t Transmit(uint8_t TxChan, const uint8_t *PacketByte, uint8_t Thresh, uint8_t MaxWait=7) { if(PacketByte==0) return 0; // if no packet to send: simply return - // if(Parameters.TxPower==(-32)) return 0; // if transmission turned OFF if(MaxWait) { for( ; MaxWait; MaxWait--) // wait for a given maximum time for a free radio channel @@ -181,9 +245,9 @@ static uint8_t Transmit(uint8_t TxChan, const uint8_t *PacketByte, uint8_t Thres Format_String(CONS_UART_Write, "ms\n"); xSemaphoreGive(CONS_Mutex); #endif -#else // of WITH_SX1262 +#else // not WITH_SX1262 TRX.ClearIrqFlags(); - TRX.OGN_WritePacket(PacketByte); // write packet into FIFO + TRX.FSK_WritePacket(PacketByte, OGN_TxPacket::Bytes); // write packet into FIFO (apply Manchester conversion) TRX.setModeTX(); // transmit vTaskDelay(5); // wait 5ms (about the OGN packet time) uint8_t Break=0; @@ -216,10 +280,22 @@ static void TimeSlot(uint8_t TxChan, uint32_t SlotLen, const uint8_t *PacketByte if( (TxTime==0) || (TxTime>=MaxTxTime) ) TxTime = RX_Random%MaxTxTime; // if TxTime out of limits, setup a random TxTime TickType_t Tx = Start + TxTime; // Tx = the moment to start transmission ReceiveUntil(Tx); // listen until this time comes - if( (TX_Credit>0) && Parameters.TxPower!=(-32) && (PacketByte) ) // when packet to transmit is given and there is still TX credit left: + if( (TX_Credit>0) && Parameters.TxPower!=(-32) && PacketByte ) // when packet to transmit is given and there is still TX credit left: if(Transmit(TxChan, PacketByte, Rx_RSSI, MaxWait)) TX_Credit-=5; // attempt to transmit the packet - ReceiveUntil(End); // listen till the end of the time-slot -} + ReceiveUntil(End); } // listen till the end of the time-slot + +#ifdef WITH_ADSL +static void TimeSlot(uint8_t TxChan, uint32_t SlotLen, const ADSL_Packet *Packet, uint8_t Rx_RSSI, uint8_t MaxWait=8, uint32_t TxTime=0) +{ TickType_t Start = xTaskGetTickCount(); // when the slot started + TickType_t End = Start + SlotLen; // when should it end + uint32_t MaxTxTime = SlotLen-8-MaxWait; // time limit when transmision could start + if( (TxTime==0) || (TxTime>=MaxTxTime) ) TxTime = RX_Random%MaxTxTime; // if TxTime out of limits, setup a random TxTime + TickType_t Tx = Start + TxTime; // Tx = the moment to start transmission + ReceiveUntil(Tx); // listen until this time comes + if( (TX_Credit>0) && Parameters.TxPower!=(-32) && Packet ) // when packet to transmit is given and there is still TX credit left: + if(Transmit(TxChan, Packet, Rx_RSSI, MaxWait)) TX_Credit-=5; // attempt to transmit the packet + ReceiveUntil(End); } // listen till the end of the time-slot +#endif static void SetFreqPlanOGN(void) // set the RF TRX according to the selected frequency hopping plan { TRX.setBaseFrequency(RF_FreqPlan.BaseFreq); // set the base frequency (recalculate to RFM69 internal synth. units) @@ -272,22 +348,24 @@ static uint8_t StartRFchip(void) // if(TRX.readBusy()) Format_String(CONS_UART_Write, "StartRFchip() sx1262 BUSY after RESET(0)\n"); // else Format_String(CONS_UART_Write, "StartRFchip() sx1262 NOT busy after reset\n"); TRX.setRegulator(1); - TRX.WaitWhileBusy_ms(5); + TRX.WaitWhileBusy_ms(10); TRX.setRFswitchDIO2(1); // enable DIO0 as RF switch - TRX.WaitWhileBusy_ms(5); - // if(TRX.readBusy()) Format_String(CONS_UART_Write, "StartRFchip() sx1262 BUSY after setRFswitchDIO2()\n"); + TRX.WaitWhileBusy_ms(10); + //if(TRX.readBusy()) Format_String(CONS_UART_Write, "StartRFchip() sx1262 BUSY after setRFswitchDIO2()\n"); // WaitWhileBusy(100); - TRX.setTCXOctrlDIO3(1, 300); // TCXO control - TRX.WaitWhileBusy_ms(5); - // if(TRX.readBusy()) Format_String(CONS_UART_Write, "StartRFchip() sx1262 BUSY after setTCXOctrlDIO3()\n"); + TRX.setTCXOctrlDIO3(2); // TCXO control: 2=1.8V + TRX.WaitWhileBusy_ms(10); + TRX.Calibrate(0x7F); // + TRX.WaitWhileBusy_ms(10); + //if(TRX.readBusy()) Format_String(CONS_UART_Write, "StartRFchip() sx1262 BUSY after setTCXOctrlDIO3()\n"); // WaitWhileBusy(100); TRX.setModeStandby(0); - TRX.WaitWhileBusy_ms(5); + TRX.WaitWhileBusy_ms(10); // if(TRX.readBusy()) Format_String(CONS_UART_Write, "StartRFchip() sx1262 BUSY after setModeStandby()\n"); - // WaitWhileBusy(100); + // TRX.WaitWhileBusy(100); // if(TRX.readBusy()) { Format_String(CONS_UART_Write, "StartRFchip() sx1262 still busy !\n"); } TRX.setFSK(); - TRX.WaitWhileBusy_ms(5); + TRX.WaitWhileBusy_ms(10); #ifdef DEBUG_PRINT { uint8_t *SYNC = TRX.Regs_Read(REG_SYNCWORD0, 8); xSemaphoreTake(CONS_Mutex, portMAX_DELAY); @@ -295,10 +373,11 @@ static uint8_t StartRFchip(void) Format_Hex(CONS_UART_Write, TRX.getStatus()); Format_String(CONS_UART_Write, " => 0x"); for(int Idx=0; Idx<8; Idx++) - { Format_Hex(CONS_UART_Write, SYNC[Idx]); } - Format_String(CONS_UART_Write, "\n"); + { Format_Hex(CONS_UART_Write, SYNC[Idx]); } // SYNC after chip reset can serve as a detection method for sx1262 + Format_String(CONS_UART_Write, "\n"); // it should be: 0x9723522556536564 xSemaphoreGive(CONS_Mutex); } #endif + TRX.StopOnPreamble(0); #else // WITH_SX1262 // not WITH_SX1262 vTaskDelay(5); // wait 10ms #endif @@ -311,13 +390,13 @@ static uint8_t StartRFchip(void) // #ifdef WITH_RFM95 // TRX.WriteDefaultReg(); // #endif - TRX.OGN_Configure(0, OGN_SYNC); // setup RF chip parameters and set to channel #0 + TRX.FSK_Configure(0, OGN_SYNC, OGN_TxPacket::Bytes); // setup RF chip parameters and set to channel #0 #ifdef WITH_SX1262 TRX.WaitWhileBusy_ms(5); #ifdef DEBUG_PRINT { uint8_t *SYNC = TRX.Regs_Read(REG_SYNCWORD0, 8); xSemaphoreTake(CONS_Mutex, portMAX_DELAY); - Format_String(CONS_UART_Write, "OGN_Configure() => 0x"); + Format_String(CONS_UART_Write, "FSK_Configure() => 0x"); Format_Hex(CONS_UART_Write, TRX.getStatus()); Format_String(CONS_UART_Write, " => 0x"); for(int Idx=0; Idx<8; Idx++) @@ -325,7 +404,7 @@ static uint8_t StartRFchip(void) Format_String(CONS_UART_Write, "\n"); xSemaphoreGive(CONS_Mutex); } #endif - TRX.Calibrate(); + TRX.CalibrateImage(); TRX.WaitWhileBusy_ms(20); if(TRX.readBusy()) Format_String(CONS_UART_Write, "StartRFchip() sx1262 BUSY after OGN_Configure() and Calibrate()\n"); #endif @@ -473,7 +552,7 @@ extern "C" WANdev.RxSilent++; if(WANdev.RxSilent>30) WANdev.Disconnect(); } // count silence when reception expected, if too many then disconnect TRX.setFSK(); // back to FSK SetFreqPlanOGN(); // OGN frequency plan - TRX.OGN_Configure(0, OGN_SYNC); // OGN config + TRX.FSK_Configure(0, OGN_SYNC, OGN_TxPacket::Bytes); // OGN config SetRxChannel(); TRX.setModeRX(); // switch to receive mode TRX.ClearIrqFlags(); @@ -570,7 +649,14 @@ extern "C" XorShift32(RX_Random); uint32_t TxTime = (RX_Random&0x3F)+1; TxTime*=6; TxTime+=50; // random transmission time: (1..64)*6+50 [ms] - +#ifdef WITH_ADSL + bool ADSL_Slot = RX_Random&0x40; + ADSL_Packet *ADSL_TxPkt = ADSL_TxFIFO.getRead(); + // if(ADSL_TxPkt) + // { Format_String(CONS_UART_Write, "ADSL:"); + // CONS_UART_Write('0'+ADSL_Slot); + // Format_String(CONS_UART_Write, "\n"); } +#endif const uint8_t *TxPktData0=0; const uint8_t *TxPktData1=0; const OGN_TxPacket *TxPkt0 = RF_TxFIFO.getRead(0); // get 1st packet from TxFIFO @@ -583,7 +669,12 @@ extern "C" { if( (RX_Channel!=TxChan) && (TxPkt0->Packet.Header.Relay==0) ) { const uint8_t *Tmp=TxPktData0; TxPktData0=TxPktData1; TxPktData1=Tmp; } // swap 1st and 2nd packet data } - TimeSlot(TxChan, 800-TimeSync_msTime(), TxPktData0, TRX.averRSSI, 0, TxTime); // run a Time-Slot till 0.800sec +#ifdef WITH_ADSL + if(ADSL_Slot==0 && ADSL_TxPkt) + TimeSlot(TxChan, 800-TimeSync_msTime(), ADSL_TxPkt, TRX.averRSSI, 0, TxTime); + else +#endif + TimeSlot(TxChan, 800-TimeSync_msTime(), TxPktData0, TRX.averRSSI, 0, TxTime); // run a Time-Slot till 0.800sec TRX.setModeStandby(); TxChan = RF_FreqPlan.getChannel(RF_SlotTime, 1, 1); // transmit channel @@ -596,7 +687,7 @@ extern "C" TRX.FNT_Configure(); // configure for FANET // TRX.setChannel(0); // configure for FANET TRX.WriteTxPower(Parameters.TxPower); // transmission power - TRX.setModeLoRaRXcont(); // continous receiver mode + TRX.setModeLoRaRXcont(); // continous receiver mode vTaskDelay(2); for(uint8_t Wait=50; Wait; Wait--) // { vTaskDelay(1); // every milisecond @@ -628,7 +719,7 @@ extern "C" // uint8_t Mode=TRX.ReadMode(); // if(Mode!=RF_OPMODE_LORA_TX) break; } TRX.setFSK(); - TRX.OGN_Configure(0, OGN_SYNC); + TRX.FSK_Configure(0, OGN_SYNC, OGN_TxPacket::Bytes); } #endif // WITH_FANET @@ -638,18 +729,21 @@ extern "C" XorShift32(RX_Random); TxTime = (RX_Random&0x3F)+1; TxTime*=6; // [ms] (1..64)*6 = 6..384ms + uint16_t SlotEnd = 1240; #ifdef WITH_LORAWAN bool WANtx = 0; - uint16_t SlotEnd = 1240; if(WAN_BackOff) WAN_BackOff--; else if(Parameters.TxPower!=(-32)) // decide to transmit in this slot { if(WANdev.State==0 || WANdev.State==2) // { WANtx=1; SlotEnd=1220; } } - TimeSlot(TxChan, SlotEnd-TimeSync_msTime(), TxPktData1, TRX.averRSSI, 0, TxTime); -#else - TimeSlot(TxChan, 1240-TimeSync_msTime(), TxPktData1, TRX.averRSSI, 0, TxTime); #endif +#ifdef WITH_ADSL + if(ADSL_Slot==1 && ADSL_TxPkt) + TimeSlot(TxChan, SlotEnd-TimeSync_msTime(), ADSL_TxPkt, TRX.averRSSI, 0, TxTime); + else +#endif + TimeSlot(TxChan, SlotEnd-TimeSync_msTime(), TxPktData1, TRX.averRSSI, 0, TxTime); #ifdef WITH_PAW static uint8_t PAWtxBackOff = 4; @@ -685,7 +779,7 @@ extern "C" if(Break>=2) break; } #endif TRX.setModeStandby(); - TRX.OGN_Configure(0, OGN_SYNC); + TRX.FSK_Configure(0, OGN_SYNC, OGN_TxPacket::Bytes); PAWtxBackOff = 2+(RX_Random%5); XorShift32(RX_Random); TX_Credit-=8; } } @@ -746,13 +840,16 @@ extern "C" } TRX.setFSK(); // back to FSK SetFreqPlanOGN(); // OGN frequency plan - TRX.OGN_Configure(0, OGN_SYNC); // OGN config + TRX.FSK_Configure(0, OGN_SYNC, OGN_TxPacket::Bytes); // OGN config SetRxChannel(); TRX.setModeRX(); // switch to receive mode TRX.ClearIrqFlags(); } #endif +#ifdef WITH_ADSL + if(ADSL_TxPkt) ADSL_TxFIFO.Read(); +#endif if(TxPkt0) RF_TxFIFO.Read(); if(TxPkt1) RF_TxFIFO.Read(); diff --git a/main/rf.h b/main/rf.h index ed7d55d..cf7efec 100644 --- a/main/rf.h +++ b/main/rf.h @@ -19,6 +19,10 @@ extern FIFO RF_RxFIFO; // buffer for received packets extern FIFO, 4> RF_TxFIFO; // buffer for transmitted packets +#ifdef WITH_ADSL + extern FIFO ADSL_TxFIFO; +#endif + #ifdef WITH_FANET extern FIFO FNT_RxFIFO; extern FIFO FNT_TxFIFO; diff --git a/main/rfm.h b/main/rfm.h index 34bd3f4..6b37f28 100644 --- a/main/rfm.h +++ b/main/rfm.h @@ -276,6 +276,8 @@ SX1262 setup calls #include "manchester.h" +// #define DEBUG_PRINT + class RFM_TRX { public: // hardware access functions @@ -302,7 +304,7 @@ class RFM_TRX return Loops; } // return number of looks left or zero (=> still busy) uint16_t WaitWhileBusy_ms(uint16_t ms=100) - { WaitWhileBusy(50); + { WaitWhileBusy(100); for( ; ms; ms--) { if(!readBusy()) break; Delay_ms(1); } @@ -320,7 +322,7 @@ class RFM_TRX Format_Hex(CONS_UART_Write, Cmd); Format_String(CONS_UART_Write, ", 0x"); for(uint8_t Idx=0; Idx 0x"); for(uint8_t Idx=0; Idx>8; Block_Buffer[2] = Addr; memcpy(Block_Buffer+3, Data, Len); @@ -374,7 +376,7 @@ class RFM_TRX Format_Hex(CONS_UART_Write, Ofs); Format_String(CONS_UART_Write, ", 0x"); for(uint8_t Idx=0; Idx>4]; // software manchester encode every byte - Packet[PktIdx++]=ManchesterEncode[Byte&0x0F]; - } + Packet[PktIdx++]=ManchesterEncode[Byte&0x0F]; } WriteFIFO(Packet, 2*Len); } void PAW_WritePacket(const uint8_t *Data, uint8_t Len=24) @@ -625,15 +626,14 @@ class RFM_TRX TransferByte(Data[Idx]); Deselect(); } - void OGN_WritePacket(const uint8_t *Data, uint8_t Len=26) const // write the packet data (26 bytes) + void FSK_WritePacket(const uint8_t *Data, uint8_t Len=26) const // write the packet data (26 bytes) { const uint8_t Addr=REG_FIFO; // write to FIFO Select(); TransferByte(Addr | 0x80); for(uint8_t Idx=0; Idx>4]); // software manchester encode every byte - TransferByte(ManchesterEncode[Byte&0x0F]); - } + TransferByte(ManchesterEncode[Byte&0x0F]); } Deselect(); } void PAW_WritePacket(const uint8_t *Data, uint8_t Len=24) @@ -688,8 +688,10 @@ class RFM_TRX #endif #ifdef WITH_SX1262 - void setTCXOctrlDIO3(uint8_t Volt=1, uint32_t Delay=320) // [0..7 => 1.6-3.3V] [1/64ms] p.82 - { uint8_t Data[4] = { Volt, (uint8_t)(Delay>>16), (uint8_t)(Delay>>8), (uint8_t)Delay }; Cmd_Write(CMD_SETDIO3ASTCXOCTRL, Data, 4); } + void setTCXOctrlDIO3(uint8_t Volt=1, uint32_t Delay=5) // [0..7 => 1.6-3.3V] [ms] p.82 + { Delay<<=6; // [ms => 1/64ms] + uint8_t Data[4] = { Volt, (uint8_t)(Delay>>16), (uint8_t)(Delay>>8), (uint8_t)Delay }; + Cmd_Write(CMD_SETDIO3ASTCXOCTRL, Data, 4); } void setRFswitchDIO2(uint8_t Mode=0) { Cmd_Write(CMD_SETDIO2ASRFSWITCHCTRL, &Mode, 1); } // enable DIO2 as RF switch void setRegulator(uint8_t Mode=0) { Cmd_Write(CMD_SETREGULATORMODE, &Mode, 1); } // 0=LDO, 1=DCDC+LDO @@ -712,9 +714,15 @@ class RFM_TRX Cmd_Write(CMD_SETTXPARAMS, TxParm, 2); } // 0x8E, Power, RampTime void WriteTxPowerMin(void) { WriteTxPower(-3); } - void Calibrate(void) // Calibrate receiver image rejection + void StopOnPreamble(uint8_t Enable) // calbration of RC, PLL and ADC + { Cmd_Write(CMD_STOPTIMERONPREAMBLE, &Enable, 1); } + + void Calibrate(uint8_t Mask) // calbration of RC, PLL and ADC + { Cmd_Write(CMD_CALIBRATE, &Mask, 1); } + + void CalibrateImage(void) // Calibrate receiver image rejection { uint8_t CalParm[2] = { 0xD7, 0xDB }; // for 868MHz // { 0xE1, 0xE9 } for 915MHz - Cmd_Write(CMD_SETTXPARAMS, CalParm, 2); } + Cmd_Write(CMD_CALIBRATEIMAGE, CalParm, 2); } static void Pack3bytes(uint8_t *Byte, uint32_t Value) { Byte[0]=Value>>16; Byte[1]=Value>>8; Byte[2]=Value; } @@ -747,8 +755,8 @@ class RFM_TRX { if(SyncTol>7) SyncTol=7; if(WriteSize>8) WriteSize=8; uint8_t Param[12]; - Param[0] = 0; // - Param[1] = 4; // [bits] preamble length + Param[0] = 0; // preamble length MSB + Param[1] = 8; // [bits] preamble length LSB Param[2] = 0x04; // preamble detect: 0x00:OFF, 0x04:8bits, 0x05:16bits, 0x06=24bits, 0x07=32bits Param[3] = WriteSize*8; // [bits] SYNC word length, write word at 0x06C0 Param[4] = 0x00; // address filtering: OFF @@ -764,7 +772,7 @@ class RFM_TRX uint8_t Param[12]; Pack3bytes(Param, 10240); // data bitrate = 32*Xtal/100e3 for OGN 100kbps Param[3] = 0x09; // 0x00:no filter, 0x08:BT=0.3, 0x09:BT=0.5, 0x0A:BT=0.7, 0x0B:BT=1.0 - Param[4] = 0x0A; // DSB RX bandwidth: 0x0A=232.3kHz, 0x19=312.2kHz, 0x1B=78.2kHz, 0x13=117.3kHz + Param[4] = 0x0A; // DSB RX bandwidth: 0x0A=232.3kHz, 0x19=312.2kHz, 0x1B=78.2kHz, 0x13=117.3kHz, 0x12=187.2kHz Pack3bytes(Param+5, 52429); // FSK deviation: 50e3*2^25/Xtal for OGN +/-50kHz Cmd_Write(CMD_SETMODULATIONPARAMS, Param, 8); // 0x8B, ModParam Param[0] = 0; // [bits] MSB @@ -801,13 +809,13 @@ class RFM_TRX setDioMode( /* IRQ_TXDONE | */ IRQ_RXDONE); Regs_Write(REG_SYNCWORD0, Sync, 8); } // Write the SYNC word - void ClearIrqFlags(uint16_t Mask=IRQ_ALL) { uint8_t Data[2]; Data[0]=Mask>>8; Data[1]=Mask; Cmd_Write(CMD_CLEARIRQSTATUS, Data, 2); } + void ClearIrqFlags(uint16_t Mask=0xFFFF) { uint8_t Data[2]; Data[0]=Mask>>8; Data[1]=Mask; Cmd_Write(CMD_CLEARIRQSTATUS, Data, 2); } uint16_t ReadIrqFlags(void) { uint8_t *Stat=Cmd_Read(CMD_GETIRQSTATUS, 2); return (((uint16_t)(Stat[0]))<<8) | Stat[1]; } void setModeSleep(void) { uint8_t Config[3] = { 0, 0, 0 }; Cmd_Write(CMD_SETSLEEP, Config, 3); } void setModeStandby(uint8_t Mode=0) { uint8_t Config[1] = { Mode }; Cmd_Write(CMD_SETSTANDBY, Config, 1); } - void setModeTX(uint32_t Timeout=0) { uint8_t T[3]; Pack3bytes(T, Timeout); Cmd_Write(CMD_SETTX, T, 3); } - void setModeRX(uint32_t Timeout=0) { uint8_t T[3]; Pack3bytes(T, Timeout); Cmd_Write(CMD_SETRX, T, 3); } + void setModeTX(uint32_t Timeout=0) { Timeout<<=6; uint8_t T[3]; Pack3bytes(T, Timeout); Cmd_Write(CMD_SETTX, T, 3); } + void setModeRX(uint32_t Timeout=0) { Timeout<<=6; uint8_t T[3]; Pack3bytes(T, Timeout); Cmd_Write(CMD_SETRX, T, 3); } void setModeSynth(void) { Cmd_Write(CMD_SETFS, 0, 0); } void setDioMode(uint16_t Mask = IRQ_RXDONE) @@ -1078,7 +1086,13 @@ class RFM_TRX return 0; } - void OGN_Configure(int16_t Channel, const uint8_t *Sync) + // void OGN_Configure(int16_t Channel, const uint8_t *Sync) + // { FSK_Configure(Channel, Sync, 2*26); } + + // void ADSL_Configure(int16_t Channel, const uint8_t *Sync) + // { FSK_Configure(Channel, Sync, 2*24); } + + void FSK_Configure(int16_t Channel, const uint8_t *Sync, uint8_t PktSize) { // WriteMode(RF_OPMODE_STANDBY); // mode: STDBY, modulation: FSK, no LoRa // usleep(1000); WriteTxPower(0); @@ -1093,8 +1107,8 @@ class RFM_TRX WriteByte( 0x85, REG_PREAMBLEDETECT); // preamble detect: 1 byte, page 92 (or 0x85 ?) WriteByte( 0x00, REG_PACKETCONFIG1); // Fixed size packet, no DC-free encoding, no CRC, no address filtering WriteByte( 0x40, REG_PACKETCONFIG2); // Packet mode - WriteByte( 2*26, REG_PAYLOADLENGTH); // Packet size = 26 bytes Manchester encoded into 52 bytes - WriteByte( 51, REG_FIFOTHRESH); // TxStartCondition=FifoNotEmpty, FIFO threshold = 51 bytes + WriteByte(PktSize*2, REG_PAYLOADLENGTH); // Packet size: Manchester encoded into twice as many bytes + WriteByte(PktSize*2-1, REG_FIFOTHRESH); // TxStartCondition=FifoNotEmpty, FIFO threshold just below the packet size WriteWord(0x3030, REG_DIOMAPPING1); // DIO signals: DIO0=00, DIO1=11, DIO2=00, DIO3=00, DIO4=00, DIO5=11, => p.64, 99 WriteByte( 0x02, REG_RXBW); // +/-125kHz Rx (single-side) bandwidth => p.27,67,83,90 WriteByte( 0x02, REG_AFCBW); // +/-125kHz AFC bandwidth @@ -1166,5 +1180,7 @@ class RFM_TRX #endif } ; +// #undef DEBUG_PRINT + #endif // __RFM_H__ diff --git a/main/sx1262.h b/main/sx1262.h index bf9f9fc..7d66b6a 100644 --- a/main/sx1262.h +++ b/main/sx1262.h @@ -27,7 +27,7 @@ #define CMD_SETRFFREQUENCY 0x86 // [4] frequency [Fxtal/2^25] #define CMD_SETPACKETTYPE 0x8A // [1] 0x00=GFSK, 0x01=LoRa (must be the first configuration command) -#define CMD_GETPACKETTYPE 0x11 // [ ] +#define CMD_GETPACKETTYPE 0x11 // [stat+1] #define CMD_SETTXPARAMS 0x8E // [2] TxPower: -17..+14 or -9..+22 [dBm] depending on PA config RampTime: 0..7: 10,20,40,80,200,800,1700,3400us #define CMD_SETMODULATIONPARAMS 0x8B // [8] depends on the protocol: FSK or LoRa #define CMD_SETPACKETPARAMS 0x8C // [9] @@ -35,23 +35,30 @@ #define CMD_SETBUFFERBASEADDRESS 0x8F // [2] Tx-base address, Rx-base address #define CMD_SETLORASYMBNUMTIMEOUT 0xA0 // [1] timeout [symbols] -#define CMD_GETSTATUS 0xC0 +#define CMD_GETSTATUS 0xC0 // #define CMD_GETRSSIINST 0x15 // [stat+1] RX-RSSI at this moment #define CMD_GETRXBUFFERSTATUS 0x13 // [stat+2] RX-packet: length, buffer pointer #define CMD_GETPACKETSTATUS 0x14 // [stat+3] RX-packet: RSSI/SNR -#define CMD_GETDEVICEERRORS 0x17 -#define CMD_CLEARDEVICEERRORS 0x07 +#define CMD_GETDEVICEERRORS 0x17 // [stat+2] rrrrrrrP rPXIAPRR +#define CMD_CLEARDEVICEERRORS 0x07 // clear the above error #define CMD_GETSTATS 0x10 // [stat+6] RX-statistics: packets, errors #define CMD_RESETSTATS 0x00 // [stat+6] RX-statistics reset // registers +#define REG_DIOXOUTPUTENABLE 0x0580 +#define REG_DIOXINPUTENABLE 0x0583 +#define REG_DIOXPULLUPCONTROL 0x0584 +#define REG_DIOXPULLDOWNCONTROL 0x0585 + #define REG_WHITENINGMSB 0x06B8 #define REG_WHITENINGLSB 0x06B9 + #define REG_CRCINITVALMSB 0x06BC #define REG_CRCINITVALLSB 0x06BD -#define REG_CRCPOLYVALMSB 0x06BE +#define REG_CRCPOLYVALMSB 0x06BE // #define REG_CRCPOLYVALLSB 0x06BF -#define REG_SYNCWORD0 0x06C0 + +#define REG_SYNCWORD0 0x06C0 // #define REG_SYNCWORD1 0x06C1 #define REG_SYNCWORD2 0x06C2 #define REG_SYNCWORD3 0x06C3 @@ -59,20 +66,32 @@ #define REG_SYNCWORD5 0x06C5 #define REG_SYNCWORD6 0x06C6 #define REG_SYNCWORD7 0x06C7 + #define REG_NODEADDRESS 0x06CD #define REG_BROADCASTADDR 0x06CE -#define REG_LORASYNCWORD 0x0740 -#define REG_LORASYNCWORDMSB 0x0740 -#define REG_LORASYNCWORDLSB 0x0741 -#define REG_RANDOMNUMBERGEN0 0x0819 + +#define REG_IQPOLARITYSETUP 0x0736 + +#define REG_LORASYNCWORD 0x0740 // +#define REG_LORASYNCWORDMSB 0x0740 // +#define REG_LORASYNCWORDLSB 0x0741 // + +#define REG_RANDOMNUMBERGEN0 0x0819 // #define REG_RANDOMNUMBERGEN1 0x081A #define REG_RANDOMNUMBERGEN2 0x081B #define REG_RANDOMNUMBERGEN3 0x081C -#define REG_RXGAIN 0x08AC + +#define REG_RXGAIN 0x08AC // 0x96 for max LNA gain, increase current by ~2mA for around ~3dB in sensivity + #define REG_OCPCONFIG 0x08E7 // max. PA current ? + +#define REG_RTCCONTROL 0x0902 + #define REG_XTATRIM 0x0911 #define REG_XTBTRIM 0x0912 +#define REG_EVENTMASK 0x0944 + // IRQ types #define IRQ_TXDONE (1 << 0) #define IRQ_RXDONE (1 << 1) @@ -86,3 +105,13 @@ #define IRQ_TIMEOUT (1 << 9) #define IRQ_ALL 0x3FF +// Operating modes +#define MODE_SLEEP 0 // The radio is in sleep mode +#define MODE_STDBY_RC 1 // The radio is in standby mode with RC oscillator +#define MODE_STDBY_XOSC 2 // The radio is in standby mode with XOSC oscillator +#define MODE_FS 3 // The radio is in frequency synthesis mode +#define MODE_TX 4 // The radio is in transmit mode +#define MODE_RX 5 // The radio is in receive mode +#define MODE_RX_DC 6 // The radio is in receive duty cycle mode +#define MODE_CAD 7 // The radio is in channel activity detection mode +