FANET+ selbst gebaut

/*
* FANET Basic modul
*
* from BitBroker – Burkhard Vogt, Muenster, Westfalia, Germany
*
* Connections:
*
* Micro USB form power supply i.e. a power bank
*
* 0 – (RX1) TX from FANET modul
* 1 – (TX1) RX to FANET modul
* 9 – (RX2) TX from GPS modul
*
* Connect PPS from GPS to FANET modul!
*/

#define UsbSerial Serial
#define FntSerial Serial1
#define GpsSerial Serial2

#define POS_DELAY 20 // wait 20 position sets for first response
#define LED 13 // the number of the onboard LED pin

// #define DEBUG
// #define DEBUG_FNT
// #define DEBUG_GPS

char FntBuf[256];
byte FntIdx;

/*
* Read the FANET statup requests
* Wait max. 1 sec for answer
*/

void ReadStartup ()
{
FntIdx = 0;

// Wait 1sec for response

unsigned long ulMillis = millis() + 1000;

while ( millis() < ulMillis )
{
if ( FntSerial.available() )
{
FntBuf[FntIdx] = FntSerial.read();

if ( FntBuf[FntIdx] == 10 )
{
#ifdef DEBUG_FNT
FntBuf[FntIdx] = 0;
UsbSerial.print ( „FANET ‚“ );
UsbSerial.print ( FntBuf );
UsbSerial.println ( „‚“ );
#endif

return;
}
else
{
if ( FntBuf[0] == ‚#‘ ) FntIdx++;

if ( FntIdx > 255 ) FntIdx = 0;
}
}
}
FntBuf[0] = 0;
}

/*
* Startup sequence
* Initialize ports, FANET modul
* and some variables
*/

void setup()
{
UsbSerial.begin ( 9600 );
FntSerial.begin ( 115200 );
GpsSerial.begin ( 9600 );

#ifdef DEBUG
while ( ! UsbSerial ) { ; }
UsbSerial.println ( „Program and funktion setup start“ );
#endif

pinMode ( LED, OUTPUT );

digitalWrite ( LED, HIGH );

// Setup the serial communication

FntSerial.clear();
GpsSerial.clear();

// Answer first ‚C‘ with ‚\n‘
// Wait 5sec for ‚C‘ to response

unsigned long ulMillis = millis() + 5000;

while ( millis() < ulMillis )
{
if ( FntSerial.available() )
{
if ( FntSerial.read() == ‚C‘ )
{
FntSerial.println ();

break;
}
}
}

// Wait for init string ‚#FNR MSG,1,initialized‘
// Comes only when booting after power on or BOOT toggle

ReadStartup ();

// ToDo: check it …

// Check for correct version

FntSerial.println ( „#DGV“ );

ReadStartup ();

// ToDo: check it …

// Get module addr

FntSerial.println ( „#FNA“ );

ReadStartup ();

// Only for information

// Check FLARM expiration

FntSerial.println ( „#FAX“ );

ReadStartup ();

// ToDo: check it and update if expired …

// Configure APP PG, online tracking

FntSerial.println ( „#FNC 1,1“ );

ReadStartup ();

// Enable receiver FANET

FntSerial.println ( „#DGP 1“ );

ReadStartup ();

// Enable receiver FLARM

FntSerial.println ( „#FAP 1“ );

ReadStartup ();

FntIdx = 0;

#ifdef DEBUG
UsbSerial.println ( „Function loop should start ;-)“ );
#endif
}

/*
* Toggle state of inbuild LED
*/

byte bGpsLED = 0;

void ToggleLED ( void )
{
if ( bGpsLED )
{
digitalWrite ( LED, LOW );
}
else
{
digitalWrite ( LED, HIGH );
}
bGpsLED = ! bGpsLED;
}

/*
* Read lat or lon inside of an NMEA sequence
* in the funny NMEA counter format
*/

double GetPos ( char *pStr, byte bLen )
{
char cDeg[4];
byte bI;

for ( bI=0; bI<bLen; bI++ )
{
if ( pStr[bI] == 0 ) return ( 0.0 );

cDeg[bI] = pStr[bI];
}

cDeg[bI] = 0;

double dDeg = atof ( cDeg );
double dMin = atof ( pStr + bLen );

return ( dDeg + dMin / 60.0 );
}

/*
* Some global variables
*/

float fHight = 0, fClimb, fSpeed, fCourse;
double dLat, dLon;
int iTime, iDate;
byte bFix = 0, bVal = 0;

static const char Hex[] = „0123456789ABCDEF“;

/*
* Function to handle the GPS serial communication
*/

char GpsBuf[256];
byte GpsIdx = 0;
byte bPosCnt = 0;

void HandleGps ()
{
GpsBuf[GpsIdx] = GpsSerial.read();

if ( GpsBuf[GpsIdx] == 13 )
{
if ( GpsIdx > 3 )
{
if ( GpsBuf[GpsIdx-3] != ‚*‘ ) return; // Wrong format!

byte bI, CRC = 0; // Calculate the CRC and check

for ( bI=1; bI<(GpsIdx-3); bI++ ) CRC = CRC ^ GpsBuf[bI] ;

if ( GpsBuf[GpsIdx-2] != Hex[(CRC & 0xf0) >> 4] ||
GpsBuf[GpsIdx-1] != Hex[(CRC & 0x0f)] ) return;

#ifdef DEBUG_GPS
GpsBuf[GpsIdx] = 0;
UsbSerial.println ( GpsBuf );
#endif

byte bCmd = 0;

for ( bI=0; bI<2; bI++ )
{
char cCmd[] = { „GPGGA,GPRMC,“ };
byte bK;

for ( bK=0; bK<6; bK++ )
{
if ( GpsBuf[bK+1] != cCmd[bI*6+bK] ) break;
}

if ( bK == 6 )
{
bCmd = bI * 20 + 20;

break;
}
}

if ( bCmd )
{
// Now the original NMEA string from GPS could send to a KOBO …
// We had to interpret the string to send infos to the FANET modul

byte bStart = 7;
byte bParam = 0;

for ( bI=bStart; bI<(GpsIdx-2); bI++ )
{
if ( GpsBuf[bI] == ‚,‘ || GpsBuf[bI] == ‚*‘ )
{
GpsBuf[bI] = 0;
bParam++;

switch ( bCmd + bParam )
{
case 21: iTime = atoi ( GpsBuf + bStart );

#ifdef DEBUG_GPS
UsbSerial.print ( „Time “ );
UsbSerial.println ( iTime );
#endif

break;

case 42: if ( GpsBuf[bStart] == ‚A‘ )
{
bVal = 1;
}
else
{
bVal = 0;
}
break;

case 22:
case 43: dLat = GetPos ( GpsBuf + bStart, 2 );

#ifdef DEBUG_GPS
UsbSerial.print ( „Lat “ );
UsbSerial.println ( dLat, 4 );
#endif

break;

case 23:
case 44: if ( GpsBuf[bStart] == ‚S‘ ) dLat *= -1.0;

break;

case 24:
case 45: dLon = GetPos ( GpsBuf + bStart, 3 );

#ifdef DEBUG_GPS
UsbSerial.print ( „Lon “ );
UsbSerial.println ( dLon, 4 );
#endif

break;

case 25:
case 46: if ( GpsBuf[bStart] == ‚W‘ ) dLon *= -1.0;

break;

case 26: bFix = atoi ( GpsBuf + bStart );

#ifdef DEBUG_GPS
UsbSerial.print ( „Fix “ );
UsbSerial.println ( bFix );
#endif

break;

case 29: {
float fHgt = atof ( GpsBuf + bStart );

fClimb = fHgt – fHight;
fHight = fHgt;
}

#ifdef DEBUG_GPS
UsbSerial.print ( „Hight “ );
UsbSerial.println ( fHight, 1 );
UsbSerial.print ( „Climb “ );
UsbSerial.println ( fClimb, 1 );
#endif

break;

case 47: fSpeed = atof ( GpsBuf + bStart ) * 1.852;

#ifdef DEBUG_GPS
UsbSerial.print ( „Speed “ );
UsbSerial.println ( fSpeed, 2 );
#endif

break;

case 48: fCourse = atof ( GpsBuf + bStart );

#ifdef DEBUG_GPS
UsbSerial.print ( „Course “ );
UsbSerial.println ( fCourse, 2 );
#endif

break;

case 49: iDate = atoi ( GpsBuf + bStart );

#ifdef DEBUG_GPS
UsbSerial.print ( „Date “ );
UsbSerial.println ( iDate );
#endif

break;

#ifdef DEBUG_GPS
default: UsbSerial.print ( bCmd + bParam );
UsbSerial.print ( „-“ );
UsbSerial.println ( GpsBuf+bStart );
#endif
}
if ( bParam > 9 ) break; // No more interesting params …

bStart = bI + 1;
}
}

if ( bCmd == 20 && bFix && bVal )
{
ToggleLED ();

if ( bPosCnt < POS_DELAY )
{
bPosCnt ++;
}
else
{
#ifdef DEBUG_GPS
UsbSerial.print ( „GPS Lat “ );
UsbSerial.print ( dLat, 4 );
UsbSerial.print ( “ Lon “ );
UsbSerial.print ( dLon, 4 );
UsbSerial.print ( “ Hight “ );
UsbSerial.print ( fHight, 1 );
UsbSerial.print ( “ Speed “ );
UsbSerial.print ( fSpeed, 2 );
UsbSerial.print ( “ Climb “ );
UsbSerial.print ( fClimb, 2 );
UsbSerial.print ( “ Course “ );
UsbSerial.print ( fCourse, 2 );
UsbSerial.print ( “ Date “ );
UsbSerial.print ( iDate );
UsbSerial.print ( “ Time “ );
UsbSerial.println ( iTime );
#endif

int iDay = iDate / 10000;
int iMonth = iDate / 100 – iDay * 100;
int iYear = iDate – iDay * 10000 – iMonth * 100;
int iHour = iTime / 10000;
int iMin = iTime / 100 – iHour * 100;
int iSec = iTime – iHour * 10000 – iMin * 100;

// examplel FANET string ‚#FNS 45.1234,10.5678,500,37,-1.5,45,117,9,30,12,35,2\n‘
// ‚#FNS lat,lon,alt,speed,climb,heading,year,month,day,hour,min,sec,turn\n‘

FntSerial.print ( „#FNS “ );
FntSerial.print ( dLat, 4 );
FntSerial.print ( „,“ );
FntSerial.print ( dLon, 4 );
FntSerial.print ( „,“ );
FntSerial.print ( fHight, 0 );
FntSerial.print ( „,“ );
FntSerial.print ( fSpeed, 0 );
FntSerial.print ( „,“ );
FntSerial.print ( fClimb, 0 );
FntSerial.print ( „,“ );
FntSerial.print ( fCourse, 0 );
FntSerial.print ( „,“ );
FntSerial.print ( iYear + 100 );
FntSerial.print ( „,“ );
FntSerial.print ( iMonth );
FntSerial.print ( „,“ );
FntSerial.print ( iDay );
FntSerial.print ( „,“ );
FntSerial.print ( iHour );
FntSerial.print ( „,“ );
FntSerial.print ( iMin );
FntSerial.print ( „,“ );
FntSerial.print ( iSec );
FntSerial.println ();
}
}
}
}
GpsIdx = 0;
}
else
{
if ( GpsBuf[0] == ‚$‘ ) GpsIdx++;

if ( GpsIdx > 255 ) GpsIdx = 0;
}
}

/*
* My own hexstr value reader
*/

int ReadHex ( char *szHex, byte bMax )
{
int iVal = 0;
byte bI;

for ( bI=0; bI<bMax; bI++ )
{
byte bByte = szHex[bI];

if ( ! bByte ) break;

// transform hex character to the 4bit equivalent number, using the ascii table indexes

if ( bByte >= ‚0‘ && bByte <= ‚9‘ ) bByte = bByte – ‚0‘;
else if ( bByte >= ‚a‘ && bByte <= ‚f‘ ) bByte = bByte – ‚a‘ + 10;
else if ( bByte >= ‚A‘ && bByte <= ‚F‘ ) bByte = bByte – ‚A‘ + 10;

// shift 4 to make space for new digit, and add the 4 bits of the new digit

iVal = ( iVal << 4 ) | bByte;
}
return iVal;
}

/*
* 3 byte little endian
*/

int ReadHexLE ( char *szHex )
{
int iVal = 0;
byte bI;

szHex += 4;

for ( bI=0; bI<3; bI++ )
{
iVal = ( iVal << 8 ) | ReadHex ( szHex – bI * 2, 2 );
}

if ( iVal & 0x800000 ) return – ( iVal & 0x7FFFFF );

return iVal;
}

/*
* Function to handle the FANET serial communication
*/

void HandleFnt ()
{
FntBuf[FntIdx] = FntSerial.read();

if ( FntBuf[FntIdx] == 10 )
{
char cCmd[] = { „FNF “ };
byte bI, bK;

char szMf[5], szId[5];
byte bTyp = 0, bLen;

for ( bK=0; bK<4; bK++ )
{
if ( FntBuf[bK+1] != cCmd[bK] ) break;
}

if ( bK == 4 )
{
#ifdef DEBUG_FNT
FntBuf[FntIdx] = 0;
UsbSerial.print ( „FANET ‚“ );
UsbSerial.print ( FntBuf );
UsbSerial.println ( „‚“ );
#endif

byte bStart = 5;
byte bParam = 0;

for ( bI=bStart; bI<(FntIdx+1); bI++ )
{
if ( FntBuf[bI] == ‚,‘ || FntBuf[bI] == 10 )
{
FntBuf[bI] = 0;
bParam++;

switch ( bParam )
{
case 1: strncpy ( szMf, FntBuf + bStart, 4 ); szMf[4] = 0;

break;

case 2: strncpy ( szId, FntBuf + bStart, 4 ); szId[4] = 0;

break;

case 3: // Not yet required

// bBc = ReadHex ( FntBuf + bStart, 2 );

break;

case 4: // Not yet required

// iSig = ReadHex ( FntBuf + bStart, 4 );

break;

case 5: bTyp = ReadHex ( FntBuf + bStart, 2 );

break;

case 6: bLen = ReadHex ( FntBuf + bStart, 2 );

break;
}
bStart = bI + 1;

if ( bParam == 6 )
{
// Here we could place a FLARM emulation send to a KOBO …
// PFLAA,<AlarmLevel>,<RelativeNorth>,<RelativeEast>, <RelativeVertical>,<IDType>,<ID>,<Track>,<TurnRate>,<GroundSpeed>, <ClimbRate>,<AcftType>
// or print a logging info to the USB serial port 😉

switch ( bTyp )
{
case 1:
{
if ( bLen != 11 )
{
UsbSerial.println ( „Tracking with wrong data info!“ );

break;
}

double dLat = ReadHexLE ( FntBuf + bStart ) / 93206.0;
double dLon = ReadHexLE ( FntBuf + bStart + 6 ) / 46603.0;

int iBuffer = ReadHex ( FntBuf + bStart + 12, 2 ) | ( ReadHex ( FntBuf + bStart + 14, 2 ) << 8 );

byte bType = ( iBuffer >> 12 ) & 7;

UsbSerial.print ( „Tracking from “ );

switch ( bType )
{
case 1: UsbSerial.print ( „Paraglider “ ); break;
case 2: UsbSerial.print ( „Hangglider “ ); break;
case 3: UsbSerial.print ( „Balloon “ ); break;
case 4: UsbSerial.print ( „Glider “ ); break;
case 5: UsbSerial.print ( „Powered Aircraft “ ); break;
case 6: UsbSerial.print ( „Helicopter “ ); break;
case 7: UsbSerial.print ( „UAV “ ); break;
default: UsbSerial.print ( „??? “ );
}

UsbSerial.print ( szMf );
UsbSerial.print ( „:“ );
UsbSerial.print ( szId );
UsbSerial.print ( “ at Pos “ );
UsbSerial.print ( dLat, 4 );
UsbSerial.print ( “ “ );
UsbSerial.println ( dLon, 4 );
break;
}

case 2:
{
UsbSerial.print ( „Name from “ );
UsbSerial.print ( szMf );
UsbSerial.print ( „:“ );
UsbSerial.print ( szId );
UsbSerial.print ( “ ‚“ );

byte bL;
char szStr[128];

for ( bL=0; bL<min(bLen,128); bL++)
{
szStr[bL] = ReadHex ( FntBuf + bStart + bL * 2, 2 );
}
szStr[bL] = 0;

UsbSerial.print ( szStr );
UsbSerial.println ( „‚“ );

break;
}

case 3:
{
UsbSerial.print ( „Message from “ );
UsbSerial.print ( szMf );
UsbSerial.print ( „:“ );
UsbSerial.print ( szId );
UsbSerial.print ( “ ‚“ );

byte bL;
char szStr[128];

for ( bL=1; bL<min(bLen,128); bL++)
{
szStr[bL-1] = ReadHex ( FntBuf + bStart + bL * 2, 2 );
}
szStr[bL-1] = 0;

UsbSerial.print ( szStr );
UsbSerial.println ( „‚“ );

break;
}
}
break; // End of reading
}
}
}
}
FntIdx = 0;
}
else
{
if ( FntBuf[0] == ‚#‘ ) FntIdx++;

if ( FntIdx > 255 ) FntIdx = 0;
}
}

/*
* Endless loop …
*/

void loop ()
{
if ( FntSerial.available() ) HandleFnt ();

if ( GpsSerial.available() ) HandleGps ();
}