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

Add basic FANET transmission: position and pilot's name if defined, alpha stage

pull/20/head
Pawel Jalocha 2020-05-01 15:01:36 +01:00
rodzic a20c55358d
commit 1d83ab73b3
9 zmienionych plików z 1127 dodań i 105 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

454
main/fanet.h 100644
Wyświetl plik

@ -0,0 +1,454 @@
#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:
uint8_t Len;
static const int MaxBytes = 32;
uint8_t Byte[MaxBytes+2];
public:
FANET_Packet() { Len=0; }
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==0x11 || Pref==0x20 || Pref==0xDD || Pref==0xDE || Pref==0xDF) return 2;
return 3; }
void setAddress(uint32_t Addr) { setAddrPref(Addr>>16); setAddrLow(Addr); }
void setAddrPref(uint8_t Prefix) { Byte[1]=Prefix; }
void setAddrLow(uint16_t Addr ) { Byte[2]=Addr; Byte[3]=Addr>>8; }
void setHeader(uint8_t Type) { Byte[0] = 0x40 | (Type&0x3F); }
void setType(uint8_t Type) { Byte[0] = (Byte[0]&0xC0) | (Type&0x3F); }
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(); }
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 cordic units to float degrees
{ // const float Conv = 90.0/0x40000000; // conversion factor (exact cordic)
const float Conv = 90.0007295677/0x40000000; // exact FANET conversion factor (not exactly cordic)
return Conv*Coord; }
static int32_t getLat(const uint8_t *Byte) // 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) // 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 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;
return QNE; }
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] [cordic] [cordic] [m] [cordic] [0.1m/s] [0.1m/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)
{ setHeader(1);
uint8_t Ofs=MsgOfs();
Len=15;
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]
// [0..15] [0..1] [cordic] [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; }
void Print(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;
int32_t Lon=getLon(Byte+Idx); Idx+=3;
printf(" [%+9.5f,%+10.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);
int32_t Lon=getLon(Byte+Idx+3);
uint16_t Alt=getAltitude(Byte+Idx+6); // [m]
uint16_t Speed=getSpeed(Byte[Idx+8]); // [0.5km/h]
int16_t Climb=getClimb(Byte[Idx+9]); // [0.1m/s]
printf(" [%+9.5f,%+10.5f] %dm %3.1fkm/h %03.0fdeg %+4.1fm/s a%X%c",
FloatCoord(Lat), FloatCoord(Lon), Alt, 0.5*Speed, (180.0/128)*Byte[Idx+10], 0.1*Climb,
AcftType&0x07, AcftType&0x08?'T':'H');
printf("\n"); return; }
if(Type()==7) // ground position
{ uint8_t Idx=MsgOfs(); uint8_t Status=Byte[Idx+6];
int32_t Lat=getLat(Byte+Idx);
int32_t Lon=getLon(Byte+Idx+3);
printf(" [%+9.5f,%+10.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 int Format_Lat(char *Str, int32_t Lat, char &HighRes) // format latitude after APRS
{ // Lat = ((int64_t)900000000*Lat+0x20000000)>>30; // convert from cordic to UBX 1e-7 deg
Lat = ((int64_t)900007296*Lat+0x20000000)>>30; // convert from FANET cordic to UBX 1e-7 deg
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 = ((int64_t)900000000*Lon+0x20000000)>>30;
Lon = ((int64_t)900007296*Lon+0x20000000)>>30;
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:
double Time; // reception time
int16_t FreqOfs; // [ 10Hz]
uint8_t SNR; // [0.25dB]
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 Print(char *Name=0) const
{ char HHMMSS[8];
Format_HHMMSS(HHMMSS, SlotTime()); HHMMSS[6]='h'; HHMMSS[7]=0;
printf("%s %3.1fdB/%de %+3.1fkHz ", HHMMSS, 0.25*SNR, BitErr, 1e-2*FreqOfs);
FANET_Packet::Print(Name); }
int WriteAPRS(char *Out)
{ bool Report=0;
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+=32;
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
uint8_t AddrType = 2; // 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
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 QNE=getQNE(Msg[11]); // [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>=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 AddrType = 2; //
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; }
}
if(SNR>0)
{ Out[Len++]=' ';
Len+=Format_UnsDec(Out+Len, ((uint16_t)SNR*10+2)/4, 2, 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; }
} ;
// =========================================================================================
class FANET_Name
{ public:
static const int MaxSize = 40;
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; }
} ;
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 // __FANET_H__

119
main/gdl90.cpp 100644
Wyświetl plik

@ -0,0 +1,119 @@
#include "gdl90.h"
static const uint16_t CRC16_CCITT_Table[256] = {
0x0000U, 0x1021U, 0x2042U, 0x3063U, 0x4084U, 0x50A5U, 0x60C6U, 0x70E7U,
0x8108U, 0x9129U, 0xA14AU, 0xB16BU, 0xC18CU, 0xD1ADU, 0xE1CEU, 0xF1EFU,
0x1231U, 0x0210U, 0x3273U, 0x2252U, 0x52B5U, 0x4294U, 0x72F7U, 0x62D6U,
0x9339U, 0x8318U, 0xB37BU, 0xA35AU, 0xD3BDU, 0xC39CU, 0xF3FFU, 0xE3DEU,
0x2462U, 0x3443U, 0x0420U, 0x1401U, 0x64E6U, 0x74C7U, 0x44A4U, 0x5485U,
0xA56AU, 0xB54BU, 0x8528U, 0x9509U, 0xE5EEU, 0xF5CFU, 0xC5ACU, 0xD58DU,
0x3653U, 0x2672U, 0x1611U, 0x0630U, 0x76D7U, 0x66F6U, 0x5695U, 0x46B4U,
0xB75BU, 0xA77AU, 0x9719U, 0x8738U, 0xF7DFU, 0xE7FEU, 0xD79DU, 0xC7BCU,
0x48C4U, 0x58E5U, 0x6886U, 0x78A7U, 0x0840U, 0x1861U, 0x2802U, 0x3823U,
0xC9CCU, 0xD9EDU, 0xE98EU, 0xF9AFU, 0x8948U, 0x9969U, 0xA90AU, 0xB92BU,
0x5AF5U, 0x4AD4U, 0x7AB7U, 0x6A96U, 0x1A71U, 0x0A50U, 0x3A33U, 0x2A12U,
0xDBFDU, 0xCBDCU, 0xFBBFU, 0xEB9EU, 0x9B79U, 0x8B58U, 0xBB3BU, 0xAB1AU,
0x6CA6U, 0x7C87U, 0x4CE4U, 0x5CC5U, 0x2C22U, 0x3C03U, 0x0C60U, 0x1C41U,
0xEDAEU, 0xFD8FU, 0xCDECU, 0xDDCDU, 0xAD2AU, 0xBD0BU, 0x8D68U, 0x9D49U,
0x7E97U, 0x6EB6U, 0x5ED5U, 0x4EF4U, 0x3E13U, 0x2E32U, 0x1E51U, 0x0E70U,
0xFF9FU, 0xEFBEU, 0xDFDDU, 0xCFFCU, 0xBF1BU, 0xAF3AU, 0x9F59U, 0x8F78U,
0x9188U, 0x81A9U, 0xB1CAU, 0xA1EBU, 0xD10CU, 0xC12DU, 0xF14EU, 0xE16FU,
0x1080U, 0x00A1U, 0x30C2U, 0x20E3U, 0x5004U, 0x4025U, 0x7046U, 0x6067U,
0x83B9U, 0x9398U, 0xA3FBU, 0xB3DAU, 0xC33DU, 0xD31CU, 0xE37FU, 0xF35EU,
0x02B1U, 0x1290U, 0x22F3U, 0x32D2U, 0x4235U, 0x5214U, 0x6277U, 0x7256U,
0xB5EAU, 0xA5CBU, 0x95A8U, 0x8589U, 0xF56EU, 0xE54FU, 0xD52CU, 0xC50DU,
0x34E2U, 0x24C3U, 0x14A0U, 0x0481U, 0x7466U, 0x6447U, 0x5424U, 0x4405U,
0xA7DBU, 0xB7FAU, 0x8799U, 0x97B8U, 0xE75FU, 0xF77EU, 0xC71DU, 0xD73CU,
0x26D3U, 0x36F2U, 0x0691U, 0x16B0U, 0x6657U, 0x7676U, 0x4615U, 0x5634U,
0xD94CU, 0xC96DU, 0xF90EU, 0xE92FU, 0x99C8U, 0x89E9U, 0xB98AU, 0xA9ABU,
0x5844U, 0x4865U, 0x7806U, 0x6827U, 0x18C0U, 0x08E1U, 0x3882U, 0x28A3U,
0xCB7DU, 0xDB5CU, 0xEB3FU, 0xFB1EU, 0x8BF9U, 0x9BD8U, 0xABBBU, 0xBB9AU,
0x4A75U, 0x5A54U, 0x6A37U, 0x7A16U, 0x0AF1U, 0x1AD0U, 0x2AB3U, 0x3A92U,
0xFD2EU, 0xED0FU, 0xDD6CU, 0xCD4DU, 0xBDAAU, 0xAD8BU, 0x9DE8U, 0x8DC9U,
0x7C26U, 0x6C07U, 0x5C64U, 0x4C45U, 0x3CA2U, 0x2C83U, 0x1CE0U, 0x0CC1U,
0xEF1FU, 0xFF3EU, 0xCF5DU, 0xDF7CU, 0xAF9BU, 0xBFBAU, 0x8FD9U, 0x9FF8U,
0x6E17U, 0x7E36U, 0x4E55U, 0x5E74U, 0x2E93U, 0x3EB2U, 0x0ED1U, 0x1EF0U
};
uint16_t GDL90_CRC16(uint8_t Byte, uint16_t CRC)
{ return CRC16_CCITT_Table[CRC>>8] ^ (CRC<<8) ^ Byte; }
uint16_t GDL90_CRC16(const uint8_t *Data, uint8_t Len, uint16_t CRC)
{ for(int Idx=0; Idx<Len; Idx++)
{ CRC = GDL90_CRC16(Data[Idx], CRC); }
return CRC; }
// inline uint16_t CRC16_CCITT(uint8_t Byte, uint16_t CRC)
// { uint16_t X = ( (CRC>>8) ^ Byte ) & 0xFF;
// X ^= X>>4;
// CRC = (CRC<<8) ^ (X<<12) ^ (X<<5) ^ X;
// return CRC; }
/*
inline uint16_t CRC16_CCITT(uint8_t Byte, uint16_t CRC)
{ CRC = (CRC>>8) | (CRC<<8);
CRC ^= Byte;
CRC ^= (CRC&0xFF)>>4;
CRC ^= CRC<<12;
CRC ^= (CRC&0xFF)<<5;
return CRC; }
*/
/*
inline uint16_t CRC16_CCITT(uint8_t Byte, uint16_t CRC)
{ // CRC = (CRC>>8) | (CRC<<8);
CRC ^= (uint16_t)Byte<<8;
CRC ^= (CRC&0xFF00)<<4;
CRC ^= CRC>>12;
CRC ^= (CRC&0xFF00)>>5;
// CRC = (CRC>>8) | (CRC<<8);
return CRC; }
*/
/*
uint16_t CRC16_CCITT(const uint8_t *Data, uint8_t Len, uint16_t CRC)
{ while (Len--)
{ uint8_t X = (CRC>>8) ^ (*Data++);
CRC = (CRC<<8) ^ CRC16_CCITT_Table[X]; }
return CRC; }
uint16_t CRC16_CCITT(const uint8_t *Data, uint8_t Len, uint16_t CRC)
{ while (Len--)
{ uint16_t X = ( (CRC>>8) ^ (*Data++) ) & 0xFF;
X ^= X>>4;
CRC = (CRC<<8) ^ (X<<12) ^ (X<<5) ^ X; }
return CRC; }
*/
/*
uint16_t CRC16_CCITT(const uint8_t *Data, uint8_t Len, uint16_t CRC)
{ while (Len--)
{ CRC = (CRC>>8) | (CRC<<8);
CRC ^= (*Data++);
CRC ^= (CRC&0xFF)>>4;
CRC ^= CRC<<12;
CRC ^= (CRC&0xFF)<<5; }
return CRC; }
*/
const uint8_t GDL90_Flag = 0x7E;
const uint8_t GDL90_Esc = 0x7D;
static int GDL90_SendEsc(void (*Output)(char), uint8_t Byte) // shall we escape control characters as well ?
{ // if(Byte<0x20 || Byte==GDL90_Flag || Byte==GDL90_Esc) { (*Output)((char)GDL90_Esc); Byte^=0x20; (*Output)((char)Byte); return 2; }
if(Byte==GDL90_Flag || Byte==GDL90_Esc) { (*Output)((char)GDL90_Esc); Byte^=0x20; (*Output)((char)Byte); return 2; } // ESCape some characters
(*Output)((char)Byte); return 1; }
int GDL90_Send(void (*Output)(char), uint8_t ID, const uint8_t *Data, int Len)
{ int Count=0; uint16_t CRC=0;
(*Output)((char)GDL90_Flag); Count++;
CRC=GDL90_CRC16(ID, CRC);
Count+=GDL90_SendEsc(Output, ID);
for( int Idx=0; Idx<Len; Idx++)
{ uint8_t Byte=Data[Idx];
CRC=GDL90_CRC16(Byte, CRC);
Count+=GDL90_SendEsc(Output, Byte); }
Count+=GDL90_SendEsc(Output, CRC&0xFF);
Count+=GDL90_SendEsc(Output, CRC>>8);
(*Output)((char)GDL90_Flag); Count++;
return Count; }

259
main/gdl90.h 100644
Wyświetl plik

@ -0,0 +1,259 @@
#ifndef __GDL90_H__
#define __GDL90_H__
#include <stdio.h>
#include <stdint.h>
#include "format.h"
// =================================================================================
uint16_t GDL90_CRC16(uint8_t Byte, uint16_t CRC); // pass a single byte through the CRC
uint16_t GDL90_CRC16(const uint8_t *Data, uint8_t Len, uint16_t CRC=0); // pass a packet of bytes through the CRC
int GDL90_Send(void (*Output)(char), uint8_t ID, const uint8_t *Data, int Len); // transmit GDL90 packet with proper framing and CRC
// =================================================================================
// https://www.faa.gov/nextgen/programs/adsb/archival/media/gdl90_public_icd_reva.pdf
// SkyRadar msg ID 101: https://github.com/etdey/gdl90/blob/master/Format%20of%20the%20SkyRadar%20receiver%20message%20ID%20101.pdf
// other msg ID: https://www.foreflight.com/connect/spec/
class GDL90_HEARTBEAT // Heart-beat packet to be send at every UTC second
{ public:
static const int Size=6;
union
{ uint8_t Status1;
struct
{ bool Initialized: 1;
bool reserved : 1;
bool RATCS : 1;
bool LowBatt : 1; // battery is LOW
bool AddrType : 1; // are we transmitting ICAO (0) or self-assigned address (1)
bool IDENT : 1;
bool MaintReq : 1;
bool PosValid : 1; // GPS position is valid
} ;
} ;
union
{ uint8_t Status2;
struct
{ bool UTCvalid : 1; // UTC timing is valid
bool reserved1 : 1;
bool reserved2 : 1;
bool reserved3 : 1;
bool reserved4 : 1;
bool CSA_Req : 1; // CSA has been requested (Conflict Situation Awareness)
bool CSA_NotAvail : 1; // CSA is not available
bool TimeStampMSB : 1; // [0x10000sec] highest TimeStamp bit
} ;
} ;
uint16_t TimeStamp; // [sec] since 0000z cut to 16-bit
uint8_t MsgCount[2]; // [/sec] counts messages received during the previous second
public:
void Clear(void)
{ Status1=0; Status2=0; TimeStamp=0; MsgCount[0]=0; MsgCount[1]=0; }
void setTimeStamp(uint32_t Time)
{ Time%=86400; TimeStamp=Time; TimeStampMSB=Time>>16; }
uint32_t getTimeStamp(void) const
{ uint32_t Time=TimeStampMSB; Time = (Time<<16) | TimeStamp; return Time; }
uint8_t getUplinkCount(void) const { return MsgCount[0]>>3; } // Uplink messages received
void setUplinkCount(uint8_t Count) { MsgCount[0] = (MsgCount[0]&0x07) | Count<<3; }
uint16_t getDownlinkCount(void) const { uint16_t Count = MsgCount[0]&0x03; return (Count<<8) | MsgCount[1]; } // Basic and Long messages received
void setDownlinkCount(uint8_t Count) { MsgCount[0] = (MsgCount[0]&0xFC) | (Count>>8); MsgCount[1] = Count; }
int Send(void (*Output)(char)) const { return GDL90_Send(Output, 0, (const uint8_t *)this, Size); }
} __attribute__((packed));
// class GSL90_CONFIG // Initialization, ID=117
// { public:
// uint8_t Data[19];
// } ;
class GDL90_GEOMALT // Geometrical altitude: ID = 11 (GPS ref. to Ellipsoid)
{ public:
static const int Size=4;
uint8_t Data[Size];
public:
void setAltitude(int32_t Alt) // [5 feet] GPS altitude (ref. to Ellipsoid)
{ if(Alt>0x7FFF) Alt=0x7FFF;
else if(Alt<(-0x8000)) Alt=(-0x8000);
Data[0]=Alt>>8; Data[1]=Alt&0x0FF; }
void setWarning(bool Warn) // [bool]
{ if(Warn) Data[2]|=0x80;
else Data[2]&=0x7F; }
void setFOM(uint16_t FOM) // [m] vertical Figure of Merit (accuracy ?)
{ Data[2] = (Data[2]&0x80) | (FOM>>8);
Data[3] = FOM&0xFF; }
} ;
class GDL90_REPORT // Position report: Traffic: ID = 20, Ownship: ID = 10
{ public:
static const int Size=27;
union
{ uint8_t Data[Size]; // 27 bytes excluding the ID and framing/CRC
/*
struct
{ uint8_t AddrType : 4; //
uint8_t Alert : 4; // 1=alert
uint32_t Address :24; // byte-reversed
uint32_t Latitude :24; // byte-reversed
uint32_t Longitude :24; // byte-reversed
uint16_t Altitude :12; // garbled
uint8_t Misc : 4; // garbled
uint8_t NACp : 4;
uint8_t NIC : 4;
uint16_t Velocity :12; // garbled
uint16_t Climb :12; // garbled
uint8_t Track : 8;
uint8_t AcftCat : 8;
char Call[8];
uint8_t Spare : 4;
uint8_t Priority : 4;
} ;
*/
} ; // __attribute__((packed));
public:
static uint32_t get3bytes(const uint8_t *Byte) { uint32_t Word=Byte[0]; Word=(Word<<8) | Byte[1]; Word=(Word<<8) | Byte[2]; return Word; } // 3-byte value
static void set3bytes(uint8_t *Byte, uint32_t Word) { Byte[0]=Word>>16; Byte[1]=Word>>8; Byte[2]=Word; }
void Clear(void) { for(int Idx=0; Idx<27; Idx++) Data[Idx]=0; } // clear all data (Lat/Lon = invalid)
uint8_t getAlertStatus(void) const { return Data[0]>>4; } // 0 = no alert, 1 = alert
void setAlertStatus(uint8_t Status) { Data[0] = (Data[0]&0x0F) | (Status<<4); }
uint8_t getAddrType(void) const { return Data[0]&0x0F; } // 0=ICAO, 1=non-ICAO, 4=surface vehicle, 5=ground beacon
void setAddrType(uint8_t AddrType) { Data[0] = (Data[0]&0xF0) | (AddrType&0x0F); }
uint32_t getAddress(void) const { return get3bytes(Data+1); }
void setAddress(uint32_t Addr) { set3bytes(Data+1, Addr); }
int32_t getLatitude(void) const { return get3bytes(Data+4)<<8; } // [cyclic]
void setLatitude(int32_t Lat) { set3bytes(Data+4, (Lat>>8)&0xFFFFFF); }
int32_t getLongitude(void) const { return get3bytes(Data+7)<<8; } // [cyclic]
void setLongitude(int32_t Lon) { set3bytes(Data+7, (Lon>>8)&0xFFFFFF); }
static int32_t CordicOGN(int32_t Coord) { return ((int64_t)Coord*83399993+(1<<21))>>22; } // [1/60000deg] => [cordic]
void setLatOGN(int32_t Lat) { setLatitude (CordicOGN(Lat)); } // [1/60000deg]
void setLonOGN(int32_t Lon) { setLongitude(CordicOGN(Lon)); } // [1/60000deg]
int32_t getAltitude(void) const { int32_t Alt=Data[10]; Alt=(Alt<<4) | (Data[11]>>4); return Alt*25-1000; } // [feet]
void setAltitude(int32_t Alt) // [feet]
{ Alt = (Alt+1000+12)/25;
if(Alt<0) Alt=0; else if(Alt>0xFFF) Alt=0xFFF;
Data[10] = Alt>>4; Alt&=0x00F; Data[11] = (Data[11]&0x0F) | (Alt<<4); }
uint8_t getMiscInd(void) const { return Data[11]&0x0F; } // Airborne | Extrapolated | TT: 00=not valid, 01=true track, 10=magnetic, 11=heading
void setMiscInd(uint8_t MiscInd) { Data[11] = (Data[11]&0xF0) | (MiscInd&0x0F); }
uint8_t getNIC(void) const { return Data[12]>>4; } // containment radius: 9=75m, 10=25m, 11=7.5m
uint8_t getNACp(void) const { return Data[12]&0x0F; } // est. pos. uncertainty: 9=30m, 10=10m, 11=3m
void setAccuracy(uint8_t NIC, uint8_t NACp) { Data[12] = (NIC<<4) | (NACp&0x0F); }
uint16_t getSpeed(void) const { uint16_t Speed=Data[13]; Speed=(Speed<<4) | (Data[14]>>4); return Speed; } // [knot]
void setSpeed(uint16_t Speed) { if(Speed>0xFFE) Speed=0xFFE; Data[13] = Speed>>4; Data[14] = (Data[14]&0x0F) | (Speed<<4); } // [knot]
void clrSpeed(void) { Data[13] = 0xFF; Data[14] = (Data[14]&0x0F) | 0xF0; } // Speed = invalid
int32_t getClimbRate(void) const
{ int16_t Climb=Data[14]&0x0F; Climb=(Climb<<8)|Data[15]; Climb<<=4; return (int32_t)Climb*4; } // [fpm]
void setClimbRate(int32_t Climb) // [fpm]
{ Climb = (Climb+32)>>6; if(Climb<(-510)) Climb=(-510); else if(Climb>510) Climb=510; // full 12-bit range is not being used
Data[15] = Climb&0xFF; Data[14] = (Data[14]&0xF0) | ((Climb>>8)&0x0F); }
void clrClimbRate(void) { Data[15]=0x00; Data[14] = (Data[14]&0xF0) | 0x08; } // set vertical rate = not available
uint8_t getHeading(void) const { return Data[16]; } // [cyclic]
void setHeading(uint8_t Heading) { Data[16]=Heading; } // [cyclic]
uint8_t getAcftCat(void) const { return Data[17]; } // 1=light, 2=small, 3=large, 4=high vortex, 5=heavy, 6=high-G, 7=rotor, 9=glider, 10=airship, 11=parachute, 12=ULM/para/hang, 14=UAV, 15=space, 17=surf, 18=service
void setAcftCat(uint8_t Cat) { Data[17]=Cat; }
void setAcftType(uint8_t AcftType) // set OGN-type aricrraft-type
{ // 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, A, B, C, D, E, F
const uint8_t OGNtype[16] = { 0, 9, 1, 7, 11, 1, 12, 12, 1, 3,15,10,10,14,18,19 } ; // conversion table from OGN aricraft-type
setAcftCat(OGNtype[AcftType&0x0F]); }
const char *getAcftCall(void) const { return (const char *)(Data+18); } // is not null-terminated
void setAcftCall(const char *Call)
{ int Idx=0;
for( ; Idx<8; Idx++)
{ char ch=Call[Idx]; if(ch==0) break;
Data[18+Idx]=ch; }
for( ; Idx<8; Idx++)
Data[18+Idx]=' ';
}
void setAcftCall(uint32_t ID)
{ const char *AddrName[4] = { "RN", "IC", "FL", "OG" } ;
char *Call = (char *)(Data+18);
const char *Name = AddrName[(ID>>24)&3]; Call[0]=Name[0]; Call[1]=Name[1];
Format_Hex(Call+2, (uint8_t)((ID>>16)&0xFF)); Format_Hex(Call+4, (uint16_t)(ID&0xFFFF)); }
uint8_t getPriority(void) const { return Data[26]>>4; } // 1=general, 2=medical, 3=fuel, 4=comms, 5=interference, 6=downed
void setPriority(uint8_t Prior) { Data[26] = (Data[26]&0x0F) | (Prior<<4); }
int Send(void (*Output)(char), uint8_t ID=10) const { return GDL90_Send(Output, ID, Data, Size); }
void Print(void) const
{ printf("%X:%06X %02X/%8s %X/%X %dft %+dfpm [%+09.5f,%+010.5f] %03.0f/%dkt\n",
getAddrType(), getAddress(), getAcftCat(), getAcftCall(),
getNIC(), getNACp(), getAltitude(), getClimbRate(),
(90.0/0x40000000)*getLatitude(), (90.0/0x40000000)*getLongitude(),
(360.0/256)*getHeading(), getSpeed()); }
} ;
// =================================================================================
class GDL90_RxMsg // receiver for the MAV messages
{ public:
static const uint8_t MaxBytes = 32; // max. number of bytes
static const uint8_t SYNC = 0x7E; // GDL90 sync byte
static const uint8_t ESC = 0x7D; // GDL90 escape byte
uint8_t Byte[MaxBytes];
uint8_t Len;
public:
void Clear(void) { Len=0; Byte[Len]=0; }
void Print(void)
{ printf("GDL90[%d] ", Len);
for(int Idx=0; Idx<Len; Idx++)
printf("%02X", Byte[Idx]);
printf("\n"); }
uint8_t ProcessByte(uint8_t RxByte) // process a single byte: add to the message or reject
{ // printf("Process[%2d] 0x%02X\n", Len, RxByte);
if(Len==0) // if the very first byte
{ if(RxByte==SYNC) { Byte[Len]=RxByte; return 1; } // is SYNC then store it
else if(Byte[Len]==SYNC)
{ Byte[Len]=RxByte; if(RxByte==ESC) return 1;
Len++; Byte[Len]=0; return 1; }
else if(Byte[Len]==ESC)
{ RxByte^=0x20; Byte[Len++]=RxByte; Byte[Len]=0; return 1; }
else return 0; }
if(RxByte==SYNC) // if not the very first byte and SYNC then the packet is possibly complete
{ if(Len<3 || Byte[Len]!=0) { Clear(); return 0; }
uint16_t CRC=0;
for(int Idx=0; Idx<(Len-2); Idx++)
{ CRC=GDL90_CRC16(Byte[Idx], CRC); }
if( (CRC&0xFF)!=Byte[Len-2] || (CRC>>8)!=Byte[Len-1] ) { Clear(); return 0; }
return 2; }
if(Byte[Len]==ESC) { RxByte^=0x20; } // if after an ESC then xor with 0x20
Byte[Len]=RxByte; if(RxByte==ESC) return 1;
Len++; // advance
if(Len>=MaxBytes) { Clear(); return 0; }
Byte[Len]=0; return 1; }
} ;
// =================================================================================
#endif // __GDL90_H__

31
main/ogn.h
Wyświetl plik

@ -26,6 +26,8 @@
#include "ogn1.h" // OGN v1
#include "ogn2.h" // OGN v2
#include "fanet.h"
#include "gdl90.h"
#include "atmosphere.h"
@ -1097,6 +1099,35 @@ class GPS_Position
Temperature = MAV->temperature/10;
hasBaro=1; }
int32_t getCordicLatitude (void) const { return ((int64_t)Latitude *83399993+(1<<21))>>22; }
int32_t getCordicLongitude(void) const { return ((int64_t)Longitude*83399993+(1<<21))>>22; }
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); }
void Encode(GDL90_REPORT &Report)
{ Report.setAccuracy(9, 9);
int32_t Lat = getCordicLatitude(); // Latitude: [0.0001/60deg] => [cordic]
int32_t Lon = getCordicLongitude(); // Longitude: [0.0001/60deg] => [cordic]
int32_t Alt = Altitude; // [0.1m]
if(hasBaro) Alt = StdAltitude;
Alt=MetersToFeet(Alt); Alt=(Alt+5)/10; // [feet]
Report.setLatitude(Lat);
Report.setLongitude(Lon);
Report.setAltitude(Alt);
uint16_t HeadAngle = ((int32_t)Heading<<12)/225; // [16-bit cordic] heading angle
int32_t SpeedKts = (3981*(int32_t)Speed+1024)>>11; // [0.1m/s] => [0.1kts]
Report.setHeading((HeadAngle+0x80)>>8); // [8-bit cordic]
Report.setMiscInd(0x2); //
Report.setSpeed((SpeedKts+5)/10); // [knot]
Report.setClimbRate(6*MetersToFeet(ClimbRate));
}
template <class OGNx_Packet>
void Encode(OGNx_Packet &Packet) const
{ Packet.Position.FixQuality = FixQuality<3 ? FixQuality:3; //

61
main/proc.cpp
Wyświetl plik

@ -21,6 +21,12 @@
#include "sound.h"
#endif
#ifdef WITH_GDL90
#include "gdl90.h"
GDL90_HEARTBEAT GDL_HEARTBEAT;
GDL90_REPORT GDL_REPORT;
#endif
#ifdef WITH_LOOKOUT // traffic awareness and warnings
#include "lookout.h"
LookOut Look;
@ -292,6 +298,7 @@ static void ReadStatus(OGN_Packet &Packet)
// static void ReadStatus(OGN_TxPacket<OGN_Packet> &StatPacket)
// { ReadStatus(StatPacket.Packet); }
#ifndef WITH_LOOKOUT // with LookOut the PFLAU is produced inside LookOut
static uint8_t WritePFLAU(char *NMEA, uint8_t GPS=1) // produce the (mostly dummy) PFLAU to satisfy XCsoar and LK8000
{ uint8_t Len=0;
Len+=Format_String(NMEA+Len, "$PFLAU,");
@ -312,6 +319,7 @@ static uint8_t WritePFLAU(char *NMEA, uint8_t GPS=1) // produce the (mostly d
Len+=NMEA_AppendCheckCRNL(NMEA, Len);
NMEA[Len]=0;
return Len; }
#endif
// ---------------------------------------------------------------------------------------------------------------------------------------
@ -339,6 +347,13 @@ static void ProcessRxPacket(OGN_RxPacket<OGN_Packet> *RxPacket, uint8_t RxPacket
#ifdef WITH_LOOKOUT
const LookOut_Target *Tgt=Look.ProcessTarget(RxPacket->Packet); // process the received target postion
if(Tgt) Warn=Tgt->WarnLevel; // remember warning level of this target
#ifdef WITH_GDL90
if(Tgt)
{ Look.Write(GDL_REPORT, Tgt); // produce GDL90 report for this target
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
GDL_REPORT.Send(CONS_UART_Write, 20); // transmit as traffic position report (not own-ship)
xSemaphoreGive(CONS_Mutex); }
#endif
#ifdef WITH_BEEPER
if(KNOB_Tick>12) Play(Play_Vol_1 | Play_Oct_2 | (7+2*Warn), 3+16*Warn);
#endif
@ -467,6 +482,7 @@ void vTaskPROC(void* pvParameters)
#else
GPS_Position *Position = GPS_getPosition(BestIdx, BestResid, SlotTime%60, 0); // get GPS position which isReady
#endif
// GPS_Position *Position = GPS_getPosition();
#ifdef DEBUG_PRINT
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
Format_String(CONS_UART_Write, "getPos(");
@ -478,7 +494,27 @@ void vTaskPROC(void* pvParameters)
Format_String(CONS_UART_Write, "s\n");
xSemaphoreGive(CONS_Mutex);
#endif
// GPS_Position *Position = GPS_getPosition();
#ifdef WITH_GDL90
GDL_HEARTBEAT.Clear();
GDL_HEARTBEAT.Initialized=1;
if(Position)
{ if(Position->isTimeValid())
{ GDL_HEARTBEAT.UTCvalid=1;
GDL_HEARTBEAT.setTimeStamp(SlotTime);
if(Position->isValid()) GDL_HEARTBEAT.PosValid = 1; }
}
GDL_REPORT.Clear();
GDL_REPORT.setAddress(Parameters.Address);
GDL_REPORT.setAddrType(Parameters.AddrType!=1);
GDL_REPORT.setAcftType(Parameters.AcftType);
if(Parameters.Reg[0]) GDL_REPORT.setAcftCall(Parameters.Reg);
// else GDL_REPORT.setAcftCall();
if(Position && Position->isValid()) Position->Encode(GDL_REPORT);
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
GDL_HEARTBEAT.Send(CONS_UART_Write);
GDL_REPORT.Send(CONS_UART_Write);
xSemaphoreGive(CONS_Mutex);
#endif
if(Position) Position->EncodeStatus(StatPacket.Packet); // encode GPS altitude and pressure/temperature/humidity
else { StatPacket.Packet.Status.FixQuality=0; StatPacket.Packet.Status.Satellites=0; } // or lack of the GPS lock
{ uint8_t SatSNR = (GPS_SatSNR+2)/4;
@ -521,6 +557,7 @@ void vTaskPROC(void* pvParameters)
#endif
OGN_TxPacket<OGN_Packet> *TxPacket = RF_TxFIFO.getWrite();
TxPacket->Packet = PosPacket.Packet; // copy the position packet to the TxFIFO
#ifdef WITH_ENCRYPT
if(Parameters.Encrypt) TxPacket->Packet.Encrypt(Parameters.EncryptKey); // if encryption is requested then encrypt
else TxPacket->Packet.Whiten(); // otherwise only whiten
@ -542,6 +579,13 @@ void vTaskPROC(void* pvParameters)
if( (AverSpeed>10) || ((RX_Random&0x3)==0) ) // send only some positions if the speed is less than 1m/s
RF_TxFIFO.Write(); // complete the write into the TxFIFO
Position->Sent=1;
#ifdef WITH_FANET
if( (SlotTime&0x07)==4 ) // every 8sec
{ FANET_Packet *FNTpkt = FNT_TxFIFO.getWrite();
FNTpkt->setAddress(Parameters.Address);
Position->EncodeAirPos(*FNTpkt, Parameters.AcftType, !Parameters.Stealth);
FNT_TxFIFO.Write(); }
#endif
#ifdef WITH_LOOKOUT
const LookOut_Target *Tgt=Look.ProcessOwn(PosPacket.Packet); // process own position, get the most dangerous target
#ifdef WITH_PFLAA
@ -583,6 +627,12 @@ void vTaskPROC(void* pvParameters)
Sound_TrafficWarn(Tgt);
#endif
}
#else // WITH_LOOKOUT
if(Parameters.Verbose)
{ uint8_t Len=Look.WritePFLAU(Line); // $PFLAU, overall status
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
Format_String(CONS_UART_Write, Line, 0, Len);
xSemaphoreGive(CONS_Mutex); }
#endif // WITH_LOOKOUT
#ifdef WITH_FLASHLOG
bool Written=FlashLog_Process(PosPacket.Packet, PosTime);
@ -624,6 +674,15 @@ void vTaskPROC(void* pvParameters)
Format_String(CONS_UART_Write, Line);
xSemaphoreGive(CONS_Mutex);
#endif
#ifdef WITH_FANET
if(Parameters.Pilot[0] && (SlotTime&0xFF)==0 ) // every 256sec
{ FANET_Packet *FNTpkt = FNT_TxFIFO.getWrite();
FNTpkt->setAddress(Parameters.Address);
FNTpkt->setName(Parameters.Pilot);
FNT_TxFIFO.Write(); }
#endif
StatPacket.Packet.HeaderWord=0;
StatPacket.Packet.Header.Address = Parameters.Address; // set address
StatPacket.Packet.Header.AddrType = Parameters.AddrType; // address-type

66
main/rf.cpp
Wyświetl plik

@ -30,6 +30,11 @@ static uint32_t RF_SlotTime; // [sec] UTC time which belongs to t
FIFO<RFM_RxPktData, 16> RF_RxFIFO; // buffer for received packets
FIFO<OGN_TxPacket<OGN_Packet>, 4> RF_TxFIFO; // buffer for transmitted packets
#ifdef WITH_FANET
FIFO<FANET_RxPacket, 8> FNT_RxFIFO;
FIFO<FANET_Packet, 4> FNT_TxFIFO;
#endif
uint16_t TX_Credit =0; // counts transmitted packets vs. time to avoid using more than 1% of the time
uint8_t RX_OGN_Packets=0; // [packets] counts received packets
@ -76,7 +81,7 @@ static uint8_t ReceivePacket(void) // see if a pack
RxPkt->msTime = TimeSync_msTime(); if(RxPkt->msTime<200) RxPkt->msTime+=1000;
RxPkt->Channel = RX_Channel; // store reception channel
RxPkt->RSSI = RxRSSI; // store signal strength
TRX.ReadPacket(RxPkt->Data, RxPkt->Err); // get the packet data from the FIFO
TRX.ReadPacketOGN(RxPkt->Data, RxPkt->Err); // get the packet data from the FIFO
// PktData.Print(); // for debug
RF_RxFIFO.Write(); // complete the write to the receiver FIFO
@ -120,7 +125,7 @@ static uint8_t Transmit(uint8_t TxChan, const uint8_t *PacketByte, uint8_t Thres
SetTxChannel(TxChan);
TRX.ClearIrqFlags();
TRX.WritePacket(PacketByte); // write packet into FIFO
TRX.WritePacketOGN(PacketByte); // write packet into FIFO
TRX.WriteMode(RF_OPMODE_TRANSMITTER); // transmit
vTaskDelay(5); // wait 5ms
uint8_t Break=0;
@ -169,10 +174,10 @@ static uint8_t StartRFchip(void)
TRX.PrintReg(CONS_UART_Write);
xSemaphoreGive(CONS_Mutex);
#endif
#ifdef WITH_RFM95
TRX.WriteDefaultReg();
#endif
TRX.Configure(0, OGN_SYNC); // setup RF chip parameters and set to channel #0
// #ifdef WITH_RFM95
// TRX.WriteDefaultReg();
// #endif
TRX.ConfigureOGN(0, OGN_SYNC); // setup RF chip parameters and set to channel #0
TRX.WriteMode(RF_OPMODE_STANDBY); // set RF chip mode to STANDBY
uint8_t Version = TRX.ReadVersion();
#ifdef DEBUG_PRINT
@ -196,6 +201,10 @@ extern "C"
{
RF_RxFIFO.Clear(); // clear receive/transmit packet FIFO's
RF_TxFIFO.Clear();
#ifdef WITH_FANET
FNT_RxFIFO.Clear();
FNT_TxFIFO.Clear();
#endif
#ifdef USE_BLOCK_SPI
TRX.TransferBlock = RFM_TransferBlock;
@ -314,8 +323,8 @@ extern "C"
const uint8_t *TxPktData0=0;
const uint8_t *TxPktData1=0;
const OGN_TxPacket<OGN_Packet> *TxPkt0 = RF_TxFIFO.getRead(0); // get 1st packet from TxFIFO
const OGN_TxPacket<OGN_Packet> *TxPkt1 = RF_TxFIFO.getRead(1); // get 2nd packet from TxFIFO
const OGN_TxPacket<OGN_Packet> *TxPkt0 = RF_TxFIFO.getRead(0); // get 1st packet from TxFIFO
const OGN_TxPacket<OGN_Packet> *TxPkt1 = RF_TxFIFO.getRead(1); // get 2nd packet from TxFIFO
if(TxPkt0) TxPktData0=TxPkt0->Byte(); // if 1st is not NULL then get its data
if(TxPkt1) TxPktData1=TxPkt1->Byte(); // if 2nd if not NULL then get its data
else TxPktData1=TxPktData0; // but if NULL then take copy of the 1st packet
@ -330,6 +339,47 @@ extern "C"
TxChan = RF_FreqPlan.getChannel(RF_SlotTime, 1, 1); // transmit channel
RX_Channel = TxChan;
#ifdef WITH_FANET
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
TRX.ConfigureFNT(); // configure for FANET
TRX.WriteMode(RF_OPMODE_LORA_RX_CONT);
vTaskDelay(2);
for(uint8_t Wait=50; Wait; Wait--)
{ vTaskDelay(1);
uint8_t Stat = TRX.ReadByte(REG_LORA_MODEM_STATUS);
if((Stat&0x0B)==0) break; }
TRX.WriteMode(RF_OPMODE_LORA_STANDBY);
TRX.SendPacketFNT(FNTpkt->Byte, FNTpkt->Len);
FNT_TxFIFO.Read(); // remove the last packet from the queue
/*
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
Format_String(CONS_UART_Write, "TRX: ");
Format_Hex(CONS_UART_Write, TRX.ReadMode());
CONS_UART_Write(':');
Format_Hex(CONS_UART_Write, TRX.ReadByte(REG_LORA_IRQ_FLAGS));
CONS_UART_Write(' ');
Format_Hex(CONS_UART_Write, TRX.ReadByte(REG_LORA_PREAMBLE_LSB));
CONS_UART_Write(':');
Format_Hex(CONS_UART_Write, TRX.ReadByte(REG_LORA_SYNC));
CONS_UART_Write(' ');
Format_Hex(CONS_UART_Write, TRX.ReadByte(REG_LORA_MODEM_CONFIG1));
CONS_UART_Write(':');
Format_Hex(CONS_UART_Write, TRX.ReadByte(REG_LORA_MODEM_CONFIG2));
Format_String(CONS_UART_Write, "\n");
xSemaphoreGive(CONS_Mutex);
*/
vTaskDelay(8);
for( uint8_t Wait=50; Wait; Wait--)
{ vTaskDelay(1);
uint8_t Mode=TRX.ReadMode();
if(Mode!=RF_OPMODE_LORA_TX) break; }
TRX.SetFSK();
TRX.ConfigureOGN(0, OGN_SYNC);
}
#endif
SetRxChannel();
TRX.WriteMode(RF_OPMODE_RECEIVER); // switch to receive mode

9
main/rf.h
Wyświetl plik

@ -12,9 +12,18 @@
#include "fifo.h"
#include "freqplan.h"
#ifdef WITH_FANET
#include "fanet.h"
#endif
extern FIFO<RFM_RxPktData, 16> RF_RxFIFO; // buffer for received packets
extern FIFO<OGN_TxPacket<OGN_Packet>, 4> RF_TxFIFO; // buffer for transmitted packets
#ifdef WITH_FANET
extern FIFO<FANET_RxPacket, 8> FNT_RxFIFO;
extern FIFO<FANET_Packet, 4> FNT_TxFIFO;
#endif
extern uint8_t RX_OGN_Packets; // [packets] counts received packets
// extern uint8_t RX_AverRSSI; // [-0.5dBm] average RSSI
// extern int8_t RF_Temp; // [degC] temperature of the RF chip: uncalibrated

128
main/rfm.h
Wyświetl plik

@ -5,7 +5,7 @@
// -----------------------------------------------------------------------------------------------------------------------
#include "config.h"
// #include "config.h"
#include "ogn.h"
class RFM_RxPktData // packet received by the RF chip
@ -226,11 +226,11 @@ class RFM_TRX
static uint16_t SwapBytes(uint16_t Word) { return (Word>>8) | (Word<<8); }
uint8_t WriteByte(uint8_t Byte, uint8_t Addr=0) // write Byte
{ // printf("WriteByte(0x%02X, 0x%02X)\n", Byte, Addr);
{ // printf("WriteByte(0x%02X => [0x%02X])\n", Byte, Addr);
uint8_t *Ret = Block_Write(&Byte, 1, Addr); return *Ret; }
void WriteWord(uint16_t Word, uint8_t Addr=0) // write Word => two bytes
{ // printf("WriteWord(0x%04X, 0x%02X)\n", Word, Addr);
{ // printf("WriteWord(0x%04X => [0x%02X])\n", Word, Addr);
uint16_t Swapped = SwapBytes(Word); Block_Write((uint8_t *)&Swapped, 2, Addr); }
uint8_t ReadByte (uint8_t Addr=0)
@ -255,7 +255,10 @@ class RFM_TRX
Buff[3] = 0;
Block_Write(Buff, 3, Addr); }
void WritePacket(const uint8_t *Data, uint8_t Len=26) // write the packet data (26 bytes)
void WriteFIFO(const uint8_t *Data, uint8_t Len)
{ Block_Write(Data, Len, REG_FIFO); }
void WritePacketOGN(const uint8_t *Data, uint8_t Len=26) // write the packet data (26 bytes)
{ uint8_t Packet[2*Len];
uint8_t PktIdx=0;
for(uint8_t Idx=0; Idx<Len; Idx++)
@ -266,7 +269,10 @@ class RFM_TRX
Block_Write(Packet, 2*Len, REG_FIFO);
}
void ReadPacket(uint8_t *Data, uint8_t *Err, uint8_t Len=26) // read packet data from FIFO
uint8_t *ReadFIFO(uint8_t Len)
{ return Block_Read(Len, REG_FIFO); }
void ReadPacketOGN(uint8_t *Data, uint8_t *Err, uint8_t Len=26) // read packet data from FIFO
{ uint8_t *Packet = Block_Read(2*Len, REG_FIFO); // read 2x26 bytes from the RF chip RxFIFO
uint8_t PktIdx=0;
for(uint8_t Idx=0; Idx<Len; Idx++) // loop over packet bytes
@ -330,7 +336,15 @@ class RFM_TRX
Deselect();
return Old; } // return the previously set frequency
void WritePacket(const uint8_t *Data, uint8_t Len=26) const // write the packet data (26 bytes)
void WriteFIFO(const uint8_t *Data, uint8_t Len)
{ const uint8_t Addr=REG_FIFO; // write to FIFO
Select();
TransferByte(Addr | 0x80);
for(uint8_t Idx=0; Idx<Len; Idx++)
TransferByte(Data[Idx]);
Deselect(); }
void WritePacketOGN(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);
@ -339,10 +353,9 @@ class RFM_TRX
TransferByte(ManchesterEncode[Byte>>4]); // software manchester encode every byte
TransferByte(ManchesterEncode[Byte&0x0F]);
}
Deselect();
}
Deselect(); }
void ReadPacket(uint8_t *Data, uint8_t *Err, uint8_t Len=26) const // read packet data from FIFO
void ReadPacketOGN(uint8_t *Data, uint8_t *Err, uint8_t Len=26) const // read packet data from FIFO
{ const uint8_t Addr=REG_FIFO;
Select(); // select the RF chip: start SPI transfer
TransferByte(Addr); // trasnfer the address/read: FIFO
@ -356,8 +369,7 @@ class RFM_TRX
Data[Idx]=(ByteH<<4) | ByteL;
Err [Idx]=(ErrH <<4) | ErrL ;
}
Deselect(); // de-select RF chip: end of SPI transfer
}
Deselect(); } // de-select RF chip: end of SPI transfer
#endif // USE_BLOCK_SPI
@ -423,7 +435,7 @@ class RFM_TRX
void WriteTxPowerMin(void) { WriteTxPower_W(-18); } // set minimal Tx power and setup for reception
int Configure(int16_t Channel, const uint8_t *Sync, bool PW=0)
int ConfigureOGN(int16_t Channel, const uint8_t *Sync, bool PW=0)
{ WriteMode(RF_OPMODE_STANDBY); // mode = STDBY
ClearIrqFlags();
WriteByte( 0x02, REG_DATAMODUL); // [0x00] Packet mode, FSK, 0x02: BT=0.5, 0x01: BT=1.0, 0x03: BT=0.3
@ -450,42 +462,6 @@ class RFM_TRX
WriteByte( 0x00, REG_AFCCTRL); // [0x00] 0x20 = AfcLowBetaOn=1 -> page 64 -> page 33
WriteByte( +10, REG_TESTAFC); // [0x00] [488Hz] if AfcLowBetaOn
return 0; }
void PrintReg(void (*CONS_UART_Write)(char))
{ Format_String(CONS_UART_Write, "RFM69 Mode:");
uint8_t RxMode=ReadMode();
Format_Hex(CONS_UART_Write, RxMode);
CONS_UART_Write(' '); CONS_UART_Write('0'+DIO0_isOn());
Format_String(CONS_UART_Write, " IRQ:");
Format_Hex(CONS_UART_Write, ReadWord(REG_IRQFLAGS1));
Format_String(CONS_UART_Write, " Pre:");
Format_Hex(CONS_UART_Write, ReadWord(REG_PREAMBLEMSB));
Format_String(CONS_UART_Write, " SYNC:");
Format_Hex(CONS_UART_Write, ReadByte(REG_SYNCCONFIG));
CONS_UART_Write('/');
for(uint8_t Idx=0; Idx<8; Idx++)
Format_Hex(CONS_UART_Write, ReadByte(REG_SYNCVALUE1+Idx));
Format_String(CONS_UART_Write, " FREQ:");
Format_Hex(CONS_UART_Write, ReadByte(REG_FRFMSB));
Format_Hex(CONS_UART_Write, ReadByte(REG_FRFMID));
Format_Hex(CONS_UART_Write, ReadByte(REG_FRFLSB));
Format_String(CONS_UART_Write, " RATE:");
Format_Hex(CONS_UART_Write, ReadWord(REG_BITRATEMSB));
Format_String(CONS_UART_Write, " FDEV:");
Format_Hex(CONS_UART_Write, ReadWord(REG_FDEVMSB));
Format_String(CONS_UART_Write, " DIO:");
Format_Hex(CONS_UART_Write, ReadWord(REG_DIOMAPPING1));
Format_String(CONS_UART_Write, " CFG:");
Format_Hex(CONS_UART_Write, ReadByte(REG_PACKETCONFIG1));
Format_Hex(CONS_UART_Write, ReadByte(REG_PACKETCONFIG2));
Format_Hex(CONS_UART_Write, ReadByte(REG_FIFOTHRESH));
Format_Hex(CONS_UART_Write, ReadByte(REG_PAYLOADLENGTH));
Format_Hex(CONS_UART_Write, ReadByte(REG_RXBW));
Format_Hex(CONS_UART_Write, ReadByte(REG_RSSICONFIG));
Format_String(CONS_UART_Write, " PA:");
Format_Hex(CONS_UART_Write, ReadByte(REG_PARAMP));
Format_Hex(CONS_UART_Write, ReadByte(REG_PALEVEL));
Format_String(CONS_UART_Write, "\n"); }
#endif
// #ifdef WITH_RFM95
@ -516,13 +492,63 @@ class RFM_TRX
void WriteTxPowerMin(void) { WriteTxPower(0); }
int Configure(int16_t Channel, const uint8_t *Sync, bool PW=0)
{ WriteMode(RF_OPMODE_STANDBY); // mode: STDBY, modulation: FSK, no LoRa
int SetLoRa(void) // switch to LoRa: has to go througth the SLEEP mode
{ WriteMode(RF_OPMODE_LORA_SLEEP);
WriteMode(RF_OPMODE_LORA_SLEEP);
return 0; }
int SetFSK(void) // switch to FSK: has to go through the SLEEP mode
{ WriteMode(RF_OPMODE_SLEEP);
WriteMode(RF_OPMODE_SLEEP);
return 0; }
int ConfigureFNT(uint8_t CR=1) // configure for FANET/LoRa
{ // WriteMode(RF_OPMODE_LORA_STANDBY);
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
WriteByte(0x80 | (CR<<1), REG_LORA_MODEM_CONFIG1); // 0x88 = 250kHz, 4+4, explicit header
WriteByte(0x74, REG_LORA_MODEM_CONFIG2); // 0x74 = SF7, CRC on
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(0x00, REG_LORA_RX_ADDR);
setChannel(0); // operating channel
return 0; }
int SendPacketFNT(const uint8_t *Data, uint8_t Len)
{ // WriteMode(RF_OPMODE_LORA_STANDBY);
// check if FIFO empty, packets could be received ?
WriteByte(0x00, REG_LORA_FIFO_ADDR); // tell write to FIFO at address 0x00
WriteFIFO(Data, Len); // write the packet data
WriteByte(0x00, REG_LORA_TX_ADDR); // tell packet address in the FIFO
WriteByte(Len, REG_LORA_PACKET_LEN); // tell packet length
WriteMode(RF_OPMODE_LORA_TX); // enter transmission mode
return 0; } // afterwards just wait for TX mode to stop
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 *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]);
return Len; }
int ConfigureOGN(int16_t Channel, const uint8_t *Sync, bool PW=0)
{ // WriteMode(RF_OPMODE_STANDBY); // mode: STDBY, modulation: FSK, no LoRa
// usleep(1000);
WriteTxPower(0);
ClearIrqFlags();
WriteWord(PW?0x0341:0x0140, REG_BITRATEMSB); // bit rate = 100kbps (32MHz/100000) (0x0341 = 38.415kbps)
WriteByte(0x00, REG_BITRATEFRAC); //
WriteByte(0x00, REG_BITRATEFRAC); // one should set exactly 38.400kbps for PW
// ReadWord(REG_BITRATEMSB);
WriteWord(PW?0x013B:0x0333, REG_FDEVMSB); // FSK deviation = +/-50kHz [32MHz/(1<<19)] (0x013B = 19.226kHz)
// ReadWord(REG_FDEVMSB);

105
main/sx1276.h
Wyświetl plik

@ -25,15 +25,15 @@ Maintainer: Miguel Luis and Gregory Cristian
#define REG_OPMODE 0x01
#define REG_BITRATEMSB 0x02
#define REG_BITRATELSB 0x03
#define REG_FDEVMSB 0x04
#define REG_FDEVMSB 0x04
#define REG_FDEVLSB 0x05
#define REG_FRFMSB 0x06
#define REG_FRFMSB 0x06 // Operational frequency: same for LoRa and FSK
#define REG_FRFMID 0x07
#define REG_FRFLSB 0x08
// Tx settings
#define REG_PACONFIG 0x09
#define REG_PACONFIG 0x09 // Same for LoRa and FSK
#define REG_PARAMP 0x0A
#define REG_OCP 0x0B
#define REG_OCP 0x0B
// Rx settings
#define REG_LNA 0x0C
#define REG_RXCONFIG 0x0D
@ -41,7 +41,7 @@ Maintainer: Miguel Luis and Gregory Cristian
#define REG_RSSICOLLISION 0x0F
#define REG_RSSITHRESH 0x10
#define REG_RSSIVALUE 0x11
#define REG_RXBW 0x12
#define REG_RXBW 0x12
#define REG_AFCBW 0x13
#define REG_OOKPEAK 0x14
#define REG_OOKFIX 0x15
@ -109,19 +109,46 @@ Maintainer: Miguel Luis and Gregory Cristian
#define REG_AGCTHRESH3 0x64
#define REG_PLL 0x70
// LoRa registers
#define REG_LORA_FIFO_ADDR 0x0D // write into this register the pointer to read/write from the FIFO
#define REG_LORA_TX_ADDR 0x0E // base address in FIFO for transmitted packets (normally 0)
#define REG_LORA_RX_ADDR 0x0F // base address in FIFO for received packets (normally 0)
#define REG_LORA_PACKET_ADDR 0x10 // address in FIFO of the new packet received ?
#define REG_LORA_IRQ_MASK 0x11 //
#define REG_LORA_IRQ_FLAGS 0x12 // 7=RxTimeout, 6=RxDone, 5=CRCerror, 4=ValidHeader, 3=TxDone, 2=CADdone, 1=FHSSchange, 0=CAD det.
#define REG_LORA_PACKET_BYTES 0x13 // number of bytes of the new packet received
#define REG_LORA_MODEM_STATUS 0x18 // RRRchxsd RRR=coding rate, c=modem clear, h=header valid, r=RX ongoing, s=signal synced, d=signal detected
#define REG_LORA_PACKET_SNR 0x19 // SNR of the last packet [0.25dB]
#define REG_LORA_PACKET_RSSI 0x1A // RSSI of the latest packet [dBm] = Value-137
#define REG_LORA_RSSI 0x1B // RSSI of the channel [dBm] = Value-137
#define REG_LORA_HOP_CHANNEL 0x1C // TCcccccc T=Timeout, C=CRC on (for a received packet), cccccc=hop channel
#define REG_LORA_MODEM_CONFIG1 0x1D // BBBBRRRI BBBB=1000(250kHz), RRR=001(4+1) =100(4+4) I=0(explicit header)
#define REG_LORA_MODEM_CONFIG2 0x1E // SSSSTCtt SSSS=SF, T=TxCont, C=CRCenable, tt=symbol timeout MSB
#define REG_LORA_SYMBOL_TIMEOUT 0x1F // symbol timeout LSB
#define REG_LORA_PREAMBLE_MSB 0x20 // preamble length MSB
#define REG_LORA_PREAMBLE_LSB 0x21 // preamble length LSB
#define REG_LORA_PACKET_LEN 0x22 // [bytes] for TX or for RX when implicit header mode
#define REG_LORA_PACKET_MAXLEN 0x23 // [bytes] to discard bad packets
#define REG_LORA_HOPPING_PERIOD 0x24 // 0
#define REG_LORA_RX_BYTE_ADDR 0x25 //
#define REG_LORA_MODEM_CONFIG3 0x26 // 0000MA00 M=Mobile/Static, A=LNA AGC
#define REG_LORA_FREQ_ERR_MSB 0x28 // freq. error estimate
#define REG_LORA_FREQ_ERR_MID 0x29
#define REG_LORA_FREQ_ERR_LSB 0x2A
#define REG_LORA_DETECT_OPTIMIZE 0x31 //
#define REG_LORA_INVERT_IQ 0x33
#define REG_LORA_DETECT_THRESHOLD 0x37 // for SF6
#define REG_LORA_SYNC 0x39 // 0xF1 for FANET, default = 0x12 (for old FANET)
/*!
* ============================================================================
* SX1276 FSK bits control definition
* ============================================================================
*/
/*!
* RegFifo
*/
// RegFifo
/*!
* RegOpMode
*/
// RegOpMode
#define RF_OPMODE_LONGRANGEMODE_MASK 0x7F
#define RF_OPMODE_LONGRANGEMODE_OFF 0x00
#define RF_OPMODE_LONGRANGEMODE_ON 0x80
@ -137,13 +164,25 @@ Maintainer: Miguel Luis and Gregory Cristian
#define RF_OPMODE_MODULATIONSHAPING_11 0x18
#define RF_OPMODE_MASK 0xF8
#define RF_OPMODE_SLEEP 0x00
#define RF_OPMODE_STANDBY 0x01 // Default
#define RF_OPMODE_SLEEP 0x00 // switch LoRa <-> FSK only in SLEEP mode
#define RF_OPMODE_STANDBY 0x01 //
#define RF_OPMODE_SYNTHESIZER_TX 0x02
#define RF_OPMODE_TRANSMITTER 0x03
#define RF_OPMODE_SYNTHESIZER_RX 0x04
#define RF_OPMODE_RECEIVER 0x05
#define RF_OPMODE_LORA 0x80 // LoRa mode, can change only in SLEEP mode
#define RF_OPMODE_LORA_SLEEP 0x80 // switch LoRa <-> FSK only in SLEEP mode
#define RF_OPMODE_LORA_STANDBY 0x81 //
#define RF_OPMODE_LORA_TX_SYNTH 0x82
#define RF_OPMODE_LORA_TX 0x83
#define RF_OPMODE_LORA_RX_SYNTH 0x84
#define RF_OPMODE_LORA_RX_CONT 0x85 //
#define RF_OPMODE_LORA_RX_SINGLE 0x86 //
#define RF_OPMODE_LORA_CAD 0x87 // probe if another device transmit
/*!
* RegBitRate ( bits/sec )
*/
@ -961,13 +1000,9 @@ Maintainer: Miguel Luis and Gregory Cristian
#define RF_IMAGECAL_TEMPMONITOR_ON 0x00 // Default
#define RF_IMAGECAL_TEMPMONITOR_OFF 0x01
/*!
* RegTemp ( Read Only )
*/
// RegTemp ( Read Only )
/*!
* RegLowBat
*/
// RegLowBat
#define RF_LOWBAT_MASK 0xF7
#define RF_LOWBAT_ON 0x08
#define RF_LOWBAT_OFF 0x00 // Default
@ -982,47 +1017,27 @@ Maintainer: Miguel Luis and Gregory Cristian
#define RF_LOWBAT_TRIM_2116 0x06
#define RF_LOWBAT_TRIM_2185 0x07
/*!
* RegIrqFlags1
*/
// RegIrqFlags1
#define RF_IRQFLAGS1_MODEREADY 0x80
#define RF_IRQFLAGS1_RXREADY 0x40
#define RF_IRQFLAGS1_TXREADY 0x20
#define RF_IRQFLAGS1_PLLLOCK 0x10
#define RF_IRQFLAGS1_RSSI 0x08
#define RF_IRQFLAGS1_TIMEOUT 0x04
#define RF_IRQFLAGS1_PREAMBLEDETECT 0x02
#define RF_IRQFLAGS1_SYNCADDRESSMATCH 0x01
/*!
* RegIrqFlags2
*/
// RegIrqFlags2
#define RF_IRQFLAGS2_FIFOFULL 0x80
#define RF_IRQFLAGS2_FIFOEMPTY 0x40
#define RF_IRQFLAGS2_FIFOLEVEL 0x20
#define RF_IRQFLAGS2_FIFOOVERRUN 0x10
#define RF_IRQFLAGS2_PACKETSENT 0x08
#define RF_IRQFLAGS2_PAYLOADREADY 0x04
#define RF_IRQFLAGS2_CRCOK 0x02
#define RF_IRQFLAGS2_LOWBAT 0x01
/*!
* RegDioMapping1
*/
// RegDioMapping1
#define RF_DIOMAPPING1_DIO0_MASK 0x3F
#define RF_DIOMAPPING1_DIO0_00 0x00 // Default
#define RF_DIOMAPPING1_DIO0_01 0x40
@ -1047,9 +1062,7 @@ Maintainer: Miguel Luis and Gregory Cristian
#define RF_DIOMAPPING1_DIO3_10 0x02
#define RF_DIOMAPPING1_DIO3_11 0x03
/*!
* RegDioMapping2
*/
// RegDioMapping2
#define RF_DIOMAPPING2_DIO4_MASK 0x3F
#define RF_DIOMAPPING2_DIO4_00 0x00 // Default
#define RF_DIOMAPPING2_DIO4_01 0x40
@ -1125,6 +1138,8 @@ Maintainer: Miguel Luis and Gregory Cristian
#define RF_PLL_BANDWIDTH_225 0x80
#define RF_PLL_BANDWIDTH_300 0xC0 // Default
#endif // __SX1276_H__