kopia lustrzana https://github.com/pjalocha/esp32-ogn-tracker
Utilities to read internal flash log as APRS and convert APRS to IGC
Pawel Jalocha
rodzic
f6f50c57f8
commit
3e8b96d40d
|
|
@ -0,0 +1,83 @@
|
|||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "format.h"
|
||||
|
||||
static int APRS2IGC(char *Out, const char *Inp, int GeoidSepar) // convert APRS positon message into IGC B-record
|
||||
{ int Len=0;
|
||||
const char *Msg = strchr(Inp, ':'); if(Msg==0) return 0; // colon: separates header and message
|
||||
Msg++; // where message starts
|
||||
if(Msg[0]!='/' || Msg[7]!='h') return 0;
|
||||
const char *Pos = Msg+8; if(Pos[4]!='.' || Pos[14]!='.') return 0; // where position starts
|
||||
const char *ExtPos = strstr(Pos+18, " !W"); if(ExtPos[5]=='!') ExtPos+=3; else ExtPos=0;
|
||||
Out[Len++]='B'; // B-record
|
||||
memcpy(Out+Len, Msg+1, 6); Len+=6; // copy UTC time
|
||||
memcpy(Out+Len, Pos, 4); Len+=4; // copy DDMM
|
||||
memcpy(Out+Len, Pos+5, 2); Len+=2; // copy fractional MM
|
||||
Out[Len++] = ExtPos?ExtPos[0]:'0'; // extended precision
|
||||
Out[Len++] = Pos[7]; // copy N/S sign
|
||||
memcpy(Out+Len, Pos+9, 5); Len+=5; // copy DDMM
|
||||
memcpy(Out+Len, Pos+15,2); Len+=2; // copy fractional MM
|
||||
Out[Len++] = ExtPos?ExtPos[1]:'0'; // extended precision
|
||||
Out[Len++] = Pos[17]; // copy E/W sign
|
||||
Out[Len++] = 'A';
|
||||
memcpy(Out+Len, " ", 10);
|
||||
const char *FL = strstr(Pos+18, " FL");
|
||||
if(FL) // pressure altitude
|
||||
{ float PressAlt=atof(FL+3); PressAlt*=30.4; int32_t Alt=floor(PressAlt+0.5);
|
||||
if(Alt<0) { Alt = (-Alt); Out[Len] = '-'; Format_UnsDec(Out+Len+1, (uint32_t)Alt, 4); }
|
||||
else { Format_UnsDec(Out+Len, (uint32_t)Alt, 5); }
|
||||
}
|
||||
Len+=5;
|
||||
if(Pos[27]=='A' && Pos[28]=='=') // geometrical altitude
|
||||
{ int32_t Alt=atol(Pos+29); Alt=(Alt*3+5)/10; Alt+=GeoidSepar; // convert to meters and add GeoidSepar for HAE
|
||||
if(Alt<0) { Alt = (-Alt); Out[Len] = '-'; Format_UnsDec(Out+Len+1, (uint32_t)Alt, 4); }
|
||||
else { Format_UnsDec(Out+Len, (uint32_t)Alt, 5); }
|
||||
}
|
||||
Len+=5;
|
||||
Out[Len]=0; return Len; }
|
||||
|
||||
static int Verbose = 1;
|
||||
static int GeoidSepar = 40;
|
||||
|
||||
static FILE *OutFile = 0;
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{ if(argc<3)
|
||||
{ printf("Usage: %s <own-aircraft-APRS-call> <input-file.aprs>\n", argv[0]);
|
||||
return 0; }
|
||||
|
||||
const char *OwnAcft = argv[1]; int OwnAcftLen = strlen(OwnAcft);
|
||||
char OutFileName[32]; strcpy(OutFileName, OwnAcft); strcat(OutFileName, ".IGC");
|
||||
OutFile=fopen(OutFileName, "wt");
|
||||
if(OutFile==0) { printf("Cannot open %s for write\n", OutFileName); return 0; }
|
||||
|
||||
const char *InpFileName = argv[2];
|
||||
|
||||
FILE *InpFile=fopen(InpFileName, "rt");
|
||||
if(InpFile==0) { printf("Cannot open %s for read\n", InpFileName); return 0; }
|
||||
|
||||
char InpLine[256];
|
||||
char OutLine[256];
|
||||
int InpLines=0;
|
||||
int OutLines=0;
|
||||
for( ; ; )
|
||||
{ if(fgets(InpLine, 256, InpFile)==0) break;
|
||||
char *EOL = strchr(InpLine, '\n'); if(EOL==0) break;
|
||||
*EOL = 0;
|
||||
InpLines++;
|
||||
if(memcmp(InpLine, OwnAcft, OwnAcftLen)) continue;
|
||||
int OutLen=APRS2IGC(OutLine, InpLine, GeoidSepar);
|
||||
if(OutLen>0)
|
||||
{ if(Verbose) printf("%s => %s [%d]\n", InpLine, OutLine, OutLen);
|
||||
fprintf(OutFile, "%s\n", OutLine); OutLines++; }
|
||||
}
|
||||
fclose(InpFile);
|
||||
printf("%d lines from %s\n", InpLines, InpFileName);
|
||||
fclose(OutFile);
|
||||
printf("%d lines to %s\n", OutLines, OutFileName);
|
||||
|
||||
return 0; }
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
#include "atmosphere.h"
|
||||
|
||||
// 200, 300, 400, 500, 600, 700, 800, 900, 1000, 1100 // [ hPa]
|
||||
const int32_t Atmosphere::StdAltTable[10] = { 117764, 91652, 71864, 55752, 42070, 30126, 19493, 9886, 1109, -6984 } ; // [0.1 m]
|
||||
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
#ifndef __ATMOSPHERE_H__
|
||||
#define __ATMOSPHERE_H__
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
class Atmosphere
|
||||
{ public:
|
||||
// int32_t Pressure; // [ Pa ]
|
||||
// int32_t Altitude; // [0.1 m ]
|
||||
// int32_t Temperature; // [0.1 degC]
|
||||
|
||||
// altitude vs. pressure // 20000, 30000, 40000, 50000, 60000, 70000, 80000, 90000, 100000, 110000 // [ Pa]
|
||||
static const int32_t StdAltTable[10]; // = { 117764, 91652, 71864, 55752, 42070, 30126, 19493, 9886, 1109, -6984 } ; // [0.1 m]
|
||||
|
||||
public:
|
||||
// dH/dP = -R/g*T => R = 287.04m^2/K/sec^2, g = 9.80655m/s^2
|
||||
|
||||
static int32_t PressureLapseRate(int32_t Pressure, int32_t Temperature=150) // [Pa], [0.1 degC] => [0.0001 m/Pa]
|
||||
{ return -((int32_t)29270*(Temperature+2732)+Pressure/2)/Pressure; }
|
||||
// int32_t PressureLapseRate(void) { return PressureLapseRate(Pressure, Temperature); }
|
||||
|
||||
static const int32_t StdPressureAtSeaLevel = 101325; // [Pa]
|
||||
static const int32_t StdPressureAt11km = 22632; // [Pa]
|
||||
static const int32_t StdPressureAt20km = 5475; // [Pa]
|
||||
static const int32_t StdTemperatureLapseRate = -65; // [0.1 degC/1000m] valid till 11km
|
||||
static const int32_t StdTemperatureAtSeaLevel = 150; // [0.1 degC ]
|
||||
static const int32_t StdTemperatureAt11km = -565; // [0.1 degC ]
|
||||
|
||||
static int32_t StdTemperature(int32_t Altitude) // [0.1 m ] valid till 20km
|
||||
{ if(Altitude>110000) return StdTemperatureAt11km;
|
||||
return StdTemperatureAtSeaLevel+(StdTemperatureLapseRate*Altitude-5000)/10000; }
|
||||
|
||||
static int32_t AltitudeDelta(int32_t PressureDelta, int32_t PressureLapseRate) // [Pa], [0.0001 m/Pa]
|
||||
{ return (PressureDelta*PressureLapseRate)/100; } // [0.01m]
|
||||
|
||||
static int32_t AltitudeDelta(int32_t PressureDelta, int32_t Pressure, int32_t Temperature) // [Pa], [Pa], [0.1degC]
|
||||
{ int32_t PLR=PressureLapseRate(Pressure, Temperature); return AltitudeDelta(PressureDelta, PLR); } // [0.01m]
|
||||
|
||||
static int32_t StdAltitude(int32_t Pressure, int32_t PressStep=100) // [Pa]
|
||||
{ int32_t Idx=(Pressure+5000)/10000; Idx-=2;
|
||||
if(Idx<0) Idx=0; else if(Idx>9) Idx=9;
|
||||
int32_t Press = 10000*(Idx+2);
|
||||
int32_t Altitude = 10*StdAltTable[Idx];
|
||||
for( ; ; )
|
||||
{ int32_t Temp=StdTemperature(Altitude/10);
|
||||
int32_t Delta=Pressure-Press; if(Delta==0) break;
|
||||
if(Delta>PressStep) Delta=PressStep;
|
||||
else if(Delta<(-PressStep)) Delta=(-PressStep);
|
||||
Altitude+=AltitudeDelta(Delta, Press, Temp);
|
||||
Press+=Delta;
|
||||
}
|
||||
return Altitude/10; } // [0.1m]
|
||||
|
||||
#ifdef NO_RTOS
|
||||
static int32_t StdAltitude_float(int32_t Pressure)
|
||||
{ return floor(443300*(1-powf((float)Pressure/(float)101325.0, (float)0.190295))+0.5); }
|
||||
#endif
|
||||
|
||||
|
||||
} ;
|
||||
|
||||
#endif // __ATMOSPHERE_H__
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
#include "bitcount.h"
|
||||
|
||||
#ifndef BITCOUNT_USE_BUILTIN
|
||||
#ifdef BITCOUNT_SAVE_FLASH
|
||||
const uint8_t ByteCount1s[ 16] = { 0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4 } ;
|
||||
#else
|
||||
const uint8_t ByteCount1s[256] = {
|
||||
0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4,
|
||||
1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5,
|
||||
1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5,
|
||||
2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
|
||||
1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5,
|
||||
2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
|
||||
2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
|
||||
3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
|
||||
1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5,
|
||||
2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
|
||||
2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
|
||||
3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
|
||||
2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
|
||||
3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
|
||||
3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
|
||||
4, 5, 5, 6, 5, 6, 6, 7, 5, 6, 6, 7, 6, 7, 7, 8
|
||||
} ;
|
||||
#endif
|
||||
#endif
|
||||
|
||||
int Count1s(const uint8_t *Byte, int Bytes)
|
||||
{ int Count=0;
|
||||
for( ; Bytes>0; Bytes--)
|
||||
{ Count += Count1s(*Byte++); }
|
||||
return Count; }
|
||||
|
||||
|
|
@ -0,0 +1,71 @@
|
|||
// Fast bit counting for (forward) error correction codes
|
||||
// (c) 2003, Pawel Jalocha, Pawel.Jalocha@cern.ch
|
||||
|
||||
#ifndef __BITCOUNT_H__
|
||||
#define __BITCOUNT_H__
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
// #define BITCOUNT_USE_BUILTIN
|
||||
#define BITCOUNT_SAVE_FLASH
|
||||
|
||||
// ==========================================================================
|
||||
// a table for fast bit counting
|
||||
|
||||
#ifdef BITCOUNT_SAVE_FLASH
|
||||
extern const uint8_t ByteCount1s[16];
|
||||
#else
|
||||
extern const uint8_t ByteCount1s[256];
|
||||
#endif
|
||||
|
||||
// ==========================================================================
|
||||
|
||||
#ifdef BITCOUNT_USE_BUILTIN
|
||||
inline uint8_t Count1s(uint8_t Byte) { return __builtin_popcount(Byte); }
|
||||
#else
|
||||
#ifdef BITCOUNT_SAVE_FLASH
|
||||
inline uint8_t Count1s(uint8_t Byte) { return ByteCount1s[Byte&0x0F] + ByteCount1s[Byte>>4]; }
|
||||
#else
|
||||
inline uint8_t Count1s(uint8_t Byte) { return ByteCount1s[Byte]; }
|
||||
#endif
|
||||
#endif
|
||||
|
||||
inline uint8_t Count1s(int8_t Byte) { return Count1s((uint8_t)Byte); }
|
||||
|
||||
#ifdef BITCOUNT_USE_BUILTIN
|
||||
inline uint8_t Count1s(uint16_t Word) { return __builtin_popcount(Word); }
|
||||
#else
|
||||
inline uint8_t Count1s(uint16_t Word)
|
||||
{ return Count1s((uint8_t)Word)
|
||||
+Count1s((uint8_t)(Word>>8)); }
|
||||
#endif
|
||||
|
||||
inline uint8_t Count1s(int16_t Word) { return Count1s((uint16_t)Word); }
|
||||
|
||||
#ifdef BITCOUNT_USE_BUILTIN
|
||||
inline uint8_t Count1s(uint32_t LongWord) { return __builtin_popcountl(LongWord); }
|
||||
#else
|
||||
inline uint8_t Count1s(uint32_t LongWord)
|
||||
{ return Count1s((uint16_t)LongWord)
|
||||
+Count1s((uint16_t)(LongWord>>16)); }
|
||||
#endif
|
||||
|
||||
inline uint8_t Count1s(int32_t LongWord) { return Count1s((uint32_t)LongWord); }
|
||||
|
||||
#ifdef BITCOUNT_USE_BUILTIN
|
||||
inline uint8_t Count1s(uint64_t LongWord) { return __builtin_popcountll(LongWord); }
|
||||
#else
|
||||
inline uint8_t Count1s(uint64_t LongWord)
|
||||
{ return Count1s((uint32_t)LongWord)
|
||||
+Count1s((uint32_t)(LongWord>>32)); }
|
||||
#endif
|
||||
|
||||
inline uint8_t Count1s(int64_t LongWord) { return Count1s((uint64_t)LongWord); }
|
||||
|
||||
int Count1s(const uint8_t *Byte, int Bytes);
|
||||
|
||||
// ==========================================================================
|
||||
|
||||
// use __builtin_popcount(unsigned int) ? http://stackoverflow.com/questions/109023/how-to-count-the-number-of-set-bits-in-a-32-bit-integer
|
||||
|
||||
#endif // of __BITCOUNT_H__
|
||||
|
|
@ -0,0 +1,618 @@
|
|||
#ifndef __FANET_H__
|
||||
#define __FANET_H__
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <map>
|
||||
|
||||
#include "format.h"
|
||||
#include "intmath.h"
|
||||
|
||||
// ===============================================================================================
|
||||
|
||||
class FANET_Packet
|
||||
{ public:
|
||||
union
|
||||
{ uint8_t Flags;
|
||||
struct
|
||||
{ uint8_t CR:3; // Coding rate used (RX) or to be used (TX)
|
||||
bool hasCRC:1; // CRC was there (RX)
|
||||
bool badCRC:1; // CRC was bad (RX)
|
||||
bool Done:1;
|
||||
} ;
|
||||
} ;
|
||||
uint8_t Len; // [bytes] packet length
|
||||
static const int MaxBytes = 40;
|
||||
uint8_t Byte[MaxBytes+2];
|
||||
|
||||
public:
|
||||
FANET_Packet() { Len=0; }
|
||||
|
||||
uint8_t Dump(char *Out)
|
||||
{ uint8_t Len=0;
|
||||
for(int Idx=0; Idx<this->Len; Idx++)
|
||||
{ Len+=Format_Hex(Out+Len, Byte[Idx]); }
|
||||
return Len; }
|
||||
|
||||
bool ExtHeader(void) const { return Byte[0]&0x80; } // is there extended header ?
|
||||
bool Forward(void) const { return Byte[0]&0x40; } // forward flag
|
||||
uint8_t Type(void) const { return Byte[0]&0x3F; } // message type
|
||||
uint32_t getAddr(void) const { uint32_t Addr=Byte[1]; Addr<<=8; Addr|=Byte[3]; Addr<<=8; Addr|=Byte[2]; return Addr; } // 24-bit source address
|
||||
uint8_t getAddrPref(void) const { return Byte[1]; }
|
||||
|
||||
uint8_t getAddrType(void) const // address-type based on prefix
|
||||
{ uint8_t Pref=getAddrPref();
|
||||
if(Pref==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 FANET cordic units to float degrees
|
||||
{ // const float Conv = 90.0/0x40000000; // conversion factor (exact cordic)
|
||||
const float Conv = 90.0007295677/0x40000000; // FANET cordic conversion factor (not exactly cordic)
|
||||
return Conv*Coord; }
|
||||
|
||||
static int32_t getLat(const uint8_t *Byte) // FANET cordic units
|
||||
{ int32_t Latitude=Byte[2]; Latitude<<=8; Latitude|=Byte[1]; Latitude<<=8; Latitude|=Byte[0]; Latitude<<=7; return Latitude; }
|
||||
static void setLat(uint8_t *Byte, int32_t Lat)
|
||||
{ Lat = (Lat+0x40)>>7; Byte[0]=Lat; Byte[1]=Lat>>8; Byte[2]=Lat>>16; }
|
||||
static int32_t getLon(const uint8_t *Byte) // FANET cordic units
|
||||
{ int32_t Longitude=Byte[2]; Longitude<<=8; Longitude|=Byte[1]; Longitude<<=8; Longitude|=Byte[0]; Longitude<<=8; return Longitude; }
|
||||
static void setLon(uint8_t *Byte, int32_t Lon)
|
||||
{ Lon = (Lon+0x80)>>8; Byte[0]=Lon; Byte[1]=Lon>>8; Byte[2]=Lon>>16; }
|
||||
|
||||
static uint16_t getSpeed(uint8_t Byte) // [0.5km/h] for Type=1
|
||||
{ if(Byte<0x80) return Byte;
|
||||
return (uint16_t)5*(Byte-0x80); }
|
||||
static void setSpeed(uint8_t *Byte, uint16_t Speed)
|
||||
{ if(Speed>=128) { Speed=(Speed+2)/5; if(Speed>=0x80) Speed=0x7F; Speed|=0x80; }
|
||||
Byte[0]=Speed; }
|
||||
|
||||
static int16_t getClimb(uint8_t Byte) // [0.1m/s]
|
||||
{ int16_t Climb = Byte&0x3F; if(Byte&0x40) Climb|=0xFFC0;
|
||||
if(Byte&0x80) return Climb*5;
|
||||
return Climb; }
|
||||
static void setClimb(uint8_t *Byte, int16_t Climb) // [0.1m/s]
|
||||
{ if(Climb>63)
|
||||
{ Climb = (Climb+2)/5; if(Climb>63) Climb=63;
|
||||
Byte[0] = 0x80 | Climb; }
|
||||
else if(Climb<(-63))
|
||||
{ Climb = (Climb-2)/5; if(Climb<(-63)) Climb=(-63);
|
||||
Byte[0] = 0x80 | Climb; }
|
||||
else
|
||||
{ Byte[0] = Climb&0x7F; }
|
||||
}
|
||||
|
||||
static uint16_t getDir(uint8_t Byte) // [deg]
|
||||
{ uint16_t Dir = ((uint16_t)45*Byte+0x10)>>5; return Dir; }
|
||||
|
||||
static uint16_t getPressure(const uint8_t *Byte) // [0.1hPa]
|
||||
{ uint16_t Press = Byte[1]; Press<<=8; Press|=Byte[0]; return Press+4300; }
|
||||
|
||||
static int16_t getTurnRate(uint8_t Byte) // [0.25deg/s]
|
||||
{ int16_t Rate = Byte&0x7F; if(Byte&0x40) Rate|=0xFF80;
|
||||
if(Byte&0x80) return Rate<<2;
|
||||
return Rate; }
|
||||
static void setTurnRate(uint8_t *Byte, int16_t Rate)
|
||||
{ if(Rate>= 64 ) { if(Rate> 254 ) Rate= 254 ; Byte[0] = 0x80 | ((Rate+2)>>2); return; }
|
||||
if(Rate<(-64)) { if(Rate<(-254)) Rate=(-254); Byte[0] = 0x80 | (((Rate+2)>>2)&0x7F); return; }
|
||||
Byte[0] = Rate&0x7F; }
|
||||
|
||||
static int16_t getQNE(uint8_t Byte) // [m] difference between pressure altitude and GPS altitude
|
||||
{ int16_t QNE = Byte&0x7F; if(Byte&0x40) QNE|=0xFF80;
|
||||
if(Byte&0x80) return QNE<<2;
|
||||
return QNE; }
|
||||
static void setQNE(uint8_t *Byte, int16_t QNE)
|
||||
{ if(QNE>= 64 ) { if(QNE> 254 ) QNE= 254 ; Byte[0] = 0x80 | ((QNE+2)>>2); return; }
|
||||
if(QNE<(-64)) { if(QNE<(-254)) QNE=(-254); Byte[0] = 0x80 | (((QNE+2)>>2)&0x7F); return; }
|
||||
Byte[0] = QNE&0x7F; }
|
||||
|
||||
static uint16_t getAltitude(const uint8_t *Byte) // [m]
|
||||
{ uint16_t Alt = Byte[1]; Alt<<=8; Alt|=Byte[0]; Alt&=0x0FFF;
|
||||
if(Alt<0x800) return Alt;
|
||||
return (Alt-0x800)<<2; }
|
||||
void setAltitude(uint8_t *Byte, uint16_t Alt) // [m]
|
||||
{ if(Alt>=0x800) { Alt>>=2; if(Alt>0x800) Alt=0x800; Alt|=0x800; }
|
||||
Byte[0]=Alt; Byte[1] = (Byte[1]&0xF0) | (Alt>>8); }
|
||||
|
||||
// [0..7] [0..1] [FANET cordic] [FANET cordic] [m] [cordic] [0.1m/s] [0.1m/s] [0.1deg/s]
|
||||
void setAirPos(uint8_t AcftType, uint8_t Track, int32_t Lat, int32_t Lon, int16_t Alt, uint8_t Dir, uint16_t Speed, int16_t Climb, int16_t Turn)
|
||||
{ setHeader(1);
|
||||
uint8_t Ofs=MsgOfs();
|
||||
Len=Ofs+12;
|
||||
setLat(Byte+Ofs, Lat); // [cordic]
|
||||
setLon(Byte+Ofs+3, Lon); // [cordic]
|
||||
Byte[Ofs+7]=(AcftType<<4) | (Track<<7);
|
||||
if(Alt<0) Alt=0;
|
||||
setAltitude(Byte+Ofs+6, Alt); // [m]
|
||||
Speed = (Speed*23+16)>>5; // [0.1m/s] => [0.5km/h]
|
||||
setSpeed(Byte+Ofs+8, Speed); // [0.5km/h]
|
||||
setClimb(Byte+Ofs+9, Climb); // [0.1m/s]
|
||||
Byte[Ofs+10] = Dir; // [cordic]
|
||||
setTurnRate(Byte+Ofs+11, Turn*2/5); } // [0.25deg/s]
|
||||
|
||||
void setQNE(int32_t StdAltitude) // [m] only for air-position
|
||||
{ uint8_t Ofs=MsgOfs();
|
||||
int32_t Alt=getAltitude(Byte+Ofs+6);
|
||||
if(Len<(Ofs+13)) Len=Ofs+13;
|
||||
setQNE(Byte+Ofs+12, StdAltitude-Alt); }
|
||||
|
||||
// [0..15] [0..1] [FANET cordic] [FANET cordic]
|
||||
void setGndPos(uint8_t Status, uint8_t Track, int32_t Lat, int32_t Lon)
|
||||
{ setHeader(7);
|
||||
uint8_t Ofs=MsgOfs();
|
||||
Len=11;
|
||||
setLat(Byte+Ofs, Lat);
|
||||
setLon(Byte+Ofs+3, Lon);
|
||||
Byte[Ofs+6] = (Status<<4) | Track; }
|
||||
|
||||
/*
|
||||
* $FNNGB,manufacturer(hex),id(hex),name(up to 32bytes),type/status,latitude,longitude,altitude,climb,speed,heading*checksum
|
||||
* manufacturer: 1-2 chars hex
|
||||
* id: 1-4 chars hex
|
||||
* name: string up to 32 chars
|
||||
* type/status: while airborne: aircraft type: 0-7 (3D tracking), else: status: 0-15 (2D tracking) +10 -> 10-25
|
||||
* latitude: %.5f in degree
|
||||
* longitude: %.5f in degree
|
||||
* altitude: %.f in meter, -1000 for ground
|
||||
* climb: %.1f in m/s
|
||||
* speed: %.1f in km/h
|
||||
* heading: %.f in degree
|
||||
*
|
||||
* for the types please see: https://github.com/3s1d/fanet-stm32/blob/master/Src/fanet/radio/protocol.txt
|
||||
*
|
||||
*/
|
||||
|
||||
uint8_t WriteFNNGB(char *Out)
|
||||
{ return 0; }
|
||||
|
||||
void Print(const char *Name=0) const
|
||||
{ if(Name) printf("%s ", Name);
|
||||
printf("[%2d:%d:%2d] FNT%06X", Len, Type(), MsgLen(), getAddr());
|
||||
if(Type()==2) // Name
|
||||
{ printf(" ");
|
||||
for(uint8_t Idx=MsgOfs(); Idx<Len; Idx++)
|
||||
printf("%c", Byte[Idx]);
|
||||
printf("\n"); return; }
|
||||
if(Type()==3) // Message
|
||||
{ uint8_t Idx=MsgOfs();
|
||||
printf(" Msg%02X: ", Byte[Idx++]);
|
||||
for( ; Idx<Len; Idx++)
|
||||
printf("%c", Byte[Idx]);
|
||||
printf("\n"); return; }
|
||||
if(Type()==4) // Service, mostly meteo
|
||||
{ uint8_t Idx=MsgOfs(); uint8_t Service=Byte[Idx++];
|
||||
int32_t Lat=getLat(Byte+Idx); Idx+=3; // [FANET cordic]
|
||||
int32_t Lon=getLon(Byte+Idx); Idx+=3; // [FANET cordic]
|
||||
printf(" [%+09.5f,%+010.5f] s%02X", FloatCoord(Lat), FloatCoord(Lon), Service);
|
||||
if(Service&0x80) printf(" Igw");
|
||||
if(Service&0x40) printf(" %+3.1fC", 0.5*(int8_t)Byte[Idx++]); // temperature
|
||||
if(Service&0x20) // wind direction and speed
|
||||
{ uint16_t Dir = Byte[Idx++]; // [cordic]
|
||||
uint16_t Wind = getSpeed(Byte[Idx++]); // [0.2km/h]
|
||||
uint16_t Gust = getSpeed(Byte[Idx++]); // [0.2km/h]
|
||||
printf(" %03.0fdeg %3.1f/%3.1fkm/h", (180.0/128)*Dir, 0.2*Wind, 0.2*Gust); }
|
||||
if(Service&0x10) printf(" %3.1f%%", 0.4*Byte[Idx++]); // humidity
|
||||
if(Service&0x08) // pressure
|
||||
{ printf(" %3.1fhPa", 0.1*getPressure(Byte+Idx)); Idx+=2; }
|
||||
if(Service&0x02) printf(" %1.0f%%", (100.0/15)*Byte[Idx++]); // charge state
|
||||
printf("\n"); return; }
|
||||
if(Type()==1) // airborne position
|
||||
{ uint8_t Idx=MsgOfs(); uint8_t AcftType=Byte[Idx+7]>>4;
|
||||
int32_t Lat=getLat(Byte+Idx); // [FANET cordic]
|
||||
int32_t Lon=getLon(Byte+Idx+3); // [FANET cordic]
|
||||
uint16_t Alt=getAltitude(Byte+Idx+6); // [m]
|
||||
uint16_t Speed=getSpeed(Byte[Idx+8]); // [0.5km/h]
|
||||
int16_t Climb=getClimb(Byte[Idx+9]); // [0.1m/s]
|
||||
printf(" [%+09.5f,%+010.5f] %dm %3.1fkm/h %03.0fdeg %+4.1fm/s a%X%c",
|
||||
FloatCoord(Lat), FloatCoord(Lon), Alt, 0.5*Speed, (180.0/128)*Byte[Idx+10], 0.1*Climb,
|
||||
AcftType&0x07, AcftType&0x08?'T':'H');
|
||||
if((Idx+11)<Len)
|
||||
{ int16_t Rate = getTurnRate(Byte[Idx+11]);
|
||||
printf(" %+3.1fdeg/s", 0.25*Rate); }
|
||||
if((Idx+12)<Len)
|
||||
{ int16_t QNE = getQNE(Byte[Idx+12]);
|
||||
printf(" %+dm", QNE); }
|
||||
printf("\n"); return; }
|
||||
if(Type()==7) // ground position
|
||||
{ uint8_t Idx=MsgOfs(); uint8_t Status=Byte[Idx+6];
|
||||
int32_t Lat=getLat(Byte+Idx); // [FANET cordic]
|
||||
int32_t Lon=getLon(Byte+Idx+3); // [FANET cordic]
|
||||
printf(" [%+09.5f,%+010.5f] s%02X", FloatCoord(Lat), FloatCoord(Lon), Status);
|
||||
printf("\n"); return; }
|
||||
if(Type()==8) // Hardware/Software
|
||||
{ uint8_t Idx=MsgOfs();
|
||||
uint8_t Hw = Byte[Idx];
|
||||
uint16_t Fw = Byte[Idx+2]; Fw<<=8; Fw|=Byte[Idx+1];
|
||||
printf(" Hw%02X Fw%02d.%02d.%04d%c", Hw, Fw&0x1F, (Fw>>5)&0x0F, 2019+((Fw>>9)&0x3F), Fw&0x8000?'d':'r');
|
||||
printf("\n"); return; }
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
uint8_t Read(const char *Inp) // read packet from a hex dump
|
||||
{ for( Len=0; Len<MaxBytes; Len++) // read as many hex bytes as you can
|
||||
{ int8_t Upp = Read_Hex1(Inp[0]); if(Upp<0) break; // 1st digit
|
||||
int8_t Low = Read_Hex1(Inp[1]); if(Low<0) break; // 2nd digit
|
||||
Byte[Len] = (Upp<<4) | Low; Inp+=2; } // new byte, count input
|
||||
return Len; } // return number of bytes read = packet length
|
||||
|
||||
static int32_t CoordUBX(int32_t Coord) { return ((int64_t)900007296*Coord+0x20000000)>>30; } // convert FANET-cordic to UBX 10e-7deg units
|
||||
// ((int64_t)900000000*Coord+0x20000000)>>30; // this is the exact formula, but FANET is not exact here
|
||||
|
||||
static int Format_Lat(char *Str, int32_t Lat, char &HighRes) // format latitude after APRS
|
||||
{ Lat = CoordUBX(Lat); // convert from FANET cordic to UBX 1e-7 deg
|
||||
char Sign;
|
||||
if(Lat>0) { Sign='N'; }
|
||||
else { Sign='S'; Lat=(-Lat); }
|
||||
int32_t Dig;
|
||||
Dig=Lat/100000000; (*Str++)='0'+Dig; Lat-=Dig*100000000;
|
||||
Dig=Lat/10000000; (*Str++)='0'+Dig; Lat-=Dig*10000000;
|
||||
Lat*=60;
|
||||
Dig=Lat/100000000; (*Str++)='0'+Dig; Lat-=Dig*100000000;
|
||||
Dig=Lat/10000000; (*Str++)='0'+Dig; Lat-=Dig*10000000;
|
||||
(*Str++)='.';
|
||||
Dig=Lat/1000000; (*Str++)='0'+Dig; Lat-=Dig*1000000;
|
||||
Dig=Lat/100000; (*Str++)='0'+Dig; Lat-=Dig*100000;
|
||||
Dig=Lat/10000; HighRes ='0'+Dig; Lat-=Dig*10000;
|
||||
(*Str++)=Sign;
|
||||
return 8; }
|
||||
|
||||
static int Format_Lon(char *Str, int32_t Lon, char &HighRes) // format longitude after APRS
|
||||
{ Lon = CoordUBX(Lon); // convert from FANET cordic to UBX 1e-7 deg
|
||||
char Sign;
|
||||
if(Lon>0) { Sign='E'; }
|
||||
else { Sign='W'; Lon=(-Lon); }
|
||||
int32_t Dig;
|
||||
Dig=Lon/1000000000; (*Str++)='0'+Dig; Lon-=Dig*1000000000;
|
||||
Dig=Lon/100000000; (*Str++)='0'+Dig; Lon-=Dig*100000000;
|
||||
Dig=Lon/10000000; (*Str++)='0'+Dig; Lon-=Dig*10000000;
|
||||
Lon*=60;
|
||||
Dig=Lon/100000000; (*Str++)='0'+Dig; Lon-=Dig*100000000;
|
||||
Dig=Lon/10000000; (*Str++)='0'+Dig; Lon-=Dig*10000000;
|
||||
(*Str++)='.';
|
||||
Dig=Lon/1000000; (*Str++)='0'+Dig; Lon-=Dig*1000000;
|
||||
Dig=Lon/100000; (*Str++)='0'+Dig; Lon-=Dig*100000;
|
||||
Dig=Lon/10000; HighRes ='0'+Dig; Lon-=Dig*10000;
|
||||
(*Str++)=Sign;
|
||||
return 9; }
|
||||
|
||||
} ;
|
||||
|
||||
class FANET_RxPacket: public FANET_Packet
|
||||
{ public:
|
||||
uint32_t sTime; // [ s] reception time
|
||||
uint16_t msTime; // [ms]
|
||||
int16_t FreqOfs; // [ 10Hz]
|
||||
int8_t SNR; // [0.25dB]
|
||||
int8_t RSSI; // [dBm]
|
||||
uint8_t BitErr; // number of bit errors
|
||||
uint8_t CodeErr; // number of block errors
|
||||
|
||||
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; }
|
||||
|
||||
void Print(char *Name=0) const
|
||||
{ char HHMMSS[8];
|
||||
Format_HHMMSS(HHMMSS, SlotTime()); HHMMSS[6]='h'; HHMMSS[7]=0;
|
||||
printf("%s CR%c%c%c %3.1fdB/%de %+3.1fkHz ", HHMMSS, '0'+CR, hasCRC?'c':'_', badCRC?'-':'+', 0.25*SNR, BitErr, 1e-2*FreqOfs);
|
||||
FANET_Packet::Print(Name); }
|
||||
|
||||
int WriteJSON(char *JSON) const
|
||||
{ int Len=0;
|
||||
Len+=Format_String(JSON+Len, "\"addr\":\"");
|
||||
Len+=Format_Hex(JSON+Len, Byte[1]);
|
||||
Len+=Format_Hex(JSON+Len, Byte[3]);
|
||||
Len+=Format_Hex(JSON+Len, Byte[2]);
|
||||
JSON[Len++]='\"';
|
||||
JSON[Len++]=',';
|
||||
Len+=Format_String(JSON+Len, "\"addr_type\":");
|
||||
JSON[Len++] = HexDigit(getAddrType());
|
||||
const uint8_t *Msg = this->Msg();
|
||||
uint8_t MsgLen = this->MsgLen();
|
||||
uint8_t Type = this->Type();
|
||||
uint32_t Time=sTime; if(msTime<300) Time--;
|
||||
Len+=Format_String(JSON+Len, ",\"time\":");
|
||||
Len+=Format_UnsDec(JSON+Len, Time);
|
||||
int64_t RxTime=(int64_t)sTime-Time; RxTime*=1000; RxTime+=msTime;
|
||||
Len+=Format_String(JSON+Len, ",\"rx_time\":");
|
||||
Len+=Format_SignDec(JSON+Len, RxTime, 4, 3, 1);
|
||||
if(Type==2)
|
||||
{ Len+=Format_String(JSON+Len, ",\"Name\":\"");
|
||||
for(int Idx=0; Idx<MsgLen; Idx++)
|
||||
{ uint8_t Byte=Msg[Idx]; if(Byte==0) break;
|
||||
JSON[Len++]=Byte; }
|
||||
JSON[Len++]='\"'; }
|
||||
if(Type==1) // for airborne position
|
||||
{ uint8_t AcftType=Msg[7]>>4; // get the aircraft-type and online-track flag
|
||||
Len+=Format_String(JSON+Len, ",\"acft_type\":\"");
|
||||
JSON[Len++] = HexDigit(AcftType&0x7);
|
||||
JSON[Len++]='\"';
|
||||
Len+=Format_String(JSON+Len, ",\"acft_cat\":\""); // GDL90 aircraft category
|
||||
// no-info, para-glider, hang-glider, balloon, glider, powered, heli, UAV
|
||||
const uint8_t AcftCat[8] = { 0, 12, 12, 10, 9, 1, 7, 14 } ;
|
||||
Len+=Format_Hex(JSON+Len, AcftCat[AcftType&0x07]);
|
||||
JSON[Len++]='\"';
|
||||
Len+=Format_String(JSON+Len, ",\"no_track\":");
|
||||
JSON[Len++]='0' + (AcftType>>3); }
|
||||
if(Type==4) // for service/weather
|
||||
{ uint8_t Service=Msg[0]; //
|
||||
Len+=Format_String(JSON+Len, ",\"service\":\"");
|
||||
if(Service&0x80) JSON[Len++]='I';
|
||||
if(Service&0x40) JSON[Len++]='T';
|
||||
if(Service&0x20) JSON[Len++]='W';
|
||||
if(Service&0x10) JSON[Len++]='H';
|
||||
if(Service&0x08) JSON[Len++]='B';
|
||||
// if(Service&0x04) JSON[Len++]='R';
|
||||
if(Service&0x02) JSON[Len++]='C';
|
||||
JSON[Len++]='\"';
|
||||
int32_t Lat = getLat(Msg+1); // [cordic] decode the latitude
|
||||
int32_t Lon = getLon(Msg+4); // [cordic] decode the longitude
|
||||
Len+=Format_String(JSON+Len, ",\"lat_deg\":");
|
||||
Len+=Format_SignDec(JSON+Len, CoordUBX(Lat), 8, 7, 1);
|
||||
Len+=Format_String(JSON+Len, ",\"lon_deg\":");
|
||||
Len+=Format_SignDec(JSON+Len, CoordUBX(Lon), 8, 7, 1); }
|
||||
if(Type==1 || Type==7) // airborne or ground position
|
||||
{ int32_t Lat = getLat(Msg); // [cordic] decode the latitude
|
||||
int32_t Lon = getLon(Msg+3); // [cordic] decode the longitude
|
||||
Len+=Format_String(JSON+Len, ",\"lat_deg\":");
|
||||
Len+=Format_SignDec(JSON+Len, CoordUBX(Lat), 8, 7, 1);
|
||||
Len+=Format_String(JSON+Len, ",\"lon_deg\":");
|
||||
Len+=Format_SignDec(JSON+Len, CoordUBX(Lon), 8, 7, 1); }
|
||||
if(Type==1) // for airpborne position
|
||||
{ uint32_t Alt=getAltitude(Msg+6); // [m] decode the altitude
|
||||
uint32_t Speed=getSpeed(Msg[8]); // [0.5km/h] ground speed
|
||||
Speed = (Speed*355+0x80)>>8; // [0.5km/h] => [0.1m/s] convert
|
||||
int32_t Climb=getClimb(Msg[9]); // [0.1m/s] climb rate
|
||||
uint16_t Dir=getDir(Msg[10]); // [deg]
|
||||
Len+=Format_String(JSON+Len, ",\"alt_msl_m\":");
|
||||
Len+=Format_UnsDec(JSON+Len, Alt);
|
||||
Len+=Format_String(JSON+Len, ",\"track_deg\":");
|
||||
Len+=Format_UnsDec(JSON+Len, Dir);
|
||||
Len+=Format_String(JSON+Len, ",\"speed_mps\":");
|
||||
Len+=Format_UnsDec(JSON+Len, Speed, 2, 1);
|
||||
Len+=Format_String(JSON+Len, ",\"climb_mps\":");
|
||||
Len+=Format_SignDec(JSON+Len, Climb, 2, 1, 1);
|
||||
if(MsgLen>11)
|
||||
{ int16_t Turn=getTurnRate(Msg[11]);
|
||||
Len+=Format_String(JSON+Len, ",\"turn_dps\":");
|
||||
Len+=Format_SignDec(JSON+Len, Turn*10/4, 2, 1, 1); }
|
||||
if(MsgLen>12)
|
||||
{ int32_t AltStd=Alt; Alt+=getQNE(Msg[12]);
|
||||
Len+=Format_String(JSON+Len, ",\"alt_std_m\":");
|
||||
Len+=Format_SignDec(JSON+Len, AltStd, 1, 0, 1); }
|
||||
Len+=Format_String(JSON+Len, ",\"on_ground\":0"); }
|
||||
if(Type==7) // for ground position
|
||||
{ uint8_t Status = Msg[6];
|
||||
Len+=Format_String(JSON+Len, ",\"no_track\":1");
|
||||
JSON[Len++]='0' + (Status&1);
|
||||
Len+=Format_String(JSON+Len, ",\"on_ground\":1"); }
|
||||
return Len; }
|
||||
|
||||
int WriteAPRS(char *Out)
|
||||
{ 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 = 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
|
||||
ID<<=24; ID |= getAddr(); // address
|
||||
int32_t Lat = getLat(Msg); // [cordic]
|
||||
int32_t Lon = getLon(Msg+3); // [cordic]
|
||||
uint32_t Alt=getAltitude(Msg+6); // [m]
|
||||
uint32_t Feet = ((int32_t)3360*Alt+512)>>10; // [feet]
|
||||
uint32_t Speed=getSpeed(Msg[8]); // [0.5km/h]
|
||||
uint32_t Knots=(Speed*553+1024)>>11; // knots
|
||||
int32_t Climb=getClimb(Msg[9]); // [0.1m/s]
|
||||
int32_t ClimbFeet = ((int32_t)1968*Climb+50)/100; // [fpm]
|
||||
uint16_t Dir=getDir(Msg[10]); // [deg]
|
||||
int16_t Turn=getQNE(Msg[11]); // [0.25deg/s]
|
||||
int16_t QNE=getQNE(Msg[12]); // [m]
|
||||
int32_t StdAlt=Alt+QNE; if(StdAlt<0) StdAlt=0; // [m]
|
||||
uint32_t StdFeet = ((int32_t)3360*StdAlt+512)>>10; // [feet]
|
||||
char hLat, hLon;
|
||||
Len+=Format_Lat(Out+Len, Lat, hLat);
|
||||
Out[Len++]=Icon[0];
|
||||
Len+=Format_Lon(Out+Len, Lon, hLon);
|
||||
Out[Len++]=Icon[1];
|
||||
Len+=Format_UnsDec(Out+Len, Dir, 3);
|
||||
Out[Len++]='/';
|
||||
Len+=Format_UnsDec(Out+Len, Knots, 3);
|
||||
Len+=Format_String(Out+Len, "/A=");
|
||||
Len+=Format_UnsDec(Out+Len, Feet, 6);
|
||||
Len+=Format_String(Out+Len, " !W");
|
||||
Out[Len++]=hLat;
|
||||
Out[Len++]=hLon;
|
||||
Out[Len++]='!';
|
||||
Len+=Format_String(Out+Len, " id");
|
||||
Len+=Format_Hex(Out+Len, ID);
|
||||
Out[Len++]=' ';
|
||||
Len+=Format_SignDec(Out+Len, ClimbFeet);
|
||||
Len+=Format_String(Out+Len, "fpm");
|
||||
if(MsgLen>11)
|
||||
{ Out[Len++]=' ';
|
||||
Len+=Format_SignDec(Out+Len, Turn*5/6, 2, 1);
|
||||
Len+=Format_String(Out+Len, "rot"); }
|
||||
if(MsgLen>12)
|
||||
{ Len+=Format_String(Out+Len, " FL");
|
||||
Len+=Format_UnsDec(Out+Len, StdFeet, 5, 2); }
|
||||
Len+=Format_String(Out+Len, " FNT1"); Out[Len++]='0'+(AcftType&7);
|
||||
Report=1; break; }
|
||||
case 7: // ground position
|
||||
{ // const char *StatusMsg[16] = { 0, "Walking", "Vehicle", "Bike", "Boot", 0, 0, 0,
|
||||
// "Need-ride", "Landed-well", 0, 0, "Need-technical",
|
||||
// "Need-medical", "Distress(man)", "Distress(auto)" } ;
|
||||
uint8_t Status = Msg[6];
|
||||
bool Track = Status&1;
|
||||
Status>>=4;
|
||||
const char *Icon = "\\n"; // static object
|
||||
if(Status>=13) Icon = "\\!"; // Emergency
|
||||
// const char *StatMsg = StatusMsg[Status];
|
||||
uint8_t AddrType = getAddrType(); //
|
||||
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 = 32;
|
||||
uint32_t Time;
|
||||
// uint8_t Type;
|
||||
char Name[MaxSize];
|
||||
|
||||
public:
|
||||
FANET_Name() { Time=0; Name[0]=0; }
|
||||
|
||||
int Copy(const uint8_t *Src, int Size)
|
||||
{ if(Size>=MaxSize) Size=MaxSize-1;
|
||||
memcpy(Name, Src, Size); Name[Size]=0;
|
||||
return Size; }
|
||||
|
||||
} ;
|
||||
|
||||
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__
|
||||
|
|
@ -0,0 +1,316 @@
|
|||
#include "format.h"
|
||||
|
||||
// ------------------------------------------------------------------------------------------
|
||||
|
||||
char HexDigit(uint8_t Val) { return Val+(Val<10?'0':'A'-10); }
|
||||
|
||||
// ------------------------------------------------------------------------------------------
|
||||
|
||||
void Format_Bytes( void (*Output)(char), const uint8_t *Bytes, uint8_t Len)
|
||||
{ for( ; Len; Len--)
|
||||
(*Output)(*Bytes++);
|
||||
}
|
||||
|
||||
void Format_String( void (*Output)(char), const char *String)
|
||||
{ if(String==0) return;
|
||||
for( ; ; )
|
||||
{ uint8_t ch = (*String++); if(ch==0) break;
|
||||
#ifdef WITH_AUTOCR
|
||||
if(ch=='\n') (*Output)('\r');
|
||||
#endif
|
||||
(*Output)(ch); }
|
||||
}
|
||||
|
||||
uint8_t Format_String(char *Out, const char *String)
|
||||
{ if(String==0) return 0;
|
||||
uint8_t OutLen=0;
|
||||
for( ; ; )
|
||||
{ char ch = (*String++); if(ch==0) break;
|
||||
#ifdef WITH_AUTOCR
|
||||
if(ch=='\n') Out[OutLen++]='\r';
|
||||
#endif
|
||||
Out[OutLen++]=ch; }
|
||||
// Out[OutLen]=0;
|
||||
return OutLen; }
|
||||
|
||||
void Format_String( void (*Output)(char), const char *String, uint8_t MinLen, uint8_t MaxLen)
|
||||
{ if(String==0) return;
|
||||
if(MaxLen<MinLen) MaxLen=MinLen;
|
||||
uint8_t Idx;
|
||||
for(Idx=0; Idx<MaxLen; Idx++)
|
||||
{ char ch = String[Idx]; if(ch==0) break;
|
||||
#ifdef WITH_AUTOCR
|
||||
if(ch=='\n') (*Output)('\r');
|
||||
#endif
|
||||
(*Output)(ch); }
|
||||
for( ; Idx<MinLen; Idx++)
|
||||
(*Output)(' ');
|
||||
}
|
||||
|
||||
uint8_t Format_String(char *Out, const char *String, uint8_t MinLen, uint8_t MaxLen)
|
||||
{ if(String==0) return 0;
|
||||
if(MaxLen<MinLen) MaxLen=MinLen;
|
||||
uint8_t OutLen=0;
|
||||
uint8_t Idx;
|
||||
for(Idx=0; Idx<MaxLen; Idx++)
|
||||
{ char ch = String[Idx]; if(ch==0) break;
|
||||
#ifdef WITH_AUTOCR
|
||||
if(ch=='\n') Out[OutLen++]='\r';
|
||||
#endif
|
||||
Out[OutLen++]=ch; }
|
||||
for( ; Idx<MinLen; Idx++)
|
||||
Out[OutLen++]=' ';
|
||||
// Out[OutLen++]=0;
|
||||
return OutLen; }
|
||||
|
||||
void Format_Hex( void (*Output)(char), uint8_t Byte )
|
||||
{ (*Output)(HexDigit(Byte>>4)); (*Output)(HexDigit(Byte&0x0F)); }
|
||||
|
||||
void Format_Hex( void (*Output)(char), uint16_t Word )
|
||||
{ Format_Hex(Output, (uint8_t)(Word>>8)); Format_Hex(Output, (uint8_t)Word); }
|
||||
|
||||
void Format_Hex( void (*Output)(char), uint32_t Word )
|
||||
{ Format_Hex(Output, (uint8_t)(Word>>24)); Format_Hex(Output, (uint8_t)(Word>>16));
|
||||
Format_Hex(Output, (uint8_t)(Word>>8)); Format_Hex(Output, (uint8_t)Word); }
|
||||
|
||||
void Format_MAC( void (*Output)(char), uint8_t *MAC, uint8_t Len)
|
||||
{ for(uint8_t Idx=0; Idx<Len; Idx++)
|
||||
{ if(Idx) (*Output)(':');
|
||||
Format_Hex(Output, MAC[Idx]); }
|
||||
}
|
||||
|
||||
uint8_t Format_HHcMMcSS(char *Out, uint32_t Time)
|
||||
{ uint32_t DayTime=Time%86400;
|
||||
uint32_t Hour=DayTime/3600; DayTime-=Hour*3600;
|
||||
uint32_t Min=DayTime/60; DayTime-=Min*60;
|
||||
uint32_t Sec=DayTime;
|
||||
uint32_t HHMMSS = 1000000*Hour + 1000*Min + Sec;
|
||||
uint8_t Len=Format_UnsDec(Out, HHMMSS, 8);
|
||||
Out[2]=':'; Out[5]=':';
|
||||
return Len; }
|
||||
|
||||
uint8_t Format_HHMMSS(char *Out, uint32_t Time)
|
||||
{ uint32_t DayTime=Time%86400;
|
||||
uint32_t Hour=DayTime/3600; DayTime-=Hour*3600;
|
||||
uint32_t Min=DayTime/60; DayTime-=Min*60;
|
||||
uint32_t Sec=DayTime;
|
||||
uint32_t HHMMSS = 10000*Hour + 100*Min + Sec;
|
||||
return Format_UnsDec(Out, HHMMSS, 6); }
|
||||
|
||||
void Format_HHMMSS(void (*Output)(char), uint32_t Time)
|
||||
{ uint32_t DayTime=Time%86400;
|
||||
uint32_t Hour=DayTime/3600; DayTime-=Hour*3600;
|
||||
uint32_t Min=DayTime/60; DayTime-=Min*60;
|
||||
uint32_t Sec=DayTime;
|
||||
uint32_t HHMMSS = 10000*Hour + 100*Min + Sec;
|
||||
Format_UnsDec(Output, HHMMSS, 6); }
|
||||
|
||||
void Format_UnsDec( void (*Output)(char), uint16_t Value, uint8_t MinDigits, uint8_t DecPoint)
|
||||
{ uint16_t Base; uint8_t Pos;
|
||||
for( Pos=5, Base=10000; Base; Base/=10, Pos--)
|
||||
{ uint8_t Dig;
|
||||
if(Value>=Base)
|
||||
{ Dig=Value/Base; Value-=Dig*Base; }
|
||||
else
|
||||
{ Dig=0; }
|
||||
if(Pos==DecPoint) (*Output)('.');
|
||||
if( (Pos<=MinDigits) || (Dig>0) || (Pos<=DecPoint) )
|
||||
{ (*Output)('0'+Dig); MinDigits=Pos; }
|
||||
}
|
||||
}
|
||||
|
||||
void Format_SignDec( void (*Output)(char), int16_t Value, uint8_t MinDigits, uint8_t DecPoint, uint8_t NoPlus)
|
||||
{ if(Value<0) { (*Output)('-'); Value=(-Value); }
|
||||
else if(!NoPlus) { (*Output)('+'); }
|
||||
Format_UnsDec(Output, (uint16_t)Value, MinDigits, DecPoint); }
|
||||
|
||||
void Format_UnsDec( void (*Output)(char), uint32_t Value, uint8_t MinDigits, uint8_t DecPoint)
|
||||
{ uint32_t Base; uint8_t Pos;
|
||||
for( Pos=10, Base=1000000000; Base; Base/=10, Pos--)
|
||||
{ uint8_t Dig;
|
||||
if(Value>=Base)
|
||||
{ Dig=Value/Base; Value-=Dig*Base; }
|
||||
else
|
||||
{ Dig=0; }
|
||||
if(Pos==DecPoint) (*Output)('.');
|
||||
if( (Pos<=MinDigits) || (Dig>0) || (Pos<=DecPoint) )
|
||||
{ (*Output)('0'+Dig); MinDigits=Pos; }
|
||||
}
|
||||
}
|
||||
|
||||
void Format_SignDec( void (*Output)(char), int32_t Value, uint8_t MinDigits, uint8_t DecPoint, uint8_t NoPlus)
|
||||
{ if(Value<0) { (*Output)('-'); Value=(-Value); }
|
||||
else if(!NoPlus) { (*Output)('+'); }
|
||||
Format_UnsDec(Output, (uint32_t)Value, MinDigits, DecPoint); }
|
||||
|
||||
void Format_UnsDec( void (*Output)(char), uint64_t Value, uint8_t MinDigits, uint8_t DecPoint)
|
||||
{ uint64_t Base; uint8_t Pos;
|
||||
for( Pos=20, Base=10000000000000000000llu; Base; Base/=10, Pos--)
|
||||
{ uint8_t Dig;
|
||||
if(Value>=Base)
|
||||
{ Dig=Value/Base; Value-=Dig*Base; }
|
||||
else
|
||||
{ Dig=0; }
|
||||
if(Pos==DecPoint) (*Output)('.');
|
||||
if( (Pos<=MinDigits) || (Dig>0) || (Pos<=DecPoint) )
|
||||
{ (*Output)('0'+Dig); MinDigits=Pos; }
|
||||
}
|
||||
}
|
||||
|
||||
void Format_SignDec( void (*Output)(char), int64_t Value, uint8_t MinDigits, uint8_t DecPoint, uint8_t NoPlus)
|
||||
{ if(Value<0) { (*Output)('-'); Value=(-Value); }
|
||||
else if(!NoPlus) { (*Output)('+'); }
|
||||
Format_UnsDec(Output, (uint32_t)Value, MinDigits, DecPoint); }
|
||||
|
||||
// ------------------------------------------------------------------------------------------
|
||||
|
||||
uint8_t Format_UnsDec(char *Out, uint32_t Value, uint8_t MinDigits, uint8_t DecPoint)
|
||||
{ uint32_t Base; uint8_t Pos, Len=0;
|
||||
for( Pos=10, Base=1000000000; Base; Base/=10, Pos--)
|
||||
{ uint8_t Dig;
|
||||
if(Value>=Base)
|
||||
{ Dig=Value/Base; Value-=Dig*Base; }
|
||||
else
|
||||
{ Dig=0; }
|
||||
if(Pos==DecPoint) { (*Out++)='.'; Len++; }
|
||||
if( (Pos<=MinDigits) || (Dig>0) || (Pos<=DecPoint) )
|
||||
{ (*Out++)='0'+Dig; Len++; MinDigits=Pos; }
|
||||
// (*Out)=0;
|
||||
}
|
||||
return Len; }
|
||||
|
||||
uint8_t Format_SignDec(char *Out, int32_t Value, uint8_t MinDigits, uint8_t DecPoint, uint8_t NoPlus)
|
||||
{ uint8_t Len=0;
|
||||
if(Value<0) { (*Out++)='-'; Len++; Value=(-Value); }
|
||||
else if(!NoPlus) { (*Out++)='+'; Len++; }
|
||||
return Len+Format_UnsDec(Out, Value, MinDigits, DecPoint); }
|
||||
|
||||
uint8_t Format_Hex( char *Output, uint8_t Byte )
|
||||
{ (*Output++) = HexDigit(Byte>>4); (*Output++)=HexDigit(Byte&0x0F); return 2; }
|
||||
|
||||
uint8_t Format_Hex( char *Output, uint16_t Word )
|
||||
{ Format_Hex(Output, (uint8_t)(Word>>8)); Format_Hex(Output+2, (uint8_t)Word); return 4; }
|
||||
|
||||
uint8_t Format_Hex( char *Output, uint32_t Word )
|
||||
{ Format_Hex(Output , (uint8_t)(Word>>24)); Format_Hex(Output+2, (uint8_t)(Word>>16));
|
||||
Format_Hex(Output+4, (uint8_t)(Word>> 8)); Format_Hex(Output+6, (uint8_t) Word ); return 8; }
|
||||
|
||||
uint8_t Format_Hex( char *Output, uint32_t Word, uint8_t Digits)
|
||||
{ for(uint8_t Idx=Digits; Idx>0; )
|
||||
{ Output[--Idx]=HexDigit(Word&0x0F);
|
||||
Word>>=4; }
|
||||
return Digits; }
|
||||
|
||||
// ------------------------------------------------------------------------------------------
|
||||
|
||||
uint8_t Format_Latitude(char *Out, int32_t Lat)
|
||||
{ uint8_t Len=0;
|
||||
char Sign='N';
|
||||
if(Lat<0) { Sign='S'; Lat=(-Lat); }
|
||||
uint32_t Deg=Lat/600000;
|
||||
Lat -= 600000*Deg;
|
||||
Len+=Format_UnsDec(Out+Len, Deg, 2, 0);
|
||||
Len+=Format_UnsDec(Out+Len, Lat, 6, 4);
|
||||
Out[Len++]=Sign;
|
||||
return Len; }
|
||||
|
||||
uint8_t Format_Longitude(char *Out, int32_t Lon)
|
||||
{ uint8_t Len=0;
|
||||
char Sign='E';
|
||||
if(Lon<0) { Sign='W'; Lon=(-Lon); }
|
||||
uint32_t Deg=Lon/600000;
|
||||
Lon -= 600000*Deg;
|
||||
Len+=Format_UnsDec(Out+Len, Deg, 3, 0);
|
||||
Len+=Format_UnsDec(Out+Len, Lon, 6, 4);
|
||||
Out[Len++]=Sign;
|
||||
return Len; }
|
||||
|
||||
// ------------------------------------------------------------------------------------------
|
||||
|
||||
int8_t Read_Hex1(char Digit)
|
||||
{ int8_t Val=Read_Dec1(Digit); if(Val>=0) return Val;
|
||||
if( (Digit>='A') && (Digit<='F') ) return Digit-'A'+10;
|
||||
if( (Digit>='a') && (Digit<='f') ) return Digit-'a'+10;
|
||||
return -1; }
|
||||
|
||||
int8_t Read_Dec1(char Digit) // convert single digit into an integer
|
||||
{ if(Digit<'0') return -1; // return -1 if not a decimal digit
|
||||
if(Digit>'9') return -1;
|
||||
return Digit-'0'; }
|
||||
|
||||
int8_t Read_Dec2(const char *Inp) // convert two digit decimal number into an integer
|
||||
{ int8_t High=Read_Dec1(Inp[0]); if(High<0) return -1;
|
||||
int8_t Low =Read_Dec1(Inp[1]); if(Low<0) return -1;
|
||||
return Low+10*High; }
|
||||
|
||||
int16_t Read_Dec3(const char *Inp) // convert three digit decimal number into an integer
|
||||
{ int8_t High=Read_Dec1(Inp[0]); if(High<0) return -1;
|
||||
int8_t Mid=Read_Dec1(Inp[1]); if(Mid<0) return -1;
|
||||
int8_t Low=Read_Dec1(Inp[2]); if(Low<0) return -1;
|
||||
return (int16_t)Low + (int16_t)10*(int16_t)Mid + (int16_t)100*(int16_t)High; }
|
||||
|
||||
int16_t Read_Dec4(const char *Inp) // convert three digit decimal number into an integer
|
||||
{ int16_t High=Read_Dec2(Inp ); if(High<0) return -1;
|
||||
int16_t Low =Read_Dec2(Inp+2); if(Low<0) return -1;
|
||||
return Low + (int16_t)100*(int16_t)High; }
|
||||
|
||||
// ------------------------------------------------------------------------------------------
|
||||
|
||||
int8_t Read_Coord(int32_t &Lat, const char *Inp)
|
||||
{ uint16_t Deg; int8_t Min, Sec;
|
||||
Lat=0;
|
||||
const char *Start=Inp;
|
||||
int8_t Len=Read_UnsDec(Deg, Inp); if(Len<0) return -1;
|
||||
Inp+=Len;
|
||||
Lat=(uint32_t)Deg*36000;
|
||||
if(Inp[0]!=(char)0xC2) return -1;
|
||||
if(Inp[1]!=(char)0xB0) return -1;
|
||||
Inp+=2;
|
||||
Min=Read_Dec2(Inp); if(Min<0) return -1;
|
||||
Inp+=2;
|
||||
Lat+=(uint32_t)Min*600;
|
||||
if(Inp[0]!=(char)'\'') return -1;
|
||||
Inp++;
|
||||
Sec=Read_Dec2(Inp); if(Sec<0) return -1;
|
||||
Inp+=2;
|
||||
Lat+=(uint32_t)Sec*10;
|
||||
if(Inp[0]=='.')
|
||||
{ Sec=Read_Dec1(Inp+1); if(Sec<0) return -1;
|
||||
Inp+=2; Lat+=Sec; }
|
||||
if(Inp[0]==(char)'\"') { Inp++; }
|
||||
else if( (Inp[0]==(char)'\'') && (Inp[1]==(char)'\'') ) { Inp+=2; }
|
||||
else return -1;
|
||||
return Inp-Start; }
|
||||
|
||||
int8_t Read_LatDDMMSS(int32_t &Lat, const char *Inp)
|
||||
{ Lat=0;
|
||||
const char *Start=Inp;
|
||||
int8_t Sign=0;
|
||||
if(Inp[0]=='N') { Sign= 1 ; Inp++; }
|
||||
else if(Inp[0]=='S') { Sign=(-1); Inp++; }
|
||||
int8_t Len=Read_Coord(Lat, Inp); if(Len<0) return -1;
|
||||
Inp+=Len;
|
||||
if(Sign==0)
|
||||
{ if(Inp[0]=='N') { Sign= 1 ; Inp++; }
|
||||
else if(Inp[0]=='S') { Sign=(-1); Inp++; }
|
||||
}
|
||||
if(Sign==0) return -1;
|
||||
if(Sign<0) Lat=(-Lat);
|
||||
return Inp-Start; }
|
||||
|
||||
int8_t Read_LonDDMMSS(int32_t &Lon, const char *Inp)
|
||||
{ Lon=0;
|
||||
const char *Start=Inp;
|
||||
int8_t Sign=0;
|
||||
if(Inp[0]=='E') { Sign= 1 ; Inp++; }
|
||||
else if(Inp[0]=='W') { Sign=(-1); Inp++; }
|
||||
int8_t Len=Read_Coord(Lon, Inp); if(Len<0) return -1;
|
||||
Inp+=Len;
|
||||
if(Sign==0)
|
||||
{ if(Inp[0]=='E') { Sign= 1 ; Inp++; }
|
||||
else if(Inp[0]=='W') { Sign=(-1); Inp++; }
|
||||
}
|
||||
if(Sign==0) return -1;
|
||||
if(Sign<0) Lon=(-Lon);
|
||||
return Inp-Start; }
|
||||
|
||||
|
|
@ -0,0 +1,124 @@
|
|||
#ifndef __FORMAT_H__
|
||||
#define __FORMAT_H__
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#define WITH_AUTOCR
|
||||
|
||||
char HexDigit(uint8_t Val);
|
||||
|
||||
void Format_Bytes ( void (*Output)(char), const uint8_t *Bytes, uint8_t Len);
|
||||
inline void Format_Bytes ( void (*Output)(char), const char *Bytes, uint8_t Len) { Format_Bytes(Output, (const uint8_t *)Bytes, Len); }
|
||||
|
||||
void Format_String( void (*Output)(char), const char *String);
|
||||
void Format_String( void (*Output)(char), const char *String, uint8_t MinLen, uint8_t MaxLen);
|
||||
|
||||
void Format_Hex( void (*Output)(char), uint8_t Byte );
|
||||
void Format_Hex( void (*Output)(char), uint16_t Word );
|
||||
void Format_Hex( void (*Output)(char), uint32_t Word );
|
||||
void Format_MAC( void (*Output)(char), uint8_t *MAC, uint8_t Len=6);
|
||||
|
||||
void Format_UnsDec ( void (*Output)(char), uint16_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
void Format_SignDec( void (*Output)(char), int16_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0, uint8_t NoPlus=0);
|
||||
|
||||
void Format_UnsDec ( void (*Output)(char), uint32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
void Format_SignDec( void (*Output)(char), int32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0, uint8_t NoPlus=0);
|
||||
|
||||
void Format_UnsDec ( void (*Output)(char), uint64_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
void Format_SignDec( void (*Output)(char), int64_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0, uint8_t NoPlus=0);
|
||||
|
||||
uint8_t Format_String(char *Out, const char *String);
|
||||
uint8_t Format_String(char *Out, const char *String, uint8_t MinLen, uint8_t MaxLen);
|
||||
|
||||
uint8_t Format_UnsDec (char *Out, uint32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
uint8_t Format_SignDec(char *Out, int32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0, uint8_t NoPlus=0);
|
||||
|
||||
uint8_t Format_Hex( char *Output, uint8_t Byte );
|
||||
uint8_t Format_Hex( char *Output, uint16_t Word );
|
||||
uint8_t Format_Hex( char *Output, uint32_t Word );
|
||||
uint8_t Format_Hex( char *Output, uint32_t Word, uint8_t Digits);
|
||||
uint8_t Format_Hex( char *Output, uint64_t Word );
|
||||
// uint8_t Format_Hex( char *Output, uint64_t Word, uint8_t Digits);
|
||||
|
||||
template <class Type>
|
||||
uint8_t Format_Hex( char *Output, Type Word, uint8_t Digits)
|
||||
{ for(uint8_t Idx=Digits; Idx>0; )
|
||||
{ Output[--Idx]=HexDigit(Word&0x0F);
|
||||
Word>>=4; }
|
||||
return Digits; }
|
||||
|
||||
uint8_t Format_HHcMMcSS(char *Out, uint32_t Time);
|
||||
uint8_t Format_HHMMSS(char *Out, uint32_t Time);
|
||||
void Format_HHMMSS(void (*Output)(char), uint32_t Time);
|
||||
|
||||
uint8_t Format_Latitude (char *Out, int32_t Lat); // [1/600000deg] => DDMM.MMMMs
|
||||
uint8_t Format_Longitude(char *Out, int32_t Lon); // [1/600000deg] => DDDMM.MMMMs
|
||||
|
||||
int8_t Read_Hex1(char Digit);
|
||||
|
||||
int8_t Read_Dec1(char Digit); // convert single digit into an integer
|
||||
inline int8_t Read_Dec1(const char *Inp) { return Read_Dec1(Inp[0]); }
|
||||
int8_t Read_Dec2(const char *Inp); // convert two digit decimal number into an integer
|
||||
int16_t Read_Dec3(const char *Inp); // convert three digit decimal number into an integer
|
||||
int16_t Read_Dec4(const char *Inp); // convert three digit decimal number into an integer
|
||||
|
||||
template <class Type>
|
||||
int8_t Read_Hex(Type &Int, const char *Inp, uint8_t MaxDig=0) // convert variable number of digits hexadecimal number into an integer
|
||||
{ if(Inp==0) return 0;
|
||||
if(MaxDig==0) MaxDig=2*sizeof(Type);
|
||||
Int=0; int8_t Len=0;
|
||||
for( ; MaxDig; MaxDig--)
|
||||
{ int8_t Dig=Read_Hex1(Inp[Len]); if(Dig<0) break;
|
||||
Int = (Int<<4) + Dig; Len++; }
|
||||
return Len; } // return number of characters read
|
||||
|
||||
template <class Type>
|
||||
int8_t Read_UnsDec(Type &Int, const char *Inp) // convert variable number of digits unsigned decimal number into an integer
|
||||
{ Int=0; int8_t Len=0;
|
||||
if(Inp==0) return 0;
|
||||
for( ; ; )
|
||||
{ int8_t Dig=Read_Dec1(Inp[Len]); if(Dig<0) break;
|
||||
Int = 10*Int + Dig; Len++; }
|
||||
return Len; } // return number of characters read
|
||||
|
||||
template <class Type>
|
||||
int8_t Read_SignDec(Type &Int, const char *Inp) // convert signed decimal number into in16_t or int32_t
|
||||
{ Int=0; int8_t Len=0;
|
||||
if(Inp==0) return 0;
|
||||
char Sign=Inp[0];
|
||||
if((Sign=='+')||(Sign=='-')) Len++;
|
||||
Len+=Read_UnsDec(Int, Inp+Len); if(Sign=='-') Int=(-Int);
|
||||
return Len; } // return number of characters read
|
||||
|
||||
template <class Type>
|
||||
int8_t Read_Int(Type &Value, const char *Inp)
|
||||
{ Value=0; int8_t Len=0;
|
||||
if(Inp==0) return 0;
|
||||
char Sign=Inp[0]; int8_t Dig;
|
||||
if((Sign=='+')||(Sign=='-')) Len++;
|
||||
if((Inp[Len]=='0')&&(Inp[Len+1]=='x'))
|
||||
{ Len+=2; Dig=Read_Hex(Value, Inp+Len); }
|
||||
else
|
||||
{ Dig=Read_UnsDec(Value, Inp+Len); }
|
||||
if(Dig<=0) return Dig;
|
||||
Len+=Dig;
|
||||
if(Sign=='-') Value=(-Value); return Len; }
|
||||
|
||||
template <class Type>
|
||||
int8_t Read_Float1(Type &Value, const char *Inp) // read floating point, take just one digit after decimal point
|
||||
{ Value=0; int8_t Len=0;
|
||||
if(Inp==0) return 0;
|
||||
char Sign=Inp[0]; int8_t Dig;
|
||||
if((Sign=='+')||(Sign=='-')) Len++;
|
||||
Len+=Read_UnsDec(Value, Inp+Len); Value*=10;
|
||||
if(Inp[Len]!='.') goto Ret;
|
||||
Len++;
|
||||
Dig=Read_Dec1(Inp[Len]); if(Dig<0) goto Ret;
|
||||
Value+=Dig; Len++;
|
||||
Dig=Read_Dec1(Inp[Len]); if(Dig>=5) Value++;
|
||||
Ret: if(Sign=='-') Value=(-Value); return Len; }
|
||||
|
||||
int8_t Read_LatDDMMSS(int32_t &Lat, const char *Inp);
|
||||
int8_t Read_LonDDMMSS(int32_t &Lon, const char *Inp);
|
||||
|
||||
#endif // __FORMAT_H__
|
||||
|
|
@ -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; }
|
||||
|
||||
|
|
@ -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__
|
||||
|
|
@ -0,0 +1,157 @@
|
|||
#include "intmath.h"
|
||||
|
||||
static const uint32_t SinePoints=256;
|
||||
// static const uint32_t SineScale=0x80000000;
|
||||
static const int32_t SineTable[SinePoints/4+1] =
|
||||
{ 0x00000000, 0x03242ABF, 0x0647D97C, 0x096A9049, 0x0C8BD35E, 0x0FAB272B, 0x12C8106F, 0x15E21445,
|
||||
0x18F8B83C, 0x1C0B826A, 0x1F19F97B, 0x2223A4C5, 0x25280C5E, 0x2826B928, 0x2B1F34EB, 0x2E110A62,
|
||||
0x30FBC54D, 0x33DEF287, 0x36BA2014, 0x398CDD32, 0x3C56BA70, 0x3F1749B8, 0x41CE1E65, 0x447ACD50,
|
||||
0x471CECE7, 0x49B41533, 0x4C3FDFF4, 0x4EBFE8A5, 0x5133CC94, 0x539B2AF0, 0x55F5A4D2, 0x5842DD54,
|
||||
0x5A82799A, 0x5CB420E0, 0x5ED77C8A, 0x60EC3830, 0x62F201AC, 0x64E88926, 0x66CF8120, 0x68A69E81,
|
||||
0x6A6D98A4, 0x6C242960, 0x6DCA0D14, 0x6F5F02B2, 0x70E2CBC6, 0x72552C85, 0x73B5EBD1, 0x7504D345,
|
||||
0x7641AF3D, 0x776C4EDB, 0x78848414, 0x798A23B1, 0x7A7D055B, 0x7B5D039E, 0x7C29FBEE, 0x7CE3CEB2,
|
||||
0x7D8A5F40, 0x7E1D93EA, 0x7E9D55FC, 0x7F0991C4, 0x7F62368F, 0x7FA736B4, 0x7FD8878E, 0x7FF62182,
|
||||
0x7FFFFFFF } ;
|
||||
|
||||
// get Sine from the SineTable
|
||||
// Angle is 0..255 which corresponds to <0..2*PI)
|
||||
int32_t IntSine(uint8_t Angle)
|
||||
{ uint8_t Idx=Angle;
|
||||
if(Angle&0x80) { Angle^=0x40; Idx=(-Idx); }
|
||||
if(Angle&0x40) Idx=0x80-Idx;
|
||||
int32_t Val=SineTable[Idx];
|
||||
if(Angle&0x80) Val=(-Val);
|
||||
return Val; }
|
||||
|
||||
// precise Sine with for 16-bit angles 2nd derivative interpolation
|
||||
// max. result error is about 2.3e-7
|
||||
int32_t IntSine(uint16_t Angle)
|
||||
{ uint8_t Int = Angle>>8;
|
||||
int32_t Frac = Angle&0x00FF;
|
||||
int32_t Value = IntSine(Int); Int+=1;
|
||||
int32_t Delta = (IntSine(Int)-Value)>>8;
|
||||
Value += Frac*Delta;
|
||||
// printf(" [%02X %02X %+11.8f] ", Int-1, Frac, (double)Value/(uint32_t)0x80000000);
|
||||
int32_t Frac2 = (Frac*(Frac-0x100));
|
||||
const int32_t Coeff = (int32_t)floor(2*M_PI*M_PI*0x80+0.5);
|
||||
int32_t Deriv2 = (Coeff*(Value>>12));
|
||||
int32_t Corr = ((Deriv2>>16)*Frac2)>>11;
|
||||
Value -= Corr;
|
||||
// printf("[%04X %+11.8f %+11.8f] ", -Frac2, (double)Deriv2/(uint32_t)0x80000000, (double)Corr/(uint32_t)0x80000000);
|
||||
return Value; }
|
||||
|
||||
// precise Sine for 32-bit angles with 2nd derivative interpolation
|
||||
// max. result error is about 2.3e-7
|
||||
int32_t IntSine(uint32_t Angle)
|
||||
{ uint8_t Int = Angle>>24;
|
||||
int32_t Frac = Angle&0x00FFFFFF;
|
||||
int32_t Value = IntSine(Int); Int+=1;
|
||||
int32_t Delta = (IntSine(Int)-Value);
|
||||
Value += ((int64_t)Frac*(int64_t)Delta)>>24;
|
||||
// printf(" [%02X %06X %+11.8f] ", Int-1, Frac, (double)Value/(uint32_t)0x80000000);
|
||||
int64_t Frac2 = ((int64_t)Frac*(Frac-0x1000000))>>32;
|
||||
const int64_t Coeff = (int64_t)floor(2*M_PI*M_PI*0x4000000+0.5);
|
||||
int64_t Deriv2 = (Coeff*Value)>>26;
|
||||
int64_t Corr = (Deriv2*Frac2)>>32;
|
||||
Value -= Corr;
|
||||
// printf(" [%04X %+11.8f %+11.8f] ", -Frac2, (double)Deriv2/(uint32_t)0x80000000, (double)Corr/(uint32_t)0x80000000);
|
||||
return Value; }
|
||||
|
||||
// Less precise sine for 16-bit angles
|
||||
// source: http://www.coranac.com/2009/07/sines/
|
||||
/// A sine approximation via a fourth-order cosine approx.
|
||||
/// @param x angle (with 2^16 units/circle)
|
||||
/// @return Sine value (Q12)
|
||||
int16_t Isin(int16_t Angle) // input: full angle = 16-bit range
|
||||
{
|
||||
int32_t x=Angle;
|
||||
int32_t c, y;
|
||||
static const int qN= 14, qA= 12, B=19900, C=3516;
|
||||
|
||||
c= x<<(30-qN); // Semi-circle info into carry.
|
||||
x -= 1<<qN; // sine -> cosine calc
|
||||
|
||||
x= x<<(31-qN); // Mask with PI
|
||||
x= x>>(31-qN); // Note: SIGNED shift! (to qN)
|
||||
x= (x*x)>>(2*qN-14); // x=x^2 To Q14
|
||||
|
||||
y= B - ((x*C)>>14); // B - x^2*C
|
||||
y= (1<<qA)-((x*y)>>16); // A - x^2*(B-x^2*C)
|
||||
|
||||
return c>=0 ? y : -y; } // result: -4096..+4096 (max. error = +/-12)
|
||||
|
||||
|
||||
/*
|
||||
int16_t IntAtan2(int16_t Y, int16_t X)
|
||||
{ uint16_t Angle=0; // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
|
||||
if(Y<0) { Angle+=0x8000; X=(-X); Y=(-Y); } // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
|
||||
if(X<0) { Angle+=0x4000; int16_t tmp=Y; Y=(-X); X=tmp; } // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
|
||||
if(X<Y) { Angle+=0x4000; int16_t tmp=Y; Y=(-X); X=tmp; } // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
|
||||
if(X==0) return 0;
|
||||
int16_t D = (((int32_t)Y<<14) + (X>>1))/ X;
|
||||
int16_t DD = ((int32_t)D*(int32_t)D)>>14;
|
||||
int16_t DDD = ((int32_t)DD*(int32_t)D)>>14;
|
||||
// printf(" %08X %08X %08X\n", D, DD, DDD);
|
||||
Angle += ((5*D)>>3) - (DDD>>3); return Angle; } // good to about 1/2 degree
|
||||
*/
|
||||
|
||||
int16_t IntAtan2(int16_t Y, int16_t X)
|
||||
{ uint16_t Angle=0; // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
|
||||
const int32_t CosPi8 = 30274; // cos(PI/8)*32768
|
||||
const int32_t SinPi8 = 12540; // sin(PI/8)*32768
|
||||
if(Y<0) { Angle+=0x8000; X=(-X); Y=(-Y); } // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
|
||||
if(X<0) { Angle+=0x4000; int16_t tmp=Y; Y=(-X); X=tmp; } // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
|
||||
if(X<Y) { Angle+=0x4000; int16_t tmp=Y; Y=(-X); X=tmp; } // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
|
||||
if(X==0) return 0;
|
||||
int16_t Yc = (Y<<1)+(Y>>1);
|
||||
if(Y<0)
|
||||
{ if(X<(-Yc))
|
||||
{ int32_t NewX = CosPi8*X - SinPi8*Y;
|
||||
int32_t NewY = SinPi8*X + CosPi8*Y;
|
||||
X=NewX>>15; if(NewX&0x4000) X+=1;
|
||||
Y=NewY>>15; if(NewY&0x4000) Y+=1;
|
||||
Angle-=0x1000; }
|
||||
} else // Y>=0
|
||||
{ if(X<Yc)
|
||||
{ int32_t NewX = CosPi8*X + SinPi8*Y;
|
||||
int32_t NewY = - SinPi8*X + CosPi8*Y;
|
||||
X=NewX>>15; if(NewX&0x4000) X+=1;
|
||||
Y=NewY>>15; if(NewY&0x4000) Y+=1;
|
||||
Angle+=0x1000; }
|
||||
} // printf(" [%+5d,%+5d] %04X\n", X, Y, Angle);
|
||||
int16_t D = (((int32_t)Y<<14) + (X>>1))/ X;
|
||||
// int16_t D = ((int32_t)Y<<14) / X;
|
||||
int16_t DD = ((int32_t)D*(int32_t)D)>>14;
|
||||
int16_t DDD = ((int32_t)DD*(int32_t)D)>>14;
|
||||
// printf(" %08X %08X %08X\n", D, DD, DDD);
|
||||
Angle += ((5*D)>>3) - (DDD>>3);
|
||||
return Angle; } // good to about 1/6 degree
|
||||
|
||||
/*
|
||||
// integer square root
|
||||
uint32_t IntSqrt(uint32_t Inp)
|
||||
{ uint32_t Out = 0;
|
||||
uint32_t Mask = 0x40000000;
|
||||
|
||||
while(Mask>Inp) Mask>>=2;
|
||||
while(Mask)
|
||||
{ if(Inp >= (Out+Mask))
|
||||
{ Inp -= Out+Mask; Out += Mask<<1; }
|
||||
Out>>=1; Mask>>=2; }
|
||||
if(Inp>Out) Out++;
|
||||
|
||||
return Out; }
|
||||
|
||||
uint64_t IntSqrt(uint64_t Inp)
|
||||
{ uint64_t Out = 0;
|
||||
uint64_t Mask = 0x4000000000000000;
|
||||
|
||||
while(Mask>Inp) Mask>>=2;
|
||||
while(Mask)
|
||||
{ if(Inp >= (Out+Mask))
|
||||
{ Inp -= Out+Mask; Out += Mask<<1; }
|
||||
Out>>=1; Mask>>=2; }
|
||||
if(Inp>Out) Out++;
|
||||
|
||||
return Out; }
|
||||
*/
|
||||
|
|
@ -0,0 +1,68 @@
|
|||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#ifndef __INTMATH_H__
|
||||
#define __INTMATH_H__
|
||||
|
||||
const uint32_t IntSine_Scale=0x80000000;
|
||||
|
||||
// get Sine from the SineTable
|
||||
// Angle is 0..255 which corresponds to <0..2*PI)
|
||||
int32_t IntSine(uint8_t Angle);
|
||||
|
||||
// precise Sine with for 16-bit angles 2nd derivative interpolation
|
||||
// max. result error is about 2.3e-7
|
||||
int32_t IntSine(uint16_t Angle);
|
||||
|
||||
// precise Sine for 32-bit angles with 2nd derivative interpolation
|
||||
// max. result error is about 2.3e-7
|
||||
int32_t IntSine(uint32_t Angle);
|
||||
|
||||
// less precise sine for 16-bit angles
|
||||
const int16_t Isine_Scale=0x4000;
|
||||
int16_t Isin(int16_t Angle);
|
||||
int16_t inline Icos(int16_t Angle) { return Isin(Angle+0x4000); }
|
||||
|
||||
// atan2(Y, X)
|
||||
// max. result error is 1/6 degree
|
||||
int16_t IntAtan2(int16_t Y, int16_t X);
|
||||
|
||||
// integer square root
|
||||
// uint32_t IntSqrt(uint32_t Inp);
|
||||
// uint64_t IntSqrt(uint64_t Inp);
|
||||
|
||||
template<class Type> // integer square root for 16-bit or 32-bit
|
||||
Type IntSqrt(Type Inp) // must be made with unsigned type or signed types with _positive_ values
|
||||
{ Type Out = 0;
|
||||
Type Mask = 1; Mask<<=(sizeof(Type)*8-2);
|
||||
|
||||
while(Mask>Inp) Mask>>=2;
|
||||
while(Mask)
|
||||
{ if(Inp >= (Out+Mask))
|
||||
{ Inp -= Out+Mask; Out += Mask<<1; }
|
||||
Out>>=1; Mask>>=2; }
|
||||
if(Inp>Out) Out++;
|
||||
|
||||
return Out; }
|
||||
|
||||
// Distance = sqrt(dX*dX+dY*dY)
|
||||
|
||||
inline uint32_t IntDistance(int32_t dX, int32_t dY) { return IntSqrt((uint64_t)((int64_t)dX*dX + (int64_t)dY*dY)); }
|
||||
inline uint16_t IntDistance(int16_t dX, int16_t dY) { return IntSqrt((uint32_t)((int32_t)dX*dX + (int32_t)dY*dY)); }
|
||||
|
||||
template <class IntType>
|
||||
IntType IntFastDistance(IntType dX, IntType dY) // after: http://www.flipcode.com/archives/Fast_Approximate_Distance_Functions.shtml
|
||||
{ IntType min, max, approx;
|
||||
|
||||
if(dX<0) dX = -dX;
|
||||
if(dY<0) dY = -dY;
|
||||
|
||||
if(dX<dY) { min = dX; max = dY; }
|
||||
else { min = dY; max = dX; }
|
||||
|
||||
approx = max*1007 + min*441;
|
||||
if( max < (min<<4) ) approx -= max*40;
|
||||
|
||||
return (approx+512)>>10; }
|
||||
|
||||
#endif // of __INTMATH_H__
|
||||
|
|
@ -0,0 +1,751 @@
|
|||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "ldpc.h"
|
||||
|
||||
#ifndef __AVR__
|
||||
#include <math.h>
|
||||
#endif
|
||||
|
||||
#ifdef __AVR__
|
||||
#include <avr/pgmspace.h>
|
||||
#endif
|
||||
|
||||
// ===================================================================================================================
|
||||
|
||||
// FindVectors65432bit(10,20,23, 500) => 208, Delta=2579
|
||||
|
||||
// every row represents a parity check to be performed on the received codeword
|
||||
static const uint32_t LDPC_ParityCheck_n208k160[48][7]
|
||||
#ifdef __AVR__
|
||||
PROGMEM
|
||||
#endif
|
||||
= { // parity check vectors: 48 vectors for 48 parity checks
|
||||
// Eaech vector applied to the data packet should yield even number of bits
|
||||
{ 0x00000805, 0x00000020, 0x04000000, 0x20000000, 0x00000040, 0x00044020, 0x00000000 },
|
||||
{ 0x00000001, 0x00800800, 0x00000000, 0x00000000, 0x00000000, 0x10010000, 0x00008C98 },
|
||||
{ 0x00004001, 0x01000080, 0x80000400, 0x00000000, 0x08000200, 0x00200000, 0x00000005 },
|
||||
{ 0x00000101, 0x20000200, 0x00000022, 0x00000000, 0x00000000, 0xCC008000, 0x00005002 },
|
||||
{ 0x00000401, 0x00000000, 0x00004900, 0x00000020, 0x00000000, 0x20C00349, 0x00000020 },
|
||||
{ 0x03140001, 0x00000002, 0x00000000, 0x40000001, 0x41534100, 0x00102C00, 0x00002000 },
|
||||
{ 0x04008800, 0x82000642, 0x00000000, 0x00000020, 0x88040020, 0x03000010, 0x00000400 },
|
||||
{ 0x00000802, 0x20000000, 0x02000014, 0x01200000, 0x04000403, 0x00800004, 0x0000A004 },
|
||||
{ 0x02020820, 0x00000000, 0x80020820, 0x10190040, 0x30000000, 0x00000002, 0x00000900 },
|
||||
{ 0x40804950, 0x00090000, 0x00000000, 0x00021204, 0x40001000, 0x10001100, 0x00000000 },
|
||||
{ 0x08000A00, 0x00020008, 0x00040000, 0x02400010, 0x01002000, 0x40280280, 0x00000010 },
|
||||
{ 0x00000000, 0x00008010, 0x118000A0, 0x00040080, 0x01000084, 0x00040100, 0x00000444 },
|
||||
{ 0x20040108, 0x18000000, 0x08608800, 0x0000000A, 0x08000010, 0x00040080, 0x00008000 },
|
||||
{ 0x00004080, 0x00422201, 0x00010000, 0x0000A400, 0x00400800, 0x00840000, 0x00000800 },
|
||||
{ 0x00000000, 0x60200000, 0x80100240, 0x08000021, 0x02800000, 0x100C0000, 0x00000000 },
|
||||
{ 0x00001000, 0x01010002, 0x00082001, 0x04000000, 0x00000001, 0x00040002, 0x00004030 },
|
||||
{ 0x00002300, 0x04000000, 0xA0080000, 0x20004000, 0x00028000, 0x00800000, 0x00000400 },
|
||||
{ 0x00004000, 0x00104100, 0x40041028, 0x24000020, 0x00200000, 0x00100000, 0x00008000 },
|
||||
{ 0x08011000, 0x20040000, 0x00000000, 0xA0800000, 0x08090000, 0x00000100, 0x00000A00 },
|
||||
{ 0x10180000, 0x00000204, 0x00002800, 0x20400800, 0x00000000, 0x10000000, 0x00000004 },
|
||||
{ 0x00000000, 0xC0000000, 0x10200000, 0x20028000, 0x20000000, 0x80000008, 0x00002011 },
|
||||
{ 0x82004000, 0x20000000, 0x04202000, 0x00000000, 0x00000000, 0x00020200, 0x00000400 },
|
||||
{ 0x08600000, 0x00001200, 0x94000000, 0x00000000, 0x40000008, 0x00000000, 0x00008020 },
|
||||
{ 0x04040000, 0x04010000, 0x04100000, 0x00000100, 0x00200000, 0x40000008, 0x00000804 },
|
||||
{ 0x00000200, 0x00000110, 0x04000100, 0x00000000, 0x28400400, 0x10000000, 0x00004000 },
|
||||
{ 0x00080000, 0x00000080, 0x04001000, 0x01882007, 0x00008024, 0x04000001, 0x00000010 },
|
||||
{ 0x20200000, 0x00000020, 0x00010040, 0x81000800, 0x10001000, 0x00300008, 0x00004400 },
|
||||
{ 0x90000010, 0x89841021, 0x00000118, 0x08080000, 0x00020000, 0x40000000, 0x00000040 },
|
||||
{ 0x04C20000, 0x10404034, 0x00000000, 0x00004000, 0x00810001, 0x04000200, 0x00000009 },
|
||||
{ 0x40102000, 0x020020A0, 0x40100000, 0x00100080, 0x00080400, 0x80030080, 0x00000020 },
|
||||
{ 0x00010000, 0x04020920, 0x00000200, 0x00060000, 0x00000218, 0x01002007, 0x00001000 },
|
||||
{ 0x00020008, 0x00A08040, 0x00080000, 0x40001400, 0x04200040, 0x80200001, 0x00000200 },
|
||||
{ 0x40000402, 0x01100000, 0x20808000, 0x00008000, 0x10100060, 0x00080000, 0x00001008 },
|
||||
{ 0x200010A0, 0x00000000, 0x01040100, 0x00000104, 0x02040042, 0x08012000, 0x00000001 },
|
||||
{ 0x01000000, 0x50000880, 0x00000092, 0x14400000, 0x00001840, 0x02400000, 0x00000000 },
|
||||
{ 0x00000010, 0x02000000, 0x00014000, 0x00200018, 0x00000240, 0x04000800, 0x00000180 },
|
||||
{ 0x00008000, 0x00880008, 0x08000044, 0x00100000, 0x00000004, 0x00400820, 0x00001001 },
|
||||
{ 0x01000000, 0x00002000, 0x02004001, 0x00000042, 0x00000000, 0x09201020, 0x00000048 },
|
||||
{ 0x00800000, 0x01000400, 0x00400002, 0xC0002000, 0x00002080, 0x00010064, 0x00000100 },
|
||||
{ 0x00000400, 0x08400840, 0x00000400, 0x00000890, 0x00008102, 0x00000020, 0x00000002 },
|
||||
{ 0x00200040, 0x00000081, 0x00000000, 0x02050000, 0x04940000, 0x20008020, 0x00000080 },
|
||||
{ 0x00000404, 0x00800000, 0x00001000, 0x00014000, 0x00082200, 0x0A000400, 0x00000000 },
|
||||
{ 0x0000A024, 0x00000000, 0x00000402, 0x08A01000, 0x00004010, 0x20000000, 0x00000008 },
|
||||
{ 0x00480046, 0x00008000, 0x00000208, 0x00000048, 0x00000000, 0x00410010, 0x00000002 },
|
||||
{ 0x0000008C, 0x00044C00, 0x00824004, 0x00000200, 0x00000000, 0x00028000, 0x00000000 },
|
||||
{ 0x10010004, 0x00080000, 0x43008000, 0x10000400, 0x80000100, 0x00000040, 0x00000080 },
|
||||
{ 0x80000000, 0x0020000C, 0x20420480, 0x00000100, 0x00000008, 0x00005410, 0x00000080 },
|
||||
{ 0x00000000, 0x00101000, 0x08000001, 0x02000200, 0x82004A80, 0x00004000, 0x00000202 }
|
||||
} ;
|
||||
|
||||
|
||||
const uint8_t LDPC_ParityCheckIndex_n208k160[48][24]
|
||||
#ifdef __AVR__
|
||||
PROGMEM
|
||||
#endif
|
||||
= { // number of, indicies to bits to be taken for parity checks
|
||||
{ 10, 0, 2, 11, 37, 90, 125, 134, 165, 174, 178 },
|
||||
{ 11, 0, 43, 55, 176, 188, 195, 196, 199, 202, 203, 207 },
|
||||
{ 11, 0, 14, 39, 56, 74, 95, 137, 155, 181, 192, 194, },
|
||||
{ 14, 0, 8, 41, 61, 65, 69, 175, 186, 187, 190, 191, 193, 204, 206 },
|
||||
{ 15, 0, 10, 72, 75, 78, 101, 160, 163, 166, 168, 169, 182, 183, 189, 197 },
|
||||
{ 21, 0, 18, 20, 24, 25, 33, 96, 126, 136, 142, 144, 145, 148, 150, 152, 158, 170, 171, 173, 180, 205 },
|
||||
{ 18, 11, 15, 26, 33, 38, 41, 42, 57, 63, 101, 133, 146, 155, 159, 164, 184, 185, 202 },
|
||||
{ 17, 1, 11, 61, 66, 68, 89, 117, 120, 128, 129, 138, 154, 162, 183, 194, 205, 207 },
|
||||
{ 18, 5, 11, 17, 25, 69, 75, 81, 95, 102, 112, 115, 116, 124, 156, 157, 161, 200, 203 },
|
||||
{ 18, 4, 6, 8, 11, 14, 23, 30, 48, 51, 98, 105, 108, 113, 140, 158, 168, 172, 188 },
|
||||
{ 17, 9, 11, 27, 35, 49, 82, 100, 118, 121, 141, 152, 167, 169, 179, 181, 190, 196 },
|
||||
{ 17, 36, 47, 69, 71, 87, 88, 92, 103, 114, 130, 135, 152, 168, 178, 194, 198, 202 },
|
||||
{ 18, 3, 8, 18, 29, 59, 60, 75, 79, 85, 86, 91, 97, 99, 132, 155, 167, 178, 207 },
|
||||
{ 16, 7, 14, 32, 41, 45, 49, 54, 80, 106, 109, 111, 139, 150, 178, 183, 203 },
|
||||
{ 15, 53, 61, 62, 70, 73, 84, 95, 96, 101, 123, 151, 153, 178, 179, 188 },
|
||||
{ 14, 12, 33, 48, 56, 64, 77, 83, 122, 128, 161, 178, 196, 197, 206 },
|
||||
{ 13, 8, 9, 13, 58, 83, 93, 95, 110, 125, 143, 145, 183, 202 },
|
||||
{ 15, 14, 40, 46, 52, 67, 69, 76, 82, 94, 101, 122, 125, 149, 180, 207 },
|
||||
{ 14, 12, 16, 27, 50, 61, 119, 125, 127, 144, 147, 155, 168, 201, 203 },
|
||||
{ 12, 19, 20, 28, 34, 41, 75, 77, 107, 118, 125, 188, 194 },
|
||||
{ 13, 62, 63, 85, 92, 111, 113, 125, 157, 163, 191, 192, 196, 205 },
|
||||
{ 10, 14, 25, 31, 61, 77, 85, 90, 169, 177, 202 },
|
||||
{ 12, 21, 22, 27, 41, 44, 90, 92, 95, 131, 158, 197, 207 },
|
||||
{ 12, 18, 26, 48, 58, 84, 90, 104, 149, 163, 190, 194, 203 },
|
||||
{ 11, 9, 36, 40, 72, 90, 138, 150, 155, 157, 188, 206 },
|
||||
{ 17, 19, 39, 76, 90, 96, 97, 98, 109, 115, 119, 120, 130, 133, 143, 160, 186, 196 },
|
||||
{ 15, 21, 29, 37, 70, 80, 107, 120, 127, 140, 156, 163, 180, 181, 202, 206 },
|
||||
{ 19, 4, 28, 31, 32, 37, 44, 50, 55, 56, 59, 63, 67, 68, 72, 115, 123, 145, 190, 198 },
|
||||
{ 18, 17, 22, 23, 26, 34, 36, 37, 46, 54, 60, 110, 128, 144, 151, 169, 186, 192, 195 },
|
||||
{ 18, 13, 20, 30, 37, 39, 45, 57, 84, 94, 103, 116, 138, 147, 167, 176, 177, 191, 197 },
|
||||
{ 18, 16, 37, 40, 43, 49, 58, 73, 113, 114, 131, 132, 137, 160, 161, 162, 173, 184, 204 },
|
||||
{ 17, 3, 17, 38, 47, 53, 55, 83, 106, 108, 126, 134, 149, 154, 160, 181, 191, 201 },
|
||||
{ 16, 1, 10, 30, 52, 56, 79, 87, 93, 111, 133, 134, 148, 156, 179, 195, 204 },
|
||||
{ 17, 5, 7, 12, 29, 72, 82, 88, 98, 104, 129, 134, 146, 153, 173, 176, 187, 192 },
|
||||
{ 16, 24, 39, 43, 60, 62, 65, 68, 71, 118, 122, 124, 134, 139, 140, 182, 185 },
|
||||
{ 13, 4, 57, 78, 80, 99, 100, 117, 134, 137, 171, 186, 199, 200 },
|
||||
{ 14, 15, 35, 51, 55, 66, 70, 91, 116, 130, 165, 171, 182, 192, 204 },
|
||||
{ 14, 24, 45, 64, 78, 89, 97, 102, 165, 172, 181, 184, 187, 195, 198 },
|
||||
{ 15, 23, 42, 56, 65, 86, 109, 126, 127, 135, 141, 162, 165, 166, 176, 200 },
|
||||
{ 14, 10, 38, 43, 54, 59, 74, 100, 103, 107, 129, 136, 143, 165, 193 },
|
||||
{ 15, 6, 21, 32, 39, 112, 114, 121, 146, 148, 151, 154, 165, 175, 189, 199 },
|
||||
{ 12, 2, 10, 55, 76, 110, 112, 137, 141, 147, 170, 185, 187 },
|
||||
{ 14, 2, 5, 13, 15, 65, 74, 108, 117, 119, 123, 132, 142, 189, 195 },
|
||||
{ 14, 1, 2, 6, 19, 22, 47, 67, 73, 99, 102, 164, 176, 182, 193 },
|
||||
{ 14, 2, 3, 7, 42, 43, 46, 50, 66, 78, 81, 87, 105, 175, 177 },
|
||||
{ 14, 2, 16, 28, 51, 79, 88, 89, 94, 106, 124, 136, 159, 166, 199 },
|
||||
{ 16, 31, 34, 35, 53, 71, 74, 81, 86, 93, 104, 131, 164, 170, 172, 174, 199 },
|
||||
{ 15, 44, 52, 64, 91, 105, 121, 135, 137, 139, 142, 153, 159, 174, 193, 201 }
|
||||
} ;
|
||||
|
||||
static const uint8_t LDPC_BitWeight_n208k160[208] // weight of parity checks for every codeword bit
|
||||
#ifdef __AVR__
|
||||
PROGMEM
|
||||
#endif
|
||||
= { 6, 3, 6, 3, 3, 3, 3, 3, 4, 3, 4, 6, 3, 3, 5, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3,
|
||||
3, 3, 3, 3, 3, 6, 3, 5, 3, 5, 3, 5, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 5, 5, 3, 3, 3, 3, 5, 3, 3,
|
||||
3, 4, 3, 3, 3, 4, 3, 3, 4, 3, 4, 4, 3, 3, 4, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 6, 3, 3, 3, 3, 5,
|
||||
3, 3, 3, 3, 3, 4, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 6, 3, 3,
|
||||
3, 3, 3, 3, 3, 3, 6, 3, 3, 5, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 5, 3, 3, 3, 3,
|
||||
4, 3, 3, 4, 3, 6, 3, 3, 4, 4, 3, 3, 3, 3, 3, 3, 5, 3, 6, 3, 3, 5, 4, 4, 3, 3, 4, 4, 5, 3, 4, 4,
|
||||
5, 4, 5, 5, 5, 4, 3, 5, 3, 3, 6, 5, 4, 3, 4, 5 } ;
|
||||
|
||||
// every row represents the generator for a parity bit
|
||||
static const uint32_t LDPC_ParityGen_n208k160[48][5]
|
||||
#ifdef __AVR__
|
||||
PROGMEM
|
||||
#endif
|
||||
= { // Parity bits generator: 48 vectors to generate 48 parity bits
|
||||
// Each vector applied to the user data yields a corresponding parity bit
|
||||
{ 0x40A90281, 0x9159D249, 0xCE9D516B, 0x2FDEED0B, 0xD9267CD4 },
|
||||
{ 0xCCBC0FC3, 0xCC4FA4BC, 0x811EC3D0, 0xB07EC1B3, 0xA3B8E8D8 },
|
||||
{ 0x66418D56, 0x3B85ADFF, 0xD2A6532E, 0x48CF52E4, 0x6A16586D },
|
||||
{ 0x44C71240, 0x2C94631F, 0x15F15A4A, 0x7459D901, 0x037863CC },
|
||||
{ 0x7386D718, 0x7F6C9623, 0x738E2E0C, 0xD2351593, 0xEF358669 },
|
||||
{ 0x7BF87232, 0x9E4CCD68, 0xBB82590E, 0x9C9292EA, 0x4CE2AEB9 },
|
||||
{ 0xAA8436BC, 0x94A61C4D, 0x1DA89B11, 0x72EAF204, 0x34D3A041 },
|
||||
{ 0xBCE77760, 0x229935B2, 0xAAF85CE3, 0xFFE7B602, 0xF26BCC64 },
|
||||
{ 0xD0C371D0, 0xA553D12F, 0xA0685BF2, 0x5C553C81, 0x0218EB48 },
|
||||
{ 0x8D29034D, 0xEB20A394, 0x1A8C82A3, 0x41B4DA0C, 0x8632F81E },
|
||||
{ 0x15A50876, 0x9BC10F59, 0xF979D1E0, 0xCFF6BD88, 0x88FE5895 },
|
||||
{ 0x037A9ED5, 0xFA5DB837, 0x61395ACA, 0xE65B5839, 0x9A2D9D02 },
|
||||
{ 0xEE70D18F, 0x8AE909C6, 0x8AF5BECA, 0x66968559, 0x1BD9B5E7 },
|
||||
{ 0xC56397AC, 0xD8FF6A30, 0x8E165AF3, 0xC01686B9, 0xEC26BEDC },
|
||||
{ 0xB2D63859, 0xFACA8CFD, 0x7EE85EB7, 0x19BFDA46, 0xC8C1CA52 },
|
||||
{ 0xD7EB4D94, 0x426104DA, 0x124FBD54, 0xBF610A1D, 0x0E615094 },
|
||||
{ 0x68EFE180, 0x15A1549C, 0x18D20289, 0xBD28AD44, 0x8DADDAEC },
|
||||
{ 0xD7EB4D18, 0x426548DA, 0x12CDFD50, 0xBF61081D, 0x0E615094 },
|
||||
{ 0xC92E426E, 0x648641B5, 0xC16A07B9, 0xA52D48AC, 0x842364AB },
|
||||
{ 0xB71DAB61, 0x2B15995C, 0x6BE7E0C1, 0x97ECE351, 0xDF622A04 },
|
||||
{ 0x55CD7406, 0x5E0F3507, 0x23F6C372, 0x7ECAFE84, 0x7E68A8DF },
|
||||
{ 0x97DB831C, 0xD46D648F, 0x14FA22B3, 0x4F875648, 0x94C23936 },
|
||||
{ 0x60D940EC, 0xFCC18797, 0xD0DE7383, 0xF38F22E5, 0x2E7A733E },
|
||||
{ 0xD8C22D55, 0x8D45EB4E, 0xAC695FF3, 0xDED59211, 0x8851288A },
|
||||
{ 0xCE9D11A1, 0xD8E8F438, 0xAF3102EE, 0xCB2FE547, 0xC11845BD },
|
||||
{ 0x61D940EC, 0xACC18F17, 0xD0DE7311, 0xE7CF22E5, 0x2E7A6B7E },
|
||||
{ 0x66AD8025, 0x493D883C, 0x538E9261, 0x5F0E116B, 0xB17492FA },
|
||||
{ 0x747C4C9E, 0x3780804E, 0x29A7B2F1, 0x2838DF6D, 0xA68C11EB },
|
||||
{ 0x7E33E90F, 0x2FB3D8E9, 0x2A9DE538, 0x3AC1ABDC, 0x59C14EAF },
|
||||
{ 0x16B6095E, 0x4883D57E, 0xF765FF4B, 0x431C6EF3, 0xF2C45F6C },
|
||||
{ 0x3F04D4F4, 0xEEA73108, 0x567ECF38, 0x15200560, 0x56AB6942 },
|
||||
{ 0x1E5ECFFE, 0x29426F53, 0x17057060, 0xA774ED7F, 0x4FE7EACB },
|
||||
{ 0xF9F02A12, 0xFADEBEE2, 0xBE67EB8B, 0x5506F594, 0xC5037599 },
|
||||
{ 0x7BF87632, 0x960CC528, 0xBB825D0E, 0x9C929A7A, 0x4CE22FBB },
|
||||
{ 0x6E2BE90F, 0x2FB3DAED, 0x2A9DCD38, 0x1A81A3DC, 0x59C14EAF },
|
||||
{ 0x16B6A97A, 0x4883D57E, 0xF765FB49, 0x4BBC7EF3, 0xF2C41F7C },
|
||||
{ 0x260C82A4, 0xD8645AF5, 0x9913D30A, 0x7158DC67, 0x68526E0A },
|
||||
{ 0x5DAD3406, 0x5E1F6607, 0xF7F2D35A, 0x5ACAFEA4, 0x3E48A8D7 },
|
||||
{ 0xAF04D4E4, 0x67232129, 0x567ECE20, 0x1D280560, 0x56A96942 },
|
||||
{ 0xBA8536B8, 0x94AE1C4D, 0x5EA81B11, 0x62EAF604, 0xB4D3A141 },
|
||||
{ 0xDF522858, 0x25CE2C46, 0x6C1E93BA, 0xDB9FBF4E, 0x9F8AACF9 },
|
||||
{ 0xC92E4E6B, 0x6CD659D5, 0xCD6A03B8, 0x872D423C, 0x0623AF69 },
|
||||
{ 0xD8C20E55, 0x8945EB4E, 0x0C615FF3, 0xFED5D211, 0x8853A88A },
|
||||
{ 0x11EC2FBB, 0xE98188FA, 0x6D02584A, 0x7BF87EBD, 0x0C324421 },
|
||||
{ 0xE1AB0619, 0x62864C22, 0xBC029B88, 0xDC501DA2, 0x3DB63518 },
|
||||
{ 0x85657508, 0xE76CE85B, 0x35A012AB, 0xD7719D8D, 0xC1CE9294 },
|
||||
{ 0x7E33EB0F, 0x2FB3D9F9, 0x2E9DE438, 0x3AC1ABDC, 0x71814AAF },
|
||||
{ 0x55CD3406, 0x5E1F7407, 0x63F2D35A, 0x5ACAFEA4, 0x7E48A8DF }
|
||||
} ;
|
||||
|
||||
|
||||
// ===================================================================================================================
|
||||
|
||||
#ifdef WITH_PPM
|
||||
|
||||
const uint32_t LDPC_ParityCheck_n354k160[194][12] // 354 codeword = 160 user bits + 194 parity checks
|
||||
#ifdef __AVR__
|
||||
PROGMEM
|
||||
#endif
|
||||
= { // parity check vectors
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x28000000, 0x00000000, 0x00000000, 0x00000400, 0x00E00006, 0x00000000, 0x00000080, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x80000000, 0x000C0040, 0x00010000, 0x00180000, 0x00000000, 0x40000040, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x20003000, 0x58000000, 0x00000000, 0x00000000, 0x00010000, 0x00000000, 0x30000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00200000, 0x18000000, 0x00000400, 0x00000000, 0x00000C00, 0x00008000, 0x00000000, 0x20000020, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00003000, 0x00002030, 0x10000800, 0x00000008, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x20000004, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000C00, 0x00000000, 0x0C000000, 0x00000000, 0x40000001, 0x00000008, 0x00004000, 0x00000000, 0x10000000, 0x00000001 },
|
||||
{ 0x00040000, 0x00800001, 0x00000380, 0x00100000, 0x04000000, 0x00000000, 0x00000000, 0x00000000, 0x00004000, 0x00000000, 0x00000010, 0x00000000 },
|
||||
{ 0x80048000, 0x00002200, 0x00000000, 0x00080200, 0x00000000, 0x00000000, 0x00010001, 0x00000800, 0x00000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000020, 0x00000000, 0x00000000, 0x01000000, 0x00000400, 0x00000000, 0x80000000, 0x00000010, 0x00000000, 0x00000000, 0x0D000100, 0x00000002 },
|
||||
{ 0x00000000, 0x00000400, 0x00000000, 0x00000440, 0x00002000, 0x20000000, 0x20000000, 0x00000000, 0x00000000, 0x00000000, 0x02000082, 0x00000000 },
|
||||
{ 0x80000000, 0x00000400, 0x00004200, 0x00000000, 0x00000000, 0x00000000, 0x00014000, 0x00000800, 0x00003000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x81000040, 0x00040000, 0x00002000, 0x8A000000, 0x00000000, 0x00000002 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00801000, 0x04060000, 0x00000800, 0x10000000, 0x01000200, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x20000100, 0x00000000, 0x0400050C, 0x00000000, 0x00000000, 0x00001000, 0x00000040, 0x00000000, 0x00000000 },
|
||||
{ 0x40000000, 0x00000000, 0x00000000, 0x00000088, 0x03000000, 0x04000000, 0x00002000, 0x00000000, 0x00000008, 0x04000000, 0x00000000, 0x00000000 },
|
||||
{ 0x40000000, 0x00008000, 0x00000070, 0x00000080, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000100, 0x00000000, 0x02200000, 0x00000000 },
|
||||
{ 0x00004000, 0x00004000, 0x01000000, 0x00000000, 0x00002400, 0x00000000, 0x80008000, 0x00000000, 0x00000000, 0x02000000, 0x00100008, 0x00000000 },
|
||||
{ 0x00002000, 0x00000000, 0x01000002, 0x00010040, 0x10000200, 0x00000400, 0x00000000, 0x00000000, 0x00000000, 0x01000000, 0x00000000, 0x00000001 },
|
||||
{ 0x40040000, 0x00000000, 0x00000006, 0x00000020, 0x02000000, 0x02000000, 0x00000800, 0x00000000, 0x20000000, 0x00800000, 0x00000000, 0x00000000 },
|
||||
{ 0x00021800, 0x80000000, 0x00000000, 0x10000000, 0x00000000, 0x00000100, 0x00000000, 0x08000008, 0x00000000, 0x00000000, 0x00000014, 0x00000000 },
|
||||
{ 0x00000601, 0x00000002, 0x00000000, 0x00000000, 0x00000000, 0x02800000, 0x00010000, 0x00000000, 0x00000008, 0x00400020, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000140, 0x08000000, 0x01800000, 0x00000000, 0x00000008, 0x00000000, 0x00000002, 0x20100000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000800, 0x0A004000, 0x00000000, 0x00000003, 0x02001004, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00800A00, 0x00000000, 0x00800240, 0x00008080, 0x00000000, 0x00000000, 0x00000000, 0x00200000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00010020, 0x00000400, 0x00010004, 0x00000000, 0x01008000, 0x00000008, 0x00000000, 0x10000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x04000000, 0x20000000, 0x08000000, 0x01400000, 0x00000000, 0x00000800, 0x00000000, 0x00004000, 0x00000082, 0x00000000 },
|
||||
{ 0x00000000, 0x40000000, 0x00400100, 0x00800000, 0x08000000, 0x00008000, 0x00000000, 0x00000008, 0x00000000, 0x00000002, 0x00000002, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00400100, 0x05000000, 0x00000000, 0x00000000, 0x00000440, 0x00010000, 0x00000000, 0x00080000, 0x00000001 },
|
||||
{ 0x00000000, 0x10000000, 0x00200080, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x80010000, 0x02002000, 0x000C0000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00100000, 0x04000000, 0x00000200, 0x00000004, 0x00040000, 0x01000000, 0x40000004, 0x00000000, 0x00000004, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x41100000, 0x00000000, 0x00000220, 0x00000000, 0x40000080, 0x00000000, 0x80000000, 0x00000000, 0x00000000, 0x00000002 },
|
||||
{ 0x00000000, 0x00000000, 0x00142000, 0x00008000, 0x00000000, 0x00000000, 0x00000080, 0x00004000, 0x00000000, 0x01000000, 0x01001000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x80000000, 0x00080000, 0x00000000, 0x00100400, 0x00000080, 0x08800000, 0x00000000, 0x00000000, 0x80000008, 0x00000000 },
|
||||
{ 0x00000000, 0x20000000, 0x00002000, 0x02800000, 0x00000000, 0x00000040, 0x00000001, 0x00000000, 0x40000000, 0x00000002, 0x00000080, 0x00000000 },
|
||||
{ 0x00000000, 0x08000000, 0x00000400, 0x00004000, 0x00000000, 0x00000000, 0x00480000, 0x00000010, 0x00000005, 0x01000000, 0x00000000, 0x00000000 },
|
||||
{ 0x40000000, 0x00000404, 0x80000000, 0x00000000, 0x00000000, 0x40000010, 0x00000000, 0x80000000, 0x00000000, 0x00400000, 0x10000000, 0x00000000 },
|
||||
{ 0x40000000, 0x00000003, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00200301, 0x20000100, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00008000, 0x00008010, 0x00000100, 0x40401000, 0x00080080, 0x00000000 },
|
||||
{ 0x30000000, 0x00000000, 0x00000000, 0x00004000, 0x00008000, 0x08000420, 0x00000000, 0x08000000, 0x00000000, 0x00002000, 0x00000000, 0x00000000 },
|
||||
{ 0x24000000, 0x00001000, 0x04000000, 0x01010020, 0x00000000, 0x00000000, 0x00000000, 0x01000000, 0x00200000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x0C0C0000, 0x00000000, 0x00000000, 0x21000000, 0x00000500, 0x00000000, 0x00000000, 0x00200000, 0x00008000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x21000000, 0x00000000, 0x00000000, 0x01000000, 0x00080000, 0x20000000, 0x00020000, 0x60000010, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x04000200, 0x00000002, 0x00000100, 0x40008000, 0x00000040, 0x00000400, 0x10000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00010000, 0x0C000400, 0x00000040, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x400000C1, 0x00001000, 0x00000000, 0x00000000 },
|
||||
{ 0x21010000, 0x00000000, 0x00000000, 0x10000800, 0x00000100, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x40000800, 0x00080000, 0x00000000 },
|
||||
{ 0x00004800, 0x00000000, 0x40080000, 0x00000008, 0x00000000, 0x00000050, 0x00000000, 0x00000000, 0x00000000, 0x00000008, 0x00040000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x01080000, 0x00000098, 0x00002000, 0x00000000, 0x00000000, 0x00000000, 0x00000800, 0x80001000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00002000, 0x00002000, 0x00008000, 0x00000000, 0x00082000, 0x04080000, 0x00000000, 0x00000000, 0x00000A00, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00086000, 0x00000008, 0x00000000, 0x00000002, 0x00042040, 0x00000200, 0x00000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000040, 0x00000000, 0x00000000, 0x00000002, 0x00000010, 0x00080000, 0x10000001, 0x00000800, 0x80020000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000400, 0x00000000, 0x02000002, 0x00000000, 0x00000000, 0x00020400, 0x00000000, 0x10200000, 0x00000800, 0x00000002, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00002000, 0x00000000, 0x00081201, 0x00000000, 0x00000000, 0x00300000, 0x00020400, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00080000, 0x00000004, 0x00000000, 0x00000000, 0x04000000, 0x10000004, 0x00400080, 0x00010000, 0x00010000, 0x00000000 },
|
||||
{ 0x00000400, 0x00400000, 0x00001800, 0x00002000, 0x00000002, 0x02000000, 0x00000000, 0x10000000, 0x18000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00080000, 0x0080A000, 0x00000080, 0x00000000, 0x00100000, 0x00000000, 0x00010000, 0x00002100, 0x00000000 },
|
||||
{ 0x00000000, 0x00000900, 0x00000000, 0x00000000, 0x00001000, 0x40080001, 0x00000000, 0x00000000, 0x00000000, 0x02000500, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00080B00, 0x20000208, 0x00000000, 0x00000000, 0x00040000, 0x00000000, 0x00004000, 0x00000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x10000002, 0x00000000, 0x00000000, 0x00000200, 0x00800040, 0x40000000, 0x20000008, 0x00000040, 0x00000000 },
|
||||
{ 0x00000000, 0x80000000, 0x10000000, 0x00000420, 0x00000080, 0x00000020, 0x00000080, 0x00000000, 0x04000000, 0x00000200, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00008400, 0x00000010, 0x00028000, 0x00000000, 0x00000000, 0x00000000, 0x80010000, 0x00040100, 0x00000000 },
|
||||
{ 0x80000000, 0x00000020, 0x80000000, 0x10000010, 0x80000000, 0x00000000, 0x24000000, 0x00000000, 0x80000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00010000, 0x00400000, 0x00000000, 0x40000010, 0x00000001, 0x00000100, 0x00000008, 0x08000008, 0x00000000 },
|
||||
{ 0x00000000, 0x00000084, 0x00000010, 0x00000100, 0x00000002, 0x00000000, 0x00000000, 0x00000000, 0x00200000, 0x30000000, 0x02000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x801D0000, 0x0020C000, 0x00000002 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x40000004, 0x00000000, 0x0082000A, 0x40020000, 0x00000000, 0x00000800, 0x00000000, 0x00000000 },
|
||||
{ 0x30000000, 0x04020000, 0x00000000, 0x00000080, 0x00401000, 0x00000000, 0x00000000, 0x20000000, 0x00000000, 0x00000000, 0x01000000, 0x00000000 },
|
||||
{ 0x00088000, 0x00000010, 0x00000000, 0x00000000, 0x10000800, 0x40000000, 0x00000000, 0x04000000, 0x00060000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00006000, 0x00000000, 0x00000000, 0x00004002, 0x00200000, 0x00000000, 0x00000200, 0x00000000, 0x00000080, 0x00000204, 0x00000000, 0x00000000 },
|
||||
{ 0x10000000, 0x00000000, 0x00000000, 0x00A00000, 0x00000000, 0x00000080, 0x00040000, 0x00000200, 0x00400000, 0x02000000, 0x00040000, 0x00000000 },
|
||||
{ 0x00000000, 0x00100000, 0x01000001, 0x00000000, 0x00000000, 0x00000000, 0x00008206, 0x00001000, 0x00000000, 0x00008000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x10000008, 0x00000000, 0x00008000, 0x00000000, 0x00000010, 0x00020000, 0x04000002, 0x04040000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00040000, 0x00000000, 0x00000001, 0x00000000, 0x00010001, 0x00000000, 0x00000040, 0x00000000, 0x00200100, 0x00010000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x20000000, 0x00000000, 0x20000000, 0x40002000, 0x00000004, 0x00100000, 0x00000028, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x20000000, 0x00000000, 0x02000000, 0x00000000, 0x44000001, 0x00000000, 0x00000004, 0x00408000, 0x00000000, 0x00000000 },
|
||||
{ 0x11200000, 0x00010000, 0x40000000, 0x00000000, 0x00000010, 0x00000000, 0x00000004, 0x00000000, 0x00000000, 0x00000000, 0x08000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000040, 0x00100000, 0x00400000, 0x00000000, 0x80000008, 0x00800000, 0x00002080, 0x00000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00400000, 0x80000010, 0x00000000, 0x00000000, 0x00000026, 0x00000000, 0x00000000, 0x00000000, 0x00002000, 0x00000400, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000042, 0x00000000, 0x00000080, 0x01000000, 0x01000000, 0x00400000, 0x00000000, 0x00000000, 0x00000400, 0x00000020, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000200, 0x00000000, 0x00000400, 0x00000000, 0x00002080, 0x00002000, 0x00008024, 0x00000000 },
|
||||
{ 0x00000000, 0x02000000, 0x00800002, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000002, 0x00004012, 0x00008000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x02800000, 0x10000000, 0x00000000, 0x00000000, 0x00000000, 0x80002000, 0x00100000, 0x00200000, 0x00000000, 0x00008000, 0x00000000 },
|
||||
{ 0x00080100, 0x00000000, 0x00080000, 0x00000010, 0x00000000, 0x08000000, 0x10000001, 0x00000400, 0x00000000, 0x00000000, 0x40000000, 0x00000000 },
|
||||
{ 0x00010080, 0x00000000, 0x00000000, 0x00040000, 0x00000000, 0x00000000, 0x00000000, 0x11000000, 0x00040001, 0x80200000, 0x00000000, 0x00000000 },
|
||||
{ 0x00002010, 0x00000000, 0x00000000, 0x00400014, 0x00004000, 0x00000000, 0x20000000, 0x20000000, 0x00000000, 0x00000000, 0x00000010, 0x00000000 },
|
||||
{ 0x00000440, 0x04000020, 0x08010000, 0x00000000, 0x00000040, 0x08000000, 0x00400000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000002, 0x48000000, 0x00000000, 0x00200000, 0x04001000, 0x02000000, 0x00000000, 0x00000000, 0x00001000, 0x00000000, 0x00000000 },
|
||||
{ 0x00200400, 0x00000000, 0x00800000, 0x02000000, 0x00000000, 0x00000020, 0x04000000, 0x00000000, 0x00000000, 0x00000004, 0x01000000, 0x00000000 },
|
||||
{ 0x01200000, 0x01000000, 0x00000800, 0x00004004, 0x00000000, 0x10000000, 0x00000000, 0x00000000, 0x00000008, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000001, 0x00400001, 0x00000000, 0x00000201, 0x00000020, 0x20000000, 0x08000000, 0x00000000 },
|
||||
{ 0x02000000, 0x00000000, 0x00000800, 0x00000001, 0x00000001, 0x00000000, 0x00000000, 0x01000000, 0x00000020, 0x20000080, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000500, 0x00000000, 0x00000000, 0x00040000, 0x00000000, 0x00000000, 0x11000210, 0x00000000, 0x40000000, 0x00000000 },
|
||||
{ 0x00000020, 0x00000000, 0x00000000, 0x00000000, 0x02000000, 0x20001000, 0x00000000, 0x00000020, 0x41000000, 0x00000400, 0x00000400, 0x00000000 },
|
||||
{ 0x00000000, 0x08000000, 0x00000001, 0x00000040, 0x00000000, 0x01000020, 0x02000000, 0x00008000, 0x80000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00100000, 0x80000100, 0x00000001, 0x00000000, 0x00104040, 0x00001000, 0x00000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000100, 0x00000008, 0x10000000, 0x00100000, 0x00000000, 0x00001000, 0x00008000, 0x00000000, 0x00000002, 0x00100001, 0x00000000, 0x00000000 },
|
||||
{ 0xA0000100, 0x10004000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00008000, 0x00000000, 0x00400000, 0x00000000, 0x00002400, 0x00000000 },
|
||||
{ 0x01800004, 0x00000010, 0x00000000, 0x00020000, 0x00200000, 0x00000040, 0x00000200, 0x00008000, 0x00000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00900000, 0x00000000, 0x00400000, 0x00000000, 0x00000020, 0x00000002, 0x00000000, 0x00100000, 0x00200000, 0x00000080, 0x02000000, 0x00000000 },
|
||||
{ 0x00000000, 0x20000008, 0x00014000, 0x00000000, 0x00000000, 0x00080000, 0x00000000, 0x00000020, 0x00020000, 0x08000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00100000, 0x1000A000, 0x00000100, 0x80000000, 0x00000000, 0x00000000, 0x00020000, 0x80000000, 0x00000000, 0x00000000, 0x00100000, 0x00000000 },
|
||||
{ 0x00020000, 0x00000000, 0x00000000, 0x00000000, 0x00100000, 0x00000000, 0x00200000, 0x00410040, 0x00000400, 0x00080000, 0x00000010, 0x00000000 },
|
||||
{ 0x00000000, 0x08000000, 0x00010000, 0x00400000, 0x40000000, 0x00800800, 0x02000000, 0x00000000, 0x00001000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00120200, 0x00800010, 0x20000000, 0x00000000, 0x00000000, 0x00000300, 0x00000000 },
|
||||
{ 0x00000000, 0x00020000, 0x00000000, 0x00000004, 0x00000080, 0x00000100, 0x00000000, 0x00000020, 0x00000020, 0x00000000, 0x80000040, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000008, 0x00002400, 0x00100800, 0x00000000, 0x00020000, 0x10000002, 0x00000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x10000000, 0x00000220, 0x80000000, 0x00000000, 0x00000000, 0x20000000, 0x00200000, 0x00000020, 0x00080000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x04000000, 0x80008000, 0x00004000, 0x00000000, 0x00000000, 0x00000000, 0x00200200, 0x00000004, 0x00000000, 0x00040000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x40100000, 0x80000800, 0x00000080, 0x00000000, 0x00000000, 0x00000000, 0x00120000, 0x00000008, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00080000, 0x00000018, 0x00100000, 0x00040000, 0x00004000, 0x00800000, 0x00000000, 0x00080000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00C00820, 0x00000000, 0x08000000, 0x00000000, 0x00000000, 0x00000408, 0x00000000, 0x00000000, 0x00000000, 0x00000100, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x20000020, 0x00000000, 0x00002000, 0x00000008, 0x00001000, 0x00004000, 0x00000000, 0x00004000, 0x00800000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00020001, 0x00000000, 0x00000040, 0x00000800, 0x00000000, 0x00001020, 0x00000000, 0x00400200, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00010000, 0x00001010, 0x00000040, 0x00000004, 0x00000000, 0x00000000, 0x00000011, 0x00010000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000400, 0x00000040, 0x00000000, 0x00000000, 0x00001000, 0x00000000, 0x02000000, 0x00002080, 0x00000000, 0x00010000, 0x04001000, 0x00000000 },
|
||||
{ 0x00220040, 0x20000044, 0x00000001, 0x00000000, 0x00000000, 0x00000200, 0x00010000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00008204, 0x00001000, 0x00000000, 0x00000000, 0x80000000, 0x00000000, 0x80000400, 0x80000000, 0x02000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00401300, 0x00000000, 0x00040000, 0x00000000, 0x05000000, 0x00000000, 0x00080000, 0x00000000, 0x00000100, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000020, 0x00000000, 0x00000000, 0x08000000, 0x00080000, 0x00008000, 0x00000900, 0x00000240, 0x04000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x10000000, 0x1C000000, 0x00000200, 0x00000000, 0x20000000, 0x00000100, 0x04000000, 0x01000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00200201, 0x00000000, 0x00000800, 0x00000000, 0x00010800, 0x00001000, 0x00000000, 0x00000008, 0x00040000, 0x00000000, 0x00000000 },
|
||||
{ 0x00080000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x08000000, 0x40010000, 0x00014040, 0x20000000, 0x00000400, 0x00000000 },
|
||||
{ 0x00000000, 0x00840000, 0x00200000, 0x04001000, 0x00000000, 0x00000000, 0x00004000, 0x00000000, 0x00000802, 0x00000000, 0x40000000, 0x00000000 },
|
||||
{ 0x00000800, 0x00014000, 0x00000000, 0x00000000, 0x00080100, 0x00000000, 0x00000000, 0x00000000, 0x00080000, 0x40200000, 0x00800000, 0x00000000 },
|
||||
{ 0x00400000, 0x00000000, 0x00400000, 0x00000000, 0x00080000, 0x00000000, 0x00000000, 0x0001000A, 0x4A000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x08000000, 0x00200000, 0x00000000, 0x80000000, 0x00000100, 0x02002004, 0x20000000, 0x04000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00280000, 0x00000024, 0x00060000, 0x00000000, 0x00000000, 0x00000000, 0x00040001, 0x00000000, 0x04000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000080, 0x00000000, 0x00010000, 0x00000000, 0x00000000, 0x00480000, 0x00000040, 0x000080A0, 0x04000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x10000000, 0x01000002, 0x000A0000, 0x00000000, 0x00000100, 0x00400000, 0x00000004, 0x00020000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00010000, 0x00440000, 0x00004000, 0x00200000, 0x02000000, 0x80000000, 0x00800010, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x20404000, 0x08000100, 0x00100000, 0x80000000, 0x00000200, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00020000, 0x40000000, 0x40000040, 0x00002000, 0x00000000, 0x00300000, 0x80000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000001, 0x00000040, 0x00000000, 0x00000100, 0x00000000, 0x08000000, 0x00000000, 0x0C000002, 0x00000000, 0x00020000, 0x00000000 },
|
||||
{ 0x00010000, 0x00000010, 0x00000000, 0x00040000, 0x00400008, 0x00020000, 0x00000000, 0x00000000, 0x00020000, 0x00800000, 0x00000000, 0x00000000 },
|
||||
{ 0x02404000, 0x00000002, 0x04000000, 0x40000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x01000080, 0x00000000, 0x00000000 },
|
||||
{ 0x00002000, 0x00000000, 0x00010000, 0x00000000, 0x00000000, 0x10000000, 0x00020020, 0x00000100, 0x00000000, 0x10000000, 0x00000020, 0x00000000 },
|
||||
{ 0x00000880, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000100, 0x00080004, 0x00000000, 0x00080010, 0x00000000, 0x00800000, 0x00000000 },
|
||||
{ 0x00008070, 0x00000000, 0x00000002, 0x00000000, 0x00000008, 0x00000002, 0x00000000, 0x00000000, 0x00420000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000200, 0x00000008, 0x80000000, 0x80010000, 0x80400000, 0x00000000, 0x00010000, 0x00000004, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x03000000, 0x00020004, 0x22000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000020, 0x10000000, 0x00100000, 0x00000000 },
|
||||
{ 0x00141000, 0x00080000, 0x00000000, 0x00000000, 0x00000000, 0x80000000, 0x00000010, 0x00000000, 0x00020800, 0x00004000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00280000, 0x48000000, 0x80000080, 0x00000400, 0x00100100, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x24000800, 0x00001020, 0x00802000, 0x02000000, 0x00000000, 0x00002000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x40030010, 0x00000084, 0x00000002, 0x00000080, 0x00000000, 0x00000200, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00010000, 0x02120000, 0x00000000, 0x00004001, 0x00100000, 0x00000000, 0x00000001, 0x00000040, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x40800000, 0x002080A0, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x20000000, 0x00820000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000080, 0x00003000, 0x00000000, 0x00000000, 0x00000000, 0x00800000, 0x00000000, 0x00200000, 0x00000800, 0x04000040, 0x00010200, 0x00000000 },
|
||||
{ 0x00000000, 0x00000100, 0x00000000, 0x00000000, 0x00000000, 0x00000010, 0x10000000, 0x42100010, 0x00100000, 0x00000020, 0x00400000, 0x00000000 },
|
||||
{ 0x04080004, 0x00102008, 0x00000400, 0x08000000, 0x00000000, 0x00000000, 0x00000020, 0x00000000, 0x00000000, 0x40000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000081, 0x00000008, 0x00000000, 0x00000002, 0x00000000, 0x00000020, 0x00000000, 0x00020400, 0x00140000, 0x00000001, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00088000, 0x00000000, 0x10040000, 0x00000000, 0x00000000, 0x02000010, 0x00000060, 0x10000000, 0x00000002 },
|
||||
{ 0x08000000, 0x00000000, 0x00000000, 0x00000800, 0x00000000, 0x10010000, 0x00050000, 0x00000000, 0x09000800, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00002008, 0x00000000, 0x02008080, 0x00004000, 0x00000000, 0x04020040, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00400000, 0x00200000, 0x00020000, 0x00000000, 0x00100000, 0x40000000, 0x00000000, 0x01080101, 0x00000000, 0x00000000 },
|
||||
{ 0x00000002, 0x00000000, 0x00068000, 0x08000000, 0x00000000, 0x00004042, 0x10000000, 0x00000000, 0x00000000, 0x00000000, 0x00000800, 0x00000000 },
|
||||
{ 0x00000000, 0x01400000, 0x02004000, 0x00000000, 0x00000020, 0x00000010, 0x00000000, 0x00001000, 0x00000200, 0x00000000, 0x80000080, 0x00000000 },
|
||||
{ 0x00000000, 0x62300000, 0x00000000, 0x00000200, 0x00000000, 0x00000000, 0x00000000, 0x00400000, 0x08800080, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x04000000, 0x000091A0, 0x02000000, 0x00200000, 0x00000000, 0x00000000, 0x00000000, 0x00000100, 0x00000000, 0x00200000, 0x00000000, 0x00000000 },
|
||||
{ 0x00730000, 0x00000040, 0x00000000, 0x00000000, 0x20000000, 0x04000000, 0x00000000, 0x00000000, 0x00000400, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000020, 0x00102000, 0x00202000, 0x00000000, 0x02000000, 0x08040010, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000100, 0x00400000, 0x00000201, 0x00000000, 0x02000200, 0x00000000, 0x00040000, 0x00001000, 0x00000010, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00008000, 0x00000000, 0x00000000, 0x00000000, 0x10100000, 0x00800000, 0x00008000, 0x00081008, 0x00200000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x40828000, 0x00800000, 0x00100000, 0x00120000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00008000, 0x00000000 },
|
||||
{ 0x80020000, 0x01100880, 0x00000000, 0x00000000, 0x00000000, 0x00100010, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000001, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000400, 0x08000000, 0x0000080A, 0x00000000, 0x00000200, 0x00020010, 0x04000000, 0x00000000 },
|
||||
{ 0x00800000, 0x00000000, 0x00000000, 0x06000000, 0x00010001, 0x00000800, 0x00000080, 0x00000000, 0x00000000, 0x08000000, 0x00000040, 0x00000000 },
|
||||
{ 0x00000000, 0x02200000, 0x00000000, 0x00008008, 0x00200000, 0x00000000, 0x00000000, 0x00004000, 0x00000000, 0x40000004, 0x00002000, 0x00000000 },
|
||||
{ 0x00800000, 0x00000000, 0x88000000, 0x84000000, 0x00040000, 0x00000000, 0x00000000, 0x00000000, 0x00800000, 0x00000000, 0x00014000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x40000001, 0x00040000, 0x00001000, 0x01200000, 0x02400004, 0x00000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00004808, 0x04000000, 0x00200000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00080000, 0x00080040, 0x00000040, 0x00000000, 0x00000000 },
|
||||
{ 0x00000310, 0x00002000, 0x00000000, 0x00800200, 0x00000000, 0x00010000, 0x00400000, 0x00000000, 0x01000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x0000100E, 0x00000018, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000002, 0x00000002, 0x00000000, 0x00000000, 0x00000002, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00820000, 0x00200100, 0x09000020, 0x00000800, 0x00000000, 0x00008000, 0x00000000, 0x00000000 },
|
||||
{ 0x08000000, 0x00000000, 0x02200000, 0x00001400, 0x80004000, 0x00000000, 0x00000004, 0x00000000, 0x00001000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00008000, 0x00000000, 0x01000000, 0x00000000, 0x20000080, 0x00000040, 0x10000000, 0x10000000, 0x00000000, 0x00000020, 0x00400000, 0x00000000 },
|
||||
{ 0x00000000, 0x40000080, 0x00020000, 0x00000000, 0x00001000, 0x00004000, 0x00000100, 0x20000000, 0x00000000, 0x00004000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00004800, 0x00000000, 0x04000800, 0x00000000, 0x00400000, 0x00000000, 0x00000000, 0x00000000, 0x00000010, 0x00400000, 0x00000001 },
|
||||
{ 0x00000000, 0x00040004, 0x00041000, 0x00000000, 0x00000008, 0x00000000, 0x00000024, 0x00000000, 0x00000000, 0x00000000, 0x00004000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x02000000, 0x00000000, 0x00040000, 0x00000000, 0x00100008, 0x00000000, 0x20000020, 0x08000040, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00001004, 0x00100000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00008000, 0x00804108, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x01010000, 0x00800000, 0x00001001, 0x00800000, 0x00000000, 0x00000000, 0x00240000, 0x00000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000200, 0x00000000, 0x00000000, 0x00002200, 0x00000002, 0x08200800, 0x00000001 },
|
||||
{ 0x00000000, 0x00000000, 0x00100000, 0x0000C000, 0x00804010, 0x00004000, 0x00000000, 0x00000000, 0x00000400, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x08000000, 0x00480000, 0x00008004, 0x00000000, 0x00040000, 0x00000000, 0x00000100, 0x00000000, 0x00000000, 0x10000000, 0x00000000, 0x00000000 },
|
||||
{ 0x08800000, 0x00040001, 0x00000000, 0x00000000, 0x00080080, 0x00000000, 0x04000000, 0x00000000, 0x04000000, 0x00000000, 0x00000000, 0x00000000 },
|
||||
{ 0x00003200, 0x00000000, 0x00000000, 0x00000000, 0x10420000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00002000, 0x00000400, 0x00000000 },
|
||||
{ 0x00000040, 0x00000000, 0x00000000, 0x00000004, 0x00000008, 0x00000000, 0x00000000, 0x00000000, 0x00000600, 0x00000000, 0x00004801, 0x00000000 },
|
||||
{ 0x00000020, 0x00000000, 0x00000410, 0x00000000, 0x00000004, 0x00000000, 0x00800000, 0x00000000, 0x00900000, 0x00000001, 0x00000000, 0x00000000 },
|
||||
{ 0x02000000, 0x00000000, 0x00000000, 0x00000000, 0x02000040, 0x00000000, 0x00000000, 0x00000000, 0x00840040, 0x00800000, 0x00000001, 0x00000000 },
|
||||
{ 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x44000800, 0x00000000, 0x08001000, 0x20001000, 0x00000000, 0x00000000, 0x00800000, 0x00000000 },
|
||||
{ 0x00000001, 0x00000000, 0x00000000, 0x00000880, 0x00000000, 0x00002000, 0x00000000, 0x00420000, 0x00040000, 0x00002020, 0x00000000, 0x00000000 },
|
||||
{ 0x00000010, 0x00000000, 0x00040000, 0x00000000, 0x20000000, 0x00040000, 0x01000000, 0x04000000, 0x00000000, 0x00000080, 0x00400000, 0x00000001 },
|
||||
{ 0x00000008, 0x00020000, 0x00000000, 0x00021000, 0x00000000, 0x00000000, 0x00000008, 0x00000000, 0x00080000, 0x00000210, 0x00000001, 0x00000000 },
|
||||
{ 0x0000000A, 0x00001080, 0x00000000, 0x00000000, 0x00004000, 0x00200004, 0x00000000, 0x00000000, 0x00008000, 0x00000004, 0x00000000, 0x00000000 },
|
||||
{ 0x00100082, 0x00000000, 0x20008000, 0x00000000, 0x00000000, 0x00004000, 0x00000800, 0x00000000, 0x00000000, 0x00001000, 0x00000000, 0x00000002 }
|
||||
} ;
|
||||
|
||||
const uint32_t LDPC_ParityGen_n354k160[194][5]
|
||||
#ifdef __AVR__
|
||||
PROGMEM
|
||||
#endif
|
||||
= { // parity bits generator
|
||||
{ 0xBF3B0B3C, 0x314E4CB2, 0x1C4219B8, 0xF87E8234, 0x6D121C05 },
|
||||
{ 0x43C31BD3, 0xA49EE0D2, 0xB613CA26, 0x4A06EB31, 0xBBBC4B69 },
|
||||
{ 0xA8E96C9C, 0x5C4BD117, 0x8877A3B7, 0x2C28FCBB, 0xC20F5C52 },
|
||||
{ 0x18E91F00, 0xABA26C2A, 0x20AFA0D3, 0x781A54FD, 0x7AAD3CD7 },
|
||||
{ 0xE2CE0C54, 0x0BBBE7EC, 0xBA8B6D72, 0x3CF41BBE, 0x9847B7BA },
|
||||
{ 0x2119517F, 0xDA38A438, 0x6A3A2341, 0x3C6B3CEE, 0xC45138E0 },
|
||||
{ 0xA03B5560, 0xB650F4D1, 0xF79542B5, 0x8B8A9EE2, 0x0008FAD3 },
|
||||
{ 0x5D2624EF, 0x10DBF749, 0x3CA1F2A0, 0x6EC49CB4, 0xE180DDB9 },
|
||||
{ 0xBFBC0710, 0xD0FA2A89, 0xFE1E22BF, 0x8DB41BAE, 0xE2A0DE6F },
|
||||
{ 0x7C97D8D6, 0xD03734C4, 0x3630E03C, 0xBC0E99D2, 0x5B1663C3 },
|
||||
{ 0xA8CB16AD, 0x006106C3, 0x64F41A4D, 0x98C281B0, 0x7E8055C4 },
|
||||
{ 0x8C2CD493, 0x2CD876DC, 0x00C39BE8, 0x0CC58836, 0xE9B55589 },
|
||||
{ 0x9ECCEF2A, 0xA648F8D7, 0xDB12CECF, 0xDECF8910, 0xFABA64FD },
|
||||
{ 0x2DD164C6, 0xE4E44F87, 0x425F5945, 0xEE48E56B, 0xE7F4349D },
|
||||
{ 0x5916345A, 0xDA049129, 0xCEEF739D, 0x47902FB7, 0x709F8AB4 },
|
||||
{ 0x987D6D27, 0x3448F4E7, 0x20DAF291, 0xF82BCE50, 0x8B591390 },
|
||||
{ 0x4DA99CD3, 0x1A13CC5E, 0xC3CE1A7E, 0xF460FD61, 0x8BA9FE66 },
|
||||
{ 0x72CED441, 0x90AAAB8D, 0xC48C6831, 0x9B52333B, 0xD835B6FC },
|
||||
{ 0x989EAEC0, 0xD9F69D6C, 0x66022DD7, 0x778FD6A5, 0x8B526FEE },
|
||||
{ 0x90861033, 0xCA18E3FB, 0x996E893B, 0x24185E8D, 0x3FD66C7C },
|
||||
{ 0xB8810387, 0x44833D00, 0xD0E0E7B3, 0xC4D91AA6, 0x6B5215F9 },
|
||||
{ 0x1FDA72C6, 0xCBD4BB6B, 0x04BE862D, 0xE1B9EA26, 0x30C01CA7 },
|
||||
{ 0xDAC99950, 0x66FC1E65, 0x734D0861, 0x8C4E7985, 0xBFCD3AD1 },
|
||||
{ 0x17BF566C, 0x26A04744, 0x2F60189D, 0x373BFCA1, 0xA08EBFF8 },
|
||||
{ 0x7EA9713E, 0x6A9A3324, 0x0E59E2A7, 0xCE7C57D0, 0xCBA40287 },
|
||||
{ 0xA442B46F, 0x182CFF0A, 0x9568B31D, 0x82015F2C, 0x4F3AB3A7 },
|
||||
{ 0x5965345A, 0xDA049169, 0xCEFF739D, 0x4790EFB7, 0x501FCAA4 },
|
||||
{ 0x5ACF0524, 0x09664861, 0xC26B69ED, 0x041EC4B6, 0xD3BE71E7 },
|
||||
{ 0xECEA5D89, 0x4C7AB36D, 0x9FA963C5, 0xFD416742, 0x1D331757 },
|
||||
{ 0x1256E1C1, 0x5C2D5E39, 0xFA349FC5, 0x98F81F1D, 0x4921E241 },
|
||||
{ 0x2AF78B26, 0xC3B88A6B, 0xE441147E, 0x68ED4E24, 0x2CD189AB },
|
||||
{ 0x899F3FCE, 0x74270F2B, 0x50C13F29, 0xD7359EEA, 0xDA12BA16 },
|
||||
{ 0x3F5A73D8, 0x71423A62, 0xD2957F27, 0xFE0F86A7, 0xAD98760B },
|
||||
{ 0x17E138A8, 0x77EA7AE4, 0xB08FEC00, 0xF51D5B10, 0x50616E89 },
|
||||
{ 0x9DEA1BA4, 0x864AA36F, 0x15AD6853, 0x4B734C20, 0xF2C935C0 },
|
||||
{ 0x1C61D24D, 0x36304D45, 0x6935994B, 0xCBB771C3, 0x74B76058 },
|
||||
{ 0x9C173562, 0xFF1735DA, 0x0C41BCB7, 0xC3195ADC, 0x60999718 },
|
||||
{ 0x30C81C35, 0x60EE7A6E, 0x9DFC8572, 0xBD28D328, 0xFCD88B0E },
|
||||
{ 0xECB8E63E, 0x8EA3EE2A, 0x61AE3F32, 0xB6B5E43C, 0xEA2DD6DE },
|
||||
{ 0x03219881, 0x126EA7A6, 0x6A130444, 0xA5313241, 0x5DD972B0 },
|
||||
{ 0xBCD7FF20, 0xB4C020BD, 0x9B77A60E, 0x91CA8631, 0xD7D1ACD2 },
|
||||
{ 0xFF80C91E, 0x5BCA2B6E, 0xF8D17E27, 0x770DC19A, 0xB1EEEDA0 },
|
||||
{ 0x7EF4C4F1, 0xC77292D4, 0x2E555DD7, 0x33DE6349, 0x6D79A577 },
|
||||
{ 0x89A41EFF, 0xE7455A6E, 0x4431B80C, 0xB47CB0B1, 0x7A2CD62A },
|
||||
{ 0x9F800F19, 0x2E497949, 0xD321D8DA, 0xF74C666B, 0x8EA103C2 },
|
||||
{ 0x46149176, 0xA17E679C, 0xC6481C0D, 0xBA6BA8B7, 0xDD9E0E80 },
|
||||
{ 0x5FE0198D, 0xE51C0C94, 0x80DA07B0, 0x8BCE8FAC, 0x119400C3 },
|
||||
{ 0x7D66BF7E, 0x75045431, 0x6927222B, 0x7FEF3B1C, 0xB2E5D39D },
|
||||
{ 0x7CB5D896, 0xF0373480, 0x3630E03D, 0xBC0E99D2, 0x5B1663C3 },
|
||||
{ 0x78DC56FF, 0x4967B31E, 0x06291302, 0xB3B4A072, 0x8BB88CEC },
|
||||
{ 0xA0354BC6, 0xD06A06D7, 0x4E724381, 0x3F07A3FD, 0x73AFE6FC },
|
||||
{ 0xF30D48DA, 0x676E5492, 0x177F288F, 0x79E936BC, 0x2016EAC9 },
|
||||
{ 0x8EF77970, 0xBD662325, 0x23C36963, 0xC4AA7264, 0x0A9BE530 },
|
||||
{ 0xFEF2A35E, 0x77C86351, 0x89A87585, 0x8F39F81C, 0xB3853AC4 },
|
||||
{ 0x5ACF0164, 0x0D664841, 0xCA6A69ED, 0x041EC4B6, 0xD3BE71A7 },
|
||||
{ 0xFC14B7B4, 0x4B5C63C5, 0xA72CC3B5, 0x6CD2FE00, 0x57EA6966 },
|
||||
{ 0x3E1E6D7B, 0x79532AE8, 0xA51A3C1B, 0xFC6FF9BD, 0x6BA67425 },
|
||||
{ 0x0E79995B, 0x843292F7, 0x382FEB26, 0x70CD2CB7, 0x7BF29FB1 },
|
||||
{ 0x6425F72F, 0xDFDC3624, 0xC593E31E, 0xE113249B, 0xB7490AFD },
|
||||
{ 0x5819FC79, 0x8A418AC9, 0xD1900B5E, 0xA01E332A, 0xE3937E73 },
|
||||
{ 0x0A475305, 0x7943BCA0, 0x666BBDE0, 0xF88290EA, 0x03436E21 },
|
||||
{ 0xEBD7D24F, 0x0ED6EC40, 0x169BF54D, 0x8F4C3C3A, 0xF288B858 },
|
||||
{ 0x15A1D5ED, 0x065B4DEE, 0xB7170FF2, 0xC110FFE6, 0x43DCAF43 },
|
||||
{ 0xF2717717, 0xD28DAD36, 0x43329E2C, 0xF244AB0A, 0x1D79E529 },
|
||||
{ 0x52FA7334, 0x7CF51534, 0xC14F1120, 0x0FB016BD, 0x11A8EDF7 },
|
||||
{ 0x63BBB512, 0xE79B813B, 0x8329901D, 0xB51B64C4, 0xE88F0253 },
|
||||
{ 0xB6BD7090, 0xF9DABA98, 0xFEFC792B, 0x0A9C05FC, 0xF3E5681E },
|
||||
{ 0xE3E8F94B, 0xB416DFF4, 0x18EA246F, 0xC4847023, 0x520C4290 },
|
||||
{ 0x9D999726, 0x6E454BA5, 0x424A1C3F, 0x9378A8C4, 0x3529A57B },
|
||||
{ 0xD96B5D54, 0xC0FDACD7, 0xC7BC13F2, 0x26604362, 0xB730C113 },
|
||||
{ 0x88F15CAA, 0xE33CD7A2, 0x6580C515, 0x4D51D804, 0x6E4D16DE },
|
||||
{ 0xACB34E0A, 0xACF46901, 0xD1E3790C, 0xCFB8150B, 0x1D24AFDF },
|
||||
{ 0xC15B49C8, 0x2493920E, 0x1EFB0A31, 0x96CF52E4, 0x6A59CC69 },
|
||||
{ 0x495A275D, 0x5B296FB3, 0x5F8FCA98, 0x79DF044F, 0xFFA075CB },
|
||||
{ 0xCB57CA6C, 0x96E56516, 0x34B373AB, 0x1DF6A1FF, 0x55FACA55 },
|
||||
{ 0xC3EB2B4E, 0x81752CE2, 0xE4A59F1A, 0x42091D75, 0xF68E15C8 },
|
||||
{ 0x3DAF86C3, 0xD6D9C19B, 0xC2A751E1, 0x79D118F4, 0xF122332C },
|
||||
{ 0xC1D1D970, 0x382D6985, 0x06B12543, 0x0C05211C, 0xEA714078 },
|
||||
{ 0x989EAEC0, 0xD9FE966C, 0x46022FDF, 0x778FD6A5, 0x8B526FEE },
|
||||
{ 0x5E3B9C7A, 0xED9ADFAF, 0x0F443C92, 0xFC855F78, 0xB1C61773 },
|
||||
{ 0x0EBF15FE, 0x1ECB1B82, 0x848E30D5, 0xE117B243, 0x6F3CE271 },
|
||||
{ 0x424E825C, 0x096A4205, 0xABF29FA4, 0x854E07BF, 0x5E998352 },
|
||||
{ 0xE0418B91, 0x4ADD50AC, 0x56511551, 0x0F0C7673, 0x821A3E84 },
|
||||
{ 0x267BDCEF, 0x04734BB4, 0xF7B4BC80, 0xCC12BA7D, 0x36C612CF },
|
||||
{ 0xC262CA37, 0x7E32C56F, 0x47845A47, 0xDF1B9BA8, 0xE0AFB8FC },
|
||||
{ 0xE0418B91, 0x4BDC50AC, 0x56D11551, 0x0F0C6672, 0x829A3E84 },
|
||||
{ 0x91F50DDE, 0x49E45E51, 0x6428D337, 0x31BCC161, 0x75402FCB },
|
||||
{ 0xB296A10D, 0xF646A5FD, 0x470B0F51, 0x1AFBB1A8, 0xDCE8A51D },
|
||||
{ 0x9848FB90, 0xDB688948, 0x8210D7E4, 0xC93EB1A8, 0x933FF050 },
|
||||
{ 0x79685C41, 0x18ED55A7, 0x6D742D4D, 0xD6B94C2D, 0xA4386DC9 },
|
||||
{ 0xB9E78046, 0xEF98DA9B, 0x74E74A58, 0xF9CCA0FB, 0xC6E7C152 },
|
||||
{ 0x1C42C120, 0x35F81A6E, 0xC633A9F9, 0x8F0AD0F1, 0xBB3588CF },
|
||||
{ 0x1B67E3ED, 0xAEFC3225, 0x85008317, 0x06AFE0B6, 0x632786BF },
|
||||
{ 0x2E623791, 0xDDA5B84C, 0x7531F094, 0x3CE5681A, 0x030D196C },
|
||||
{ 0x9659B78B, 0x6031178A, 0xDB017702, 0x5554FBCE, 0x33525AAC },
|
||||
{ 0x60B57C8B, 0x58A1BF6B, 0x57EE5D97, 0x6F976042, 0x43948F0A },
|
||||
{ 0x03220BC9, 0x2D105DCF, 0x1A2F231E, 0x14F15B28, 0x114227CB },
|
||||
{ 0xB388A264, 0x4A7D76C5, 0x6ABBF6D9, 0x5556254C, 0xEB7DF23E },
|
||||
{ 0x5DE9B360, 0x0C1008CC, 0xA7020795, 0x3A6C552C, 0x8AE262B6 },
|
||||
{ 0xEDCA5D89, 0x4D7AB36D, 0x9FA96BC5, 0xFD412746, 0x1D331757 },
|
||||
{ 0x20C00117, 0x75B2C4BC, 0x7DCBE920, 0x9CE9E0FE, 0xA6F64701 },
|
||||
{ 0xAFAED554, 0xB54C8CCC, 0xE8B0699F, 0xCDE94601, 0x1FF9AF34 },
|
||||
{ 0x684CC6F3, 0x90577B5A, 0x28F4656A, 0x90149AD1, 0x7D065F13 },
|
||||
{ 0xEA63020E, 0x10073485, 0xF7738AF7, 0xECD9DF63, 0xEF52967C },
|
||||
{ 0xF34D5BDA, 0x676E5492, 0x177B288F, 0x79E936BC, 0x2516EAC9 },
|
||||
{ 0x9ED015B6, 0xC305A3CA, 0xC5C5C293, 0x3F78BB2D, 0x45738306 },
|
||||
{ 0x5916345A, 0xDA049129, 0xCEFF739D, 0x4790EFB7, 0x701FCAA4 },
|
||||
{ 0x48E5557C, 0x3867D1E4, 0xA86F29E8, 0xC5FDC9F4, 0xD2765165 },
|
||||
{ 0x95EA1BA4, 0x864AA36F, 0x178D6853, 0x4B735820, 0x72C975C0 },
|
||||
{ 0xF554F1F1, 0x1214B399, 0x45C252C4, 0x3EBA532B, 0xCEC50308 },
|
||||
{ 0xC5B1C5CE, 0xD3F3540D, 0x63AA0659, 0xB3F95434, 0xF585E134 },
|
||||
{ 0xEC4D8B91, 0x4BDC50AC, 0x56D11551, 0x2E0C6672, 0x829A3B84 },
|
||||
{ 0xCF63E412, 0x12B15E91, 0x83051D0F, 0x9CC8BE39, 0x24814888 },
|
||||
{ 0x96DAB33B, 0x403EE24D, 0xD4448F71, 0x3B6ECC5E, 0xCCBE5361 },
|
||||
{ 0x05C2385B, 0x6C1EB2AD, 0x44E2D157, 0xAA4F2281, 0x36881398 },
|
||||
{ 0x25CF1FCB, 0xEB18939F, 0x3420F9B2, 0x31A1A463, 0x4D941996 },
|
||||
{ 0xE9588C02, 0x7BBCECFF, 0x7AA5B0E7, 0x7E5FE165, 0x9984E1B4 },
|
||||
{ 0xBC48FB90, 0xDB689948, 0x8610D7E4, 0xC83FB188, 0x933FF050 },
|
||||
{ 0xD5192898, 0xE4A0029F, 0x62574555, 0x7168276F, 0x77021800 },
|
||||
{ 0x51D69601, 0x62F5524B, 0x16B5D9BC, 0x624E462D, 0xAE500B1D },
|
||||
{ 0x17669EA7, 0x1775A41F, 0x09A47393, 0xF0FE3BD7, 0x58178FC1 },
|
||||
{ 0xEC304D69, 0x4D5E9089, 0x3A899E6C, 0xAE0DA801, 0xB394CF54 },
|
||||
{ 0x6CA5F72F, 0xDFD83625, 0xC593E31E, 0xE113249B, 0xB7410A7D },
|
||||
{ 0x2A4099D1, 0x5926389F, 0x85EE807C, 0xBF2B5A2F, 0x3442B2AA },
|
||||
{ 0x9565CA53, 0xEFB6F5B0, 0x9586A876, 0x3B85C5B5, 0x185F87B0 },
|
||||
{ 0x89F7CDEA, 0x2FFDECB4, 0xC191EC57, 0x964510D8, 0x23DD8018 },
|
||||
{ 0x48DC8D1F, 0x593EED5B, 0xA06A9AB7, 0x81AE548A, 0x5261DF4C },
|
||||
{ 0x0FF22560, 0xD10ADA44, 0x53081653, 0x7E5F18B1, 0xC5C1B2A5 },
|
||||
{ 0x449AAD97, 0x5215DD71, 0xCB3CAEFE, 0x70C35948, 0x603E83CB },
|
||||
{ 0x0FCF09D8, 0x502FD0D4, 0x0BD6ABE3, 0x7C2981A7, 0x69BB3DDA },
|
||||
{ 0x5B7E95C1, 0xDC432A50, 0xDA1830CB, 0xE39D70EF, 0x70553B71 },
|
||||
{ 0x80B7031D, 0x008ACF8A, 0xD6A52F6D, 0xA32E741A, 0x70F87C14 },
|
||||
{ 0xAD7EFC8C, 0x048C390D, 0xE2C42E23, 0x7A70EAAC, 0xE4FF9B20 },
|
||||
{ 0x04F750C8, 0x2EB3118A, 0xC3F13D99, 0xDF48E0AD, 0x28FF9F90 },
|
||||
{ 0x6BF84DDF, 0x7B3CA371, 0xEB402058, 0x6DA784CF, 0x0654544A },
|
||||
{ 0x68D02631, 0x2D4D0277, 0x26727DD6, 0x801214CA, 0x5D217B68 },
|
||||
{ 0x434C131D, 0x68A3DFAE, 0xB017E87F, 0xC7B52C91, 0x42EA8D0F },
|
||||
{ 0x4E9D3ED1, 0x978E35BB, 0xD5BAC41B, 0x78492E14, 0x2EC940AD },
|
||||
{ 0xF514F1F1, 0x9214B389, 0x45C252C4, 0x3EBA532B, 0xCEC5032E },
|
||||
{ 0x5B5F3E79, 0x2ABAB2F5, 0x084C4D5A, 0x31D688D0, 0xD54F32A1 },
|
||||
{ 0xC9D0422B, 0xF87EFB4B, 0x65C25674, 0xE9924A10, 0xD17731E8 },
|
||||
{ 0xFF5F83D6, 0xE6C7F0F4, 0x0A96F918, 0x2FBDE919, 0xD25A140C },
|
||||
{ 0xCBA3FCEB, 0xF3610958, 0x20AB2507, 0xEABFC19C, 0xA4432F0A },
|
||||
{ 0x3542D3AF, 0x09A7674F, 0xF77389BF, 0xCF5DF542, 0x50815658 },
|
||||
{ 0x0AF1DF54, 0xF80803CD, 0x3DA3F21D, 0x6084E0FC, 0x1F9EDFFB },
|
||||
{ 0x6DE2AA90, 0xBFE9A564, 0xD1798BB7, 0x367DEFBD, 0x3716658D },
|
||||
{ 0xB3CF1AD0, 0x55D872A7, 0x8F853289, 0xF2A83C7A, 0xF18EBF7A },
|
||||
{ 0x75E81640, 0xCAAC3405, 0xA1DB2536, 0xE6BAE8B9, 0x463ECA30 },
|
||||
{ 0x14B85EA7, 0xCB24075A, 0x03B2B4C3, 0x84A5CE08, 0xC31CC695 },
|
||||
{ 0xC55B49C8, 0x249303AE, 0x1CFB0A31, 0x96EF52E4, 0x6A59CC69 },
|
||||
{ 0x267531D5, 0xAD722E2B, 0xD0601DE1, 0x2B3DFDB4, 0x816EE75B },
|
||||
{ 0xE415677A, 0xD09449D0, 0x10C8E740, 0xA038FF65, 0x14CBE595 },
|
||||
{ 0x6A906631, 0x2D4D0275, 0x22727DD6, 0xC01214CA, 0x5D217B68 },
|
||||
{ 0xB31272C5, 0xC2594005, 0x94B83E46, 0x4D84ED27, 0xF23A67F3 },
|
||||
{ 0xB2BBF8A5, 0x36004598, 0x971E0455, 0x00BA60CE, 0x93B2D373 },
|
||||
{ 0xDF37FE5C, 0x6ADBAD69, 0x8A9755B8, 0x3916D1B1, 0x4458FE0E },
|
||||
{ 0xB4D7FF20, 0xB48820BD, 0x9B77260A, 0x91CA8631, 0xD7D5ACD2 },
|
||||
{ 0x5D3608F5, 0x436907F3, 0x4CD2CBAD, 0x84C5E362, 0xD1E7240D },
|
||||
{ 0x34C01C31, 0x60FE5A66, 0x9DFC8172, 0xB528D328, 0xFCD88B0E },
|
||||
{ 0x409562A7, 0x100957A1, 0x4526AC8B, 0xE7C47A53, 0xBD8B8ED5 },
|
||||
{ 0xDA4D0FD3, 0x4E28D26C, 0x6A6B8AC1, 0xF82D0118, 0xF315A243 },
|
||||
{ 0x745A9DB4, 0x9071FBC7, 0x33A67C1D, 0x40063FD4, 0xB8EE6CDA },
|
||||
{ 0x85A1E2B5, 0x0267BB1F, 0x436DAAF0, 0x65D3EF48, 0xFA1CF504 },
|
||||
{ 0x247CD0D8, 0x64165577, 0xEE6ACDD4, 0x9C0CBD01, 0x958A76A1 },
|
||||
{ 0xC5B5C5CE, 0xD373540C, 0x63AA05D9, 0xB3E95434, 0xF185E134 },
|
||||
{ 0xD17281AB, 0xF5E8C8AE, 0x81F1D98E, 0xF4D8C0CD, 0xD7DF700E },
|
||||
{ 0x50BAB24E, 0x546D7C13, 0xE047CA14, 0x96E26BC6, 0xF035D936 },
|
||||
{ 0xD872A27F, 0xEE03F33C, 0x8EBC2CC6, 0x8A82CD68, 0x964A6E4E },
|
||||
{ 0x629516BC, 0xF182D7B1, 0x4760C49C, 0xF069128A, 0x19CEC52F },
|
||||
{ 0x9A2CACEB, 0x9C729BAB, 0xBB6024B4, 0x80C26E03, 0xC5C1E2FB },
|
||||
{ 0xFF5FB1D6, 0xE6C7F0F4, 0x0A96F918, 0x2FBDE919, 0xC218140C },
|
||||
{ 0xB0A929EE, 0xB189398A, 0xE904C6EE, 0x769ECA8E, 0xC868552F },
|
||||
{ 0xEF4D67E1, 0x3F7A8BF3, 0x7B4686D9, 0x2E4918B4, 0x88E76F5A },
|
||||
{ 0xF7202730, 0x6763E65A, 0x01E69E66, 0x213AF56A, 0x07FFDF91 },
|
||||
{ 0xAD220791, 0xE6A0D905, 0x8855FD21, 0xF65B9F08, 0x0E11BEC6 },
|
||||
{ 0xCA4FD7C6, 0xD429968D, 0x54EE0F82, 0x5F0B299D, 0xB377A305 },
|
||||
{ 0xFC749190, 0x84558B4E, 0x16E0249D, 0x1015D925, 0xA045B5DB },
|
||||
{ 0xAD7430E3, 0x46C2F2B7, 0xFB569EA5, 0xAB7068D2, 0x8BED359A },
|
||||
{ 0xC2421229, 0xBD61DCB7, 0xDBB300AA, 0x1450F14E, 0xE8B7317D },
|
||||
{ 0x4E9E2248, 0x4A44E893, 0x95B0CC28, 0x94FE53F8, 0x2997B8AF },
|
||||
{ 0x18792A74, 0x01C6AC75, 0x51C74F95, 0x5C23C030, 0xC82C03E6 },
|
||||
{ 0xE6E4579F, 0x4BE76A10, 0x46CE12AC, 0xA4D9E3E7, 0xB01B9244 },
|
||||
{ 0xB5EC3540, 0x4F5C6BDE, 0xD60F41DB, 0xAAEF0E13, 0x68938D5D },
|
||||
{ 0xD4544232, 0xAF748A57, 0xB52772F1, 0x126625AF, 0xDB1D5FF1 },
|
||||
{ 0x1E623791, 0xD9A7B84C, 0x7531F094, 0x3CE5689A, 0x034D096C },
|
||||
{ 0x55A90C45, 0x2C89BE82, 0x51B53A53, 0xDD30D5DB, 0x950D788D },
|
||||
{ 0x86A7B294, 0xD7991A0D, 0xA998C3AD, 0xFDBDE0E8, 0x1BDED0B7 },
|
||||
{ 0x8CCA1BA4, 0x864BA36F, 0x55AD6853, 0x4B734C20, 0xF2C935D0 },
|
||||
{ 0xCEF9CA2C, 0x3DD0F8C3, 0x5944397A, 0x10B3C86C, 0x766C5640 },
|
||||
{ 0x9D48FDB5, 0xA9C5D735, 0x63C23A23, 0x1DC99B85, 0x90B1C1D3 },
|
||||
{ 0xA48DEE95, 0x9782ABB5, 0x422ED881, 0x1F657314, 0x289FA398 },
|
||||
{ 0x99C33D5E, 0xF1247681, 0x315592C6, 0xF0DF750F, 0xBA5C69FE },
|
||||
{ 0xC25B509C, 0x2D2C04B6, 0x47866799, 0x58D1953A, 0x33A12CAC },
|
||||
{ 0x1972680C, 0xC53F300C, 0xCF1C1DE5, 0x1A7ED516, 0xDBC46D76 }
|
||||
} ;
|
||||
|
||||
#endif // WITH_PPM
|
||||
|
||||
// ===================================================================================================================
|
||||
|
||||
#ifdef __AVR__
|
||||
|
||||
// encode Parity from Data: Data is 5x 32-bit words = 160 bits, Parity is 1.5x 32-bit word = 48 bits
|
||||
void LDPC_Encode(const uint32_t *Data, uint32_t *Parity, const uint32_t ParityGen[48][5])
|
||||
{ uint8_t ParIdx=0; Parity[ParIdx]=0; uint32_t Mask=1;
|
||||
for(uint8_t Row=0; Row<48; Row++)
|
||||
{ uint8_t Count=0;
|
||||
const uint32_t *Gen=ParityGen[Row];
|
||||
for(uint8_t Idx=0; Idx<5; Idx++)
|
||||
{ Count+=Count1s(Data[Idx]&pgm_read_dword(Gen+Idx)); }
|
||||
if(Count&1) Parity[ParIdx]|=Mask; Mask<<=1;
|
||||
if(Mask==0) { ParIdx++; Parity[ParIdx]=0; Mask=1; }
|
||||
}
|
||||
}
|
||||
|
||||
void LDPC_Encode(const uint32_t *Data, uint32_t *Parity)
|
||||
{ LDPC_Encode(Data, Parity, LDPC_ParityGen); }
|
||||
|
||||
// encode Parity from Data: Data is 20 bytes = 160 bits, Parity is 6 bytes = 48 bits
|
||||
void LDPC_Encode(const uint8_t *Data, uint8_t *Parity, const uint32_t ParityGen[48][5])
|
||||
{ uint8_t ParIdx=0; Parity[ParIdx]=0; uint8_t Mask=1;
|
||||
for(uint8_t Row=0; Row<48; Row++)
|
||||
{ uint8_t Count=0;
|
||||
const uint8_t *Gen = (uint8_t *)ParityGen[Row];
|
||||
for(uint8_t Idx=0; Idx<20; Idx++)
|
||||
{ Count+=Count1s(Data[Idx]&pgm_read_byte(Gen+Idx)); }
|
||||
if(Count&1) Parity[ParIdx]|=Mask; Mask<<=1;
|
||||
if(Mask==0) { ParIdx++; Parity[ParIdx]=0; Mask=1; }
|
||||
}
|
||||
}
|
||||
|
||||
void LDPC_Encode(const uint8_t *Data, uint8_t *Parity)
|
||||
{ LDPC_Encode(Data, Parity, LDPC_ParityGen); }
|
||||
|
||||
void LDPC_Encode(uint8_t *Data)
|
||||
{ LDPC_Encode(Data, Data+20, LDPC_ParityGen); }
|
||||
|
||||
// check Data against Parity (run 48 parity checks) - return number of failed checks
|
||||
int8_t LDPC_Check(const uint8_t *Data) // 20 data bytes followed by 6 parity bytes
|
||||
{ uint8_t Errors=0;
|
||||
for(uint8_t Row=0; Row<48; Row++)
|
||||
{ uint8_t Count=0;
|
||||
const uint8_t *Check = (uint8_t *)LDPC_ParityCheck[Row];
|
||||
for(uint8_t Idx=0; Idx<26; Idx++)
|
||||
{ Count+=Count1s(Data[Idx] & pgm_read_byte(Check+Idx)); }
|
||||
if(Count&1) Errors++; }
|
||||
return Errors; }
|
||||
|
||||
int8_t LDPC_Check(const uint32_t *Packet)
|
||||
{ return LDPC_Check( (uint8_t *)Packet ); }
|
||||
|
||||
#else // if not 8-bit AVR
|
||||
|
||||
void LDPC_Encode(const uint8_t *Data, uint8_t *Parity, const uint32_t ParityGen[48][5])
|
||||
{ uint8_t ParIdx=0; uint8_t ParByte=0; uint8_t Mask=1;
|
||||
for(uint8_t Row=0; Row<48; Row++)
|
||||
{ uint8_t Count=0;
|
||||
const uint8_t *Gen = (uint8_t *)(ParityGen[Row]);
|
||||
for(uint8_t Idx=0; Idx<20; Idx++)
|
||||
{ Count+=Count1s((uint8_t)(Data[Idx]&Gen[Idx])); }
|
||||
if(Count&1) ParByte|=Mask; Mask<<=1;
|
||||
if(Mask==0) { Parity[ParIdx++]=ParByte; Mask=1; ParByte=0; }
|
||||
}
|
||||
// if(Mask!=1) Parity[ParIdx]=ParByte;
|
||||
}
|
||||
|
||||
void LDPC_Encode(const uint8_t *Data, uint8_t *Parity)
|
||||
{ LDPC_Encode(Data, Parity, LDPC_ParityGen_n208k160); }
|
||||
|
||||
void LDPC_Encode(uint8_t *Data)
|
||||
{ LDPC_Encode(Data, Data+20); }
|
||||
|
||||
// encode Parity from Data: Data is 5x 32-bit words = 160 bits, Parity is 1.5x 32-bit word = 48 bits
|
||||
static void LDPC_Encode(const uint32_t *Data, uint32_t *Parity, uint8_t DataWords, uint8_t Checks, const uint32_t *ParityGen)
|
||||
{ // printf("LDPC_Encode: %08X %08X %08X %08X %08X", Data[0], Data[1], Data[2], Data[3], Data[4] );
|
||||
uint8_t ParIdx=0; Parity[ParIdx]=0; uint32_t Mask=1;
|
||||
const uint32_t *Gen=ParityGen;
|
||||
for(uint8_t Row=0; Row<Checks; Row++)
|
||||
{ uint8_t Count=0;
|
||||
for(uint8_t Idx=0; Idx<DataWords; Idx++)
|
||||
{ Count+=Count1s(Data[Idx]&Gen[Idx]); }
|
||||
if(Count&1) Parity[ParIdx]|=Mask; Mask<<=1;
|
||||
if(Mask==0) { ParIdx++; Parity[ParIdx]=0; Mask=1; }
|
||||
Gen+=DataWords; }
|
||||
// printf(" => %08X %08X\n", Parity[0], Parity[1] );
|
||||
}
|
||||
|
||||
void LDPC_Encode(const uint32_t *Data, uint32_t *Parity) { LDPC_Encode(Data, Parity, 5, 48, (uint32_t *)LDPC_ParityGen_n208k160); }
|
||||
void LDPC_Encode( uint32_t *Data) { LDPC_Encode(Data, Data+5, 5, 48, (uint32_t *)LDPC_ParityGen_n208k160); }
|
||||
|
||||
#ifdef WITH_PPM
|
||||
void LDPC_Encode_n354k160(const uint32_t *Data, uint32_t *Parity) { LDPC_Encode(Data, Parity, 5, 194, (uint32_t *)LDPC_ParityGen_n354k160); }
|
||||
void LDPC_Encode_n354k160( uint32_t *Data) { LDPC_Encode(Data, Data+5, 5, 194, (uint32_t *)LDPC_ParityGen_n354k160); }
|
||||
#endif
|
||||
|
||||
// check Data against Parity (run 48 parity checks) - return number of failed checks
|
||||
uint8_t LDPC_Check(const uint32_t *Data, const uint32_t *Parity) // Data and Parity are 32-bit words
|
||||
{ uint8_t Errors=0;
|
||||
for(uint8_t Row=0; Row<48; Row++)
|
||||
{ uint8_t Count=0;
|
||||
const uint32_t *Check=LDPC_ParityCheck_n208k160[Row];
|
||||
uint8_t Idx;
|
||||
for(Idx=0; Idx<5; Idx++)
|
||||
{ Count+=Count1s(Data[Idx]&Check[Idx]); }
|
||||
Count+=Count1s(Parity[0]&Check[Idx++]);
|
||||
Count+=Count1s((Parity[1]&Check[Idx++])&0xFFFF);
|
||||
if(Count&1) Errors++; }
|
||||
return Errors; }
|
||||
|
||||
uint8_t LDPC_Check(const uint32_t *Data) { return LDPC_Check(Data, Data+5); }
|
||||
|
||||
uint8_t LDPC_Check(const uint8_t *Data) // 20 data bytes followed by 6 parity bytes
|
||||
{ uint8_t Errors=0;
|
||||
for(uint8_t Row=0; Row<48; Row++)
|
||||
{ uint8_t Count=0;
|
||||
const uint8_t *Check = (uint8_t *)LDPC_ParityCheck_n208k160[Row];
|
||||
for(uint8_t Idx=0; Idx<26; Idx++)
|
||||
{ uint8_t And = Data[Idx]&Check[Idx]; Count+=Count1s(And); }
|
||||
if(Count&1) Errors++; }
|
||||
return Errors; }
|
||||
#ifdef WITH_PPM
|
||||
uint8_t LDPC_Check_n354k160(const uint32_t *Data, const uint32_t *Parity) // Data and Parity are 32-bit words
|
||||
{ uint8_t Errors=0;
|
||||
for(uint8_t Row=0; Row<194; Row++)
|
||||
{ uint8_t Count=0;
|
||||
const uint32_t *Check=LDPC_ParityCheck_n354k160[Row];
|
||||
uint8_t Idx;
|
||||
for(Idx=0; Idx<5; Idx++)
|
||||
{ Count+=Count1s(Data[Idx]&Check[Idx]); }
|
||||
uint8_t ParIdx;
|
||||
for(ParIdx=0; ParIdx<6; ParIdx++, Idx++)
|
||||
{ Count+=Count1s(Parity[ParIdx]&Check[Idx]); }
|
||||
Count+=Count1s((Parity[ParIdx]&Check[Idx])&0x0003);
|
||||
if(Count&1) Errors++; }
|
||||
return Errors; }
|
||||
|
||||
uint8_t LDPC_Check_n354k160(const uint32_t *Data) { return LDPC_Check_n354k160(Data, Data+5); }
|
||||
#endif // WITH_PPM
|
||||
|
||||
#endif // __AVR__
|
||||
|
||||
|
|
@ -0,0 +1,425 @@
|
|||
#ifndef __LDPC_H__
|
||||
#define __LDPC_H__
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include <limits>
|
||||
|
||||
#include "bitcount.h"
|
||||
|
||||
#ifndef __AVR__
|
||||
// #include <stdio.h>
|
||||
#include <math.h>
|
||||
#endif
|
||||
|
||||
#ifdef __AVR__
|
||||
#include <avr/pgmspace.h>
|
||||
#endif
|
||||
|
||||
// extern const uint32_t LDPC_ParityGen_n208k160[48][5];
|
||||
// extern const uint32_t LDPC_ParityCheck_n208k160[48][7];
|
||||
// extern const uint8_t LDPC_ParityCheckIndex_n208k160[48][24];
|
||||
// extern const uint8_t LDPC_BitWeight_n208k160[208];
|
||||
#ifdef WITH_PPM
|
||||
extern const uint32_t LDPC_ParityGen_n354k160[194][5];
|
||||
extern const uint32_t LDPC_ParityCheck_n354k160[194][12];
|
||||
#endif
|
||||
|
||||
#ifdef __AVR__
|
||||
|
||||
// encode Parity from Data: Data is 5x 32-bit words = 160 bits, Parity is 1.5x 32-bit word = 48 bits
|
||||
void LDPC_Encode(const uint32_t *Data, uint32_t *Parity, const uint32_t ParityGen[48][5]);
|
||||
void LDPC_Encode(const uint32_t *Data, uint32_t *Parity);
|
||||
|
||||
// encode Parity from Data: Data is 20 bytes = 160 bits, Parity is 6 bytes = 48 bits
|
||||
void LDPC_Encode(const uint8_t *Data, uint8_t *Parity, const uint32_t ParityGen[48][5]);
|
||||
void LDPC_Encode(const uint8_t *Data, uint8_t *Parity);
|
||||
void LDPC_Encode( uint8_t *Data);
|
||||
// check Data against Parity (run 48 parity checks) - return number of failed checks
|
||||
uint8_t LDPC_Check(const uint8_t *Data); // 20 data bytes followed by 6 parity bytes
|
||||
uint8_t LDPC_Check(const uint32_t *Packet);
|
||||
|
||||
#else // if not 8-bit AVR
|
||||
|
||||
void LDPC_Encode(const uint8_t *Data, uint8_t *Parity, const uint32_t ParityGen[48][5]);
|
||||
void LDPC_Encode(const uint8_t *Data, uint8_t *Parity);
|
||||
void LDPC_Encode( uint8_t *Data);
|
||||
// encode Parity from Data: Data is 5x 32-bit words = 160 bits, Parity is 1.5x 32-bit word = 48 bits
|
||||
// void LDPC_Encode(const uint32_t *Data, uint32_t *Parity, const uint32_t ParityGen[48][5]);
|
||||
|
||||
// void LDPC_Encode(const uint32_t *Data, uint32_t *Parity, uint8_t DataWords, uint8_t Checks, const uint32_t *ParityGen);
|
||||
// inline void LDPC_Encode(const uint32_t *Data, uint32_t *Parity) { LDPC_Encode(Data, Parity, 5, 48, (uint32_t *)LDPC_ParityGen_n208k160); }
|
||||
// inline void LDPC_Encode( uint32_t *Data) { LDPC_Encode(Data, Data+5, 5, 48, (uint32_t *)LDPC_ParityGen_n208k160); }
|
||||
// inline void LDPC_Encode_n394k160(const uint32_t *Data, uint32_t *Parity) { LDPC_Encode(Data, Parity, 5, 194, (uint32_t *)LDPC_ParityGen_n354k160); }
|
||||
// inline void LDPC_Encode_n394k160( uint32_t *Data) { LDPC_Encode(Data, Data+5, 5, 194, (uint32_t *)LDPC_ParityGen_n354k160); }
|
||||
|
||||
void LDPC_Encode(const uint32_t *Data, uint32_t *Parity);
|
||||
void LDPC_Encode( uint32_t *Data);
|
||||
#ifdef WITH_PPM
|
||||
void LDPC_Encode_n354k160(const uint32_t *Data, uint32_t *Parity);
|
||||
void LDPC_Encode_n354k160( uint32_t *Data);
|
||||
#endif
|
||||
// check Data against Parity (run 48 parity checks) - return number of failed checks
|
||||
uint8_t LDPC_Check(const uint32_t *Data, const uint32_t *Parity); // Data and Parity are 32-bit words
|
||||
uint8_t LDPC_Check(const uint32_t *Data);
|
||||
uint8_t LDPC_Check(const uint8_t *Data); // 20 data bytes followed by 6 parity bytes
|
||||
#ifdef WITH_PPM
|
||||
uint8_t LDPC_Check_n354k160(const uint32_t *Data, const uint32_t *Parity); // Data and Parity are 32-bit words
|
||||
uint8_t LDPC_Check_n354k160(const uint32_t *Data);
|
||||
#endif
|
||||
|
||||
#endif // __AVR__
|
||||
|
||||
#ifndef __AVR__
|
||||
|
||||
extern const uint8_t LDPC_ParityCheckIndex_n208k160[48][24];
|
||||
|
||||
class LDPC_Decoder
|
||||
{ public:
|
||||
const static uint8_t UserBits = 160; // 5 32-bit bits = 20 bytes
|
||||
const static uint8_t UserWords = UserBits/32;
|
||||
const static uint8_t ParityBits = 48; // 6 bytes (total packet is 26 bytes)
|
||||
const static uint8_t CodeBits = UserBits+ParityBits; // 160+48 = 208 code bits = 26 bytes
|
||||
const static uint8_t CodeBytes = (CodeBits+ 7)/ 8; //
|
||||
const static uint8_t CodeWords = (CodeBits+31)/32; //
|
||||
const static uint8_t MaxCheckWeight = 24;
|
||||
// const static uint8_t MaxBitWeight = 8;
|
||||
|
||||
public:
|
||||
|
||||
int16_t InpBit[CodeBits]; // a-priori bits
|
||||
int16_t ExtBit[CodeBits]; // extrinsic inf.
|
||||
int16_t OutBit[CodeBits]; // a-posteriori bits
|
||||
|
||||
void Input(const uint8_t *Data, const uint8_t *Err)
|
||||
{ uint8_t Mask=1; uint8_t Idx=0; uint8_t DataByte=0; uint8_t ErrByte=0;
|
||||
for(uint8_t Bit=0; Bit<CodeBits; Bit++)
|
||||
{ if(Mask==1) { DataByte=Data[Idx]; ErrByte=Err[Idx]; }
|
||||
int16_t Inp;
|
||||
if(ErrByte&Mask) Inp=0;
|
||||
else Inp=(DataByte&Mask) ? +128:-128;
|
||||
OutBit[Bit] = InpBit[Bit] = Inp; ExtBit[Bit]=0;
|
||||
Mask<<=1; if(Mask==0) { Idx++; Mask=1; }
|
||||
}
|
||||
}
|
||||
|
||||
void Input(const uint32_t Data[CodeWords])
|
||||
{ uint32_t Mask=1; uint8_t Idx=0; uint32_t Word=Data[Idx];
|
||||
for(uint8_t Bit=0; Bit<CodeBits; Bit++)
|
||||
{ OutBit[Bit] = InpBit[Bit] = (Word&Mask) ? +128:-128;
|
||||
ExtBit[Bit]=0;
|
||||
Mask<<=1; if(Mask==0) { Word=Data[++Idx]; Mask=1; }
|
||||
}
|
||||
}
|
||||
|
||||
void Input(const float *Data, float RefAmpl=1.0)
|
||||
{ for(int Bit=0; Bit<CodeBits; Bit++)
|
||||
{ int Inp = floor(128*Data[Bit^7]/RefAmpl+0.5);
|
||||
if(Inp>32767) Inp=32767; else if(Inp<(-32767)) Inp=(-32767);
|
||||
OutBit[Bit] = InpBit[Bit] = Inp;
|
||||
ExtBit[Bit]=0; }
|
||||
}
|
||||
|
||||
void Output(uint32_t Data[CodeWords])
|
||||
{ uint32_t Mask=1; uint8_t Idx=0; uint32_t Word=0;
|
||||
for(uint8_t Bit=0; Bit<CodeBits; Bit++)
|
||||
{ if(OutBit[Bit]>0) Word|=Mask;
|
||||
Mask<<=1; if(Mask==0) { Data[Idx++]=Word; Word=0; Mask=1; }
|
||||
} if(Mask>1) Data[Idx++]=Word;
|
||||
}
|
||||
|
||||
void Output(uint8_t Data[CodeBytes])
|
||||
{ uint8_t Mask=1; uint8_t Idx=0; uint8_t Byte=0;
|
||||
for(uint8_t Bit=0; Bit<CodeBits; Bit++)
|
||||
{ if(OutBit[Bit]>0) Byte|=Mask;
|
||||
Mask<<=1; if(Mask==0) { Data[Idx++]=Byte; Byte=0; Mask=1; }
|
||||
} if(Mask>1) Data[Idx++]=Byte;
|
||||
}
|
||||
|
||||
int8_t ProcessChecks(void)
|
||||
{ for(uint8_t Bit=0; Bit<CodeBits; Bit++)
|
||||
ExtBit[Bit]=0;
|
||||
uint8_t Count=0;
|
||||
for(uint8_t Row=0; Row<ParityBits; Row++)
|
||||
{ int16_t Ret=ProcessCheck(Row);
|
||||
if(Ret<=0) Count++; }
|
||||
// printf("%d parity checks fail\n", Count);
|
||||
if(Count==0) return 0;
|
||||
for(uint8_t Bit=0; Bit<CodeBits; Bit++)
|
||||
{ OutBit[Bit] = InpBit[Bit] + (ExtBit[Bit]>>1); }
|
||||
return Count; }
|
||||
|
||||
int16_t ProcessCheck(uint8_t Row)
|
||||
{ int16_t MinAmpl=32767; uint8_t MinBit=0; int16_t MinAmpl2=MinAmpl;
|
||||
uint32_t Word=0; uint32_t Mask=1;
|
||||
const uint8_t *CheckIndex = LDPC_ParityCheckIndex_n208k160[Row];
|
||||
uint8_t CheckWeight = *CheckIndex++;
|
||||
for(uint8_t Bit=0; Bit<CheckWeight; Bit++)
|
||||
{ uint8_t BitIdx=CheckIndex[Bit];
|
||||
int16_t Ampl=OutBit[BitIdx];
|
||||
if(Ampl>0) Word|=Mask;
|
||||
Mask<<=1;
|
||||
if(Ampl<0) Ampl=(-Ampl);
|
||||
if(Ampl<MinAmpl) { MinAmpl2=MinAmpl; MinAmpl=Ampl; MinBit=Bit; }
|
||||
else if(Ampl<MinAmpl2) { MinAmpl2=Ampl; }
|
||||
}
|
||||
uint8_t CheckFails = Count1s(Word)&1;
|
||||
Mask=1;
|
||||
for(uint8_t Bit=0; Bit<CheckWeight; Bit++)
|
||||
{ uint8_t BitIdx=CheckIndex[Bit];
|
||||
int16_t Ampl = Bit==MinBit ? MinAmpl2 : MinAmpl;
|
||||
if(CheckFails) Ampl=(-Ampl);
|
||||
ExtBit[BitIdx] += (Word&Mask) ? Ampl:-Ampl;
|
||||
Mask<<=1; }
|
||||
return CheckFails?-MinAmpl:MinAmpl; }
|
||||
|
||||
} ;
|
||||
|
||||
template <class Float=float>
|
||||
class LDPC_FloatDecoder
|
||||
{ public:
|
||||
|
||||
const static int MaxCodeBits=512;
|
||||
const static int MaxParityBits=256;
|
||||
const static int MaxParityWeight=32; //
|
||||
int CodeBits; // number of code bits
|
||||
int ParityBits; // number of parity bits
|
||||
uint16_t ParityCheckIndex[MaxParityBits][MaxParityWeight]; // list of 1's in the ParityCheck matrix
|
||||
uint8_t ParityCheckRowWeight[MaxParityBits]; // number of 1's in ParityCheck rows
|
||||
uint8_t ParityCheckColWeight[MaxCodeBits]; // number of 1's in ParityCheck columns
|
||||
|
||||
Float InpBit[MaxCodeBits]; // a-priori bits
|
||||
Float ExtBit[MaxCodeBits]; // extrinsic inf.
|
||||
Float OutBit[MaxCodeBits]; // a-posteriori bits
|
||||
Float Feedback;
|
||||
|
||||
public:
|
||||
|
||||
LDPC_FloatDecoder()
|
||||
{ CodeBits=0; ParityBits=0; Feedback=0.33; }
|
||||
|
||||
void Clear(void)
|
||||
{ for(int Bit=0; Bit<CodeBits; Bit++)
|
||||
{ OutBit[Bit] = InpBit[Bit] = ExtBit[Bit]=0; }
|
||||
}
|
||||
|
||||
int Configure(int NewCodeBits, int NewParityBits, const uint32_t *PackedParityCheck )
|
||||
{ if(CodeBits>MaxCodeBits) return -1;
|
||||
CodeBits=NewCodeBits;
|
||||
if(ParityBits>MaxParityBits) return -1;
|
||||
ParityBits=NewParityBits;
|
||||
for(int Bit=0; Bit<CodeBits; Bit++)
|
||||
ParityCheckColWeight[Bit]=0;
|
||||
const uint32_t *Check=PackedParityCheck;
|
||||
for(int ParBit=0; ParBit<ParityBits; ParBit++)
|
||||
{ int RowWeight=0;
|
||||
uint32_t Word=0; uint32_t Mask=0;
|
||||
for(int Bit=0; Bit<CodeBits; Bit++)
|
||||
{ if(Mask==0) { Mask=1; Word=(*Check++); }
|
||||
if(Word&Mask)
|
||||
{ ParityCheckIndex[ParBit][RowWeight++]=Bit;
|
||||
ParityCheckColWeight[Bit]++; }
|
||||
Mask<<=1;
|
||||
}
|
||||
ParityCheckRowWeight[ParBit]=RowWeight;
|
||||
}
|
||||
return 1; }
|
||||
|
||||
void PrintConfig(void) const
|
||||
{ printf("LDPC_FloatDecoder[%d,%d] Check index table:\n", CodeBits, ParityBits);
|
||||
for(int ParBit=0; ParBit<ParityBits; ParBit++)
|
||||
{ printf("Check[%3d]:", ParityCheckRowWeight[ParBit]);
|
||||
for(int Bit=0; Bit<ParityCheckRowWeight[ParBit]; Bit++)
|
||||
{ printf(" %3d", ParityCheckIndex[ParBit][Bit]); }
|
||||
printf("\n");
|
||||
}
|
||||
printf("ColWeight[%d]:\n", CodeBits);
|
||||
int Bit;
|
||||
for(Bit=0; Bit<CodeBits; Bit++)
|
||||
{ if((Bit&0x1F)==0x00) printf("%03d:", Bit);
|
||||
printf(" %d", ParityCheckColWeight[Bit]);
|
||||
if((Bit&0x1F)==0x1F) printf("\n"); }
|
||||
if((Bit&0x1F)!=0x00) printf("\n");
|
||||
}
|
||||
|
||||
void PrintOutBits(void)
|
||||
{ printf("OutBit[%d]\n", CodeBits);
|
||||
for(int Bit=0; Bit<CodeBits; Bit++)
|
||||
{ if((Bit&0xF)==0x0) printf("%03d:", Bit);
|
||||
printf(" %+6.3f", OutBit[Bit]);
|
||||
if((Bit&0xF)==0xF) printf("\n"); }
|
||||
}
|
||||
|
||||
void addInput(int Bit, Float Ampl)
|
||||
{ InpBit[Bit]+=Ampl; OutBit[Bit] = InpBit[Bit]; }
|
||||
|
||||
void Input(const uint8_t *Data, const uint8_t *Err, Float Ampl=1.0) // get bits from series of bytes and the error pattern (from Manchester decoder)
|
||||
{ uint8_t Mask=1; int Idx=0; uint8_t DataByte=0; uint8_t ErrByte=0;
|
||||
for(int Bit=0; Bit<CodeBits; Bit++)
|
||||
{ if(Mask==1) { DataByte=Data[Idx]; ErrByte=Err[Idx]; }
|
||||
Float Inp;
|
||||
if(ErrByte&Mask) Inp=0;
|
||||
else Inp=(DataByte&Mask) ? +Ampl:-Ampl;
|
||||
OutBit[Bit] = InpBit[Bit] = Inp; ExtBit[Bit]=0;
|
||||
Mask<<=1; if(Mask==0) { Idx++; Mask=1; }
|
||||
}
|
||||
}
|
||||
|
||||
void Input(const uint32_t *Data, Float Ampl=1.0) // get bits from a series of 32-bit words
|
||||
{ uint32_t Mask=0; int Idx=0; uint32_t Word=0;
|
||||
for(int Bit=0; Bit<CodeBits; Bit++)
|
||||
{ if(Mask==0) { Word=Data[Idx++]; Mask=1; }
|
||||
OutBit[Bit] = InpBit[Bit] = (Word&Mask) ? +Ampl:-Ampl; ExtBit[Bit]=0;
|
||||
Mask<<=1;
|
||||
}
|
||||
}
|
||||
|
||||
void Output(uint32_t *Data) // format decoded bits as a series of 32-bit words
|
||||
{ uint32_t Mask=1; int Idx=0; uint32_t Word=0;
|
||||
for(int Bit=0; Bit<CodeBits; Bit++)
|
||||
{ if(OutBit[Bit]>0) Word|=Mask;
|
||||
Mask<<=1; if(Mask==0) { Data[Idx++]=Word; Word=0; Mask=1; }
|
||||
} if(Mask>1) Data[Idx++]=Word;
|
||||
}
|
||||
|
||||
void Output(uint8_t *Data) // format decoded bits as a series of bytes
|
||||
{ uint8_t Mask=1; int Idx=0; uint8_t Byte=0;
|
||||
for(int Bit=0; Bit<CodeBits; Bit++)
|
||||
{ if(OutBit[Bit]>0) Byte|=Mask;
|
||||
Mask<<=1; if(Mask==0) { Data[Idx++]=Byte; Byte=0; Mask=1; }
|
||||
} if(Mask>1) Data[Idx++]=Byte;
|
||||
}
|
||||
|
||||
int ProcessChecks(void)
|
||||
{ for(int Bit=0; Bit<CodeBits; Bit++) // clear the extrinsic inf. for bits
|
||||
ExtBit[Bit]=0;
|
||||
int Count=0;
|
||||
for(int Row=0; Row<ParityBits; Row++) // process all parity checks and count how many have failed
|
||||
{ Float Ret=ProcessCheck(Row);
|
||||
if(Ret<=0) Count++; }
|
||||
// printf("%d parity checks fail\n", Count);
|
||||
if(Count==0) return 0; // if all passed, then return
|
||||
for(int Bit=0; Bit<CodeBits; Bit++) // add Input+Extrinsic and store in Output
|
||||
{ OutBit[Bit] = InpBit[Bit] + Feedback*ExtBit[Bit]; }
|
||||
return Count; }
|
||||
|
||||
Float ProcessCheck(uint8_t Row)
|
||||
{ Float MinAmpl=std::numeric_limits<Float>::max(); int MinBit=0; Float MinAmpl2=MinAmpl; // look for 1st and 2nd smallest LL
|
||||
uint32_t Word=0; uint32_t Mask=1;
|
||||
const uint16_t *CheckIndex = ParityCheckIndex[Row]; // indeces of bits in this parity check
|
||||
int CheckWeight = ParityCheckRowWeight[Row]; // number of bits in this parity check
|
||||
for(int Bit=0; Bit<CheckWeight; Bit++) // loop over bits in the parity check
|
||||
{ int BitIdx=CheckIndex[Bit]; // index of the bit
|
||||
Float Ampl=OutBit[BitIdx]; // LL of the bit
|
||||
if(Ampl>0) Word|=Mask; // store hard bits in the Word
|
||||
Mask<<=1;
|
||||
if(Ampl<0) Ampl=(-Ampl); // strip the LL sign
|
||||
if(Ampl<MinAmpl) { MinAmpl2=MinAmpl; MinAmpl=Ampl; MinBit=Bit; } // find 1st and 2nd smallest
|
||||
else if(Ampl<MinAmpl2) { MinAmpl2=Ampl; }
|
||||
}
|
||||
int CheckFails = __builtin_parityl(Word); // tell if this parity check failed
|
||||
Mask=1;
|
||||
for(int Bit=0; Bit<CheckWeight; Bit++) // loop over bits in this parity check
|
||||
{ int BitIdx=CheckIndex[Bit]; // inndex of the bit
|
||||
Float Ampl = Bit==MinBit ? MinAmpl2 : MinAmpl; // if this is the weakest bit, then use 2nd smallest LL, otherwise 1st
|
||||
if(CheckFails) Ampl=(-Ampl);
|
||||
ExtBit[BitIdx] += (Word&Mask) ? Ampl:-Ampl; // add to the extrinsic inf. with the correct sign
|
||||
Mask<<=1; }
|
||||
return CheckFails?-MinAmpl:MinAmpl; }
|
||||
|
||||
int CountErrors(void)
|
||||
{ int Count=0;
|
||||
for(int Idx=0; Idx<CodeBits; Idx++)
|
||||
{ bool Inp=InpBit[Idx]>0;
|
||||
bool Out=OutBit[Idx]>0;
|
||||
if(Inp!=Out) Count++; }
|
||||
return Count; }
|
||||
|
||||
} ;
|
||||
|
||||
#ifdef WITH_PPM
|
||||
template <class Float>
|
||||
class OGN_PPM_Decoder
|
||||
{ public:
|
||||
static const int DataBits = 32*5; // 5 words = 160 data bits = OGN packet
|
||||
static const int ParityBits = 194; // 194 parity bits (Gallager code)
|
||||
static const int CodeBits = DataBits+ParityBits; // 354 total bits per Gallager code block
|
||||
static const int BitsPerSymbol = 6; // 6 bits per symbol for PPM modulation
|
||||
static const int PulsesPerSlot = 1<<BitsPerSymbol; // 64 (possible) pulses per time slot = 1 symbol = 6 bits
|
||||
static const int CodeSymbols = CodeBits/BitsPerSymbol; // 59 time slots to form complete packet
|
||||
|
||||
LDPC_FloatDecoder<Float> LDPC_Decoder; // inner LDPC code decoder
|
||||
|
||||
Float InpSymb[CodeSymbols][PulsesPerSlot]; // input from the demodulator
|
||||
Float ExtSymb[CodeSymbols][PulsesPerSlot]; // output from the LDPC decoder
|
||||
Float OutSymb[CodeSymbols][PulsesPerSlot]; // input x extrinsic inf.
|
||||
|
||||
public:
|
||||
OGN_PPM_Decoder()
|
||||
{ LDPC_Decoder.Configure(CodeBits, ParityBits, (uint32_t *)LDPC_ParityCheck_n354k160);
|
||||
Clear(); }
|
||||
|
||||
void Clear(void)
|
||||
{ for(int Symb=0; Symb<CodeSymbols; Symb++)
|
||||
{ Float Ext=1.0/PulsesPerSlot;
|
||||
for(int Pulse=0; Pulse<PulsesPerSlot; Pulse++)
|
||||
{ InpSymb[Symb][Pulse]=0; ExtSymb[Symb][Pulse]=Ext; OutSymb[Symb][Pulse]=0; }
|
||||
}
|
||||
}
|
||||
|
||||
void addSymbol(unsigned int Slot, unsigned int Symbol, Float Power=1.0)
|
||||
{ if( (Slot>=CodeSymbols) || (Symbol>=PulsesPerSlot) ) return;
|
||||
InpSymb[Slot][Symbol]+=Power; }
|
||||
|
||||
int Process(int Loops=48)
|
||||
{ LDPC_Decoder.Clear();
|
||||
|
||||
for(int Symb=0; Symb<CodeSymbols; Symb++)
|
||||
{ for(int Pulse=0; Pulse<PulsesPerSlot; Pulse++)
|
||||
{ Float Pwr=InpSymb[Symb][Pulse]*ExtSymb[Symb][Pulse];
|
||||
if(Pwr==0) continue;
|
||||
Pwr=Pwr*Pwr;
|
||||
int Bin=Binary(Pulse);
|
||||
int Idx=Symb;
|
||||
for(int Bit=0; Bit<BitsPerSymbol; Bit++, Idx+=CodeSymbols)
|
||||
{ LDPC_Decoder.addInput(Idx, (Bin&1) ? +Pwr:-Pwr);
|
||||
Bin>>=1; }
|
||||
}
|
||||
}
|
||||
int CheckErr=0;
|
||||
for( int Loop=0; Loop<Loops; Loop++)
|
||||
{ CheckErr=LDPC_Decoder.ProcessChecks();
|
||||
printf("%3d: OGN_PPM_Decoder.Process() => %3d\n", Loop, CheckErr);
|
||||
if(CheckErr==0) break; }
|
||||
return CheckErr; }
|
||||
|
||||
static uint8_t Gray(uint8_t Binary) { return Binary ^ (Binary>>1); }
|
||||
|
||||
static uint8_t Binary(uint8_t Gray)
|
||||
{ Gray = Gray ^ (Gray >> 4);
|
||||
Gray = Gray ^ (Gray >> 2);
|
||||
Gray = Gray ^ (Gray >> 1);
|
||||
return Gray; }
|
||||
|
||||
void NormExtSymb(Float Norm=1.0)
|
||||
{ for(int Symb=0; Symb<CodeSymbols; Symb++)
|
||||
{ NormExtSymb(Symb, Norm); }
|
||||
}
|
||||
|
||||
void NormExtSymp(int Symb, Float Norm=1.0)
|
||||
{ Float Sum=0;
|
||||
for(int Pulse=0; Pulse<PulsesPerSlot; Pulse++)
|
||||
{ Sum+=ExtSymb[Symb][Pulse]; }
|
||||
Sum=Norm/Sum;
|
||||
for(int Pulse=0; Pulse<PulsesPerSlot; Pulse++)
|
||||
{ ExtSymb[Symb][Pulse]*=Sum; }
|
||||
}
|
||||
|
||||
} ;
|
||||
#endif // WITH_PPM
|
||||
|
||||
#endif // __AVR__
|
||||
|
||||
#endif // of __LDPC_H__
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
read_log: read_log.cc
|
||||
g++ -Wall -Wno-misleading-indentation -g -O2 -o read_log read_log.cc format.cpp
|
||||
|
||||
aprs2igc: aprs2igc.cc
|
||||
g++ -Wall -Wno-misleading-indentation -O2 -o aprs2igc aprs2igc.cc format.cpp
|
||||
|
||||
clean:
|
||||
rm read_log aprs2igc
|
||||
|
||||
|
|
@ -0,0 +1,320 @@
|
|||
#ifndef __MAVLINK_H__
|
||||
#define __MAVLINK_H__
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "intmath.h"
|
||||
|
||||
// ============================================================================
|
||||
|
||||
// https://github.com/mavlink/mavlink/blob/master/message_definitions/v1.0/common.xml
|
||||
|
||||
// System-ID
|
||||
|
||||
|
||||
// Component-ID
|
||||
const uint8_t MAV_COMP_ID_AUTOPILOT1 = 1; // auto-pilot
|
||||
const uint8_t MAV_COMP_ID_ADSB = 156; // ADS-B receiver
|
||||
const uint8_t MAV_COMP_ID_GPS = 220;
|
||||
|
||||
// Message-ID
|
||||
const uint8_t MAV_ID_HEARTBEAT = 0; // +
|
||||
const uint8_t MAV_ID_SYS_STATUS = 1; // + power data
|
||||
const uint8_t MAV_ID_SYSTEM_TIME = 2; // + boot and UTC time
|
||||
const uint8_t MAV_ID_PARAM_VALUE = 22; // + value of an parameter
|
||||
const uint8_t MAV_ID_GPS_RAW_INT = 24; // + position after the GPS
|
||||
const uint8_t MAV_ID_RAW_IMU = 27; // +
|
||||
const uint8_t MAV_ID_SCALED_PRESSURE = 29; // + pressure+temperature
|
||||
const uint8_t MAV_ID_ATTITUDE = 30; //
|
||||
const uint8_t MAV_ID_GLOBAL_POSITION_INT = 33; // + combined position estimate
|
||||
const uint8_t MAV_ID_RC_CHANNELS_RAW = 35; //
|
||||
const uint8_t MAV_ID_SERVO_OUTPUT_RAW = 36; //
|
||||
const uint8_t MAV_ID_MISSION_CURRENT = 42; //
|
||||
const uint8_t MAV_ID_NAV_CONTROLLER_OUTPUT = 62; //
|
||||
const uint8_t MAV_ID_VFR_HUD = 74; //
|
||||
const uint8_t MAV_ID_TIMESYNC = 111;
|
||||
const uint8_t MAV_ID_HIL_GPS = 113;
|
||||
const uint8_t MAV_ID_SCALED_IMU2 = 116; //
|
||||
const uint8_t MAV_ID_POWER_STATUS = 125; // 5V and servo power and status
|
||||
const uint8_t MAV_ID_TERRAIN_REPORT = 136; //
|
||||
const uint8_t MAV_ID_BATTERY_STATUS = 147; //
|
||||
const uint8_t MAV_ID_ADSB_VEHICLE = 246; // + traffic information sent by an ADS-B receiver
|
||||
const uint8_t MAV_ID_COLLISION = 247; // + collision threat detected by auto-pilot
|
||||
const uint8_t MAV_ID_STATUSTEXT = 253; // +
|
||||
const uint8_t MAV_ID_DEBUG = 254;
|
||||
|
||||
// --------------------------------------------------------------------------------
|
||||
|
||||
class MAV_HEARTBEAT // 0
|
||||
{ public:
|
||||
uint32_t custom_mode;
|
||||
uint8_t type; // MAV-aircraft-type: 1=fixed wing, 2=quadrotor, 7=airship, 8=balloon, D=hexarotor
|
||||
uint8_t autopilot; // 3=ArduPilotMega, 4=OpenPilot, 13=PX4
|
||||
union
|
||||
{ uint8_t base_mode;
|
||||
struct
|
||||
{ bool custom:1;
|
||||
bool test:1;
|
||||
bool autonomous:1;
|
||||
bool guided:1;
|
||||
bool stabilize:1;
|
||||
bool hil_simulation:1;
|
||||
bool remote_control:1;
|
||||
bool armed:1;
|
||||
} ;
|
||||
} ;
|
||||
uint8_t system_status; // 1=boot, 2=calib., 3=standby, 4=active, 5=critical, 6=emergency, 7=power-off, 8=terminate
|
||||
uint8_t mavlink_version;
|
||||
public:
|
||||
void Print(void) const
|
||||
{ printf("HEARTBEAT: t%X v%d\n", type, mavlink_version); }
|
||||
} ;
|
||||
|
||||
class MAV_SYS_STATUS // 1
|
||||
{ public:
|
||||
uint32_t onboard_control_sensors_present;
|
||||
uint32_t onboard_control_sensors_enabled;
|
||||
uint32_t onboard_control_sensors_health;
|
||||
uint16_t load; // [0.1%]
|
||||
uint16_t battery_voltage; // [mV]
|
||||
int16_t battery_current; // [10mA]
|
||||
uint16_t error_comm;
|
||||
uint16_t error_comm1;
|
||||
uint16_t error_comm2;
|
||||
uint16_t error_comm3;
|
||||
uint16_t error_comm4;
|
||||
uint8_t battery_remaining; // [%]
|
||||
void Print(void) const
|
||||
{ printf("SYS_STATUS: %3.1f%% %5.3fV %+5.2fA\n", 0.1*load, 1e-3*battery_voltage, 0.01*battery_current); }
|
||||
} ;
|
||||
|
||||
class MAV_SYSTEM_TIME // 2
|
||||
{ public:
|
||||
uint64_t time_unix_usec; // [usec]
|
||||
uint32_t time_boot_ms; // [ms]
|
||||
public:
|
||||
void Print(void) const
|
||||
{ printf("SYSTEM_TIME: %14.3f-%8.3f [sec]\n", 1e-6*time_unix_usec, 1e-3*time_boot_ms); }
|
||||
} ;
|
||||
|
||||
class MAV_PARAM_VALUE
|
||||
{ public:
|
||||
float param_value;
|
||||
uint16_t param_count;
|
||||
uint16_t param_index;
|
||||
char param_id[16];
|
||||
uint8_t param_type; // (1)2=(u)int8, (3)/4=(u)int16_t, (5)/6=(u)int32_t, (7)/8=(u)int64_t, 9=float, 10=double
|
||||
public:
|
||||
void Print(void) const
|
||||
{ printf("PARAM_VALUE: %.16s t%X %d/%d\n", param_id, param_type, param_index, param_count); }
|
||||
} ;
|
||||
|
||||
class MAV_GPS_RAW_INT // 24
|
||||
{ public:
|
||||
uint64_t time_usec; // [usec] Time-since-boot time or UTC
|
||||
int32_t lat; // [1e-7deg] Latitude
|
||||
int32_t lon; // [1e-7deg] Longitude
|
||||
int32_t alt; // [mm] Altitude AMSL
|
||||
// int32_t alt_ellipsoid; // [mm]
|
||||
// uint32_t h_acc; // [mm]
|
||||
// uint32_t v_acc; // [mm]
|
||||
// uint32_t vel_acc; // [mm/s]
|
||||
// uint32_t hdg_acc; // [0.00001deg]
|
||||
uint16_t eph; // [0.01] HDOP
|
||||
uint16_t epv; // [0.01] VDOP
|
||||
uint16_t vel; // [0.01m/s] Valocity
|
||||
uint16_t cog; // [0.01deg] course-over-Ground
|
||||
uint8_t fix_type; // [] 0=no GPS, 1=no fix, 2=2D, 3=3D, 4=DGPS
|
||||
uint8_t satellites_visible; // [] Number of satellites
|
||||
public:
|
||||
void Print(void) const
|
||||
{ printf("GPS_RAW_INT: %14.3fsec [%+9.5f, %+10.5f]deg %+5.1fm %3.1fm/s %05.1fdeg %d/%dsat\n",
|
||||
1e-6*time_usec, 1e-7*lat, 1e-7*lon, 1e-3*alt, 0.01*vel, 0.01*cog, fix_type, satellites_visible); }
|
||||
} ;
|
||||
|
||||
class MAV_RAW_IMU
|
||||
{ public:
|
||||
uint64_t time_usec; // [usec] Time-since-boot time or UTC
|
||||
int16_t xacc; // [] Accelerometer
|
||||
int16_t yacc;
|
||||
int16_t zacc;
|
||||
int16_t xgyro; // [] Gyroskop
|
||||
int16_t ygyro;
|
||||
int16_t zgyro;
|
||||
int16_t xmag; // [] Magnetometer
|
||||
int16_t ymag;
|
||||
int16_t zmag;
|
||||
} ;
|
||||
|
||||
class MAV_GLOBAL_POSITION_INT
|
||||
{ public:
|
||||
uint32_t time_boot_ms; // [ms]
|
||||
int32_t lat; // [1e-7deg]
|
||||
int32_t lon; // [1e-7deg]
|
||||
int32_t alt; // [mm] AMSL
|
||||
int32_t relative_alt; // [mm] Above-takeoff ?
|
||||
int16_t vx; // [cm/s] along latitude: positive => north
|
||||
int16_t vy; // [cm/s] along longitude: positive => east
|
||||
int16_t vz; // [cm/s] sink rate: positive => down
|
||||
int16_t hdg; // [0.01deg] yaw angle
|
||||
public:
|
||||
void Print(void) const
|
||||
{ uint16_t Track = IntAtan2(vy, vx);
|
||||
printf("GLOBAL_POSITION_INT: %10.3fsec [%+9.5f, %+10.5f]deg %5.1f(%+4.1f)m %3.1fm/s %05.1f/%05.1fdeg %+4.1fm/s\n",
|
||||
1e-3*time_boot_ms, 1e-7*lat, 1e-7*lon, 1e-3*alt, 1e-3*relative_alt,
|
||||
0.01*IntSqrt((int32_t)vx*vx+(int32_t)vy*vy), 360.0/0x10000*Track, 0.01*hdg,
|
||||
0.01*vz); }
|
||||
} ;
|
||||
|
||||
class MAV_SCALED_PRESSURE
|
||||
{ public:
|
||||
uint32_t time_boot_ms; // [msec]
|
||||
float press_abs; // [hPa]
|
||||
float press_diff; // [hPa]
|
||||
int16_t temperature; // [0.01degC]
|
||||
public:
|
||||
void Print(void) const
|
||||
{ printf("SCALED_PRESSURE: %8.3f [sec] %7.2f %+4.2f [hPa] %+5.2f [degC]\n",
|
||||
1e-3*time_boot_ms, press_abs, press_diff, 0.01*temperature); } // with ms5607 there is a bug/feature: pressure is twice as low
|
||||
} ;
|
||||
|
||||
class MAV_ADSB_VEHICLE // this message is sent by ADS-B or other traffic receiver
|
||||
{ public:
|
||||
uint32_t ICAO_address; // ICAO ID (for ADS-B), other ID's for FLARM/OGN/...
|
||||
int32_t lat; // [1e-7deg]
|
||||
int32_t lon; // [1e-7deg]
|
||||
int32_t altitude; // [mm]
|
||||
uint16_t heading; // [0.01deg]
|
||||
uint16_t hor_velocity; // [cm/sec]
|
||||
int16_t ver_velocity; // [cm/sec]
|
||||
union
|
||||
{ uint16_t flags; // validity: 1=coord. 2=alt. 4=heading 8=velocity 16=callsign 32=squawk 64=simulated
|
||||
struct
|
||||
{ bool CoordValid:1; // #0
|
||||
bool AltValid:1; // #1
|
||||
bool HeadingValid:1; // #2
|
||||
bool CallsignValid:1; // #3
|
||||
bool VelocityValid:1; // #4
|
||||
bool SquawkValid:1; // #5
|
||||
bool isSimulated:1; // #6
|
||||
} ;
|
||||
} ;
|
||||
uint16_t squawk;
|
||||
uint8_t altitude_type; // 0 = pressure/QNH, 1 = GPS
|
||||
char callsign[9]; // 8+null
|
||||
uint8_t emiter_type; // 0=no-info, 1=light, 2=small. 3=large, 4=high-vortex, 5=heavy, 6=manuv, 7=rotor, 9=glider, 10=balloon/airship, 11=parachute, 12=ULM, 14=UAV, 15=space, 19=obstacle
|
||||
uint8_t tslc; // [sec] time since last communication
|
||||
public:
|
||||
void Print(void) const
|
||||
{ printf("ADSB_VEHICLE: %.9s %02X:%08lX [%+9.5f, %+10.5f]deg %5.1fm %3.1fm/s %05.1fdeg %+4.1fm/s %dsec\n",
|
||||
callsign, (int)emiter_type, (long int)ICAO_address,
|
||||
1e-7*lat, 1e-7*lon, 1e-3*altitude, 0.01*hor_velocity, 360.0/0x10000*heading, 0.01*ver_velocity,
|
||||
tslc); }
|
||||
} ;
|
||||
|
||||
class COLLISION // this message is sent by the autopilot when it detects a collision threat
|
||||
{ public:
|
||||
uint32_t id;
|
||||
float time_to_minimum_delta;
|
||||
float altitude_minimum_delta;
|
||||
float horizontal_minimum_delta;
|
||||
uint8_t src;
|
||||
uint8_t action;
|
||||
uint8_t threat_level;
|
||||
} ;
|
||||
|
||||
// =============================================================================
|
||||
|
||||
// https://groups.google.com/forum/#!topic/mavlink/-ipDgVeYSiU
|
||||
static const uint8_t mavlink_message_crcs[256] = {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 78, 196, 0, 0, 15, 3, 0, 0, 0, 0, 0, 153, 183, 51, 59, 118, 148, 21, 0, 243, 124, 0, 0, 38, 20, 158, 152, 143, 0, 0, 0, 106, 49, 22, 143, 140, 5, 150, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 138, 108, 32, 185, 84, 34, 174, 124, 237, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 25, 226, 46, 29, 223, 85, 6, 229, 203, 1, 195, 109, 168, 181, 47, 72, 131, 127, 0, 103, 154, 178, 200, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 34, 71, 15, 0, 0, 0, 0, 0, 0, 0, 163, 105, 0, 35, 0, 0, 0, 0, 0, 0, 0, 90, 104, 85, 95, 130, 184, 0, 8, 204, 49, 170, 44, 83, 46, 0};
|
||||
|
||||
class MAV_RxMsg // receiver for the MAV messages
|
||||
{ public:
|
||||
static const uint8_t MaxBytes = 65; // max. number of bytes
|
||||
static const uint8_t Sync = 0xFE; // MAV sync byte
|
||||
|
||||
uint16_t Check;
|
||||
uint8_t Byte[MaxBytes];
|
||||
uint8_t Idx;
|
||||
|
||||
public:
|
||||
void Clear(void) { Idx=0; CheckInit(Check); }
|
||||
|
||||
uint8_t getLen (void) const { return Byte[1]; } // Payload length (not whole packet length)
|
||||
uint8_t getSeq (void) const { return Byte[2]; } // Sequence (increments with every new message)
|
||||
uint8_t getSysID (void) const { return Byte[3]; } // System-ID
|
||||
uint8_t getCompID(void) const { return Byte[4]; } // Component-ID
|
||||
uint8_t getMsgID (void) const { return Byte[5]; } // Message-ID
|
||||
void *getPayload(void) const { return (void *)(Byte+6); } // message (pointer to) Payload
|
||||
|
||||
void Print(bool Ext=1) const
|
||||
{ printf("MAV[%2d:%2d] [%02X] %02X:%02X %3d:", Idx, getLen(), getSeq(), getSysID(), getCompID(), getMsgID() );
|
||||
if( (getMsgID()==MAV_ID_STATUSTEXT) && isComplete() )
|
||||
{ printf("(%d) %s\n", Byte[6], Byte+7); }
|
||||
else
|
||||
{ for(uint8_t i=6; i<Idx; i++)
|
||||
printf(" %02X", Byte[i]);
|
||||
printf(" %04X (%c)\n", Check, isComplete()?'+':'-');
|
||||
if(Ext)
|
||||
{ if(getMsgID()==MAV_ID_HEARTBEAT ) { ((const MAV_HEARTBEAT *)getPayload())->Print(); }
|
||||
else if(getMsgID()==MAV_ID_SYS_STATUS ) { ((const MAV_SYS_STATUS *)getPayload())->Print(); }
|
||||
else if(getMsgID()==MAV_ID_SYSTEM_TIME ) { ((const MAV_SYSTEM_TIME *)getPayload())->Print(); }
|
||||
else if(getMsgID()==MAV_ID_SCALED_PRESSURE ) { ((const MAV_SCALED_PRESSURE *)getPayload())->Print(); }
|
||||
else if(getMsgID()==MAV_ID_GPS_RAW_INT ) { ((const MAV_GPS_RAW_INT *)getPayload())->Print(); }
|
||||
else if(getMsgID()==MAV_ID_GLOBAL_POSITION_INT ) { ((const MAV_GLOBAL_POSITION_INT *)getPayload())->Print(); }
|
||||
else if(getMsgID()==MAV_ID_ADSB_VEHICLE ) { ((const MAV_ADSB_VEHICLE *)getPayload())->Print(); }
|
||||
else if(getMsgID()==MAV_ID_PARAM_VALUE ) { ((const MAV_PARAM_VALUE *)getPayload())->Print(); }
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t ProcessByte(uint8_t RxByte) // process a single byte: add to the message or reject
|
||||
{ // printf("Process[%2d] 0x%02X\n", Idx, RxByte);
|
||||
if(Idx==0) // the very first byte: we only accept SYNC
|
||||
{ if(RxByte==Sync) { Byte[Idx++]=RxByte; return 1; }
|
||||
else { return 0; }
|
||||
}
|
||||
if(Idx==1) // second byte: payload length
|
||||
{ Byte[Idx++]=RxByte; CheckPass(Check, RxByte); return 1; }
|
||||
if(Idx>=MaxBytes) { Clear(); return 0; } // take following bytes
|
||||
Byte[Idx++]=RxByte; if(Idx<(getLen()+7)) CheckPass(Check, RxByte);
|
||||
if(Idx==(getLen()+8))
|
||||
{ CheckPass(Check, mavlink_message_crcs[getMsgID()]);
|
||||
// printf("[%2d]", Idx); for(uint8_t i=0; i<Idx; i++) printf(" %02X", Byte[i]); printf(" %04X\n", Check);
|
||||
if( ((Check&0xFF)!=Byte[Idx-2]) || ((Check>>8)!=Byte[Idx-1]) ) { Clear(); return 0; }
|
||||
}
|
||||
return 1; }
|
||||
|
||||
uint8_t isComplete(void) const { return Idx==(getLen()+8); }
|
||||
|
||||
void static CheckInit(uint16_t &Check) { Check=0xFFFF; }
|
||||
void static CheckPass(uint16_t &Check, uint8_t Byte)
|
||||
{ uint8_t Tmp = Byte ^ (uint8_t)(Check&0xFF);
|
||||
Tmp ^= (Tmp<<4);
|
||||
Check = (Check>>8) ^ ((uint16_t)Tmp<<8) ^ ((uint16_t)Tmp<<3) ^ (Tmp>>4);
|
||||
// printf("CheckPass: 0x%02X => 0x%04X\n", Byte, Check);
|
||||
}
|
||||
|
||||
static uint8_t Send(uint8_t Len, uint8_t Seq, uint8_t SysID, uint8_t CompID, uint8_t MsgID, const uint8_t *Payload, void (*SendByte)(char) )
|
||||
{ uint16_t Check; CheckInit(Check);
|
||||
(*SendByte)(Sync);
|
||||
(*SendByte)(Len); CheckPass(Check, Len);
|
||||
(*SendByte)(Seq); CheckPass(Check, Seq);
|
||||
(*SendByte)(SysID); CheckPass(Check, SysID);
|
||||
(*SendByte)(CompID); CheckPass(Check, CompID);
|
||||
(*SendByte)(MsgID); CheckPass(Check, MsgID);
|
||||
for(uint8_t Idx=0; Idx<Len; Idx++)
|
||||
{ (*SendByte)(Payload[Idx]); CheckPass(Check, Payload[Idx]); }
|
||||
CheckPass(Check, mavlink_message_crcs[MsgID]);
|
||||
(*SendByte)(Check&0xFF); (*SendByte)(Check>>8);
|
||||
return 8+Len; }
|
||||
|
||||
uint8_t Send(void (*SendByte)(char)) const
|
||||
{ return Send(Byte[1], Byte[2], Byte[3], Byte[4], Byte[5], Byte+6, SendByte); }
|
||||
|
||||
} ;
|
||||
|
||||
// ============================================================================
|
||||
|
||||
#endif // __MAVLINK_H__
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
#include "nmea.h"
|
||||
|
||||
uint8_t NMEA_Check(uint8_t *NMEA, uint8_t Len) // NMEA check-sum
|
||||
{ uint8_t Check=0; // to be calculated over characters between '$' and '*' but _excluding_ those.
|
||||
for(uint8_t Idx=0; Idx<Len; Idx++)
|
||||
Check^=NMEA[Idx];
|
||||
return Check; }
|
||||
|
||||
uint8_t NMEA_AppendCheck(uint8_t *NMEA, uint8_t Len)
|
||||
{ uint8_t Check=NMEA_Check(NMEA+1, Len-1);
|
||||
NMEA[Len ]='*';
|
||||
uint8_t Digit=Check>>4; NMEA[Len+1] = Digit<10 ? Digit+'0':Digit+'A'-10;
|
||||
Digit=Check&0xF; NMEA[Len+2] = Digit<10 ? Digit+'0':Digit+'A'-10;
|
||||
return 3; }
|
||||
|
||||
uint8_t NMEA_AppendCheckCRNL(uint8_t *NMEA, uint8_t Len)
|
||||
{ uint8_t CheckLen=NMEA_AppendCheck(NMEA, Len);
|
||||
Len+=CheckLen; NMEA[Len]='\n';
|
||||
return CheckLen+1; }
|
||||
|
|
@ -0,0 +1,213 @@
|
|||
#ifndef __NMEA_H__
|
||||
#define __NMEA_H__
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
uint8_t NMEA_Check(uint8_t *NMEA, uint8_t Len);
|
||||
uint8_t NMEA_AppendCheck(uint8_t *NMEA, uint8_t Len);
|
||||
inline uint8_t NMEA_AppendCheck(char *NMEA, uint8_t Len) { return NMEA_AppendCheck((uint8_t*)NMEA, Len); }
|
||||
uint8_t NMEA_AppendCheckCRNL(uint8_t *NMEA, uint8_t Len);
|
||||
inline uint8_t NMEA_AppendCheckCRNL(char *NMEA, uint8_t Len) { return NMEA_AppendCheckCRNL((uint8_t*)NMEA, Len); }
|
||||
|
||||
class NMEA_RxMsg // receiver for the NMEA sentences
|
||||
{ public:
|
||||
static const uint8_t MaxLen=104; // maximum length
|
||||
static const uint8_t MaxParms=24; // maximum number of parameters (commas)
|
||||
uint8_t Data[MaxLen]; // the message itself
|
||||
uint8_t Len; // number of bytes
|
||||
uint8_t Parms; // number of commas
|
||||
uint8_t Parm[MaxParms]; // offset to each comma
|
||||
uint8_t State; // bits: 0:loading, 1:complete, 2:locked,
|
||||
uint8_t Check; // check sum: should be a XOR of all bytes between '$' and '*'
|
||||
|
||||
public:
|
||||
void Clear(void) // Clear the frame: discard all data, ready for next message
|
||||
{ State=0; Len=0; Parms=0; }
|
||||
|
||||
void Send(void (*SendByte)(char) ) const
|
||||
{ for(uint8_t Idx=0; Idx<Len; Idx++)
|
||||
{ (*SendByte)(Data[Idx]); }
|
||||
(*SendByte)('\r'); (*SendByte)('\n'); }
|
||||
|
||||
void ProcessByte(uint8_t Byte) // pass all bytes through this call and it will build the frame
|
||||
{
|
||||
if(isComplete()) return; // if already a complete frame, ignore extra bytes
|
||||
if(Len==0) // if data is empty
|
||||
{ if(Byte!='$') return; // then ignore all bytes but '$'
|
||||
Data[Len++]=Byte; // start storing the frame
|
||||
setLoading(); Check=0x00; Parms=0; // set state to "isLoading", clear checksum
|
||||
} else // if not empty (being loaded)
|
||||
{ if((Byte=='\r')||(Byte=='\n')) // if CR (or NL ?) then frame is complete
|
||||
{ setComplete(); if(Len<MaxLen) Data[Len]=0;
|
||||
return; }
|
||||
else if(Byte<=' ') // other control bytes treat as errors
|
||||
{ Clear(); return; } // and drop the frame
|
||||
else if(Byte==',') // save comma positions to later get back to the fields
|
||||
{ if(Parms<MaxParms) Parm[Parms++]=Len+1; } // save the position just after the comma
|
||||
if(Len<MaxLen) { Data[Len++]=Byte; Check^=Byte; } // store data but if too much then treat as an error
|
||||
else Clear(); // if too long, then drop the frame completely
|
||||
}
|
||||
return; }
|
||||
|
||||
uint8_t isLoading(void) const { return State &0x01; }
|
||||
void setLoading(void) { State|=0x01; }
|
||||
|
||||
uint8_t isComplete(void) const { return State &0x02; }
|
||||
void setComplete(void) { State|=0x02; }
|
||||
|
||||
uint8_t isLocked(void) const { return State&0x04; }
|
||||
|
||||
uint8_t isEmpty(void) const { return Len==0; }
|
||||
|
||||
uint8_t hasCheck(void) const
|
||||
{ if(Len<4) return 0;
|
||||
return Data[Len-3]=='*'; }
|
||||
|
||||
uint8_t isChecked(void) const // is the NMEA checksum OK ?
|
||||
{ if(!hasCheck()) return 0;
|
||||
uint8_t DataCheck = Check^Data[Len-3]^Data[Len-2]^Data[Len-1];
|
||||
int8_t HighDigit=HexValue(Data[Len-2]); if(HighDigit<0) return 0;
|
||||
int8_t LowDigit=HexValue(Data[Len-1]); if(LowDigit<0) return 0;
|
||||
uint8_t FrameCheck = (HighDigit<<4) | LowDigit;
|
||||
return DataCheck==FrameCheck; }
|
||||
/*
|
||||
static int8_t DecValue(uint8_t Char)
|
||||
{ if(Char<'0') return -1;
|
||||
if(Char<='9') return Char-'0';
|
||||
return -1; }
|
||||
*/
|
||||
static int8_t HexValue(uint8_t Char)
|
||||
{ if(Char<'0') return -1;
|
||||
if(Char<='9') return Char-'0';
|
||||
if(Char<'A') return -1;
|
||||
if(Char<='F') return Char-('A'-10);
|
||||
// if(Char<'a') return -1; // check-sum uses uppercase latters
|
||||
// if(Char<='f') return Char-('a'-10);
|
||||
return -1; }
|
||||
|
||||
const uint8_t *ParmPtr(uint8_t Field) const // get a pointer to given (comma separated) field
|
||||
{ if(Field>=Parms) return 0;
|
||||
return Data+Parm[Field]; }
|
||||
|
||||
uint8_t ParmLen(uint8_t Field) const
|
||||
{ if(Field>=Parms) return 0;
|
||||
if(Field==(Parms-1)) return Len-hasCheck()*3-Parm[Field];
|
||||
return Parm[Field+1]-Parm[Field]-1; }
|
||||
|
||||
uint8_t isGP(void) const // GPS sentence ?
|
||||
{ if(Data[1]!='G') return 0;
|
||||
return Data[2]=='P'; }
|
||||
|
||||
uint8_t isGN(void) const
|
||||
{ if(Data[1]!='G') return 0;
|
||||
return Data[2]=='N'; }
|
||||
|
||||
uint8_t isGx(void) const // GPS or GLONASS sentence ?
|
||||
{ return Data[1]=='G'; }
|
||||
|
||||
uint8_t isGPRMC(void) const // GPS recomended minimum data
|
||||
{ if(!isGP()) return 0;
|
||||
if(Data[3]!='R') return 0;
|
||||
if(Data[4]!='M') return 0;
|
||||
return Data[5]=='C'; }
|
||||
|
||||
uint8_t isGNRMC(void) const // GPS recomended minimum data
|
||||
{ if(!isGN()) return 0;
|
||||
if(Data[3]!='R') return 0;
|
||||
if(Data[4]!='M') return 0;
|
||||
return Data[5]=='C'; }
|
||||
|
||||
uint8_t isGxRMC(void) const // GPS recomended minimum data
|
||||
{ if(!isGx()) return 0;
|
||||
if(Data[3]!='R') return 0;
|
||||
if(Data[4]!='M') return 0;
|
||||
return Data[5]=='C'; }
|
||||
|
||||
uint8_t isGPGGA(void) const // GPS 3-D fix data
|
||||
{ if(!isGP()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='G') return 0;
|
||||
return Data[5]=='A'; }
|
||||
|
||||
uint8_t isGNGGA(void) const // GPS 3-D fix data
|
||||
{ if(!isGN()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='G') return 0;
|
||||
return Data[5]=='A'; }
|
||||
|
||||
uint8_t isGxGGA(void) const //
|
||||
{ if(!isGx()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='G') return 0;
|
||||
return Data[5]=='A'; }
|
||||
|
||||
uint8_t isGPGSA(void) const //
|
||||
{ if(!isGP()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='S') return 0;
|
||||
return Data[5]=='A'; }
|
||||
|
||||
uint8_t isGNGSA(void) const //
|
||||
{ if(!isGN()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='S') return 0;
|
||||
return Data[5]=='A'; }
|
||||
|
||||
uint8_t isGxGSA(void) const //
|
||||
{ if(!isGx()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='S') return 0;
|
||||
return Data[5]=='A'; }
|
||||
|
||||
uint8_t isGxGSV(void) const // GPS satellite data
|
||||
{ if(!isGx()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='S') return 0;
|
||||
return Data[5]=='V'; }
|
||||
|
||||
uint8_t isGPGSV(void) const // GPS satellite data
|
||||
{ if(!isGP()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='S') return 0;
|
||||
return Data[5]=='V'; }
|
||||
|
||||
uint8_t isGNGSV(void) const // GPS satellite data
|
||||
{ if(!isGN()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='S') return 0;
|
||||
return Data[5]=='V'; }
|
||||
|
||||
uint8_t isGPTXT(void) const // GPS satellite data
|
||||
{ if(!isGP()) return 0;
|
||||
if(Data[3]!='T') return 0;
|
||||
if(Data[4]!='X') return 0;
|
||||
return Data[5]=='T'; }
|
||||
|
||||
uint8_t isP(void) const
|
||||
{ return Data[1]=='P'; }
|
||||
|
||||
uint8_t isPOGN(void) const // OGN dedicated NMEA sentence
|
||||
{ if(Data[1]!='P') return 0;
|
||||
if(Data[2]!='O') return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
return Data[4]=='N'; }
|
||||
|
||||
uint8_t isPOGNB(void) // barometric report from the OGN tracker
|
||||
{ if(!isPOGN()) return 0;
|
||||
return Data[5]=='B'; }
|
||||
|
||||
uint8_t isPOGNT(void) // other aircraft position (tracking) report from OGN trackers
|
||||
{ if(!isPOGN()) return 0;
|
||||
return Data[5]=='T'; }
|
||||
|
||||
uint8_t isPOGNS(void) // tracker parameters setup
|
||||
{ if(!isPOGN()) return 0;
|
||||
return Data[5]=='S'; }
|
||||
|
||||
uint8_t isPOGNL(void) // log file list request
|
||||
{ if(!isPOGN()) return 0;
|
||||
return Data[5]=='L'; }
|
||||
|
||||
} ;
|
||||
|
||||
#endif // of __NMEA_H__
|
||||
Plik diff jest za duży
Load Diff
|
|
@ -0,0 +1,846 @@
|
|||
#ifndef __OGN1_H__
|
||||
#define __OGN1_H__
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
#include <string.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "ognconv.h"
|
||||
|
||||
#include "intmath.h"
|
||||
|
||||
#include "bitcount.h"
|
||||
#include "nmea.h"
|
||||
#include "mavlink.h"
|
||||
|
||||
#include "format.h"
|
||||
|
||||
// the packet description here is how it look on the little-endian CPU before sending it to the RF chip
|
||||
// nRF905, CC1101, SPIRIT1, RFM69 chips actually reverse the bit order within every byte
|
||||
// thus on the air the bits appear MSbit first for every byte transmitted
|
||||
|
||||
class OGN1_Packet // Packet structure for the OGN tracker
|
||||
{ public:
|
||||
|
||||
static const int Words = 5;
|
||||
static const int Bytes = 20;
|
||||
|
||||
union
|
||||
{ uint32_t HeaderWord; // ECRR PMTT AAAA AAAA AAAA AAAA AAAA AAAA
|
||||
// E=Emergency, C=enCrypt/Custom, RR=Relay count, P=Parity, M=isMeteo/Other, TT=address Type, AA..=Address:24-bit
|
||||
// When enCrypt/Custom is set the data (position or whatever) can only be decoded by the owner
|
||||
// This option is indented to pass any type of custom data not foreseen otheriwse
|
||||
struct
|
||||
{ unsigned int Address :24; // aircraft address
|
||||
unsigned int AddrType : 2; // address type: 0 = random, 1 = ICAO, 2 = FLARM, 3 = OGN
|
||||
unsigned int NonPos : 1; // 0 = position packet, 1 = other information like status
|
||||
unsigned int Parity : 1; // parity takes into account bits 0..27 thus only the 28 lowest bits
|
||||
unsigned int Relay : 2; // 0 = direct packet, 1 = relayed once, 2 = relayed twice, ...
|
||||
unsigned int Encrypted : 1; // packet is encrypted
|
||||
unsigned int Emergency : 1; // aircraft in emergency (not used for now)
|
||||
} Header ;
|
||||
|
||||
} ;
|
||||
|
||||
union
|
||||
{ uint32_t Data[4]; // 0: QQTT TTTT LLLL LLLL LLLL LLLL LLLL LLLL QQ=fix Quality:2, TTTTTT=time:6, LL..=Latitude:20
|
||||
// 1: MBDD DDDD LLLL LLLL LLLL LLLL LLLL LLLL F=fixMode:1 B=isBaro:1, DDDDDD=DOP:6, LL..=Longitude:20
|
||||
// 2: RRRR RRRR SSSS SSSS SSAA AAAA AAAA AAAA RR..=turn Rate:8, SS..=Speed:10, AA..=Alt:14
|
||||
// 3: BBBB BBBB YYYY PCCC CCCC CCDD DDDD DDDD BB..=Baro altitude:8, YYYY=AcftType:4, P=Stealth:1, CC..=Climb:9, DD..=Heading:10
|
||||
|
||||
// meteo/telemetry types: Meteo conditions, Thermal wind/climb, Device status, Precise time,
|
||||
|
||||
// meteo report: Humidity, Barometric pressure, Temperature, wind Speed/Direction
|
||||
// 2: HHHH HHHH SSSS SSSS SSAA AAAA AAAA AAAA
|
||||
// 3: TTTT TTTT YYYY BBBB BBBB BBDD DDDD DDDD YYYY = report tYpe (meteo, thermal, water level, other telemetry)
|
||||
|
||||
// Device status: Time, baro pressure+temperature, GPS altitude, supply voltage, TX power, RF noise, software version, software features, hardware features,
|
||||
// 0: UUUU UUUU UUUU UUUU UUUU UUUU UUUU UUUU UU..=Unix time
|
||||
// 1: SSSS SSSS SSSS SSSS TTTT TTTT HHHH HHHH SS..=slot time, TT..=temperature, HH..=humidity
|
||||
// 2: BBBB BBBB BBBB BBBB BBAA AAAA AAAA AAAA Baro pressure[0.5Pa], GPS altitude
|
||||
// 3: VVVV VVVV YYYY HHHH HHHH XXXX VVVV VVVV VV..=firmware version, YYYY = report type, TT..=Temperatature, XX..=TxPower, VV..=battery voltage
|
||||
|
||||
// Pilot status:
|
||||
// 0: NNNN NNNN NNNN NNNN NNNN NNNN NNNN NNNN Name: 9 char x 7bit or 10 x 6bit or Huffman encoding ?
|
||||
// 1: NNNN NNNN NNNN NNNN NNNN NNNN NNNN NNNN
|
||||
struct
|
||||
{ signed int Latitude:24; // // QQTT TTTT LLLL LLLL LLLL LLLL LLLL LLLL QQ=fix Quality:2, TTTTTT=time:6, LL..=Latitude:24
|
||||
unsigned int Time: 6; // [sec] // time, just second thus ambiguity every every minute
|
||||
unsigned int FixQuality: 2; // // 0 = none, 1 = GPS, 2 = Differential GPS (can be WAAS)
|
||||
signed int Longitude:24; // // MBDD DDDD LLLL LLLL LLLL LLLL LLLL LLLL F=fixMode:1 B=isBaro:1, DDDDDD=DOP:6, LL..=Longitude:24
|
||||
unsigned int DOP: 6; // // GPS Dilution of Precision
|
||||
unsigned int BaroMSB: 1; // // negated bit #8 of the altitude difference between baro and GPS
|
||||
unsigned int FixMode: 1; // // 0 = 2-D, 1 = 3-D
|
||||
unsigned int Altitude:14; // [m] VR // RRRR RRRR SSSS SSSS SSAA AAAA AAAA AAAA RR..=turn Rate:8, SS..=Speed:10, AA..=Alt:14
|
||||
unsigned int Speed:10; // [0.1m/s] VR
|
||||
unsigned int TurnRate: 8; // [0.1deg/s] VR
|
||||
unsigned int Heading:10; // [360/1024deg] // BBBB BBBB YYYY PCCC CCCC CCDD DDDD DDDD BB..=Baro altitude:8, YYYY=AcftType:4, P=Stealth:1, CC..=Climb:9, DD..=Heading:10
|
||||
unsigned int ClimbRate: 9; // [0.1m/s] VR // rate of climb/decent from GPS or from baro sensor
|
||||
unsigned int Stealth: 1; // // not really used till now
|
||||
unsigned int AcftType: 4; // [0..15] // type of aircraft: 1 = glider, 2 = towplane, 3 = helicopter, ...
|
||||
unsigned int BaroAltDiff: 8; // [m] // lower 8 bits of the altitude difference between baro and GPS
|
||||
} Position;
|
||||
|
||||
struct
|
||||
{ unsigned int Pulse : 8; // [bpm] // pilot: heart pulse rate
|
||||
unsigned int Oxygen : 7; // [%] // pilot: oxygen level in the blood
|
||||
unsigned int SatSNR : 5; // [dB] // average SNR of GPS signals
|
||||
// unsigned int FEScurr : 5; // [A] // FES current
|
||||
unsigned int RxRate : 4; // [/min] // log2 of received packet rate
|
||||
unsigned int Time : 6; // [sec] // same as in the position packet
|
||||
unsigned int FixQuality: 2;
|
||||
unsigned int AudioNoise: 8; // [dB] //
|
||||
unsigned int RadioNoise: 8; // [dBm] // noise seen by the RF chip
|
||||
unsigned int Temperature:8; // [0.1degC] VR // temperature by the baro or RF chip
|
||||
unsigned int Humidity : 8; // [0.1%] VR // humidity
|
||||
unsigned int Altitude :14; // [m] VR // same as in the position packet
|
||||
unsigned int Pressure :14; // [0.08hPa] // barometric pressure
|
||||
unsigned int Satellites: 4; // [ ]
|
||||
unsigned int Firmware : 8; // [ ] // firmware version
|
||||
unsigned int Hardware : 8; // [ ] // hardware version
|
||||
unsigned int TxPower : 4; // [dBm] // RF trancmitter power (offset = 4)
|
||||
unsigned int ReportType: 4; // [0] // 0 for the status report
|
||||
unsigned int Voltage : 8; // [1/64V] VR // supply/battery voltage
|
||||
} Status;
|
||||
|
||||
union
|
||||
{ uint8_t Byte[16];
|
||||
struct
|
||||
{ uint8_t Data[14]; // [16x7bit]packed string of 16-char: 7bit/char
|
||||
uint8_t DataChars: 4; // [int] number of characters in the packed string
|
||||
uint8_t ReportType: 4; // [1] // 1 for the Info packets
|
||||
uint8_t Check; // CRC check
|
||||
} ;
|
||||
} Info;
|
||||
|
||||
struct
|
||||
{ signed int Latitude:24; // // Latitude of the measurement
|
||||
unsigned int Time: 6; // [sec] // time, just second thus ambiguity every every minute
|
||||
unsigned int : 2; // // spare
|
||||
signed int Longitude:24; // // Longitude of the measurement
|
||||
unsigned int : 6; // // spare
|
||||
unsigned int BaroMSB: 1; // // negated bit #8 of the altitude difference between baro and GPS
|
||||
unsigned int : 1; // // spare
|
||||
unsigned int Altitude:14; // [m] VR // Altitude of the measurement
|
||||
unsigned int Speed:10; // [0.1m/s] VR // Horizontal wind speed
|
||||
unsigned int : 8; // // spare
|
||||
unsigned int Heading:10; // // Wind direction
|
||||
unsigned int ClimbRate: 9; // [0.1m/s] VR // Vertical wind speed
|
||||
unsigned int : 1; // // spare
|
||||
unsigned int ReportType: 4; // // 2 for wind/thermal report
|
||||
unsigned int BaroAltDiff: 8; // [m] // lower 8 bits of the altitude difference between baro and GPS
|
||||
} Wind;
|
||||
|
||||
} ;
|
||||
|
||||
uint8_t *Byte(void) const { return (uint8_t *)&HeaderWord; } // packet as bytes
|
||||
uint32_t *Word(void) const { return (uint32_t *)&HeaderWord; } // packet as words
|
||||
|
||||
// void recvBytes(const uint8_t *SrcPacket) { memcpy(Byte(), SrcPacket, Bytes); } // load data bytes e.g. from a demodulator
|
||||
|
||||
static const uint8_t InfoParmNum = 14; // [int] number of info-parameters and their names
|
||||
static const char *InfoParmName(uint8_t Idx) { static const char *Name[InfoParmNum] =
|
||||
{ "Pilot", "Manuf", "Model", "Type", "SN", "Reg", "ID", "Class",
|
||||
"Task" , "Base" , "ICE" , "PilotID", "Hard", "Soft" } ;
|
||||
return Idx<InfoParmNum ? Name[Idx]:0; }
|
||||
|
||||
#ifndef __AVR__
|
||||
|
||||
void Dump(void) const
|
||||
{ printf("%08lX: %08lX %08lX %08lX %08lX\n",
|
||||
(long int)HeaderWord, (long int)Data[0], (long int)Data[1],
|
||||
(long int)Data[2], (long int)Data[3] ); }
|
||||
|
||||
uint8_t Read(const char *Inp)
|
||||
{ uint8_t Len=0;
|
||||
if(Inp[0]==' ') Inp++;
|
||||
int Chars = Read_Hex(HeaderWord, Inp); if(Chars!=8) return 0;
|
||||
Inp+=Chars; Len+=4;
|
||||
for( uint8_t Idx=0; Idx<4; Idx++)
|
||||
{ if(Inp[0]==' ') Inp++;
|
||||
int Chars = Read_Hex(Data[Idx], Inp); if(Chars!=8) return 0;
|
||||
Inp+=Chars; Len+=4; }
|
||||
return Len; }
|
||||
|
||||
uint8_t Dump(char *Out)
|
||||
{ uint8_t Len=0;
|
||||
Len+=Format_Hex(Out+Len, HeaderWord);
|
||||
for(int Idx=0; Idx<4; Idx++)
|
||||
{ Out[Len++]=' '; Len+=Format_Hex(Out+Len, Data[Idx]); }
|
||||
return Len; }
|
||||
|
||||
void DumpBytes(void) const
|
||||
{ for(uint8_t Idx=0; Idx<Bytes; Idx++)
|
||||
{ printf(" %02X", Byte()[Idx]); }
|
||||
printf("\n"); }
|
||||
|
||||
uint8_t WriteStatus(char *Out)
|
||||
{ uint8_t 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';
|
||||
Out[Len++]='/'; Out[Len++]='0'+Status.FixQuality;
|
||||
Out[Len++]='/'; Len+=Format_UnsDec(Out+Len, Status.SatSNR+8); Out[Len++]='d'; Out[Len++]='B';
|
||||
Out[Len++]=' '; Len+=Format_SignDec(Out+Len, DecodeAltitude(), 1, 0, 1); Out[Len++]='m';
|
||||
Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, (((uint32_t)Status.Pressure<<3)+5)/10, 2, 1); Out[Len++]='h'; Out[Len++]='P'; Out[Len++]='a';
|
||||
Out[Len++]=' '; Len+=Format_SignDec(Out+Len, DecodeTemperature(), 2, 1); Out[Len++]='d'; Out[Len++]='e'; Out[Len++]='g'; Out[Len++]='C';
|
||||
Out[Len++]=' '; Len+=Format_SignDec(Out+Len, DecodeHumidity(), 2, 1); Out[Len++]='%';
|
||||
Out[Len++]=' '; Len+=Format_SignDec(Out+Len, ((uint32_t)DecodeVoltage()*100+32)>>6, 3, 2); Out[Len++]='V';
|
||||
Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Status.TxPower+4);
|
||||
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<<Status.RxRate)-1); Out[Len++]='/'; Out[Len++]='m'; Out[Len++]='i'; Out[Len++]='n';
|
||||
return Len; }
|
||||
|
||||
uint8_t WriteDeviceStatus(char *Out)
|
||||
{ return sprintf(Out, " h%02X v%02X %dsat/%d/%ddB %ldm %3.1fhPa %+4.1fdegC %3.1f%% %4.2fV %d/%+4.1fdBm %d/min",
|
||||
Status.Hardware, Status.Firmware, Status.Satellites, Status.FixQuality, 8+Status.SatSNR, (long int)DecodeAltitude(),
|
||||
0.08*Status.Pressure, 0.1*DecodeTemperature(), 0.1*DecodeHumidity(),
|
||||
(1.0/64)*DecodeVoltage(), Status.TxPower+4, -0.5*Status.RadioNoise, (1<<Status.RxRate)-1 );
|
||||
}
|
||||
|
||||
uint8_t WriteDeviceInfo(char *Out)
|
||||
{ int Len=0;
|
||||
char Value[16];
|
||||
uint8_t InfoType;
|
||||
uint8_t Idx=0;
|
||||
for( ; ; )
|
||||
{ uint8_t Chars = readInfo(Value, InfoType, Idx);
|
||||
if(Chars==0) break;
|
||||
if(InfoType<InfoParmNum)
|
||||
{ Len += sprintf(Out+Len, " %s=%s", InfoParmName(InfoType), Value); }
|
||||
else
|
||||
{ Len += sprintf(Out+Len, " #%d=%s", InfoType, Value); }
|
||||
Idx+=Chars; }
|
||||
Out[Len]=0; return Len; }
|
||||
|
||||
void Print(void) const
|
||||
{ if(!Header.NonPos) { PrintPosition(); return; }
|
||||
if(Status.ReportType==0) { PrintDeviceStatus(); return; }
|
||||
if(Status.ReportType==1) { PrintDeviceInfo(); return; }
|
||||
}
|
||||
|
||||
void PrintDeviceInfo(void) const
|
||||
{ printf("%c:%06lX R%c%c%c:",
|
||||
'0'+Header.AddrType, (long int)Header.Address, '0'+Header.Relay, Header.Emergency?'E':' ', goodInfoCheck()?'+':'-');
|
||||
char Value[16];
|
||||
uint8_t InfoType;
|
||||
uint8_t Idx=0;
|
||||
for( ; ; )
|
||||
{ uint8_t Chars = readInfo(Value, InfoType, Idx);
|
||||
if(Chars==0) break;
|
||||
if(InfoType<InfoParmNum)
|
||||
{ printf(" %s=%s", InfoParmName(InfoType), Value); }
|
||||
else
|
||||
{ printf(" #%d=%s", InfoType, Value); }
|
||||
Idx+=Chars; }
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
void PrintDeviceStatus(void) const
|
||||
{ printf("%c:%06lX R%c%c %02ds:",
|
||||
'0'+Header.AddrType, (long int)Header.Address, '0'+Header.Relay, Header.Emergency?'E':' ', Status.Time);
|
||||
printf(" h%02X v%02X %dsat/%d/%ddB %ldm %3.1fhPa %+4.1fdegC %3.1f%% %4.2fV Tx:%ddBm Rx:%+4.1fdBm %d/min",
|
||||
Status.Hardware, Status.Firmware, Status.Satellites, Status.FixQuality, 8+Status.SatSNR, (long int)DecodeAltitude(),
|
||||
0.08*Status.Pressure, 0.1*DecodeTemperature(), 0.1*DecodeHumidity(),
|
||||
(1.0/64)*DecodeVoltage(), Status.TxPower+4, -0.5*Status.RadioNoise, (1<<Status.RxRate)-1 );
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
void PrintPosition(void) const
|
||||
{ printf("%c%X:%c:%06lX R%c%c",
|
||||
Position.Stealth ?'s':' ', (int)Position.AcftType, '0'+Header.AddrType, (long int)Header.Address, '0'+Header.Relay,
|
||||
Header.Emergency?'E':' ');
|
||||
printf(" %d/%dD/%4.1f", (int)Position.FixQuality, (int)Position.FixMode+2, 0.1*(10+DecodeDOP()) );
|
||||
if(Position.Time<60) printf(" %02ds:", (int)Position.Time);
|
||||
else printf(" ---:");
|
||||
printf(" [%+010.6f,%+011.6f]deg %ldm",
|
||||
0.0001/60*DecodeLatitude(), 0.0001/60*DecodeLongitude(), (long int)DecodeAltitude() );
|
||||
if(hasBaro())
|
||||
{ printf("[%+dm]", (int)getBaroAltDiff() ); }
|
||||
printf(" %3.1fm/s %05.1fdeg %+4.1fm/s %+4.1fdeg/s",
|
||||
0.1*DecodeSpeed(), 0.1*DecodeHeading(), 0.1*DecodeClimbRate(), 0.1*DecodeTurnRate() );
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
int WriteJSON(char *JSON) const
|
||||
{ int Len=0;
|
||||
Len+=Format_String(JSON+Len, "\"addr\":\"");
|
||||
Len+=Format_Hex(JSON+Len, (uint8_t) (Header.Address>>16));
|
||||
Len+=Format_Hex(JSON+Len, (uint16_t)(Header.Address));
|
||||
JSON[Len++]='\"';
|
||||
JSON[Len++]=',';
|
||||
Len+=Format_String(JSON+Len, "\"addr_type\":");
|
||||
JSON[Len++] = HexDigit(Header.AddrType);
|
||||
if(!Header.Encrypted && !Header.NonPos) // if non-encrypted position
|
||||
{ Len+=Format_String(JSON+Len, ",\"acft_type\":\"");
|
||||
JSON[Len++] = HexDigit(Position.AcftType);
|
||||
JSON[Len++]='\"';
|
||||
Len+=Format_String(JSON+Len, ",\"acft_cat\":\""); // GDL90 aircraft category
|
||||
// no-info, glider, tow, heli, parachute, drop-plane, hang-glider, para-glider, powered, jet, UFO, balloon, Zeppelin, UAV, ground vehicle, static } ;
|
||||
const uint8_t AcftCat[16] = { 0, 9, 1, 7, 11, 1, 12, 12, 1, 2, 0, 10, 10, 14, 18, 19 } ;
|
||||
Len+=Format_Hex(JSON+Len, AcftCat[Position.AcftType]);
|
||||
JSON[Len++]='\"';
|
||||
Len+=Format_String(JSON+Len, ",\"stealth\":");
|
||||
JSON[Len++] = '0'+Position.Stealth;
|
||||
Len+=Format_String(JSON+Len, ",\"lat_deg\":");
|
||||
Len+=Format_SignDec(JSON+Len, (int32_t)(((int64_t)50*DecodeLatitude()+1)/3), 8, 7, 1);
|
||||
Len+=Format_String(JSON+Len, ",\"lon_deg\":");
|
||||
Len+=Format_SignDec(JSON+Len, (int32_t)(((int64_t)50*DecodeLongitude()+1)/3), 8, 7, 1);
|
||||
int32_t Altitude=DecodeAltitude();
|
||||
Len+=Format_String(JSON+Len, ",\"alt_msl_m\":");
|
||||
Len+=Format_UnsDec(JSON+Len, (uint32_t)Altitude);
|
||||
if(hasBaro())
|
||||
{ Altitude+=getBaroAltDiff();
|
||||
Len+=Format_String(JSON+Len, ",\"alt_std_m\":");
|
||||
Len+=Format_SignDec(JSON+Len, Altitude, 1, 0, 1); }
|
||||
Len+=Format_String(JSON+Len, ",\"track_deg\":");
|
||||
Len+=Format_UnsDec(JSON+Len, DecodeHeading(), 2, 1);
|
||||
Len+=Format_String(JSON+Len, ",\"speed_mps\":");
|
||||
Len+=Format_UnsDec(JSON+Len, DecodeSpeed(), 2, 1);
|
||||
Len+=Format_String(JSON+Len, ",\"climb_mps\":");
|
||||
Len+=Format_SignDec(JSON+Len, DecodeClimbRate(), 2, 1, 1);
|
||||
Len+=Format_String(JSON+Len, ",\"turn_dps\":");
|
||||
Len+=Format_SignDec(JSON+Len, DecodeTurnRate(), 2, 1, 1);
|
||||
Len+=Format_String(JSON+Len, ",\"DOP\":");
|
||||
Len+=Format_UnsDec(JSON+Len, 10+DecodeDOP(), 2, 1); }
|
||||
if(!Header.Encrypted && Header.NonPos) // non-encrypted status and info
|
||||
{ if(Status.ReportType==0) // status
|
||||
{ }
|
||||
if(Status.ReportType==1) // info
|
||||
{ char Value[16];
|
||||
uint8_t InfoType;
|
||||
uint8_t Idx=0;
|
||||
for( ; ; )
|
||||
{ uint8_t Chars = readInfo(Value, InfoType, Idx);
|
||||
if(Chars==0) break;
|
||||
if(InfoType<InfoParmNum)
|
||||
{ Len+=sprintf(JSON+Len, ",\"%s\":\"%s\"", InfoParmName(InfoType), Value); }
|
||||
Idx+=Chars; }
|
||||
}
|
||||
}
|
||||
return Len; }
|
||||
/*
|
||||
void Encode(MAV_ADSB_VEHICLE *MAV)
|
||||
{ MAV->ICAO_address = HeaderWord&0x03FFFFFF;
|
||||
MAV->lat = ((int64_t)50*DecodeLatitude()+1)/3;
|
||||
MAV->lon = ((int64_t)50*DecodeLongitude()+1)/3;
|
||||
MAV->altitude = 1000*DecodeAltitude();
|
||||
MAV->heading = 10*DecodeHeading();
|
||||
MAV->hor_velocity = 10*DecodeSpeed();
|
||||
MAV->ver_velocity = 10*DecodeClimbRate();
|
||||
MAV->flags = 0x17;
|
||||
MAV->altitude_type = 1;
|
||||
MAV->callsign[0] = 0;
|
||||
MAV->tslc = 0;
|
||||
MAV->emiter_type = 0; }
|
||||
*/
|
||||
void Encode(MAV_ADSB_VEHICLE *MAV)
|
||||
{ MAV->ICAO_address = Header.Address;
|
||||
MAV->lat = ((int64_t)50*DecodeLatitude()+1)/3; // convert coordinates to [1e-7deg]
|
||||
MAV->lon = ((int64_t)50*DecodeLongitude()+1)/3;
|
||||
MAV->altitude = 1000*DecodeAltitude(); // convert to [mm[
|
||||
MAV->heading = 10*DecodeHeading(); // [cdeg/s]
|
||||
MAV->hor_velocity = 10*DecodeSpeed(); // [cm/s]
|
||||
MAV->ver_velocity = 10*DecodeClimbRate(); // [cm/s]
|
||||
MAV->flags = 0x1F; // all valid except for Squawk, not simulated
|
||||
MAV->altitude_type = 1; // GPS altitude
|
||||
const static char Prefix[4] = { 'R', 'I', 'F', 'O' }; // prefix for Random, ICAO, Flarm and OGN address-types
|
||||
MAV->callsign[0] = Prefix[Header.AddrType]; // create a call-sign from address-type and address
|
||||
Format_Hex((char *)MAV->callsign+1, ( uint8_t)(Header.Address>>16)); // highest byte
|
||||
Format_Hex((char *)MAV->callsign+3, (uint16_t)(Header.Address&0xFFFF)); // two lower bytes
|
||||
MAV->callsign[7] = 0; // end-of-string for call-sign
|
||||
MAV->squawk = 0; // what shall we put there for OGN ?
|
||||
MAV->tslc = 1; // 1sec for now but should be more precise
|
||||
const static uint8_t EmitterType[16] = // conversion table from OGN aircraft-type
|
||||
{ 0, 9, 2, 7, // unknown, glider, towplane, helicopter
|
||||
11, 3, 9, 11, // parachute, drop plane, hang-glider, para-glider
|
||||
2, 3, 15, 10, // powered aircraft, jet aircraft, UFO, balloon
|
||||
10, 14, 2, 19 }; // airship, UAV, ground vehiele, fixed object
|
||||
MAV->emiter_type = EmitterType[Position.AcftType]; // convert from the OGN
|
||||
}
|
||||
|
||||
static const char *getAprsIcon(uint8_t AcftType)
|
||||
{ static const char *AprsIcon[16] = // Icons for various FLARM acftType's
|
||||
{ "/z", // 0 = ?
|
||||
"/'", // 1 = (moto-)glider (most frequent)
|
||||
"/'", // 2 = tow plane (often)
|
||||
"/X", // 3 = helicopter (often)
|
||||
"/g" , // 4 = parachute (rare but seen - often mixed with drop plane)
|
||||
"\\^", // 5 = drop plane (seen)
|
||||
"/g" , // 6 = hang-glider (rare but seen)
|
||||
"/g" , // 7 = para-glider (rare but seen)
|
||||
"\\^", // 8 = powered aircraft (often)
|
||||
"/^", // 9 = jet aircraft (rare but seen)
|
||||
"/z", // A = UFO (people set for fun)
|
||||
"/O", // B = balloon (seen once)
|
||||
"/O", // C = airship (seen once)
|
||||
"/'", // D = UAV (drones, can become very common)
|
||||
"/z", // E = ground support (ground vehicles at airfields)
|
||||
"\\n" // F = static object (ground relay ?)
|
||||
} ;
|
||||
return AcftType<16 ? AprsIcon[AcftType]:0;
|
||||
}
|
||||
|
||||
int8_t ReadAPRS(const char *Msg) // read an APRS position message
|
||||
{ Clear();
|
||||
|
||||
const char *Data = strchr(Msg, ':'); if(Data==0) return -1; // where the time/position data starts
|
||||
Data++;
|
||||
const char *Dest = strchr(Msg, '>'); if(Dest==0) return -1; // where the destination call is
|
||||
Dest++;
|
||||
const char *Comma = strchr(Dest, ','); // the first comma after the destination call
|
||||
|
||||
Position.AcftType=0xF;
|
||||
|
||||
uint8_t AddrType;
|
||||
uint32_t Address;
|
||||
if(memcmp(Msg, "RND", 3)==0) AddrType=0;
|
||||
else if(memcmp(Msg, "ICA", 3)==0) AddrType=1;
|
||||
else if(memcmp(Msg, "FLR", 3)==0) AddrType=2;
|
||||
else if(memcmp(Msg, "OGN", 3)==0) AddrType=3;
|
||||
else AddrType=4;
|
||||
if(AddrType<4)
|
||||
{ if(Read_Hex(Address, Msg+3)==6) Header.Address=Address;
|
||||
Header.AddrType=AddrType; }
|
||||
|
||||
if(Comma)
|
||||
{ if(memcmp(Comma+1, "RELAY*" , 6)==0) Header.Relay=1;
|
||||
else if(Comma[10]=='*') Header.Relay=1;
|
||||
}
|
||||
|
||||
if(Data[0]!='/') return -1;
|
||||
int8_t Time;
|
||||
if(Data[7]=='h') // HHMMSS UTC time
|
||||
{ Time=Read_Dec2(Data+5); if(Time<0) return -1; }
|
||||
else if(Data[7]=='z') // DDHHMM UTC time
|
||||
{ Time=0; }
|
||||
else return -1;
|
||||
|
||||
Position.Time=Time;
|
||||
Data+=8;
|
||||
|
||||
Position.FixMode=1;
|
||||
Position.FixQuality=1;
|
||||
EncodeDOP(0xFF);
|
||||
|
||||
int8_t LatDeg = Read_Dec2(Data); if(LatDeg<0) return -1;
|
||||
int8_t LatMin = Read_Dec2(Data+2); if(LatMin<0) return -1;
|
||||
if(Data[4]!='.') return -1;
|
||||
int8_t LatFrac = Read_Dec2(Data+5); if(LatFrac<0) return -1;
|
||||
int32_t Latitude = (int32_t)LatDeg*600000 + (int32_t)LatMin*10000 + (int32_t)LatFrac*100;
|
||||
char LatSign = Data[7];
|
||||
Data+=8+1;
|
||||
|
||||
int16_t LonDeg = Read_Dec3(Data); if(LonDeg<0) return -1;
|
||||
int8_t LonMin = Read_Dec2(Data+3); if(LonMin<0) return -1;
|
||||
if(Data[5]!='.') return -1;
|
||||
int8_t LonFrac = Read_Dec2(Data+6); if(LonFrac<0) return -1;
|
||||
int32_t Longitude = (int32_t)LonDeg*600000 + (int32_t)LonMin*10000 + (int32_t)LonFrac*100;
|
||||
char LonSign = Data[8];
|
||||
Data+=9+1;
|
||||
|
||||
int16_t Speed=0;
|
||||
int16_t Heading=0;
|
||||
if(Data[3]=='/')
|
||||
{ Heading=Read_Dec3(Data);
|
||||
Speed=Read_Dec3(Data+4);
|
||||
Data+=7; }
|
||||
EncodeHeading(Heading*10);
|
||||
EncodeSpeed(((int32_t)Speed*337146+0x8000)>>16);
|
||||
|
||||
uint32_t Altitude=0;
|
||||
if( (Data[0]=='/') && (Data[1]=='A') && (Data[2]=='=') && (Read_UnsDec(Altitude, Data+3)==6) )
|
||||
{ Data+=9; }
|
||||
EncodeAltitude(FeetToMeters(Altitude));
|
||||
|
||||
for( ; ; )
|
||||
{ if(Data[0]!=' ') break;
|
||||
Data++;
|
||||
|
||||
if( (Data[0]=='!') && (Data[1]=='W') && (Data[4]=='!') )
|
||||
{ Latitude += (Data[2]-'0')*10;
|
||||
Longitude += (Data[3]-'0')*10;
|
||||
Data+=5; continue; }
|
||||
|
||||
if( (Data[0]=='i') && (Data[1]=='d') )
|
||||
{ uint32_t ID; Read_Hex(ID, Data+2);
|
||||
Header.Address = ID&0x00FFFFFF;
|
||||
Header.AddrType = (ID>>24)&0x03;
|
||||
Position.AcftType = (ID>>26)&0x0F;
|
||||
Position.Stealth = ID>>31;
|
||||
Data+=10; continue; }
|
||||
|
||||
if( (Data[0]=='F') && (Data[1]=='L') && (Data[5]=='.') )
|
||||
{ int16_t FLdec=Read_Dec3(Data+2);
|
||||
int16_t FLfrac=Read_Dec2(Data+6);
|
||||
if( (FLdec>=0) && (FLfrac>=0) )
|
||||
{ uint32_t StdAlt = FLdec*100+FLfrac;
|
||||
EncodeStdAltitude(FeetToMeters(StdAlt)); }
|
||||
Data+=8; continue; }
|
||||
|
||||
if( (Data[0]=='+') || (Data[0]=='-') )
|
||||
{ int32_t Value; int8_t Len=Read_Float1(Value, Data);
|
||||
if(Len>0)
|
||||
{ Data+=Len;
|
||||
if(memcmp(Data, "fpm", 3)==0) { EncodeClimbRate((333*Value+0x8000)>>16); Data+=3; continue; }
|
||||
if(memcmp(Data, "rot", 3)==0) { EncodeTurnRate(3*Value); Data+=3; continue; }
|
||||
}
|
||||
}
|
||||
|
||||
if( (Data[0]=='g') && (Data[1]=='p') && (Data[2]=='s') )
|
||||
{ int16_t HorPrec=Read_Dec2(Data+3);
|
||||
if(HorPrec<0) HorPrec=Read_Dec1(Data[3]);
|
||||
if(HorPrec>=0)
|
||||
{ uint16_t DOP=HorPrec*5; if(DOP<10) DOP=10; else if(DOP>230) DOP=230;
|
||||
EncodeDOP(DOP-10); Data+=5; }
|
||||
}
|
||||
while(Data[0]>' ') Data++;
|
||||
}
|
||||
|
||||
if(LatSign=='S') Latitude=(-Latitude); else if(LatSign!='N') return -1;
|
||||
EncodeLatitude(Latitude);
|
||||
if(LonSign=='W') Longitude=(-Longitude); else if(LonSign!='E') return -1;
|
||||
EncodeLongitude(Longitude);
|
||||
|
||||
return 0; }
|
||||
|
||||
uint8_t WriteAPRS(char *Msg, uint32_t Time, const char *ProtName="APRS") // write an APRS position message
|
||||
{ uint8_t Len=0;
|
||||
static const char *AddrTypeName[4] = { "RND", "ICA", "FLR", "OGN" } ;
|
||||
memcpy(Msg+Len, AddrTypeName[Header.AddrType], 3); Len+=3;
|
||||
Len+=Format_Hex(Msg+Len, (uint8_t)(Header.Address>>16));
|
||||
Len+=Format_Hex(Msg+Len, (uint16_t)(Header.Address));
|
||||
Msg[Len++] = '>';
|
||||
uint8_t ProtLen = strlen(ProtName);
|
||||
memcpy(Msg+Len, ProtName, ProtLen); Len+=ProtLen;
|
||||
if(Header.Relay)
|
||||
{ memcpy(Msg+Len, ",RELAY*", 7); Len+=7; }
|
||||
Msg[Len++] = ':';
|
||||
|
||||
if(Header.NonPos && Status.ReportType!=0) return 0; // give up if neither position nor status
|
||||
|
||||
if(Position.Time<60)
|
||||
{ uint32_t DayTime=Time%86400; int Sec=DayTime%60; // second of the time the packet was recevied
|
||||
int DiffSec=Position.Time-Sec; if(DiffSec>4) DiffSec-=60; // difference should always be zero or negative, but can be small positive for predicted positions
|
||||
Time+=DiffSec; } // get out the correct position time
|
||||
Msg[Len++] = Header.NonPos?'>':'/';
|
||||
Len+=Format_HHMMSS(Msg+Len, Time);
|
||||
Msg[Len++] = 'h';
|
||||
|
||||
if(Header.NonPos) { Len+=WriteStatus(Msg+Len); Msg[Len++]='\n'; Msg[Len]=0; return Len; }
|
||||
|
||||
const char *Icon = getAprsIcon(Position.AcftType);
|
||||
|
||||
int32_t Lat = DecodeLatitude();
|
||||
bool NegLat = Lat<0; if(NegLat) Lat=(-Lat);
|
||||
uint32_t LatDeg = Lat/600000;
|
||||
Len+=Format_UnsDec(Msg+Len, LatDeg, 2);
|
||||
Lat -= LatDeg*600000;
|
||||
uint32_t LatMin = Lat/100;
|
||||
Len+=Format_UnsDec(Msg+Len, LatMin, 4, 2);
|
||||
Lat -= LatMin*100;
|
||||
Msg[Len++] = NegLat ? 'S':'N';
|
||||
Msg[Len++] = Icon[0];
|
||||
|
||||
int32_t Lon = DecodeLongitude();
|
||||
bool NegLon = Lon<0; if(NegLon) Lon=(-Lon);
|
||||
uint32_t LonDeg = Lon/600000;
|
||||
Len+=Format_UnsDec(Msg+Len, LonDeg, 3);
|
||||
Lon -= LonDeg*600000;
|
||||
uint32_t LonMin = Lon/100;
|
||||
Len+=Format_UnsDec(Msg+Len, LonMin, 4, 2);
|
||||
Lon -= LonMin*100;
|
||||
Msg[Len++] = NegLon ? 'W':'E';
|
||||
Msg[Len++] = Icon[1];
|
||||
|
||||
Len+=Format_UnsDec(Msg+Len, (DecodeHeading()+5)/10, 3);
|
||||
Msg[Len++] = '/';
|
||||
Len+=Format_UnsDec(Msg+Len, ((uint32_t)DecodeSpeed()*199+512)>>10, 3);
|
||||
Msg[Len++] = '/'; Msg[Len++] = 'A'; Msg[Len++] = '='; Len+=Format_UnsDec(Msg+Len, MetersToFeet(DecodeAltitude()), 6);
|
||||
|
||||
Msg[Len++] = ' ';
|
||||
Msg[Len++] = '!';
|
||||
Msg[Len++] = 'W';
|
||||
Msg[Len++] = '0'+Lat/10;
|
||||
Msg[Len++] = '0'+Lon/10;
|
||||
Msg[Len++] = '!';
|
||||
|
||||
Msg[Len++] = ' '; Msg[Len++] = 'i'; Msg[Len++] = 'd'; Len+=Format_Hex(Msg+Len, ((uint32_t)Position.AcftType<<26) | ((uint32_t)Header.AddrType<<24) | Header.Address);
|
||||
|
||||
Msg[Len++] = ' '; Len+=Format_SignDec(Msg+Len, ((int32_t)DecodeClimbRate()*10079+256)>>9, 3); Msg[Len++] = 'f'; Msg[Len++] = 'p'; Msg[Len++] = 'm';
|
||||
Msg[Len++] = ' '; Len+=Format_SignDec(Msg+Len, DecodeTurnRate()/3, 2, 1); Msg[Len++] = 'r'; Msg[Len++] = 'o'; Msg[Len++] = 't';
|
||||
|
||||
if(hasBaro())
|
||||
{ int32_t Alt = DecodeStdAltitude();
|
||||
if(Alt<0) Alt=0;
|
||||
Msg[Len++] = ' '; Msg[Len++] = 'F'; Msg[Len++] = 'L';
|
||||
Len+=Format_UnsDec(Msg+Len, MetersToFeet((uint32_t)Alt), 5, 2); }
|
||||
|
||||
uint16_t DOP=10+DecodeDOP();
|
||||
uint16_t HorPrec=(DOP*2+5)/10; if(HorPrec>63) HorPrec=63;
|
||||
uint16_t VerPrec=(DOP*3+5)/10; if(VerPrec>63) VerPrec=63;
|
||||
Msg[Len++] = ' '; Msg[Len++] = 'g'; Msg[Len++] = 'p'; Msg[Len++] = 's';
|
||||
Len+=Format_UnsDec(Msg+Len, HorPrec); Msg[Len++] = 'x'; Len+=Format_UnsDec(Msg+Len, VerPrec);
|
||||
|
||||
Msg[Len++]='\n';
|
||||
Msg[Len]=0;
|
||||
return Len; }
|
||||
|
||||
#endif // __AVR__
|
||||
|
||||
// calculate distance vector [LatDist, LonDist] from a given reference [RefLat, Reflon]
|
||||
int calcDistanceVector(int32_t &LatDist, int32_t &LonDist, int32_t RefLat, int32_t RefLon, uint16_t LatCos=3000, int32_t MaxDist=0x7FFF)
|
||||
{ LatDist = ((DecodeLatitude()-RefLat)*1517+0x1000)>>13; // convert from 1/600000deg to meters (40000000m = 360deg) => x 5/27 = 1517/(1<<13)
|
||||
if(abs(LatDist)>MaxDist) return -1;
|
||||
LonDist = ((DecodeLongitude()-RefLon)*1517+0x1000)>>13;
|
||||
if(abs(LonDist)>(4*MaxDist)) return -1;
|
||||
LonDist = (LonDist*LatCos+0x800)>>12;
|
||||
if(abs(LonDist)>MaxDist) return -1;
|
||||
return 1; }
|
||||
|
||||
// sets position [Lat, Lon] according to given distance vector [LatDist, LonDist] from a reference point [RefLat, RefLon]
|
||||
void setDistanceVector(int32_t LatDist, int32_t LonDist, int32_t RefLat, int32_t RefLon, uint16_t LatCos=3000)
|
||||
{ EncodeLatitude(RefLat+(LatDist*27)/5);
|
||||
LonDist = (LonDist<<12)/LatCos; // LonDist/=cosine(Latitude)
|
||||
EncodeLongitude(RefLon+(LonDist*27)/5); }
|
||||
|
||||
// Centripetal acceleration
|
||||
static int16_t calcCPaccel(int16_t Speed, int16_t TurnRate) { return ((int32_t)TurnRate*Speed*229+0x10000)>>17; } // [0.1m/s^2]
|
||||
int16_t calcCPaccel(void) { return calcCPaccel(DecodeSpeed(), DecodeTurnRate()); }
|
||||
|
||||
// Turn radius
|
||||
static int16_t calcTurnRadius(int16_t Speed, int16_t TurnRate, int16_t MaxRadius=0x7FFF) // [m] ([0.1m/s], [], [m])
|
||||
{ if(TurnRate==0) return 0;
|
||||
int32_t Radius = 14675*Speed;
|
||||
Radius /= TurnRate; Radius = (Radius+128)>>8;
|
||||
if(abs(Radius)>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++]=':';
|
||||
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++]=' ';
|
||||
// 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);
|
||||
Out[Len++]=' ';
|
||||
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++]='\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; }
|
||||
|
||||
uint32_t getAddressAndType(void) const { return HeaderWord&0x03FFFFFF; } // Address with address-type: 26-bit
|
||||
void setAddressAndType(uint32_t AddrAndType) { HeaderWord = (HeaderWord&0xFC000000) | (AddrAndType&0x03FFFFFF); }
|
||||
|
||||
bool goodAddrParity(void) const { return ((Count1s(HeaderWord&0x0FFFFFFF)&1)==0); } // Address parity should be EVEN
|
||||
void calcAddrParity(void) { if(!goodAddrParity()) HeaderWord ^= 0x08000000; } // if not correct parity, flip the parity bit
|
||||
|
||||
void EncodeLatitude(int32_t Latitude) // encode Latitude: units are 0.0001/60 degrees
|
||||
{ Position.Latitude = Latitude>>3; }
|
||||
|
||||
int32_t DecodeLatitude(void) const
|
||||
{ int32_t Latitude = Position.Latitude;
|
||||
// if(Latitude&0x00800000) Latitude|=0xFF000000;
|
||||
Latitude = (Latitude<<3)+4; return Latitude; }
|
||||
|
||||
void EncodeLongitude(int32_t Longitude) // encode Longitude: units are 0.0001/60 degrees
|
||||
{ Position.Longitude = Longitude>>=4; }
|
||||
|
||||
int32_t DecodeLongitude(void) const
|
||||
{ int32_t Longitude = Position.Longitude;
|
||||
Longitude = (Longitude<<4)+8; return Longitude; }
|
||||
|
||||
bool hasBaro(void) const { return Position.BaroMSB || Position.BaroAltDiff; }
|
||||
void clrBaro(void) { Position.BaroMSB=0; Position.BaroAltDiff=0; }
|
||||
int16_t getBaroAltDiff(void) const { int16_t AltDiff=Position.BaroAltDiff; if(Position.BaroMSB==0) AltDiff|=0xFF00; return AltDiff; }
|
||||
void setBaroAltDiff(int32_t AltDiff)
|
||||
{ if(AltDiff<(-255)) AltDiff=(-255); else if(AltDiff>255) AltDiff=255;
|
||||
Position.BaroMSB = (AltDiff&0xFF00)==0; Position.BaroAltDiff=AltDiff&0xFF; }
|
||||
void EncodeStdAltitude(int32_t StdAlt) { setBaroAltDiff((StdAlt-DecodeAltitude())); }
|
||||
int32_t DecodeStdAltitude(void) const { return (DecodeAltitude()+getBaroAltDiff()); }
|
||||
|
||||
void EncodeAltitude(int32_t Altitude) // encode altitude in meters
|
||||
{ if(Altitude<0) Altitude=0;
|
||||
Position.Altitude = UnsVRencode<uint16_t, 12>((uint16_t)Altitude); }
|
||||
// Position.Altitude = EncodeUR2V12((uint16_t)Altitude); }
|
||||
|
||||
int32_t DecodeAltitude(void) const // return Altitude in meters
|
||||
{ return UnsVRdecode<uint16_t, 12>(Position.Altitude); }
|
||||
// { return DecodeUR2V12(Position.Altitude); }
|
||||
|
||||
void EncodeDOP(uint8_t DOP)
|
||||
{ Position.DOP = UnsVRencode<uint8_t, 4>(DOP); }
|
||||
// { Position.DOP = EncodeUR2V4(DOP); }
|
||||
|
||||
uint8_t DecodeDOP(void) const
|
||||
{ return UnsVRdecode<uint8_t, 4>(Position.DOP); }
|
||||
// { return DecodeUR2V4(Position.DOP); }
|
||||
|
||||
void EncodeSpeed(int16_t Speed) // speed in 0.2 knots (or 0.1m/s)
|
||||
{ if(Speed<0) Speed=0;
|
||||
else Speed = UnsVRencode<uint16_t, 8>(Speed); // EncodeUR2V8(Speed);
|
||||
Position.Speed = Speed; }
|
||||
|
||||
int16_t DecodeSpeed(void) const // return speed in 0.2 knots or 0.1m/s units
|
||||
{ return UnsVRdecode<uint16_t, 8>(Position.Speed); }
|
||||
// { return DecodeUR2V8(Position.Speed); } // => max. speed: 3832*0.2 = 766 knots
|
||||
|
||||
int16_t DecodeHeading(void) const // return Heading in 0.1 degree units 0..359.9 deg
|
||||
{ int32_t Heading = Position.Heading;
|
||||
return (Heading*3600+512)>>10; }
|
||||
|
||||
void EncodeHeading(int16_t Heading)
|
||||
{ Position.Heading = (((int32_t)Heading<<10)+180)/3600; }
|
||||
|
||||
void setHeadingAngle(uint16_t HeadingAngle)
|
||||
{ Position.Heading = (((HeadingAngle+32)>>6)); }
|
||||
|
||||
uint16_t getHeadingAngle(void) const
|
||||
{ return (uint16_t)Position.Heading<<6; }
|
||||
|
||||
void clrTurnRate(void) { Position.TurnRate=0x80; }
|
||||
bool hasTurnRate(void) const { return Position.TurnRate==0x80; }
|
||||
|
||||
void EncodeTurnRate(int16_t Turn) // [0.1 deg/sec]
|
||||
{ Position.TurnRate = EncodeSR2V5(Turn); }
|
||||
|
||||
int16_t DecodeTurnRate(void) const
|
||||
{ return DecodeSR2V5(Position.TurnRate); }
|
||||
|
||||
void clrClimbRate(void) { Position.ClimbRate=0x100; }
|
||||
bool hasClimbRate(void) const { return Position.ClimbRate==0x100; }
|
||||
|
||||
void EncodeClimbRate(int16_t Climb)
|
||||
{ Position.ClimbRate = EncodeSR2V6(Climb); }
|
||||
|
||||
int16_t DecodeClimbRate(void) const
|
||||
{ return DecodeSR2V6(Position.ClimbRate); }
|
||||
|
||||
// --------------------------------------------------------------------------------------------------------------
|
||||
// Status fields
|
||||
|
||||
void clrTemperature(void) { Status.Temperature=0x80; }
|
||||
bool hasTemperature(void) const { return Status.Temperature!=0x80; }
|
||||
void EncodeTemperature(int16_t Temp) { Status.Temperature=EncodeSR2V5(Temp-200); } // [0.1degC]
|
||||
int16_t DecodeTemperature(void) const { return 200+DecodeSR2V5(Status.Temperature); }
|
||||
|
||||
void EncodeVoltage(uint16_t Voltage) { Status.Voltage=EncodeUR2V6(Voltage); } // [1/64V]
|
||||
uint16_t DecodeVoltage(void) const { return DecodeUR2V6(Status.Voltage); }
|
||||
|
||||
void clrHumidity(void) { Status.Humidity=0x80; }
|
||||
bool hasHumidity(void) const { return Status.Humidity!=0x80; }
|
||||
void EncodeHumidity(uint16_t Hum) { Status.Humidity=EncodeSR2V5((int16_t)(Hum-520)); } // [0.1%]
|
||||
uint16_t DecodeHumidity(void) const { return 520+DecodeSR2V5(Status.Humidity); }
|
||||
|
||||
// --------------------------------------------------------------------------------------------------------------
|
||||
// Info fields: pack and unpack 7-bit char into the Info packets
|
||||
|
||||
void setInfoChar(uint8_t Char, uint8_t Idx) // put 7-bit Char onto give position
|
||||
{ if(Idx>=16) return; // Idx = 0..15
|
||||
Char&=0x7F;
|
||||
uint8_t BitIdx = Idx*7; // [bits] bit index to the target field
|
||||
Idx = BitIdx>>3; // [bytes] index of the first byte to change
|
||||
uint8_t Ofs = BitIdx&0x07;
|
||||
if(Ofs==0) { Info.Data[Idx] = (Info.Data[Idx]&0x80) | Char ; return; }
|
||||
if(Ofs==1) { Info.Data[Idx] = (Info.Data[Idx]&0x01) | (Char<<1) ; return; }
|
||||
uint8_t Len1 = 8-Ofs;
|
||||
uint8_t Len2 = Ofs-1;
|
||||
uint8_t Msk1 = 0xFF; Msk1<<=Ofs;
|
||||
uint8_t Msk2 = 0x01; Msk2 = (Msk2<<Len2)-1;
|
||||
Info.Data[Idx ] = (Info.Data[Idx ]&(~Msk1)) | (Char<<Ofs);
|
||||
Info.Data[Idx+1] = (Info.Data[Idx+1]&(~Msk2)) | (Char>>Len1); }
|
||||
|
||||
uint8_t getInfoChar(uint8_t Idx) const // get 7-bit Char from given position
|
||||
{ if(Idx>=16) return 0; // Idx = 0..15
|
||||
uint8_t BitIdx = Idx*7; // [bits] bit index to the target field
|
||||
Idx = BitIdx>>3; // [bytes] index of the first byte to change
|
||||
uint8_t Ofs = BitIdx&0x07;
|
||||
if(Ofs==0) return Info.Data[Idx]&0x7F;
|
||||
if(Ofs==1) return Info.Data[Idx]>>1;
|
||||
uint8_t Len = 8-Ofs;
|
||||
return (Info.Data[Idx]>>Ofs) | ((Info.Data[Idx+1]<<Len)&0x7F); }
|
||||
|
||||
void clrInfo(void) // clear the info packet
|
||||
{ Info.DataChars=0; // clear number of characters
|
||||
Info.ReportType=1; } // just in case: set the report-type
|
||||
|
||||
uint8_t addInfo(const char *Value, uint8_t InfoType) // add an info field
|
||||
{ uint8_t Idx=Info.DataChars; // number of characters already in the info packet
|
||||
if(Idx) Idx++; // if at least one already, then skip over the terminator
|
||||
if(Idx>=15) return 0;
|
||||
uint8_t Len=0;
|
||||
for( ; ; )
|
||||
{ uint8_t Char = Value[Len]; if(Char==0) break;
|
||||
if(Idx>=15) return 0;
|
||||
setInfoChar(Char, Idx++);
|
||||
Len++; }
|
||||
setInfoChar(InfoType, Idx); // terminating character
|
||||
Info.DataChars=Idx; // update number of characters
|
||||
return Len+1; } // return number of added Value characters
|
||||
|
||||
uint8_t readInfo(char *Value, uint8_t &InfoType, uint8_t ValueIdx=0) const
|
||||
{ uint8_t Len=0; // count characters in the info-string
|
||||
uint8_t Chars = Info.DataChars; // total number of characters in the record
|
||||
char Char=0;
|
||||
for( ; ; ) // loop over characters
|
||||
{ if((ValueIdx+Len)>Chars) return 0; // return failure if overrun the data
|
||||
Char = getInfoChar(ValueIdx+Len); // get the character
|
||||
if(Char<0x20) break; // if less than 0x20 (space) then this is the terminator
|
||||
Value[Len++]=Char; }
|
||||
Value[Len]=0; // null-terminate the infor string
|
||||
InfoType=Char; // get the info-type: Pilot, Type, etc.
|
||||
return Len+1; } // return number of character taken thus info length + terminator
|
||||
|
||||
uint8_t InfoCheck(void) const
|
||||
{ uint8_t Check=0;
|
||||
for( uint8_t Idx=0; Idx<15; Idx++)
|
||||
{ Check ^= Info.Byte[Idx]; }
|
||||
// printf("Check = %02X\n", Check);
|
||||
return Check; }
|
||||
|
||||
void setInfoCheck(void)
|
||||
{ Info.Check = InfoCheck();
|
||||
// printf("Check = %02X\n", Info.Check);
|
||||
}
|
||||
|
||||
uint8_t goodInfoCheck(void) const
|
||||
{ return Info.Check == InfoCheck(); }
|
||||
|
||||
// --------------------------------------------------------------------------------------------------------------
|
||||
|
||||
void Encrypt (const uint32_t Key[4]) { XXTEA_Encrypt(Data, 4, Key, 8); } // encrypt with given Key
|
||||
void Decrypt (const uint32_t Key[4]) { XXTEA_Decrypt(Data, 4, Key, 8); } // decrypt with given Key
|
||||
|
||||
void Whiten (void) { TEA_Encrypt_Key0(Data, 8); TEA_Encrypt_Key0(Data+2, 8); } // whiten the position
|
||||
void Dewhiten(void) { TEA_Decrypt_Key0(Data, 8); TEA_Decrypt_Key0(Data+2, 8); } // de-whiten the position
|
||||
|
||||
uint8_t getTxSlot(uint8_t Idx) const // Idx=0..15
|
||||
{ const uint32_t *DataPtr = Data;
|
||||
uint32_t Mask=1; Mask<<=Idx;
|
||||
uint8_t Slot=0;
|
||||
for(uint8_t Bit=0; Bit<6; Bit++)
|
||||
{ Slot>>=1;
|
||||
if(DataPtr[Bit]&Mask) Slot|=0x20;
|
||||
Mask<<=1; Slot>>=1; }
|
||||
return EncodeGray(Slot); }
|
||||
|
||||
} ;
|
||||
|
||||
#endif // of __OGN1_H__
|
||||
|
||||
|
|
@ -0,0 +1,434 @@
|
|||
#ifndef __OGN2_H__
|
||||
#define __OGN2_H__
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
#include <string.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "intmath.h"
|
||||
|
||||
#include "ognconv.h"
|
||||
|
||||
#include "bitcount.h"
|
||||
#include "nmea.h"
|
||||
#include "mavlink.h"
|
||||
|
||||
#include "format.h"
|
||||
|
||||
// uint32_t OGN2_SYNC = 0xF56D3738;
|
||||
|
||||
// the packet description here is how it look on the little-endian CPU before sending it to the RF chip
|
||||
// nRF905, CC1101, SPIRIT1, RFM69 chips actually reverse the bit order within every byte
|
||||
// thus on the air the bits appear MSbit first for every byte transmitted
|
||||
|
||||
class OGN2_Packet // Packet structure for the OGN tracker
|
||||
{ public:
|
||||
|
||||
static const int Words = 5;
|
||||
static const int Bytes = 20;
|
||||
|
||||
union
|
||||
{ uint32_t HeaderWord; // ECNR POTT AAAA AAAA AAAA AAAA AAAA AAAA
|
||||
// E=Emergency, C=enCrypt/Custom, R=Relay, P=Parity, O=Other, TT=address Type, AA..=Address:24-bit
|
||||
// When enCrypt/Custom is set the data (position or whatever) can only be decoded by the owner
|
||||
// This option is indented to pass any type of custom data not foreseen otheriwse
|
||||
|
||||
struct
|
||||
{ unsigned int Address :24; // aircraft address
|
||||
unsigned int AddrType : 2; // address type: 0 = random, 1 = ICAO, 2 = FLARM, 3 = OGN
|
||||
unsigned int Relay : 1; // 0 = direct packet, 1 = relayed packet
|
||||
unsigned int Parity : 1; // parity takes into account bits 0..27 thus only the 28 lowest bits
|
||||
unsigned int NonPos : 1; // 0 = position packet, 1 = other information like status
|
||||
// unsigned int Auth : 2; // Authentication: 00 = no auth. 01 = auth. this packet, 10/11 = auth response
|
||||
// Auth:NonPos: 000 = position, 010 = position, to be followed by crypto-response,
|
||||
// 101 = response #0, 111 = response #1
|
||||
// 001 = non-position: status, info, etc.
|
||||
// 100 = ???, 110 = ???, 011 = ???
|
||||
unsigned int NonOGN : 1; // 0 = OGN packet, 1 = other systems, like MAVlink
|
||||
unsigned int Encrypted : 1; // packet is encrypted
|
||||
unsigned int Emergency : 1; // aircraft in emergency (not used for now)
|
||||
} Header ;
|
||||
|
||||
} ;
|
||||
|
||||
union
|
||||
{ uint32_t Data[4];
|
||||
|
||||
struct
|
||||
{ unsigned int AcftType: 4; // [0..15] // type of aircraft: 1 = glider, 2 = towplane, 3 = helicopter, ...
|
||||
unsigned int Heading:10; // [0.35deg] // Ground track in cordic units
|
||||
unsigned int ClimbRate: 9; // [0.1m/s] VR
|
||||
unsigned int BaroAltDiff: 9; // [m] // lower 8 bits of the altitude difference between baro and GPS
|
||||
|
||||
unsigned int Altitude:14; // [m] VR // Altitude
|
||||
unsigned int Speed:10; // [0.1m/s] VR // Ground speed
|
||||
unsigned int TurnRate: 8; // [0.1deg/s] VR // Ground heading (track) rate
|
||||
|
||||
unsigned int Latitude:24; // [1.2m] // Latitude in cordic units
|
||||
unsigned int Time: 6; // [sec] // time, just second thus ambiguity every every minute
|
||||
unsigned int FixQuality: 2; // // 0 = no fix, 1 = GPS, 2 = diff. GPS, 3 = other
|
||||
|
||||
unsigned int Longitude:25; // [1.2m] // Longitude in cordic units
|
||||
unsigned int DOP: 6; // // GPS Dilution of Precision
|
||||
unsigned int FixMode: 1; // [2-D/3-D]
|
||||
} Position;
|
||||
|
||||
|
||||
struct
|
||||
{
|
||||
unsigned int ReportType: 4; // [0] // 0 for the status report
|
||||
unsigned int TxPower : 4; // [dBm] // RF trancmitter power
|
||||
unsigned int Firmware : 8; // [ ] // firmware version
|
||||
unsigned int Hardware : 8; // [ ] // hardware version
|
||||
unsigned int Voltage : 8; // [1/64V] VR // supply voltager
|
||||
|
||||
unsigned int Altitude :14; // [m] VR // same as in the position packet
|
||||
unsigned int Pressure :14; // [0.08hPa] // barometric pressure
|
||||
unsigned int Satellites: 4; // [ ]
|
||||
|
||||
unsigned int Pulse : 8; // [bpm] // pilot: heart pulse rate
|
||||
unsigned int Oxygen : 7; // [%] // pilot: oxygen level in the blood
|
||||
// unsigned int FEScurr : 5; // [A] //
|
||||
unsigned int SatSNR : 5; // [dB] // average SNR of GPS signals
|
||||
unsigned int RxRate : 4; // [/min] // log2 of received packet rate
|
||||
unsigned int Time : 6; // [sec] // same as in the position packet
|
||||
unsigned int FixQuality: 2;
|
||||
|
||||
unsigned int AudioNoise: 8; // [dB] //
|
||||
unsigned int RadioNoise: 8; // [dBm] // noise seen by the RF chip
|
||||
unsigned int Temperature:8; // [0.1degC] VR // temperature by the baro or RF chip
|
||||
unsigned int Humidity : 8; // [%] // humidity
|
||||
} Status ;
|
||||
|
||||
union
|
||||
{ uint8_t Byte[16];
|
||||
struct
|
||||
{ uint8_t ReportType: 4; // [1] // 1 for the Info packets
|
||||
uint8_t DataChars: 4; // [int] number of characters in the packed string
|
||||
uint8_t Data[14]; // [16x7bit]packed string of 16-char: 7bit/char
|
||||
uint8_t Check; // CRC check
|
||||
} ;
|
||||
} Info;
|
||||
|
||||
} ;
|
||||
|
||||
uint8_t *Byte(void) const { return (uint8_t *)&HeaderWord; } // packet as bytes
|
||||
uint32_t *Word(void) const { return (uint32_t *)&HeaderWord; } // packet as words
|
||||
|
||||
static const uint8_t InfoParmNum = 14; // [int] number of info-parameters and their names
|
||||
static const char *InfoParmName(uint8_t Idx) { static const char *Name[InfoParmNum] =
|
||||
{ "Pilot", "Manuf", "Model", "Type", "SN", "Reg", "ID", "Class",
|
||||
"Task" , "Base" , "ICE" , "PilotID", "Hard", "Soft" } ;
|
||||
return Idx<InfoParmNum ? Name[Idx]:0; }
|
||||
|
||||
void Dump(void) const
|
||||
{ printf("%08lX: %08lX %08lX %08lX %08lX\n",
|
||||
(long int)HeaderWord, (long int)Data[0], (long int)Data[1],
|
||||
(long int)Data[2], (long int)Data[3] ); }
|
||||
|
||||
void DumpBytes(void) const
|
||||
{ for(uint8_t Idx=0; Idx<Bytes; Idx++)
|
||||
{ printf(" %02X", Byte()[Idx]); }
|
||||
printf("\n"); }
|
||||
|
||||
int WriteDeviceStatus(char *Out)
|
||||
{ return sprintf(Out, " h%02X v%02X %dsat/%d %ldm %3.1fhPa %+4.1fdegC %3.1f%% %4.2fV %d/%+4.1fdBm %d/min",
|
||||
Status.Hardware, Status.Firmware, Status.Satellites, Status.FixQuality, (long int)DecodeAltitude(),
|
||||
0.08*Status.Pressure, 0.1*DecodeTemperature(), 0.1*DecodeHumidity(),
|
||||
(1.0/64)*DecodeVoltage(), Status.TxPower+4, -0.5*Status.RadioNoise, (1<<Status.RxRate)-1 );
|
||||
}
|
||||
|
||||
int WriteDeviceInfo(char *Out)
|
||||
{ int Len=0;
|
||||
char Value[16];
|
||||
uint8_t InfoType;
|
||||
uint8_t Idx=0;
|
||||
for( ; ; )
|
||||
{ uint8_t Chars = readInfo(Value, InfoType, Idx);
|
||||
if(Chars==0) break;
|
||||
if(InfoType<InfoParmNum)
|
||||
{ Len += sprintf(Out+Len, " %s=%s", InfoParmName(InfoType), Value); }
|
||||
else
|
||||
{ Len += sprintf(Out+Len, " #%d=%s", InfoType, Value); }
|
||||
Idx+=Chars; }
|
||||
Out[Len]=0; return Len; }
|
||||
|
||||
void Print(void) const
|
||||
{ if(!Header.NonPos) { PrintPosition(); return; }
|
||||
if(Status.ReportType==0) { PrintDeviceStatus(); return; }
|
||||
}
|
||||
|
||||
void PrintDeviceStatus(void) const
|
||||
{ printf("%c:%06lX R%c%c %02ds:",
|
||||
'0'+Header.AddrType, (long int)Header.Address, '0'+Header.Relay, Header.Emergency?'E':' ', Status.Time);
|
||||
printf(" h%02X v%02X %dsat/%d %ldm %3.1fhPa %+4.1fdegC %3.1f%% %4.2fV Tx:%ddBm Rx:%+4.1fdBm %d/min",
|
||||
Status.Hardware, Status.Firmware, Status.Satellites, Status.FixQuality, (long int)DecodeAltitude(),
|
||||
0.08*Status.Pressure, 0.1*DecodeTemperature(), 0.1*DecodeHumidity(),
|
||||
(1.0/64)*DecodeVoltage(), Status.TxPower+4, -0.5*Status.RadioNoise, (1<<Status.RxRate)-1 );
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
void PrintPosition(void) const
|
||||
{ printf(" %X:%c:%06lX R%c%c",
|
||||
(int)Position.AcftType, '0'+Header.AddrType, (long int)Header.Address, '0'+Header.Relay,
|
||||
Header.Emergency?'E':' ');
|
||||
printf(" %d/%dD/%4.1f", (int)Position.FixQuality, (int)Position.FixMode+2, 0.1*(10+DecodeDOP()) );
|
||||
if(Position.Time<60) printf(" %02ds:", (int)Position.Time);
|
||||
else printf(" ---:");
|
||||
printf(" [%+10.6f, %+11.6f]deg %ldm",
|
||||
0.0001/60*DecodeLatitude(), 0.0001/60*DecodeLongitude(), (long int)DecodeAltitude() );
|
||||
if(hasBaro())
|
||||
{ printf("[%+dm]", (int)getBaroAltDiff() ); }
|
||||
printf(" %3.1fm/s %05.1fdeg %+4.1fm/s %+4.1fdeg/s",
|
||||
0.1*DecodeSpeed(), 0.1*DecodeHeading(), 0.1*DecodeClimbRate(), 0.1*DecodeTurnRate() );
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
// calculate distance vector [LatDist, LonDist] from a given reference [RefLat, Reflon]
|
||||
int calcDistanceVector(int32_t &LatDist, int32_t &LonDist, int32_t RefLat, int32_t RefLon, uint16_t LatCos=3000, int32_t MaxDist=0x7FFF)
|
||||
{ LatDist = ((DecodeLatitude()-RefLat)*1517+0x1000)>>13; // convert from 1/600000deg to meters (40000000m = 360deg) => x 5/27 = 151$
|
||||
if(abs(LatDist)>MaxDist) return -1;
|
||||
LonDist = ((DecodeLongitude()-RefLon)*1517+0x1000)>>13;
|
||||
if(abs(LonDist)>(4*MaxDist)) return -1;
|
||||
LonDist = (LonDist*LatCos+0x800)>>12;
|
||||
if(abs(LonDist)>MaxDist) return -1;
|
||||
return 1; }
|
||||
|
||||
// sets position [Lat, Lon] according to given distance vector [LatDist, LonDist] from a reference point [RefLat, RefLon]
|
||||
void setDistanceVector(int32_t LatDist, int32_t LonDist, int32_t RefLat, int32_t RefLon, uint16_t LatCos=3000)
|
||||
{ EncodeLatitude(RefLat+(LatDist*27)/5);
|
||||
LonDist = (LonDist<<12)/LatCos; // LonDist/=cosine(Latitude)
|
||||
EncodeLongitude(RefLon+(LonDist*27)/5); }
|
||||
|
||||
// Centripetal acceleration
|
||||
static int16_t calcCPaccel(int16_t Speed, int16_t TurnRate) { return ((int32_t)TurnRate*Speed*229+0x10000)>>17; } // [0.1m/s^2]
|
||||
int16_t calcCPaccel(void) { return calcCPaccel(DecodeSpeed(), DecodeTurnRate()); }
|
||||
|
||||
// Turn radius
|
||||
static int16_t calcTurnRadius(int16_t Speed, int16_t TurnRate, int16_t MaxRadius=0x7FFF) // [m] ([0.1m/s], [], [m])
|
||||
{ if(TurnRate==0) return 0;
|
||||
int32_t Radius = 14675*Speed;
|
||||
Radius /= TurnRate; Radius = (Radius+128)>>8;
|
||||
if(abs(Radius)>MaxRadius) return 0;
|
||||
return Radius; }
|
||||
int16_t calcTurnRadius(int16_t MaxRadius=0x7FFF) { return calcTurnRadius(DecodeSpeed(), DecodeTurnRate(), MaxRadius); }
|
||||
|
||||
void Clear(void) { HeaderWord=0; Data[0]=0; Data[1]=0; Data[2]=0; Data[3]=0; }
|
||||
|
||||
uint32_t getAddressAndType(void) const { return HeaderWord&0x03FFFFFF; } // Address with address-type: 26-bit
|
||||
void setAddressAndType(uint32_t AddrAndType) { HeaderWord = (HeaderWord&0xFC000000) | (AddrAndType&0x03FFFFFF); }
|
||||
|
||||
bool goodAddrParity(void) const { return ((Count1s(HeaderWord&0x0FFFFFFF)&1)==0); } // Address parity should be EVEN
|
||||
void calcAddrParity(void) { if(!goodAddrParity()) HeaderWord ^= 0x08000000; } // if not correct parity, flip the parity bit
|
||||
|
||||
void clrBaro(void) { Position.BaroAltDiff=EncodeGray((uint16_t)0x100); }
|
||||
bool hasBaro(void) const { return DecodeGray((uint16_t)Position.BaroAltDiff)!=0x100; }
|
||||
|
||||
void setBaroAltDiff(int32_t AltDiff)
|
||||
{ if(AltDiff<(-255)) AltDiff=(-255);
|
||||
else if(AltDiff>255) AltDiff=255;
|
||||
Position.BaroAltDiff=EncodeGray((uint16_t)(AltDiff&0x1FF)); }
|
||||
int16_t getBaroAltDiff(void) const
|
||||
{ int16_t AltDiff=DecodeGray((uint16_t)Position.BaroAltDiff);
|
||||
if(AltDiff & 0x100) AltDiff |= 0xFE00;
|
||||
return AltDiff; }
|
||||
|
||||
void EncodeStdAltitude(int32_t StdAlt) { setBaroAltDiff((StdAlt-DecodeAltitude())); }
|
||||
int32_t DecodeStdAltitude(void) const { return (DecodeAltitude()+getBaroAltDiff()); }
|
||||
|
||||
void EncodeAltitude(int32_t Altitude) // encode altitude in meters
|
||||
{ Altitude += 1024;
|
||||
if(Altitude<0) Altitude=0;
|
||||
Position.Altitude = EncodeGray((uint16_t)UnsVRencode<uint16_t, 12>((uint16_t)Altitude)); }
|
||||
|
||||
int32_t DecodeAltitude(void) const // return Altitude in meters
|
||||
{ return (int32_t)UnsVRdecode<uint16_t, 12>(DecodeGray((uint16_t)Position.Altitude))-1024; }
|
||||
|
||||
static const int32_t FullAngle = 360*600000; // Latitude and Longitude are encoded in units of 1/10000 arcmin
|
||||
static const int32_t HalfAngle = FullAngle/2;
|
||||
|
||||
// void EncodeLatitude(int32_t Latitude)
|
||||
// { Position.Latitude = ((int64_t)Latitude*83399993+(1<<28))>>29; }
|
||||
|
||||
// int32_t DecodeLatitude(void) const
|
||||
// { int32_t Latitude = Position.Latitude;
|
||||
// return ((int64_t)Latitude*HalfAngle+(1<<23))>>24; }
|
||||
|
||||
void EncodeLatitude(int32_t Latitude)
|
||||
{ Latitude = ((int64_t)Latitude*83399993+(1<<28))>>29; // convert to cordic units
|
||||
Latitude &= 0x00FFFFFF;
|
||||
Position.Latitude = EncodeGray((uint32_t)Latitude); }
|
||||
|
||||
int32_t DecodeLatitude(void) const
|
||||
{ int32_t Latitude = DecodeGray((uint32_t)Position.Latitude);
|
||||
if(Latitude&0x00800000) Latitude |= 0xFF000000;
|
||||
return ((int64_t)Latitude*HalfAngle+(1<<23))>>24; } // convert from cordic units to 1/10000 arcmin
|
||||
|
||||
void EncodeLongitude(int32_t Longitude)
|
||||
{ Longitude = ((int64_t)Longitude*83399993+(1<<28))>>29;
|
||||
Longitude &= 0x01FFFFFF;
|
||||
Position.Longitude = EncodeGray((uint32_t)Longitude); }
|
||||
|
||||
int32_t DecodeLongitude(void) const
|
||||
{ int32_t Longitude = DecodeGray((uint32_t)Position.Longitude);
|
||||
if(Longitude&0x01000000) Longitude |= 0xFE000000;
|
||||
Longitude = ((int64_t)Longitude*HalfAngle+(1<<23))>>24;
|
||||
return Longitude; }
|
||||
|
||||
void EncodeDOP(uint8_t DOP)
|
||||
{ Position.DOP = EncodeGray(UnsVRencode<uint8_t, 4>(DOP)); }
|
||||
|
||||
uint8_t DecodeDOP(void) const
|
||||
{ return UnsVRdecode<uint8_t, 4>(DecodeGray((uint8_t)Position.DOP)); }
|
||||
|
||||
void EncodeSpeed(int16_t Speed) // speed in 0.2 knots (or 0.1m/s)
|
||||
{ if(Speed<0) Speed=0;
|
||||
else Speed=UnsVRencode<uint16_t, 8>(Speed);
|
||||
Position.Speed = EncodeGray((uint16_t)Speed); }
|
||||
|
||||
int16_t DecodeSpeed(void) const // return speed in 0.2 knots or 0.1m/s units
|
||||
{ return UnsVRdecode<uint16_t, 8>(DecodeGray((uint16_t)Position.Speed)); } // => max. speed: 3832*0.2 = 766 knots
|
||||
|
||||
void EncodeHeading(int16_t Heading)
|
||||
{ if(Heading<0) Heading+=3600;
|
||||
Heading = ((int32_t)Heading*1165+(1<<11))>>12;
|
||||
Position.Heading = EncodeGray((uint16_t)Heading); }
|
||||
|
||||
int16_t DecodeHeading(void) const // return Heading in 0.1 degree units 0..359.9 deg
|
||||
{ int32_t Heading = DecodeGray((uint16_t)Position.Heading);
|
||||
return (Heading*3600+512)>>10; }
|
||||
|
||||
void setHeadingAngle(uint16_t HeadingAngle)
|
||||
{ Position.Heading = EncodeGray((uint16_t)((HeadingAngle+32)>>6)); }
|
||||
|
||||
uint16_t getHeadingAngle(void) const
|
||||
{ return (uint16_t)DecodeGray((uint16_t)Position.Heading)<<6; }
|
||||
|
||||
void clrTurnRate(void) { Position.TurnRate=0x80; }
|
||||
bool hasTurnRate(void) const { return Position.TurnRate==0x80; }
|
||||
|
||||
void EncodeTurnRate(int16_t Turn) // [0.1 deg/sec]
|
||||
{ Position.TurnRate = SignVRencode<int16_t, 5>(Turn); }
|
||||
|
||||
int16_t DecodeTurnRate(void) const
|
||||
{ return SignVRdecode<int16_t, 5>(Position.TurnRate); }
|
||||
|
||||
void clrClimbRate(void) { Position.ClimbRate=0x100; }
|
||||
bool hasClimbRate(void) const { return Position.ClimbRate==0x100; }
|
||||
|
||||
void EncodeClimbRate(int16_t Climb) // [0.1m/s]
|
||||
{ Position.ClimbRate = SignVRencode<int16_t, 6>(Climb); }
|
||||
|
||||
int16_t DecodeClimbRate(void) const
|
||||
{ return SignVRdecode<int16_t, 6>(Position.ClimbRate); }
|
||||
|
||||
// --------------------------------------------------------------------------------------------------------------
|
||||
// Status fields
|
||||
|
||||
void clrTemperature(void) { Status.Temperature=EncodeGray((uint8_t)0x80); }
|
||||
bool hasTemperature(void) const { return DecodeGray((uint8_t)Status.Temperature)!=0x80; }
|
||||
void EncodeTemperature(int16_t Temp) { Status.Temperature=EncodeGray(EncodeSR2V5(Temp-200)); } // [0.1degC]
|
||||
int16_t DecodeTemperature(void) const { return 200+DecodeSR2V5(DecodeGray((uint8_t)Status.Temperature)); }
|
||||
|
||||
void clrHumidity(void) { Status.Humidity=EncodeGray((uint8_t)0x80); }
|
||||
bool hasHumidity(void) const { return DecodeGray((uint8_t)Status.Humidity)!=0x80; }
|
||||
void EncodeHumidity(uint16_t Hum) { Status.Humidity=EncodeGray(EncodeSR2V5((int16_t)(Hum-525))); } // [0.1%]
|
||||
uint16_t DecodeHumidity(void) const { return 525+DecodeSR2V5(DecodeGray((uint8_t)Status.Humidity)); }
|
||||
|
||||
void EncodeVoltage_mV(uint16_t Voltage) { EncodeVoltage((Voltage*8+63)/125); }
|
||||
void EncodeVoltage(uint16_t Voltage) // [1/64V]
|
||||
{ if(Voltage<80) Voltage = 0;
|
||||
else Voltage-=80;
|
||||
Status.Voltage=EncodeGray(EncodeUR2V6(Voltage)); }
|
||||
uint16_t DecodeVoltage(void) const { return 80+DecodeUR2V6(DecodeGray((uint8_t)Status.Voltage)); }
|
||||
|
||||
// --------------------------------------------------------------------------------------------------------------
|
||||
// Info fields: pack and unpack 7-bit char into the Info packets
|
||||
|
||||
void setInfoChar(uint8_t Char, uint8_t Idx) // put 7-bit Char onto give position
|
||||
{ if(Idx>=16) return; // Idx = 0..15
|
||||
Char&=0x7F;
|
||||
uint8_t BitIdx = Idx*7; // [bits] bit index to the target field
|
||||
Idx = BitIdx>>3; // [bytes] index of the first byte to change
|
||||
uint8_t Ofs = BitIdx&0x07;
|
||||
if(Ofs==0) { Info.Data[Idx] = (Info.Data[Idx]&0x80) | Char ; return; }
|
||||
if(Ofs==1) { Info.Data[Idx] = (Info.Data[Idx]&0x01) | (Char<<1) ; return; }
|
||||
uint8_t Len1 = 8-Ofs;
|
||||
uint8_t Len2 = Ofs-1;
|
||||
uint8_t Msk1 = 0xFF; Msk1<<=Ofs;
|
||||
uint8_t Msk2 = 0x01; Msk2 = (Msk2<<Len2)-1;
|
||||
Info.Data[Idx ] = (Info.Data[Idx ]&(~Msk1)) | (Char<<Ofs);
|
||||
Info.Data[Idx+1] = (Info.Data[Idx+1]&(~Msk2)) | (Char>>Len1); }
|
||||
|
||||
uint8_t getInfoChar(uint8_t Idx) const // get 7-bit Char from given position
|
||||
{ if(Idx>=16) return 0; // Idx = 0..15
|
||||
uint8_t BitIdx = Idx*7; // [bits] bit index to the target field
|
||||
Idx = BitIdx>>3; // [bytes] index of the first byte to change
|
||||
uint8_t Ofs = BitIdx&0x07;
|
||||
if(Ofs==0) return Info.Data[Idx]&0x7F;
|
||||
if(Ofs==1) return Info.Data[Idx]>>1;
|
||||
uint8_t Len = 8-Ofs;
|
||||
return (Info.Data[Idx]>>Ofs) | ((Info.Data[Idx+1]<<Len)&0x7F); }
|
||||
|
||||
void clrInfo(void) // clear the info packet
|
||||
{ Info.DataChars=0; // clear number of characters
|
||||
Info.ReportType=1; } // just in case: set the report-type
|
||||
|
||||
uint8_t addInfo(const char *Value, uint8_t InfoType) // add an info field
|
||||
{ uint8_t Idx=Info.DataChars; // number of characters already in the info packet
|
||||
if(Idx) Idx++; // if at least one already, then skip over the terminator
|
||||
if(Idx>=15) return 0;
|
||||
uint8_t Len=0;
|
||||
for( ; ; )
|
||||
{ uint8_t Char = Value[Len]; if(Char==0) break;
|
||||
if(Idx>=15) return 0;
|
||||
setInfoChar(Char, Idx++);
|
||||
Len++; }
|
||||
setInfoChar(InfoType, Idx); // terminating character
|
||||
Info.DataChars=Idx; // update number of characters
|
||||
return Len+1; } // return number of added Value characters
|
||||
|
||||
uint8_t readInfo(char *Value, uint8_t &InfoType, uint8_t ValueIdx=0) const
|
||||
{ uint8_t Len=0; // count characters in the info-string
|
||||
uint8_t Chars = Info.DataChars; // total number of characters in the record
|
||||
char Char=0;
|
||||
for( ; ; ) // loop over characters
|
||||
{ if((ValueIdx+Len)>Chars) return 0; // return failure if overrun the data
|
||||
Char = getInfoChar(ValueIdx+Len); // get the character
|
||||
if(Char<0x20) break; // if less than 0x20 (space) then this is the terminator
|
||||
Value[Len++]=Char; }
|
||||
Value[Len]=0; // null-terminate the infor string
|
||||
InfoType=Char; // get the info-type: Pilot, Type, etc.
|
||||
return Len+1; } // return number of character taken thus info length + terminator
|
||||
|
||||
uint8_t InfoCheck(void) const
|
||||
{ uint8_t Check=0;
|
||||
for( uint8_t Idx=0; Idx<15; Idx++)
|
||||
{ Check ^= Info.Byte[Idx]; }
|
||||
// printf("Check = %02X\n", Check);
|
||||
return Check; }
|
||||
|
||||
void setInfoCheck(void)
|
||||
{ Info.Check = InfoCheck();
|
||||
// printf("Check = %02X\n", Info.Check);
|
||||
}
|
||||
|
||||
uint8_t goodInfoCheck(void) const
|
||||
{ return Info.Check == InfoCheck(); }
|
||||
|
||||
// --------------------------------------------------------------------------------------------------------------
|
||||
|
||||
void Whiten (uint8_t Prefix=0x05)
|
||||
{ uint32_t Mask = ((uint32_t)Prefix<<26) | (HeaderWord & 0x03FFFFFF);
|
||||
for( uint8_t Idx=0; Idx<4; Idx++)
|
||||
{ XorShift32(Mask); Mask += 0x01234567;
|
||||
Data[Idx] ^= Mask; }
|
||||
}
|
||||
|
||||
void Dewhiten(void) { Whiten(); }
|
||||
|
||||
} ;
|
||||
|
||||
#endif // __OGN2_H__
|
||||
|
||||
|
|
@ -0,0 +1,271 @@
|
|||
#include <stdint.h>
|
||||
|
||||
#include "ognconv.h"
|
||||
|
||||
// ==============================================================================================
|
||||
|
||||
uint32_t FeetToMeters(uint32_t Altitude) { return (Altitude*312+512)>>10; } // [feet] => [m]
|
||||
uint32_t MetersToFeet(uint32_t Altitude) { return (Altitude*3360+512)>>10; } // [m] => [feet]
|
||||
|
||||
// ==============================================================================================
|
||||
|
||||
uint16_t EncodeUR2V8(uint16_t Value) // Encode unsigned 12bit (0..3832) as 10bit
|
||||
{ if(Value<0x100) { }
|
||||
else if(Value<0x300) Value = 0x100 | ((Value-0x100)>>1);
|
||||
else if(Value<0x700) Value = 0x200 | ((Value-0x300)>>2);
|
||||
else if(Value<0xF00) Value = 0x300 | ((Value-0x700)>>3);
|
||||
else Value = 0x3FF;
|
||||
return Value; }
|
||||
|
||||
uint16_t DecodeUR2V8(uint16_t Value) // Decode 10bit 0..0x3FF
|
||||
{ uint16_t Range = Value>>8;
|
||||
Value &= 0x0FF;
|
||||
if(Range==0) return Value; // 000..0FF
|
||||
if(Range==1) return 0x101+(Value<<1); // 100..2FE
|
||||
if(Range==2) return 0x302+(Value<<2); // 300..6FC
|
||||
return 0x704+(Value<<3); } // 700..EF8 // in 12bit (0..3832)
|
||||
|
||||
|
||||
uint8_t EncodeUR2V5(uint16_t Value) // Encode unsigned 9bit (0..472) as 7bit
|
||||
{ if(Value<0x020) { }
|
||||
else if(Value<0x060) Value = 0x020 | ((Value-0x020)>>1);
|
||||
else if(Value<0x0E0) Value = 0x040 | ((Value-0x060)>>2);
|
||||
else if(Value<0x1E0) Value = 0x060 | ((Value-0x0E0)>>3);
|
||||
else Value = 0x07F;
|
||||
return Value; }
|
||||
|
||||
uint16_t DecodeUR2V5(uint16_t Value) // Decode 7bit as unsigned 9bit (0..472)
|
||||
{ uint8_t Range = (Value>>5)&0x03;
|
||||
Value &= 0x1F;
|
||||
if(Range==0) { } // 000..01F
|
||||
else if(Range==1) { Value = 0x021+(Value<<1); } // 020..05E
|
||||
else if(Range==2) { Value = 0x062+(Value<<2); } // 060..0DC
|
||||
else { Value = 0x0E4+(Value<<3); } // 0E0..1D8 => max. Value = 472
|
||||
return Value; }
|
||||
|
||||
uint8_t EncodeSR2V5(int16_t Value) // Encode signed 10bit (-472..+472) as 8bit
|
||||
{ uint8_t Sign=0; if(Value<0) { Value=(-Value); Sign=0x80; }
|
||||
Value = EncodeUR2V5(Value);
|
||||
return Value | Sign; }
|
||||
|
||||
int16_t DecodeSR2V5( int16_t Value) // Decode
|
||||
{ int16_t Sign = Value&0x80;
|
||||
Value = DecodeUR2V5(Value&0x7F);
|
||||
return Sign ? -Value: Value; }
|
||||
|
||||
uint16_t EncodeUR2V6(uint16_t Value) // Encode unsigned 10bit (0..952) as 8 bit
|
||||
{ if(Value<0x040) { }
|
||||
else if(Value<0x0C0) Value = 0x040 | ((Value-0x040)>>1);
|
||||
else if(Value<0x1C0) Value = 0x080 | ((Value-0x0C0)>>2);
|
||||
else if(Value<0x3C0) Value = 0x0C0 | ((Value-0x1C0)>>3);
|
||||
else Value = 0x0FF;
|
||||
return Value; }
|
||||
|
||||
uint16_t DecodeUR2V6(uint16_t Value) // Decode 8bit as unsigned 10bit (0..952)
|
||||
{ uint16_t Range = (Value>>6)&0x03;
|
||||
Value &= 0x3F;
|
||||
if(Range==0) { } // 000..03F
|
||||
else if(Range==1) { Value = 0x041+(Value<<1); } // 040..0BE
|
||||
else if(Range==2) { Value = 0x0C2+(Value<<2); } // 0C0..1BC
|
||||
else { Value = 0x1C4+(Value<<3); } // 1C0..3B8 => max. Value = 952
|
||||
return Value; }
|
||||
|
||||
uint16_t EncodeSR2V6(int16_t Value) // Encode signed 11bit (-952..+952) as 9bit
|
||||
{ uint16_t Sign=0; if(Value<0) { Value=(-Value); Sign=0x100; }
|
||||
Value = EncodeUR2V6(Value);
|
||||
return Value | Sign; }
|
||||
|
||||
int16_t DecodeSR2V6( int16_t Value) // Decode 9bit as signed 11bit (-952..+952)
|
||||
{ int16_t Sign = Value&0x100;
|
||||
Value = DecodeUR2V6(Value&0x00FF);
|
||||
return Sign ? -Value: Value; }
|
||||
|
||||
uint8_t EncodeUR2V4(uint8_t DOP)
|
||||
{ if(DOP<0x10) { }
|
||||
else if(DOP<0x30) DOP = 0x10 | ((DOP-0x10)>>1);
|
||||
else if(DOP<0x70) DOP = 0x20 | ((DOP-0x30)>>2);
|
||||
else if(DOP<0xF0) DOP = 0x30 | ((DOP-0x70)>>3);
|
||||
else DOP = 0x3F;
|
||||
return DOP; }
|
||||
|
||||
uint8_t DecodeUR2V4(uint8_t DOP)
|
||||
{ uint8_t Range = DOP>>4;
|
||||
DOP &= 0x0F;
|
||||
if(Range==0) return DOP; // 00..0F
|
||||
if(Range==1) return 0x11+(DOP<<1); // 10..2E
|
||||
if(Range==2) return 0x32+(DOP<<2); // 30..6C
|
||||
return 0x74+(DOP<<3); } // 70..E8 => max. DOP = 232*0.1=23.2
|
||||
|
||||
uint16_t EncodeUR2V12(uint16_t Value) // encode unsigned 16-bit (0..61432) as 14-bit
|
||||
{ if(Value<0x1000) { }
|
||||
else if(Value<0x3000) Value = 0x1000 | ((Value-0x1000)>>1);
|
||||
else if(Value<0x7000) Value = 0x2000 | ((Value-0x3000)>>2);
|
||||
else if(Value<0xF000) Value = 0x3000 | ((Value-0x7000)>>3);
|
||||
else Value = 0x3FFF;
|
||||
return Value; }
|
||||
|
||||
uint16_t DecodeUR2V12(uint16_t Value)
|
||||
{ uint16_t Range = Value>>12;
|
||||
Value &=0x0FFF;
|
||||
if(Range==0) return Value; // 0000..0FFF
|
||||
if(Range==1) return 0x1001+(Value<<1); // 1000..2FFE
|
||||
if(Range==2) return 0x3002+(Value<<2); // 3000..6FFC
|
||||
return 0x7004+(Value<<3); } // 7000..EFF8 => max: 61432
|
||||
|
||||
// ==============================================================================================
|
||||
|
||||
uint8_t EncodeGray(uint8_t Binary)
|
||||
{ return Binary ^ (Binary>>1); }
|
||||
|
||||
uint8_t DecodeGray(uint8_t Gray)
|
||||
{ Gray ^= (Gray >> 4);
|
||||
Gray ^= (Gray >> 2);
|
||||
Gray ^= (Gray >> 1);
|
||||
return Gray; }
|
||||
|
||||
uint16_t EncodeGray(uint16_t Binary)
|
||||
{ return Binary ^ (Binary>>1); }
|
||||
|
||||
uint16_t DecodeGray(uint16_t Gray)
|
||||
{ Gray ^= (Gray >> 8);
|
||||
Gray ^= (Gray >> 4);
|
||||
Gray ^= (Gray >> 2);
|
||||
Gray ^= (Gray >> 1);
|
||||
return Gray; }
|
||||
|
||||
uint32_t EncodeGray(uint32_t Binary)
|
||||
{ return Binary ^ (Binary>>1); }
|
||||
|
||||
uint32_t DecodeGray(uint32_t Gray)
|
||||
{ Gray ^= (Gray >>16);
|
||||
Gray ^= (Gray >> 8);
|
||||
Gray ^= (Gray >> 4);
|
||||
Gray ^= (Gray >> 2);
|
||||
Gray ^= (Gray >> 1);
|
||||
return Gray; }
|
||||
|
||||
// ==============================================================================================
|
||||
// TEA encryption/decryption
|
||||
// Data is 2 x 32-bit word
|
||||
// Key is 4 x 32-bit word
|
||||
|
||||
void TEA_Encrypt (uint32_t* Data, const uint32_t *Key, int Loops)
|
||||
{ uint32_t v0=Data[0], v1=Data[1]; // set up
|
||||
const uint32_t delta=0x9e3779b9; uint32_t sum=0; // a key schedule constant
|
||||
uint32_t k0=Key[0], k1=Key[1], k2=Key[2], k3=Key[3]; // cache key
|
||||
for (int i=0; i < Loops; i++) // basic cycle start
|
||||
{ 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;
|
||||
}
|
||||
|
||||
void TEA_Decrypt (uint32_t* Data, const uint32_t *Key, int Loops)
|
||||
{ uint32_t v0=Data[0], v1=Data[1]; // set up
|
||||
const uint32_t delta=0x9e3779b9; uint32_t sum=delta*Loops; // a key schedule constant
|
||||
uint32_t k0=Key[0], k1=Key[1], k2=Key[2], k3=Key[3]; // cache key
|
||||
for (int i=0; i < Loops; i++) // basic cycle start */
|
||||
{ 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;
|
||||
}
|
||||
|
||||
void TEA_Encrypt_Key0 (uint32_t* Data, int Loops)
|
||||
{ uint32_t v0=Data[0], v1=Data[1]; // set up
|
||||
const uint32_t delta=0x9e3779b9; uint32_t sum=0; // a key schedule constant
|
||||
for (int i=0; i < Loops; i++) // basic cycle start
|
||||
{ sum += delta;
|
||||
v0 += (v1<<4) ^ (v1 + sum) ^ (v1>>5);
|
||||
v1 += (v0<<4) ^ (v0 + sum) ^ (v0>>5); } // end cycle
|
||||
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
|
||||
const uint32_t delta=0x9e3779b9; uint32_t sum=delta*Loops; // a key schedule constant
|
||||
for (int i=0; i < Loops; i++) // basic cycle start
|
||||
{ v1 -= (v0<<4) ^ (v0 + sum) ^ (v0>>5);
|
||||
v0 -= (v1<<4) ^ (v1 + sum) ^ (v1>>5);
|
||||
sum -= delta; } // end cycle
|
||||
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))); }
|
||||
|
||||
void XXTEA_Encrypt(uint32_t *Data, uint8_t Words, const uint32_t Key[4], uint8_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++)
|
||||
{ Y = Data[P+1];
|
||||
Z = Data[P] += XXTEA_MX(E, Y, Z, P, Sum, Key); }
|
||||
Y = Data[0];
|
||||
Z = Data[Words-1] += XXTEA_MX(E, Y, Z, Words-1, Sum, Key);
|
||||
}
|
||||
}
|
||||
|
||||
void XXTEA_Decrypt(uint32_t *Data, uint8_t Words, const uint32_t Key[4], uint8_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--)
|
||||
{ Z = Data[P-1];
|
||||
Y = Data[P] -= XXTEA_MX(E, Y, Z, P, Sum, Key); }
|
||||
Z = Data[Words-1];
|
||||
Y = Data[0] -= XXTEA_MX(E, Y, Z, 0, Sum, Key);
|
||||
Sum -= Delta;
|
||||
}
|
||||
}
|
||||
|
||||
// ==============================================================================================
|
||||
|
||||
void XorShift32(uint32_t &Seed) // simple random number generator
|
||||
{ Seed ^= Seed << 13;
|
||||
Seed ^= Seed >> 17;
|
||||
Seed ^= Seed << 5; }
|
||||
|
||||
void xorshift64(uint64_t &Seed)
|
||||
{ Seed ^= Seed >> 12;
|
||||
Seed ^= Seed << 25;
|
||||
Seed ^= Seed >> 27; }
|
||||
|
||||
// ==============================================================================================
|
||||
|
||||
const static unsigned char MapAscii85[86] = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz!#$%&()*+-;<=>?@^_`{|}~";
|
||||
|
||||
const static uint8_t UnmapAscii85[128] =
|
||||
{ 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85,
|
||||
85, 62, 85, 63, 64, 65, 66, 85, 67, 68, 69, 70, 85, 71, 85, 85, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 85, 72, 73, 74, 75, 76,
|
||||
77, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 85, 85, 85, 78, 79,
|
||||
80, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 81, 82, 83, 84, 85 };
|
||||
|
||||
uint8_t EncodeAscii85(char *Ascii, uint32_t Word)
|
||||
{ for( uint8_t Idx=5; Idx; )
|
||||
{ uint32_t Div = Word/85;
|
||||
Idx--;
|
||||
Ascii[Idx]=MapAscii85[Word-Div*85];
|
||||
Word=Div; }
|
||||
Ascii[5]=0;
|
||||
return 5; }
|
||||
|
||||
uint8_t DecodeAscii85(uint32_t &Word, const char *Ascii)
|
||||
{ Word=0;
|
||||
for( uint8_t Idx=0; Idx<5; Idx++)
|
||||
{ char Char = Ascii[Idx]; if(Char<=0) return 0;
|
||||
uint8_t Dig = UnmapAscii85[(uint8_t)Char];
|
||||
if(Dig>=85) return 0;
|
||||
Word = Word*85+Dig; }
|
||||
return 5; }
|
||||
|
||||
// ==============================================================================================
|
||||
|
||||
|
|
@ -0,0 +1,85 @@
|
|||
#ifndef __OGNCONV_H__
|
||||
#define __OGNCONV_H__
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
uint32_t FeetToMeters(uint32_t Altitude); //
|
||||
uint32_t MetersToFeet(uint32_t Altitude); //
|
||||
|
||||
uint16_t EncodeUR2V8(uint16_t Value); // Encode unsigned 12bit (0..3832) as 10bit
|
||||
uint16_t DecodeUR2V8(uint16_t Value); // Decode 10bit 0..0x3FF
|
||||
|
||||
uint8_t EncodeUR2V5(uint16_t Value); // Encode unsigned 9bit (0..472) as 7bit
|
||||
uint16_t DecodeUR2V5(uint16_t Value); // Decode 7bit as unsigned 9bit (0..472)
|
||||
|
||||
uint8_t EncodeSR2V5(int16_t Value); // Encode signed 10bit (-472..+472) as 8bit
|
||||
int16_t DecodeSR2V5( int16_t Value); // Decode
|
||||
|
||||
uint16_t EncodeUR2V6(uint16_t Value); // Encode unsigned 10bit (0..952) as 8 bit
|
||||
uint16_t DecodeUR2V6(uint16_t Value); // Decode 8bit as unsigned 10bit (0..952)
|
||||
|
||||
uint16_t EncodeSR2V6(int16_t Value); // Encode signed 11bit (-952..+952) as 9bit
|
||||
int16_t DecodeSR2V6( int16_t Value); // Decode 9bit as signed 11bit (-952..+952)
|
||||
|
||||
// uint16_t EncodeUR2V12(uint16_t Value); // encode unsigned 16-bit (0..61432) as 14-bit
|
||||
// uint16_t DecodeUR2V12(uint16_t Value);
|
||||
|
||||
// uint8_t EncodeUR2V4(uint8_t DOP);
|
||||
// uint8_t DecodeUR2V4(uint8_t DOP);
|
||||
|
||||
template <class Type, int Bits>
|
||||
Type UnsVRdecode(Type Value)
|
||||
{ const Type Thres = 1<<Bits;
|
||||
uint8_t Range = Value>>Bits;
|
||||
Value &= Thres-1;
|
||||
if(Range==0) return Value;
|
||||
if(Range==1) return Thres+1+(Value<<1);
|
||||
if(Range==2) return 3*Thres+2+(Value<<2);
|
||||
return 7*Thres+4+(Value<<3); }
|
||||
|
||||
template <class Type, int Bits>
|
||||
Type UnsVRencode(Type Value)
|
||||
{ const Type Thres = 1<<Bits;
|
||||
if(Value< Thres) return Value;
|
||||
if(Value< 3*Thres) return Thres | ((Value- Thres)>>1);
|
||||
if(Value< 7*Thres) return 2*Thres | ((Value-3*Thres)>>2);
|
||||
if(Value<15*Thres) return 3*Thres | ((Value-7*Thres)>>3);
|
||||
return 4*Thres-1; }
|
||||
|
||||
template <class Type, int Bits>
|
||||
Type SignVRencode(Type Value)
|
||||
{ const Type SignMask = 1<<(Bits+2);
|
||||
Type Sign=0; if(Value<0) { Value=(-Value); Sign=SignMask; }
|
||||
Value = UnsVRencode<Type, Bits>(Value);
|
||||
return Value | Sign; }
|
||||
|
||||
template <class Type, int Bits>
|
||||
Type SignVRdecode(Type Value)
|
||||
{ const Type SignMask = 1<<(Bits+2);
|
||||
Type Sign = Value&SignMask;
|
||||
Value = UnsVRdecode<Type, Bits>(Value&(SignMask-1));
|
||||
return Sign ? -Value: Value; }
|
||||
|
||||
uint8_t EncodeGray(uint8_t Binary);
|
||||
uint8_t DecodeGray(uint8_t Gray);
|
||||
uint16_t EncodeGray(uint16_t Binary);
|
||||
uint16_t DecodeGray(uint16_t Gray);
|
||||
uint32_t EncodeGray(uint32_t Binary);
|
||||
uint32_t DecodeGray(uint32_t Gray);
|
||||
|
||||
void TEA_Encrypt (uint32_t* Data, const uint32_t *Key, int Loops);
|
||||
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 XorShift32(uint32_t &Seed); // simple random number generator
|
||||
void xorshift64(uint64_t &Seed);
|
||||
|
||||
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
|
||||
|
||||
#endif // __OGNCONV_H__
|
||||
|
|
@ -0,0 +1,132 @@
|
|||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
#include <unistd.h>
|
||||
#include <time.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include "serial.h"
|
||||
#include "ogn.h"
|
||||
|
||||
static double getTime(void) // read the system time at this very moment
|
||||
{ struct timespec now; clock_gettime(CLOCK_REALTIME, &now); return now.tv_sec + 1e-9*now.tv_nsec; }
|
||||
|
||||
static const char *PortName = "/dev/ttyUSB0"; // default serial port name
|
||||
static int BaudRate = 115200; // default baud rate on the serial port
|
||||
static int ListOnly = 0;
|
||||
static uint32_t SelectFile = 0;
|
||||
static int Help = 0;
|
||||
|
||||
static SerialPort Port;
|
||||
|
||||
const int MaxLineLen = 512;
|
||||
static char Line[MaxLineLen];
|
||||
|
||||
static std::vector<uint32_t> LogFileList; // list of log file start times
|
||||
|
||||
static int List(void)
|
||||
{
|
||||
sleep(1);
|
||||
Port.Write(12); // send Ctrl-L to list the log files
|
||||
|
||||
uint8_t Index[32];
|
||||
int LineIdx=0;
|
||||
double Start=getTime(); // sart counting the time
|
||||
for( ; ; )
|
||||
{ char Byte;
|
||||
if(Port.Read(Byte)<=0) // get a byte from the serial port
|
||||
{ double Now=getTime(); // if non, then check time
|
||||
if((Now-Start)>=10.0) break; // if idle for more than 4 sec then stop recording the log files
|
||||
usleep(1000); continue; } // if no new bytes on the serial port sleep a little
|
||||
// printf("%3d: %02X %c\n", LineIdx, Byte, Byte<=' '?' ':Byte);
|
||||
if(Byte<' ') // if a control (non-printable) character
|
||||
{ Line[LineIdx]=0; // terminate the line and process it
|
||||
if(LineIdx<7) { LineIdx=0; continue; }
|
||||
// printf("%s\n", Line);
|
||||
if(memcmp(Line, "$POGNL,", 7)!=0) { LineIdx=0; continue; } // only take POGNL sentences
|
||||
int8_t Err=GPS_Position::IndexNMEA(Index, Line); // index the parameters
|
||||
if(Err<2) { LineIdx=0; continue; } // if less than two parameters, then skip this message
|
||||
if((Index[1]-Index[0])!=13) { LineIdx=0; continue; } // filename must be 13 characters long
|
||||
uint32_t UnixTime=0;
|
||||
if(Read_Hex(UnixTime, Line+Index[0])!=8) { LineIdx=0; continue; } // get the starting time: 8 hex characters
|
||||
time_t Time = UnixTime;
|
||||
struct tm *TM = gmtime(&Time);
|
||||
printf("%s (%d,%d) %02d:%02d:%02d %02d.%02d.%04d\n",
|
||||
Line, Err, Index[1]-Index[0], TM->tm_hour, TM->tm_min, TM->tm_sec, TM->tm_mday, TM->tm_mon+1, TM->tm_year+1900);
|
||||
Start=getTime(); // set new start time counter
|
||||
LogFileList.push_back(Time);
|
||||
LineIdx=0; continue; }
|
||||
if(LineIdx<MaxLineLen) Line[LineIdx++]=Byte; // add the byte to the line, keep collecting more bytes
|
||||
}
|
||||
printf("%d log files\n", LogFileList.size());
|
||||
|
||||
return LogFileList.size(); }
|
||||
|
||||
static int Download(uint32_t LogFile)
|
||||
{ char Cmd[32];
|
||||
|
||||
sleep(1);
|
||||
sprintf(Cmd, "$POGNL,%08X.TLG\n", LogFile);
|
||||
Port.Write(Cmd);
|
||||
printf("Port <- %s", Cmd);
|
||||
|
||||
int Records=0;
|
||||
int LineIdx=0;
|
||||
double Start=getTime();
|
||||
for( ; ; )
|
||||
{ char Byte;
|
||||
if(Port.Read(Byte)<=0)
|
||||
{ double Now=getTime();
|
||||
if((Now-Start)>=8.0) break;
|
||||
usleep(1000); continue; } // if no new bytes on the serial port sleep a little
|
||||
if(Byte<' ') // if a control (non-printable) character
|
||||
{ Line[LineIdx]=0;
|
||||
if(Line[0]=='$') { LineIdx=0; continue; }
|
||||
if(LineIdx<12) { LineIdx=0; continue; }
|
||||
printf("%s\n", Line);
|
||||
Start=getTime();
|
||||
Records++;
|
||||
LineIdx=0; continue; }
|
||||
if(LineIdx<MaxLineLen) Line[LineIdx++]=Byte;
|
||||
}
|
||||
|
||||
printf("%d log records for %08X.TLG\n", Records, LogFile);
|
||||
return Records; }
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
int arg=1;
|
||||
for( ; arg<argc; arg++)
|
||||
{ const char *Val = argv[arg]; if(Val[0]!='-') break;
|
||||
switch(Val[1])
|
||||
{ case 'h': Help=1; break;
|
||||
case 'l': ListOnly=1; break;
|
||||
case 'f': SelectFile=strtol(Val+2, 0, 16); break;
|
||||
}
|
||||
}
|
||||
|
||||
if(arg<argc)
|
||||
{ char *Colon = strchr(argv[arg],':');
|
||||
if(Colon) { Colon[0]=0; BaudRate=atoi(Colon+1); }
|
||||
PortName = argv[arg]; }
|
||||
|
||||
if(Port.Open(PortName, BaudRate)<0) { printf("Can't open %s at %dbps\n", PortName, BaudRate); return -1; }
|
||||
// printf("Open %s at %dbps\n", PortName, BaudRate);
|
||||
|
||||
if(SelectFile==0) List();
|
||||
|
||||
if(ListOnly) { Port.Close(); return 0; }
|
||||
|
||||
if(SelectFile)
|
||||
{ Download(SelectFile); }
|
||||
else
|
||||
{ for( size_t File=0; File<LogFileList.size(); File++)
|
||||
Download(LogFileList[File]); }
|
||||
|
||||
Port.Close();
|
||||
return 0; }
|
||||
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
#include <unistd.h>
|
||||
#include <stdint.h>
|
||||
#include <fcntl.h>
|
||||
#include <termios.h>
|
||||
#include <string.h>
|
||||
|
||||
class SerialPort
|
||||
{ public:
|
||||
|
||||
int DeviceHandle;
|
||||
|
||||
public:
|
||||
|
||||
SerialPort()
|
||||
{ DeviceHandle=(-1); }
|
||||
|
||||
~SerialPort()
|
||||
{ Close(); }
|
||||
|
||||
int isOpen(void) const { return DeviceHandle>=0; }
|
||||
|
||||
int Open(const char *DeviceName="/dev/ttyS0", int BaudRate=9600)
|
||||
{ Close();
|
||||
|
||||
DeviceHandle=open(DeviceName, O_RDWR | O_NOCTTY | O_NDELAY );
|
||||
if(DeviceHandle<0) return -1;
|
||||
|
||||
int Speed; // speed_t Speed;
|
||||
if(BaudRate==4800) Speed=B4800;
|
||||
else if(BaudRate==9600) Speed=B9600;
|
||||
else if(BaudRate==19200) Speed=B19200;
|
||||
else if(BaudRate==38400) Speed=B38400;
|
||||
else if(BaudRate==57600) Speed=B57600;
|
||||
else if(BaudRate==115200) Speed=B115200;
|
||||
else if(BaudRate==230400) Speed=B230400;
|
||||
// else if(BaudRate==128000) Speed=B128000;
|
||||
// else if(BaudRate==256000) Speed=B256000;
|
||||
else return -2;
|
||||
|
||||
struct termios Options;
|
||||
tcgetattr(DeviceHandle, &Options);
|
||||
|
||||
if(cfsetispeed(&Options, Speed)<0) return -2;
|
||||
if(cfsetospeed(&Options, Speed)<0) return -2;
|
||||
// cfsetspeed(&Options, Speed);
|
||||
|
||||
Options.c_cflag |= (CLOCAL | CREAD);
|
||||
|
||||
Options.c_cflag &= ~PARENB; // 8-bits, no parity
|
||||
Options.c_cflag &= ~CSTOPB;
|
||||
Options.c_cflag &= ~CSIZE;
|
||||
Options.c_cflag |= CS8;
|
||||
|
||||
Options.c_cc[VTIME] = 0;
|
||||
Options.c_cc[VMIN] = 1;
|
||||
|
||||
tcsetattr(DeviceHandle, TCSANOW, &Options);
|
||||
/*
|
||||
int Bytes=sizeof(Options);
|
||||
uint8_t *Ptr = (uint8_t *)(&Options);
|
||||
printf("Options[%d] =", Bytes);
|
||||
for( ; Bytes; Bytes--, Ptr++)
|
||||
{ printf(" %02X", *Ptr); }
|
||||
printf("\n");
|
||||
*/
|
||||
return 0; }
|
||||
|
||||
int OpenFileForRead(const char *FileName)
|
||||
{ Close();
|
||||
DeviceHandle=open(FileName, O_RDONLY);
|
||||
if(DeviceHandle<0) return -1;
|
||||
return 0; }
|
||||
|
||||
int Close(void)
|
||||
{ if(DeviceHandle>=0)
|
||||
{ close(DeviceHandle); DeviceHandle=(-1); }
|
||||
return 0; }
|
||||
|
||||
int Read(char *Buffer, size_t MaxChars)
|
||||
{ int Len=read(DeviceHandle, Buffer, MaxChars);
|
||||
return Len<=0 ? 0:Len; }
|
||||
|
||||
int Read(char &Char)
|
||||
{ return Read(&Char, 1); }
|
||||
|
||||
int Write(const char *Buffer, size_t Chars)
|
||||
{ int Len=write(DeviceHandle, Buffer, Chars);
|
||||
return Len<=0 ? 0:Len; }
|
||||
|
||||
int Write(const char *String)
|
||||
{ return Write(String, strlen(String)); }
|
||||
|
||||
int Write(char Char)
|
||||
{ return Write(&Char,1); }
|
||||
|
||||
} ;
|
||||
|
||||
|
|
@ -0,0 +1,316 @@
|
|||
#ifndef __UBX_H__
|
||||
#define __UBX_H__
|
||||
|
||||
class UBX_NAV_POSLLH // 0x01 0x02
|
||||
{ public:
|
||||
uint32_t iTOW; // [ms] Time-of-Week
|
||||
int32_t lon; // [1e-7 deg] Longitude
|
||||
int32_t lat; // [1e-7 deg] Latitude
|
||||
int32_t height; // [mm] height above elipsoid (GPS altitude)
|
||||
int32_t hMSL; // [mm] height above Mean Sea Level
|
||||
uint32_t hAcc; // [mm] horizontal accuracy
|
||||
uint32_t vAcc; // [mm] vertical accuracy
|
||||
} ;
|
||||
|
||||
class UBX_NAV_STATUS // 0x01 0x03
|
||||
{ public:
|
||||
uint32_t iTOW; // [ms] Time-of-Week
|
||||
uint8_t gpsFix; // Fix type: 0:none, 1=dead reckoning, 2:2-D, 3:3-D, 4:GPS+dead reckoning, 5:time-only
|
||||
uint8_t flags; // xxxxTWDF => T:Time-of-Week is valid, W:Week-Number is valid, D:Diff. GPS is used, F:Fix valid
|
||||
uint8_t diffStat; // DD => 00:none, 01:PR+PRR corr., 10: PR+PRR+CP corr., 11: high accuracy PR+PRR+CP
|
||||
uint8_t res; // reserved PR=Pseudo-Range, PRR=Pseudo-Range Rate
|
||||
uint32_t ttff; // [ms] Time To First Fix
|
||||
uint32_t msss; // [ms] Since Startup
|
||||
} ;
|
||||
|
||||
class UBX_NAV_DOP // 0x01 0x04
|
||||
{ public:
|
||||
uint32_t iTOW; // [ms] Time-of-Week
|
||||
uint16_t gDOP; // [1/100] geometrical
|
||||
uint16_t pDOP; // [1/100] position
|
||||
uint16_t tDOP; // [1/100] time
|
||||
uint16_t vDOP; // [1/100] vertical
|
||||
uint16_t hDOP; // [1/100] horizontal
|
||||
uint16_t nDOP; // [1/100] north-south
|
||||
uint16_t eDOP; // [1/100] east-west
|
||||
uint16_t padding; // padding for round size
|
||||
} ;
|
||||
|
||||
class UBX_NAV_SOL // 0x01 0x06
|
||||
{ public:
|
||||
uint32_t iTOW; // [ms] Time-of-Week
|
||||
int32_t fTOW; // [ns] Time-of-Week
|
||||
int16_t Week; // [week]
|
||||
uint8_t gpsFix; // 0=none, 1=DR, 2=2D, 3=3D, 4=DR+GPS, 5=time-only
|
||||
uint8_t flags;
|
||||
int32_t ecefX; // [cm] ECEF position
|
||||
int32_t ecefY; // [cm]
|
||||
int32_t ecefZ; // [cm]
|
||||
uint32_t pAcc; // [cm] position accuracy
|
||||
int32_t ecefVX; // [cm/s] ECEF velocity
|
||||
int32_t ecefVY; // [cm/s]
|
||||
int32_t ecefVZ; // [cm/s]
|
||||
uint32_t sAcc; // [cm/s] speed accuracy
|
||||
uint16_t PDOP; // [1/100]
|
||||
uint8_t reserved1; //
|
||||
uint8_t numSV; // [satellites]
|
||||
uint8_t reserved2[4]; //
|
||||
} ;
|
||||
|
||||
class UBX_NAV_VELNED // 0x01 0x12
|
||||
{ public:
|
||||
uint32_t iTOW; // [ms] Time-of-Week
|
||||
int32_t velN; // [cm/s] velocity North
|
||||
int32_t velE; // [cm/s] velocity East
|
||||
int32_t velD; // [cm/s] velocity Down
|
||||
uint32_t Speed; // [cm/s] velocity
|
||||
uint32_t gSpeed; // [cm/s] ground speed (horizontal velocity)
|
||||
int32_t heading; // [1e-5 deg] ground heading
|
||||
uint32_t sAcc; // [cm/s] speed accuracy
|
||||
uint32_t cAcc; // [1e-5 deg] heading accuracy
|
||||
} ;
|
||||
|
||||
class UBX_NAV_TIMEGPS // 0x01 0x020
|
||||
{ public:
|
||||
uint32_t iTOW; // [ms] Time-of-Week
|
||||
int32_t fTOW; // [ns] reminder of Time-of-Week
|
||||
uint16_t week;
|
||||
uint8_t leapS;
|
||||
uint8_t valid; // bits: 0:ToW, 1:week, 2:leapS
|
||||
uint32_t tAcc; // [ns]
|
||||
|
||||
public:
|
||||
static const uint32_t SecsPerWeek = 7*24*60*60;
|
||||
uint8_t Valid(void) const
|
||||
{ return (valid&0x03)==0x03; }
|
||||
uint32_t UnixTime() const
|
||||
{ return (iTOW+10)/1000 + week*SecsPerWeek + 315964785; } // http://www.andrews.edu/~tzs/timeconv/timedisplay.php
|
||||
} ;
|
||||
|
||||
class UBX_NAV_TIMEUTC // 0x01 0x21
|
||||
{ public:
|
||||
uint32_t iTOW; // [ms] Time-of-Week
|
||||
uint32_t tAcc; // [ns] accurary estimate
|
||||
int32_t nano; // [ns]
|
||||
uint16_t year; // 1999..2099
|
||||
uint8_t month; // 1..12
|
||||
uint8_t day; // 1..31
|
||||
uint8_t hour;
|
||||
uint8_t min;
|
||||
uint8_t sec;
|
||||
uint8_t valid; // bits: 0:ToW, 1:WN, 2:UTC
|
||||
} ;
|
||||
|
||||
class UBX_RXM_PMREQ // 0x02 0x41
|
||||
{ public:
|
||||
uint32_t duration; // [ms]
|
||||
uint32_t flags; // bit #1 = enter backup mode
|
||||
} ;
|
||||
|
||||
class UBX_CFG_CFG
|
||||
{ public:
|
||||
uint32_t clearMask;
|
||||
uint32_t saveMask;
|
||||
uint32_t loadMask;
|
||||
uint8_t deviceMask;
|
||||
} ;
|
||||
|
||||
class UBX_CFG_PRT // 0x06 0x00
|
||||
{ public:
|
||||
uint8_t portID; // 0 = I2C, 1 = UART1, 2 = UART2, 3 = USB, 4 = SPI
|
||||
uint8_t reserved1;
|
||||
uint16_t txReady; // 0 = disable this feature
|
||||
int32_t mode; // 00 10x x 11 x 1 xxxx => 0x08D0
|
||||
uint32_t baudRate; // [bps]
|
||||
int16_t inProtoMask; // bit 0:UBX, bit 1:NMEA
|
||||
int16_t outProtoMask; // bit 0:UBX, bit 1:NMEA
|
||||
uint16_t flags; // bit 1:extendedTxTimeout
|
||||
uint16_t reserved2;
|
||||
} ;
|
||||
|
||||
class UBX_CFG_MSG // 0x06 0x01
|
||||
{ public:
|
||||
uint8_t msgClass; // 0xF0:00=GGA, 0xF0:02=GSA, 0xF0:04=RMC, 0xF0:41=TXT
|
||||
uint8_t msgID;
|
||||
uint8_t rate; // message send rate
|
||||
} ;
|
||||
|
||||
/*
|
||||
class UBX_CFG_MSG // 0x06 0x01
|
||||
{ public:
|
||||
uint8_t msgClass; // 0xF0:00=GGA, 0xF0:02=GSA, 0xF0:04=RMC, 0xF0:41=TXT
|
||||
uint8_t msgID;
|
||||
uint8_t rate[6]; // message send rate
|
||||
} ;
|
||||
*/
|
||||
|
||||
class UBX_CFG_RATE // 0x06 0x08
|
||||
{ public:
|
||||
uint16_t measRate; // [ms] measurement rate
|
||||
uint16_t navRate; // [cycles] = 1
|
||||
uint16_t timeRef; // 0=UTC, 1=GPS
|
||||
} ;
|
||||
|
||||
class UBX_CFG_NAV5 // 0x06 0x24
|
||||
{ public:
|
||||
uint16_t mask; // bit #0 = apply dynamic mode settings, #1 = apply min. elev. settings, #2 = apply fix mode settings
|
||||
uint8_t dynModel; // 6 = airborne 1g, 7 = 2g, 8 = 4g
|
||||
uint8_t fixMode; // 1=2D only, 2=3D only, 3=auto 2/3D
|
||||
int32_t fixAlt; // [0.01m]
|
||||
uint32_t fixAltVar; // [0.001m]
|
||||
int8_t minElev; // [deg] minimum satelite elevation
|
||||
uint8_t drLimit; // [sec] Dead Reconning time limit
|
||||
uint16_t pDop;
|
||||
uint16_t tDop;
|
||||
uint16_t pAcc; // [m]
|
||||
uint16_t tAcc; // [m]
|
||||
uint8_t staticHoldThres; // [cm/s]
|
||||
uint8_t dgpsTimeout; // [s]
|
||||
uint8_t cnoThreshNumSVs; //
|
||||
uint8_t cnoThresh; // [dBHz]
|
||||
uint8_t reserved2[2];
|
||||
uint32_t reserved3;
|
||||
uint32_t reserved4;
|
||||
} ;
|
||||
|
||||
// UBX Class packet numbers
|
||||
const uint8_t UBX_NAV = 0x01; // navigation
|
||||
const uint8_t UBX_ACK = 0x05; // acknoledgement of configuration
|
||||
const uint8_t UBX_CFG = 0x06; // configuration
|
||||
|
||||
class UBX_RxMsg // receiver for the UBX sentences
|
||||
{ public:
|
||||
// most information in the UBX packets is already aligned to 32-bit boundary
|
||||
// thus it makes sense to have the packet so aligned when receiving it.
|
||||
static const uint8_t MaxWords=32; // 10; // maximum number of 32-bit words (excl. head and tail)
|
||||
static const uint8_t MaxBytes=4*MaxWords; // max. number of bytes
|
||||
static const uint8_t SyncL=0xB5; // UBX sync bytes
|
||||
static const uint8_t SyncH=0x62;
|
||||
|
||||
union
|
||||
{ uint32_t Word[MaxWords]; // here we store the UBX packet (excl. head and tail)
|
||||
uint8_t Byte[MaxBytes]; } ;
|
||||
uint8_t Class; // Class (01=NAV)
|
||||
uint8_t ID; // ID
|
||||
uint8_t Bytes; // number of bytes in the packet (excl. head and tail)
|
||||
|
||||
private:
|
||||
uint8_t Padding; // just to make the structure size be a multiple of 4-bytes
|
||||
uint8_t State; // bits: 0:loading, 1:complete, 2:locked,
|
||||
uint8_t Idx; // loading index
|
||||
|
||||
uint8_t CheckA; // UBX check sum (two bytes)
|
||||
uint8_t CheckB;
|
||||
void CheckInit(void) { CheckA=0; CheckB=0; } // initialize the checksum
|
||||
void CheckPass(uint8_t Byte) { CheckA+=Byte; CheckB+=CheckA; } // pass a byte through the checksum
|
||||
|
||||
public:
|
||||
void RecalcCheck(void)
|
||||
{ CheckInit();
|
||||
CheckPass(Class); CheckPass(ID); CheckPass(Bytes); CheckPass(0x00);
|
||||
for(uint8_t Idx=0; Idx<Bytes; Idx++) CheckPass(Byte[Idx]); }
|
||||
|
||||
inline void Clear(void) { Idx=0; State=0; CheckInit(); }
|
||||
|
||||
uint8_t isLoading(void) const
|
||||
{ return State&0x01; }
|
||||
|
||||
uint8_t isComplete(void) const
|
||||
{ return State&0x02; }
|
||||
|
||||
void ProcessByte(uint8_t RxByte) // pass all bytes through this call and it will build the frame
|
||||
{
|
||||
if(isComplete()) Clear(); // if already a complete frame, clear it
|
||||
switch(Idx)
|
||||
{ case 0: // expect SyncL
|
||||
if(RxByte!=SyncL) { Clear(); return; }
|
||||
State=0x01; break; // declare "isLoading" state
|
||||
case 1: // expect SyncH
|
||||
if(RxByte!=SyncH) { Clear(); return; }
|
||||
break;
|
||||
case 2: // Class
|
||||
Class=RxByte; CheckPass(RxByte);
|
||||
break;
|
||||
case 3: // ID
|
||||
ID=RxByte; CheckPass(RxByte);
|
||||
break;
|
||||
case 4: // LSB of packet length
|
||||
Bytes=RxByte; CheckPass(RxByte); if(Bytes>MaxBytes) { Clear(); return; }
|
||||
break;
|
||||
case 5: // MSB of packet length (expect zero)
|
||||
CheckPass(RxByte); if(RxByte!=0) { Clear(); return; }
|
||||
break;
|
||||
default: // past the header, now load the packet content
|
||||
uint8_t ByteIdx=Idx-6;
|
||||
if(ByteIdx<Bytes)
|
||||
{ Byte[ByteIdx]=RxByte; CheckPass(RxByte); }
|
||||
else if(ByteIdx==Bytes) // already past the content, now the first checksum byte
|
||||
{ if(RxByte!=CheckA) { Clear(); return; } }
|
||||
else if(ByteIdx==(Bytes+1)) // second checksum byte
|
||||
{ if(RxByte!=CheckB) { Clear(); return; }
|
||||
State=0x02; } // declare "isComplete" state
|
||||
else
|
||||
{ Clear(); return; }
|
||||
break;
|
||||
}
|
||||
Idx++;
|
||||
}
|
||||
|
||||
void Send(void (*SendByte)(char)) const
|
||||
{ (*SendByte)(SyncL);
|
||||
(*SendByte)(SyncH);
|
||||
(*SendByte)(Class);
|
||||
(*SendByte)(ID);
|
||||
(*SendByte)(Bytes);
|
||||
(*SendByte)(0x00);
|
||||
for(uint8_t Idx=0; Idx<Bytes; Idx++)
|
||||
{ (*SendByte)(Byte[Idx]); }
|
||||
(*SendByte)(CheckA);
|
||||
(*SendByte)(CheckB);
|
||||
}
|
||||
|
||||
static void Send(uint8_t Class, uint8_t ID, void (*SendByte)(char), const uint8_t *Data=0, uint8_t DataLen=0)
|
||||
{ (*SendByte)(SyncL);
|
||||
(*SendByte)(SyncH);
|
||||
(*SendByte)(Class);
|
||||
uint8_t CheckA = Class; // pass Class through check sum
|
||||
uint8_t CheckB = CheckA;
|
||||
(*SendByte)(ID);
|
||||
CheckA += ID; // pass ID through check sum
|
||||
CheckB += CheckA;
|
||||
(*SendByte)(DataLen);
|
||||
CheckA += DataLen;
|
||||
CheckB += CheckA; // pass PollDataLen
|
||||
(*SendByte)(0x00);
|
||||
CheckB += CheckA; // pass 0x00
|
||||
if(Data)
|
||||
{ for(uint8_t Idx=0; Idx<DataLen; Idx++)
|
||||
{ (*SendByte)(Data[Idx]);
|
||||
CheckA += Data[Idx];
|
||||
CheckB += CheckA; }
|
||||
}
|
||||
(*SendByte)(CheckA); // send the check sum
|
||||
(*SendByte)(CheckB);
|
||||
}
|
||||
|
||||
bool isNAV(void) const { return Class==0x01; }
|
||||
bool isACK(void) const { return Class==0x05; }
|
||||
bool isCFG(void) const { return Class==0x06; }
|
||||
|
||||
bool isNAV_POSLLH (void) const { return isNAV() && (ID==0x02) && (Bytes==sizeof(UBX_NAV_POSLLH)); }
|
||||
bool isNAV_STATUS (void) const { return isNAV() && (ID==0x03); }
|
||||
bool isNAV_DOP (void) const { return isNAV() && (ID==0x04); }
|
||||
bool isNAV_SOL (void) const { return isNAV() && (ID==0x06) && (Bytes==sizeof(UBX_NAV_SOL)); }
|
||||
bool isNAV_VELNED (void) const { return isNAV() && (ID==0x12); }
|
||||
bool isNAV_TIMEGPS(void) const { return isNAV() && (ID==0x20); }
|
||||
bool isNAV_TIMEUTC(void) const { return isNAV() && (ID==0x21) && (Bytes==sizeof(UBX_NAV_TIMEUTC)); }
|
||||
|
||||
bool isACK_NAK (void) const { return isACK() && (ID==0x00); }
|
||||
bool isACK_ACK (void) const { return isACK() && (ID==0x01); }
|
||||
|
||||
bool isCFG_PRT (void) const { return isCFG() && (ID==0x00); }
|
||||
bool isCFG_NAV5 (void) const { return isCFG() && (ID==0x24); }
|
||||
} ;
|
||||
|
||||
|
||||
#endif // __UBX_H__
|
||||
Ładowanie…
Reference in New Issue