Modellbau

/*
* FryLink Telemetry Converter Includes
*
* from Burkhard Vogt, Muenster, Westfalia, Germany
*
* These are the defines for FrSky S.Port
*
*/

#define FrSkyPIdAnz 26

const byte FrSkyPId[] =
{
0x00,  // Physical ID  1 – Vario2 (altimeter high precision)
0xA1,  // Physical ID  2 – FLVSS Lipo sensor (can be sent with one or two cell voltages)
0x22,  // Physical ID  3 – FAS-40S current sensor
0x83,  // Physical ID  4 – GPS / altimeter (normal precision)
0xE4,  // Physical ID  5 – RPM
0x45,  // Physical ID  6 – SP2UART(Host)
0xC6,  // Physical ID  7 – SPUART(Remote)
0x67,  // Physical ID  8 –
0x48,  // Physical ID  9 –
0xE9,  // Physical ID 10 –
0x6A,  // Physical ID 11 –
0xCB,  // Physical ID 12 –
0xAC,  // Physical ID 13 –
0x0D,  // Physical ID 14 –
0x8E,  // Physical ID 15 –
0x2F,  // Physical ID 16 –
0xD0,  // Physical ID 17 –
0x71,  // Physical ID 18 –
0xF2,  // Physical ID 19 –
0x53,  // Physical ID 20 –
0x34,  // Physical ID 21 –
0x95,  // Physical ID 22 –
0x16,  // Physical ID 23 –
0xB7,  // Physical ID 24 – Receiver quality ?
0x98,  // Physical ID 25 – Fuel ?
0x39,  // Physical ID 26 –
0xBA,  // Physical ID 27 – Timer ?
0x1B   // Physical ID 28 –
};

/*
* FryLink Telemetry Converter
*
* from Burkhard Vogt, Munster, Westfalia, Germany
*
* Idea and sources from Jochen Tuchbreiter
*
* Connections:
* Vin – Input (Bus) power by RX of MPX
* Gnd
* A1  – Signal wire MLINK
* A3  – Signal LED (green)
* A6  – Setup  LED (yellow)
* A10 – Signal wire FrSky
* A12 – Signal LED (red)
*/

#include <EEPROM.h>

struct Sensor
{
unsigned short uTypID;
short iMin;
short iMax;
byte  bAlert;
short iValue;
byte  bClass;
byte  bBuzz;
unsigned long  ulMsec;
}
SensDef[16];

// #define DEBUG
// #define DEBUG_SETUP
// #define DEBUG_MLINK
// #define DEBUG_FRSKY

// Serial interfaces used

#define DebugSerial Serial
#define MLinkSerial Serial1
#define FrSkySerial Serial2

#define MLinkSerial_C1 UART0_C1
#define MLinkSerial_C3 UART0_C3
#define FrSkySerial_C1 UART1_C1
#define FrSkySerial_C3 UART1_C3

// Some defines for the serial communication

#define FRSKY_POLLING 10 // Time between two polling requests
#define MLINK_DELAY    3 // Time between request and answer

// Some global variables for the LEDs

const byte MLinkLED =  3;  // The green  MLink LED
const byte SetupLED =  6;  // The yellow Setup LED
const byte FrSkyLED = 12;  // The red    FrSky LED

// Some global variables for the setup

#define SETUP_BEGIN  0xBA
#define SETUP_READ   0xBB
#define SETUP_WRITE  0xBC
#define SETUP_END    0xBE
#define SETUP_ESC    0xBF

// Write sensor definitions

void WriteSetting ( byte bIdx )
{
#ifdef DEBUG_SETUP
DebugSerial.print ( „Write setting to EEPROM for “ );
DebugSerial.println ( bIdx );
#endif

byte bI, *bBytes = (byte *) &SensDef[bIdx];

for ( bI=0; bI<8; bI++ )
{
#ifdef DEBUG_SETUP
DebugSerial.print ( bBytes[bI], HEX );
DebugSerial.print ( “ “ );
#endif

if ( EEPROM[bIdx*8+bI] != bBytes[bI] ) EEPROM[bIdx*8+bI] = bBytes[bI];
}

#ifdef DEBUG_SETUP
DebugSerial.println ();
#endif
}

// Sets the right class for given TypID

void SetSensClass ( byte bIdx )
{
switch ( SensDef[bIdx].uTypID )
{
case 0x0200:

SensDef[bIdx].bClass = 2;   // Current

break;

default:

SensDef[bIdx].bClass = 1;   // Voltage
}
return;
}

// Read sensor definitions

void ReadSettings ()
{
byte bI, bIdx = 0;

#ifdef DEBUG_SETUP
DebugSerial.println ( „Read setting from EEPROM“ );
#endif

for ( bI=0; bI<16; bI++)
{
byte bK, *bBytes;

bBytes = (byte *) &SensDef[bI];

#ifdef DEBUG_SETUP
DebugSerial.print ( bIdx );
DebugSerial.print ( “ – “ );
#endif

for ( bK=0; bK<8; bK++ )
{
bBytes[bK] = EEPROM[bIdx++];

#ifdef DEBUG_SETUP
DebugSerial.print ( bBytes[bK], HEX );
DebugSerial.print ( “ “ );
#endif

SetSensClass ( bI );

SensDef[bI].iValue = 0x8000;
SensDef[bI].bBuzz  = 0;
SensDef[bI].ulMsec = 0;
}
#ifdef DEBUG_SETUP
DebugSerial.println ();
#endif
}
}

// Startup function to initialize serial communication,
// global settings, some led blinking and so on

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

// Setup the pin states

pinMode ( MLinkLED, OUTPUT );
pinMode ( FrSkyLED, OUTPUT );
pinMode ( SetupLED, OUTPUT );

// Setup the serial communication

DebugSerial.begin ( 9600 );
MLinkSerial.begin ( 38400 );
FrSkySerial.begin ( 57600, SERIAL_8N1_RXINV_TXINV );

MLinkSerial_C1 |= 0x20 + 0x80; // Single wire mode

#ifdef DEBUG_SETUP
DebugSerial.println ( „Set MLink single wire mode“ );
#endif

FrSkySerial_C1 |= 0x20 + 0x80; // Single wire mode

#ifdef DEBUG_SETUP
DebugSerial.println ( „Set FrSky single wire mode“ );
DebugSerial.println ( „Set FrSky TX/RX invert“ );
#endif

MLinkSerial.clear();
FrSkySerial.clear();

byte bI;

ReadSettings ();

byte bCmd = 0;

for ( bI=0; bI<12; bI++)
{
byte bLed = bI % 3;

digitalWrite ( MLinkLED, bLed == 0 ? HIGH : LOW );
digitalWrite ( SetupLED, bLed == 1 ? HIGH : LOW );
digitalWrite ( FrSkyLED, bLed == 2 ? HIGH : LOW );

bCmd = MLinkSerial.read();

if ( bCmd == SETUP_BEGIN ) break;

delay ( 500 );
}

digitalWrite ( FrSkyLED, LOW );

#ifdef DEBUG_SETUP
DebugSerial.println ( „End of wait for setup start“ );
#endif

if ( bI < 12 && bCmd == SETUP_BEGIN )
{
int nOnOff = HIGH;

digitalWrite ( MLinkLED, HIGH );
digitalWrite ( FrSkyLED, LOW  );

DebugSerial.println ( „Setup loop start“ );

while ( bCmd != SETUP_ESC )
{
switch ( bCmd )
{
case SETUP_BEGIN:
{
#ifdef DEBUG_SETUP
DebugSerial.println ( „Write info message to MLink serial“ );
#endif

MLinkSerial_C3 |= 0x20;

MLinkSerial.write ( SETUP_BEGIN );
MLinkSerial.print ( „FryLink Version  1.0 from Burkhard R. Vogt, Muenster, Germany“ );
MLinkSerial.write ( SETUP_END );
MLinkSerial.flush ();          // make sure send is complete

MLinkSerial_C3 ^= 0x20;

MLinkSerial.clear();          // and clear the pipe

break;
}

case SETUP_READ:  // The PC program will get the EEPROM infos
{
#ifdef DEBUG_SETUP
DebugSerial.println ( „Write setting to MLink serial“ );
#endif

MLinkSerial_C3 |= 0x20;

MLinkSerial.write ( SETUP_READ );

for ( bI=0; bI<16; bI++)
{
byte bK, *bBytes = (byte *) &SensDef[bI];

#ifdef DEBUG_SETUP
DebugSerial.print ( bI );
DebugSerial.print ( “ – “ );
#endif

for ( bK=0; bK<8; bK++ )
{
#ifdef DEBUG_SETUP
DebugSerial.print ( bBytes[bK], HEX );
DebugSerial.print ( “ “ );
#endif

MLinkSerial.write ( bBytes[bK] );
}
#ifdef DEBUG_SETUP
DebugSerial.println ();
#endif
}

MLinkSerial.write ( SETUP_END );

MLinkSerial.flush ();          // make sure send is complete

MLinkSerial_C3 ^= 0x20;

MLinkSerial.clear();          // and clear the pipe

break;
}

case SETUP_WRITE:               // The PC program sends one new sensor info
{
#ifdef DEBUG_SETUP
DebugSerial.println ( „Read setting from MLink serial“ );
#endif

byte bBytes[10];

for ( bI=0; bI<10; bI++ )
{
bBytes[bI] = MLinkSerial.read();

#ifdef DEBUG_SETUP
DebugSerial.print ( bBytes[bI], HEX );
DebugSerial.print ( “ “ );
#endif

}
#ifdef DEBUG_SETUP
DebugSerial.println ();
#endif

if ( bBytes[9] == SETUP_END )
{
byte bIdx = bBytes[0];  // Only for better reading …

#ifdef DEBUG_SETUP
DebugSerial.println ( „New settings OK“ );
#endif

memcpy ( &SensDef[bIdx], bBytes + 1, 8 );

SetSensClass ( bIdx );

WriteSetting ( bIdx );
}
break;
}
}

bCmd = MLinkSerial.read ();

digitalWrite ( SetupLED, nOnOff );

nOnOff = nOnOff == HIGH ? LOW : HIGH;

delay ( 200 );
}
digitalWrite ( SetupLED, LOW );

#ifdef DEBUG_SETUP
DebugSerial.println ( „Setup loop end“ );
#endif
}

// Delete all data in pipes …

MLinkSerial.clear();
FrSkySerial.clear();

digitalWrite ( MLinkLED, LOW );
digitalWrite ( FrSkyLED, LOW );

for ( bI=0; bI<(FrSkyPIdAnz*5); bI++ )
{
HandleFrSky ();
}

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

// Some global variables for the FrSky serial handling

unsigned long ulFrSkyMsec = 0;
byte bFrSkySId   = 0;
byte bFrSkyRead  = 0;
byte bFrSkyByte[8];

// Saves the value read from FrSky in sensor struct for MLink request

void SaveValue ( unsigned int uTypID, short iValue )
{
for ( byte bI = 0; bI < 16; bI++ )
{
if ( SensDef[bI].uTypID == uTypID )
{
SensDef[bI].iValue = iValue;

if ( SensDef[bI].bAlert )
{
if ( iValue < SensDef[bI].iMin )
{
SensDef[bI].bBuzz = 1;
}
else
{
if ( iValue > SensDef[bI].iMax )
{
SensDef[bI].bBuzz = 1;
}
else
{
SensDef[bI].bBuzz = 0;
}
}
}
SensDef[bI].ulMsec = millis ();

#ifdef DEBUG_FRSKY
DebugSerial.print ( „FrSky input value saved as “ );
DebugSerial.println ( bI );
#endif
}
}
return;
}

// Function to handle the FrSky serial communication

void HandleFrSky ()
{
if ( FrSkySerial.available() )
{
// Set LED on when data reading …

if ( ! bFrSkyRead ) digitalWrite ( FrSkyLED, HIGH );

bFrSkyByte[bFrSkyRead] = FrSkySerial.read();

// Check for FrSky stuffing spezial char

if ( bFrSkyByte[bFrSkyRead] == 0x7D )
{
while ( ! FrSkySerial.available() ) { ; }

// Read the „real“ value

bFrSkyByte[bFrSkyRead] = FrSkySerial.read() | 0x20;
}

#ifdef DEBUG_FRSKY
DebugSerial.print ( bFrSkyByte[bFrSkyRead], HEX );
DebugSerial.print ( “ “ );
#endif

if ( bFrSkyRead++ < 7 ) return;

#ifdef DEBUG_FRSKY
DebugSerial.println ();
#endif

// Parameter set is complete

if ( bFrSkyByte[0] == 0x10 )
{
unsigned short uTypID = ( bFrSkyByte[2] << 8 ) + bFrSkyByte[1];

#ifdef DEBUG_FRSKY
DebugSerial.print ( „TypID: “ );
DebugSerial.println ( uTypID, HEX );
#endif

switch ( uTypID )
{
case 0x0110:  // vario         | 0.01m/s | int32
break;

case 0x0200:  // current       | 0.01A   | uint32
{
unsigned long uValue;

memcpy ( &uValue, bFrSkyByte + 3, 4 );

#ifdef DEBUG_FRSKY
DebugSerial.print ( „Current 0.1A: “ );
DebugSerial.println ( uValue / 10 );
#endif

SaveValue ( uTypID, uValue / 10 );

break;
}

case 0x0210:  // voltage       | 0.01V   | uint32
{
unsigned long uValue;

memcpy ( &uValue, bFrSkyByte + 3, 4 );

#ifdef DEBUG_FRSKY
DebugSerial.print ( „Voltage 0.1V: “ );
DebugSerial.println ( uValue / 10 );
#endif

SaveValue ( uTypID, uValue / 10 );

break;
}

case 0x0300:  // voltage       | 0.01V   | uint32
{
unsigned long uValue;

memcpy ( &uValue, bFrSkyByte + 3, 4 );

byte bCell = uValue & 0x0F;     // First cell of two

uValue >>= 4;

byte bCells = uValue & 0x0F;    // Num of cells

uValue >>= 4;

#ifdef DEBUG_FRSKY
DebugSerial.print ( „Voltage Cell “ );
DebugSerial.print ( bCell );
DebugSerial.print ( “ 0.1V: “ );
DebugSerial.println ( ( uValue & 0x0FFF ) / 50 );
#endif

SaveValue ( uTypID + bCell, ( uValue & 0x0FFF ) / 50 );

bCell ++;

if ( bCell < bCells )
{
uValue >>= 12;

#ifdef DEBUG_FRSKY
DebugSerial.print ( „Voltage Cell “ );
DebugSerial.print ( bCell );
DebugSerial.print ( “ 0.1V: “ );
DebugSerial.println ( ( uValue & 0x0FFF ) / 50 );
#endif

SaveValue ( uTypID + bCell, ( uValue & 0x0FFF ) / 50 );
}
break;
}

case 0x0500:  // RPM           | 1rpm    | uint32
break;

case 0x0600:  // capacity used | 1mah    | uint32
break;

case 0x0610:  // altitude      | 1m      | int32
break;

case 0x0830:  // gps speed     | 1km/h   | uint32
break;

#ifdef DEBUG_FRSKY
default:
DebugSerial.println ( „Wrong TypID!“ );
#endif
}
}
else
{
#ifdef DEBUG_FRSKY
DebugSerial.println ( „Wrong byte input!“ );
#endif
}
bFrSkyRead = 0;
}

if ( ulFrSkyMsec < ( millis () – FRSKY_POLLING ) )
{
ulFrSkyMsec = millis ();

FrSkySerial_C3 |= 32;
FrSkySerial.write( 0x7E );
FrSkySerial.write( FrSkyPId[bFrSkySId] );
FrSkySerial.flush();      // make shure send is complete
FrSkySerial_C3 ^= 32;

#ifdef DEBUG_FRSKY
DebugSerial.print ( „Sending FrSky request “ );
DebugSerial.print ( bFrSkySID );
DebugSerial.print ( “ “ );
DebugSerial.print ( FrSkyPID[bFrSkySID], HEX );
DebugSerial.println ();
#endif

if ( bFrSkySId++ > FrSkyPIdAnz ) bFrSkySId = 0;

bFrSkyRead = 0;

digitalWrite ( FrSkyLED, LOW );
}
}

// Function to handle the MLink serial communication

void HandleMLink ()
{
byte bMLinkID = MLinkSerial.read();

if ( bMLinkID < 16 )
{
#ifdef DEBUG_MLINK
DebugSerial.print ( bMLinkID, HEX );
DebugSerial.print ( “ “ );
#endif

delay ( 3 );

// For secure, if another sensor uses the same ID

if ( ! MLinkSerial.available() )
{
// Is this sensor ID defined?

if ( SensDef[bMLinkID].uTypID )
{
digitalWrite ( MLinkLED, HIGH );

byte  bRet = ( bMLinkID << 4 ) + SensDef[bMLinkID].bClass;
short iVal;

// Check if the last response is to old !

if ( SensDef[bMLinkID].ulMsec < ( millis () – FRSKY_POLLING * 100 ) )
{
iVal = 0x8000;
}
else
{
iVal = SensDef[bMLinkID].iValue * 2;

if ( SensDef[bMLinkID].bAlert ) iVal += SensDef[bMLinkID].bBuzz;
}

MLinkSerial_C3 |= 0x20;

MLinkSerial.write ( bRet );   // send sensor address and typ
MLinkSerial.write ( lowByte  ( iVal ) );
MLinkSerial.write ( highByte ( iVal ) );
MLinkSerial.flush ();          // make sure send is complete

MLinkSerial_C3 ^= 0x20;

#ifdef DEBUG_MLINK
DebugSerial.print ( „MLink response“ );
#endif

digitalWrite ( MLinkLED, LOW );
}
}
else
{
byte bReq = 0;

while ( MLinkSerial.available() )
{
#ifdef DEBUG_MLINK
DebugSerial.print ( MLinkSerial.read(), HEX );
DebugSerial.print ( “ “ );
#else
MLinkSerial.read();
#endif

if ( bReq++ > 2 ) break;
}
}
}
else
{
#ifdef DEBUG_MLINK
DebugSerial.print ( „!- “ );
DebugSerial.print ( bMLinkID, HEX );
DebugSerial.print ( “ “ );
#endif

while ( MLinkSerial.available() )
{
#ifdef DEBUG_MLINK
DebugSerial.print ( MLinkSerial.read(), HEX );
DebugSerial.print ( “ “ );
#else
MLinkSerial.read();
#endif
}
}
#ifdef DEBUG_MLINK
DebugSerial.println ();
#endif
}

// The endless loop

void loop ()
{
while ( MLinkSerial.available() ) { HandleMLink (); }

HandleFrSky (); // Time for the FrSky S.Port
}

/*
* Multiplex GPS Sensor
*
* from Burkhard Vogt, Muenster, Westfalia, Germany
*
* Connections:
* Vin – Input (Bus) power by RX of MPX
* Gnd
* A1  – Serial wire MPX
* A3  – Signal LED (green)
* A6  – GPS LED (red)
* A10 – TX GPS
*/

#include <EEPROM.h>
#include <string.h>
#include <ctype.h>
#include <math.h>

const double M_RO = 180.0 / M_PI;

#define DebSerial Serial
#define MpxSerial Serial1
#define GpsSerial Serial2

#define MpxSerial_C1 UART0_C1
#define MpxSerial_C3 UART0_C3

// Some defines for the serial communication

#define MPX_DELAY    0 // Time between request and answer

// Some define for the start position

#define POS_DELAY   20 // Position sentence to wait for start
#define MID_DELAY    5 // Position sentence to middel (must lower than wait)

// the minimal speed to start

#define MIN_SPEED  5.0 // Minimum start speed

// Some global variables for the LEDs

const byte GpsLED =  3;  // The yellow GPS LED
const byte MpxLED =  6;  // The green Mpx LED

/* Sensor definitions

Types are:
– 01 – Speed
– 02 – Hight
– 03 – Distance
– 04 – Sattelites in view
– 05 – Max. Speed
– 06 – Max. Hight
– 07 – Max. Distance
– 08 – Max. Sattelites in view */

struct Sensor
{
byte  bTyp;
byte  bAlert;
short iValue;
byte  bClass;
byte  bBuzz;
}
SensDef[16];

byte SensRef[8]; // Reference for the real values (types)

// #define DEBUG
// #define DEBUG_SETUP
// #define DEBUG_MPX
// #define DEBUG_GPS

// Startadresse der Sensorausgabe
#define SADR 7

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

for ( byte bI=0; bI<10; bI++ )
{
SensDef[bI].bTyp = 0;
}

SensDef[SADR+0].bTyp   =      1; // Speed
SensDef[SADR+0].bAlert =      0; // No alarm
SensDef[SADR+0].iValue = 0x8000; // No value
SensDef[SADR+0].bClass =      4; // Speed 0,1km/h
SensDef[SADR+0].bBuzz  =      0; // No buzzer

SensDef[SADR+1].bTyp   =      2; // Hight
SensDef[SADR+1].bAlert =      0; // No alarm
SensDef[SADR+1].iValue = 0x8000; // No value
SensDef[SADR+1].bClass =      8; // Hight 1m
SensDef[SADR+1].bBuzz  =      0; // No buzzer

SensDef[SADR+2].bTyp   =      3; // Distance
SensDef[SADR+2].bAlert =      0; // No alarm
SensDef[SADR+2].iValue = 0x8000; // No value
SensDef[SADR+2].bClass =      8; // short distance 1m
SensDef[SADR+2].bBuzz  =      0; // No buzzer

SensDef[SADR+3].bTyp   =      4; // Sattelites
SensDef[SADR+3].bAlert =      0; // No alarm
SensDef[SADR+3].iValue = 0x8000; // No value
SensDef[SADR+3].bClass =     15; // Num. Sattelites
SensDef[SADR+3].bBuzz  =      0; // No buzzer

SensDef[SADR+4].bTyp   =      5; // Speed
SensDef[SADR+4].bAlert =      0; // No alarm
SensDef[SADR+4].iValue = 0x8000; // No value
SensDef[SADR+4].bClass =      4; // Speed 0,1km/h
SensDef[SADR+4].bBuzz  =      0; // No buzzer

SensDef[SADR+5].bTyp   =      8; // Distance
SensDef[SADR+5].bAlert =      0; // No alarm
SensDef[SADR+5].iValue = 0x8000; // No value
SensDef[SADR+5].bClass =      8; // Short distance 1m
SensDef[SADR+5].bBuzz  =      0; // No buzzer

for ( byte bI=0; bI<8; bI++ )
{
SensRef[bI] = 0;
}

for ( byte bI=0; bI<16; bI++ )
{
if ( SensDef[bI].bTyp ) SensRef[SensDef[bI].bTyp-1] = bI + 1;
}

// Setup the pin states

pinMode ( MpxLED, OUTPUT );
pinMode ( GpsLED, OUTPUT );

// Setup the serial communication

MpxSerial.begin ( 38400 );
GpsSerial.begin (  9600 );

MpxSerial_C1 |= 0x20 + 0x80; // Single wire mode

for ( byte bI=0; bI<6; bI++)
{
byte bLed = bI % 2;

digitalWrite ( MpxLED, bLed == 0 ? HIGH : LOW );
digitalWrite ( GpsLED, bLed == 1 ? HIGH : LOW );

delay ( 500 );
}

digitalWrite ( MpxLED, LOW );
digitalWrite ( GpsLED, LOW );

MpxSerial.clear();
GpsSerial.clear();

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

char GpsBuf[256];
byte GpsIdx = 0;

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

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 );
}

float  fHgt, fHgtS = 0.0;
double dLat, dLon, dLatS = 0.0, dLonS = 0.0, dRad;
float  fHight, fHightM = 0.0, fSpeed, fSpeedM = 0.0, fDist, fDistM = 0.0;
byte   bFix = 0, bSat = 0, bSatM = 0, bVal = 0;

byte   bMpxLED = 0;
byte   bGpsLED = 0;
byte   bPosCnt = 0;

// Function to handle the Mpx serial communication

void HandleMpx ()
{
byte bMLinkID = MpxSerial.read();

if ( bMLinkID < 16 )
{
#ifdef DEBUG_MPX
DebSerial.print ( bMLinkID, HEX );
DebSerial.print ( “ “ );
#endif

delay ( MPX_DELAY );

if ( ! bMLinkID )
{
if ( bMpxLED )
{
digitalWrite ( MpxLED, LOW );

bMpxLED = 0;
}
else
{
digitalWrite ( MpxLED, HIGH );

bMpxLED = 1;
}
}
if ( bMLinkID == 7 )
{
if ( ! bFix || ! bSat || ! bVal ||  bPosCnt < POS_DELAY )
{
digitalWrite ( MpxLED, LOW );

bMpxLED = 0;
}
}

// For secure, if another sensor uses the same ID

if ( ! MpxSerial.available() )
{
// Is this sensor ID defined?

if ( SensDef[bMLinkID].bTyp )
{
byte  bRet = ( bMLinkID << 4 ) + SensDef[bMLinkID].bClass;

short iVal;

if ( bFix && bSat && bVal && bPosCnt >= POS_DELAY )
{
iVal = SensDef[bMLinkID].iValue * 2;
}
else
{
iVal = 0x8000;
}

if ( SensDef[bMLinkID].bAlert ) iVal += SensDef[bMLinkID].bBuzz;

MpxSerial_C3 |= 0x20;

MpxSerial.write ( bRet );   // send sensor address and typ
MpxSerial.write ( lowByte  ( iVal ) );
MpxSerial.write ( highByte ( iVal ) );
MpxSerial.flush ();         // make sure send is complete

MpxSerial_C3 ^= 0x20;

#ifdef DEBUG_MPX
DebSerial.print ( „MLink response“ );
#endif
}
}
else
{
byte bReq = 0;

while ( MpxSerial.available() )
{
#ifdef DEBUG_MPX
DebSerial.print ( MpxSerial.read(), HEX );
DebSerial.print ( “ “ );
#else
MpxSerial.read();
#endif

if ( bReq++ > 2 ) break;
}
}
}
else
{
#ifdef DEBUG_MPX
DebSerial.print ( „!- “ );
DebSerial.print ( bMLinkID, HEX );
DebSerial.print ( “ “ );
#endif

#ifdef DEBUG_MPX
while ( MpxSerial.available() )
{
DebSerial.print ( MpxSerial.read(), HEX );
DebSerial.print ( “ “ );
}
#else
if ( MpxSerial.available() ) MpxSerial.clear();
#endif
}
#ifdef DEBUG_MPX
DebSerial.println ();
#endif
}

void SaveVal ( byte bTyp, short iVal )
{
if ( ! SensRef[bTyp-1] ) return;

SensDef[SensRef[bTyp-1]-1].iValue = iVal;
}

// Function to handle the GPS serial communication

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

if ( GpsBuf[GpsIdx] == 255 ) return;

if ( GpsBuf[GpsIdx] == 10 || GpsBuf[GpsIdx] == 13 )
{
if ( GpsIdx > 3 )
{
#ifdef DEBUG_GPS
GpsBuf[GpsIdx+1] = 0;
#endif

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
DebSerial.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 )
{
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 41:  if ( bGpsLED )
{
digitalWrite ( GpsLED, LOW );

bGpsLED = 0;
}
else
{
if ( bFix && bSat && bVal )
{
digitalWrite ( GpsLED, HIGH );

bGpsLED = 1;
}
}
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
DebSerial.print   ( „Lat   “ );
DebSerial.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
DebSerial.print   ( „Lon   “ );
DebSerial.println ( dLon, 4 );
#endif

break;

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

break;

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

break;

case 27:  bSat = atoi ( GpsBuf + bStart );

if ( bSat > bSatM ) bSatM = bSat;

SaveVal ( 4, min ( bSat,  10 ) );
SaveVal ( 8, min ( bSatM, 10 ) );

#ifdef DEBUG_GPS
DebSerial.print   ( „Sats  “ );
DebSerial.println ( bSat );
#endif

break;

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

if ( bFix && bSat && bVal && bPosCnt >= POS_DELAY )
{
fHight = fHgt – fHgtS;

if ( fHight > fHightM ) fHightM = fHight;

#ifdef DEBUG_GPS
DebSerial.print   ( „Hight “ );
DebSerial.println ( fHight, 1 );
#endif
}
else
{
fHight = 0x8000;
}

SaveVal ( 2, max ( 0, fHight  ) );
SaveVal ( 6,          fHightM   );
break;

case 47:  if ( bFix && bSat && bVal && bPosCnt >= POS_DELAY )
{
fSpeed = atof ( GpsBuf + bStart ) * 1.852;

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

if ( fSpeed < MIN_SPEED )
{
fSpeed = 0.0;
}
else
{
if ( fSpeed > fSpeedM ) fSpeedM = fSpeed;
}
}
else
{
fSpeed = 0x8000;
}

SaveVal ( 1, fSpeed  * 10 );
SaveVal ( 5, fSpeedM * 10 );

#ifdef DEBUG_GPS
case 21:
case 28:
case 48:
case 49:
break;
default:  DebSerial.print   ( bCmd + bParam );
DebSerial.print   ( „-“ );
DebSerial.println ( GpsBuf+bStart );
#endif
}
if ( bParam > 8 ) break;  // No more interesting params …

bStart = bI + 1;
}
}

if ( bFix && bSat && bVal )
{
if ( bPosCnt < POS_DELAY )
{
bPosCnt ++;

if ( bPosCnt > ( POS_DELAY – MID_DELAY ) )
{
#ifdef DEBUG_GPS
DebSerial.print   ( „Save start position … “ );
DebSerial.println ( bPosCnt – POS_DELAY + MID_DELAY );
#endif

fHgtS += fHgt / MID_DELAY;
dLatS += dLat / MID_DELAY;
dLonS += dLon / MID_DELAY;

if ( bPosCnt ==  POS_DELAY )
{
dRad = 111300 * cos ( dLatS / M_RO );

#ifdef DEBUG_GPS
DebSerial.println ( „Startpoint“ );
DebSerial.print   ( „Lat   “ );
DebSerial.println ( dLatS, 4 );
DebSerial.print   ( „Lon   “ );
DebSerial.println ( dLonS, 4 );
DebSerial.print   ( „Hight “ );
DebSerial.println ( fHgtS, 1 );
DebSerial.print   ( „Rad m “ );
DebSerial.println ( dRad,  0 );
#endif
}
}
}
else
{
#ifdef DEBUG_GPS
DebSerial.println ( „Calculate distance“ );
#endif

double dX = ( dLon – dLonS ) * dRad;
double dY = ( dLat – dLatS ) * 111300;

fDist = sqrt ( dX * dX + dY * dY );

if ( fDist <= 2000 )
{
if ( fDist > fDistM ) fDistM = fDist;
}
else
{
fDist = 0x8000;
}

SaveVal ( 3, fDist  );
SaveVal ( 7, fDistM );

#ifdef DEBUG_GPS
DebSerial.print   ( „Dist  “ );
DebSerial.println ( fDist, 2 );
#endif
}
}
}
}
GpsIdx = 0;
}
else
{
if ( GpsBuf[0] == ‚$‘ ) GpsIdx++;

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

void loop ()
{
if ( MpxSerial.available() ) HandleMpx ();

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

/*
* Multiplex Telemetry Display Logger
*
* from Burkhard Vogt, Munster, Westfalia, Germany
*
* Connections:
* Vin – Power by TX (HFM3) of MPX
* Gnd
* A1  – Signal wire MLINK
*
* SD card attached to SPI bus as follows:
* MOSI – pin 11
* MISO – pin 12
* CLK  – pin 13
* CS   – pin 4
*
* A1  – Signal wire MLINK
* A3  – Signal LED (green)
* A6  – Setup  LED (yellow)
*/

#include <SPI.h>
#include <SD.h>

struct SensDef
{
byte  bClass;
short iValue;
}
Sensor[16];

char szFileName[13];

// #define DEBUG
// #define DEBUG_FILE
// #define DEBUG_MLINK

// Serial interfaces used

#define Debug Serial
#define MLink Serial1
#define SENS_WAIT 100

const byte GreenLED  =  3;  // The green  LED
const byte YellowLED =  6;  // The yellow LED

byte bBuffer[20], bBufIdx = 0;
int  iMinA =  20; // Min power to start (1/10A)
int  iMinR = 500; // Min rmp to start
char cDeci = ‚,‘; // Decimal in Excel CSV

void setup()
{
// Setup the pin states

pinMode ( GreenLED, OUTPUT );
pinMode ( YellowLED, OUTPUT );

digitalWrite ( GreenLED,  HIGH );
digitalWrite ( YellowLED, HIGH );

// For best compatiblity with all SD cards

pinMode (  4, INPUT_PULLUP );
pinMode ( 10, INPUT_PULLUP );

delay (1);  // allow time for both pins to reach 3.3V

#ifdef DEBUG_FILE
Debug.begin ( 9600 );
while ( ! Serial ) {;}
#endif

#ifdef DEBUG_FILE
Debug.print ( „Initializing SD card …“ );
#endif

if ( ! SD.begin ( 4 ) )
{
#ifdef DEBUG_FILE
Debug.println ( “ failed!“ );
#endif
return;
}
#ifdef DEBUG_FILE
Debug.println ( “ done.“ );
#endif

File fRoot = SD.open ( „/“ );
File fEntry;

unsigned int uLastIdx = 0;

while ( fEntry = fRoot.openNextFile () )
{
if ( ! fEntry.isDirectory())
{
if ( strlen ( fEntry.name() ) == 12 )
{
strcpy ( szFileName, fEntry.name() );

if ( ! strncmp ( szFileName,      „LOG“, 3 ) &&
! strncmp ( szFileName + 8, „.CSV“, 4 ) )
{
unsigned int uIdx = atoi ( szFileName + 3 );

if ( uLastIdx < uIdx ) uLastIdx = uIdx;
}
}
}
fEntry.close();
}

sprintf ( szFileName, „LOG%05u.CSV“, ++uLastIdx );

#ifdef DEBUG_FILE
Debug.print   ( „Filename “ );
Debug.println ( szFileName );
#endif

File SetFile = SD.open ( „SETTINGS.TXT“, FILE_READ );

if ( SetFile )
{
byte bByte;

do
{
bByte = SetFile.read();

if ( bByte == 0    ||
bByte == 0xFF ||
bByte == ‚\n‘ ||
bByte == ‚\r‘ )
{
bBuffer[bBufIdx] = 0;

if ( strlen ( (char *) bBuffer ) )
{
#ifdef DEBUG_FILE
Debug.println ( (char *) bBuffer );
#endif

if ( strncmp ( (char *) bBuffer, „MinA=“, 5 ) == 0 )
{
iMinA = atoi ( (char *) bBuffer + 5 );
#ifdef DEBUG_FILE
Debug.println ( „MinA->“ );
Debug.println ( iMinA );
#endif
}
else if ( strncmp ( (char *) bBuffer, „MinR=“, 5 ) == 0 )
{
iMinR = atoi ( (char *) bBuffer + 5 );
#ifdef DEBUG_FILE
Debug.println ( „MinR->“ );
Debug.println ( iMinR );
#endif
}
else if ( strncmp ( (char *) bBuffer, „Deci=“, 5 ) == 0 )
{
cDeci = bBuffer[5];

if ( cDeci != ‚.‘ || cDeci != ‚,‘ ) cDeci = ‚,‘;
#ifdef DEBUG_FILE
Debug.println ( „Deci“ );
Debug.println ( cDeci );
#endif
}
}
bBufIdx = 0;
}
else
{
if ( bBufIdx < 18 )
{
bBuffer[bBufIdx++] = bByte;
}
}
} while ( bByte != 0xFF );

SetFile.close ();
}
else
{
File SetFile = SD.open ( „SETTINGS.TXT“, FILE_WRITE );

if ( SetFile )
{
Debug.println ( „MinA=20“  );
Debug.println ( „MinR=500“ );
Debug.println ( „Deci=,“   );

SetFile.close ();
}
}

bBufIdx = 0;

MLink.begin ( 38400 );
MLink.clear ();

digitalWrite ( GreenLED, LOW );
}

byte bLastIdx = 0;
byte bWriteSet = 0;
byte bToggle = 0;
unsigned int  uWait = 0;
unsigned long ulMillis;

void ToggleLED ( byte LEDon, byte LEDoff )
{
if ( bToggle )
{
bToggle = 0;

digitalWrite ( LEDon, LOW );
}
else
{
bToggle = 1;

digitalWrite ( LEDon, HIGH );
}
digitalWrite ( LEDoff, LOW );
}

void HandleOutput ( void )
{
if ( uWait > SENS_WAIT )
{
if ( bWriteSet )
{
ToggleLED ( GreenLED, YellowLED );

File LogFile = SD.open ( szFileName, FILE_WRITE );

if ( LogFile )
{
char szOutput[12];

unsigned long ulDM   = ( millis () – ulMillis ) / 100; // 1/10 sek
unsigned int  uiSM   = ulDM % 10;
ulDM  /=        10;
unsigned int  uiSec  = ulDM % 60;
ulDM  /=        60;
unsigned int  uiMin  = ulDM % 60;
ulDM  /=        60;

sprintf ( szOutput, „%02lu:%02u:%02u%c%01u;“, ulDM, uiMin, uiSec, cDeci, uiSM );

LogFile.print ( szOutput );

for ( byte bI=0; bI<16; bI++ )
{
switch ( Sensor[bI].bClass )
{
case  1:
case  2:
case  3:
case  4:
case  6:
case  7:
case 13:

sprintf ( szOutput, „%i%c%i;“, Sensor[bI].iValue / 10, cDeci, Sensor[bI].iValue % 10 );
LogFile.print ( szOutput );
break;

case  5:

if ( Sensor[bI].iValue < 0 )
{
LogFile.print ( abs ( Sensor[bI].iValue * 10 ) );
}
else
{
LogFile.print ( Sensor[bI].iValue * 100 );
}
LogFile.print ( „;“ );
break;

case  8:
case  9:
case 10:
case 11:
case 12:
case 14:
case 15:

LogFile.print ( Sensor[bI].iValue );
LogFile.print ( „;“ );
}
}
LogFile.println ();
LogFile.close ();
}
}
else
{
ToggleLED ( YellowLED, GreenLED );
}
}
else
{
if ( uWait == SENS_WAIT )
{
ToggleLED ( GreenLED, YellowLED );

ulMillis = millis ();

File LogFile = SD.open ( szFileName, FILE_WRITE );

if ( LogFile )
{
LogFile.print ( „Zeit (1/10s);“ );

for ( byte bI=0; bI<16; bI++ )
{
switch ( Sensor[bI].bClass )
{
case  1: LogFile.print ( „Spannung (V);“ );           break;
case  2: LogFile.print ( „Strom (A);“ );              break;
case  3: LogFile.print ( „Steigen/Sinken (m/s);“ );   break;
case  4: LogFile.print ( „Geschwindigkeit (km/h);“ ); break;
case  5: LogFile.print ( „Drehzahl (1/min);“ );       break;
case  6: LogFile.print ( „Temperatur (\xB0\x43);“ );  break;
case  7: LogFile.print ( „Richtung (Grad \xB0);“ );   break;
case  8: LogFile.print ( „H\xF6he/Distanz (m);“ );    break;
case  9: LogFile.print ( „F\xFCllstand (%);“ );       break;
case 10: LogFile.print ( „LQI (%);“ );                break;
case 11: LogFile.print ( „Stromverbrauch (mAh);“ );   break;
case 12: LogFile.print ( „Fl\xFCssigkeiten (mL);“ );  break;
case 13: LogFile.print ( „Distanz (km);“ );           break;
case 14: LogFile.print ( „unbekannt;“ );              break;
case 15: LogFile.print ( „Wert;“ );
}
}
LogFile.println ();
LogFile.close ();
}
}
else
{
ToggleLED ( YellowLED, GreenLED );
}
uWait ++;
}
}

void HandleSensor ( void )
{
byte bIdx   = bBuffer[0];
byte bClass = bBuffer[1] & 0x0F;

// If wait long enough
// Write the header or the values …

if ( bLastIdx > bIdx )
{
HandleOutput ();

bWriteSet = 0;    // Reset Outputflag
}

bLastIdx = bIdx;

// Save the new sensor values

if ( uWait <= SENS_WAIT )
{
Sensor[bIdx].bClass = bClass;
}
else
{
if ( Sensor[bIdx].bClass != bClass ) return;
}

memcpy ( &Sensor[bIdx].iValue, bBuffer + 2, 2 );

Sensor[bIdx].iValue /= 2;

if ( Sensor[bIdx].bClass == 2 )
{
if ( iMinA <= Sensor[bIdx].iValue ) bWriteSet = 1;
}
else if ( Sensor[bIdx].bClass == 5 )
{
if ( Sensor[bIdx].iValue < 0 )
{
if ( iMinR <= abs ( Sensor[bIdx].iValue *  10 ) ) bWriteSet = 1;
}
else
{
if ( iMinR <=     ( Sensor[bIdx].iValue * 100 ) ) bWriteSet = 1;
}
}

#ifdef DEBUG
if ( bWriteSet )
{
Debug.print ( bBuffer[0], HEX );
Debug.print ( “ “ );
Debug.print ( bBuffer[1], HEX );
Debug.print ( “ “ );
Debug.print ( bBuffer[2], HEX );
Debug.print ( “ “ );
Debug.print ( bBuffer[3], HEX );
Debug.println ();
}
#endif

return;
}

void loop()
{
if ( ! MLink.available() ) return;  // Nothing to do !

bBuffer[0] = MLink.read();

if ( bBuffer[0] <= 0x0F )
{
#ifdef DEBUG_MLINK
Debug.print ( bBuffer[0], HEX );
Debug.print ( “ “ );
#endif

// For secure, we wait for a full data set

delay ( 4 );

if ( MLink.available() )
{
bBufIdx = 1;

while ( MLink.available() )
{
bBuffer[bBufIdx] = MLink.read();

#ifdef DEBUG_MLINK
Debug.print ( bBuffer[bBufIdx], HEX );
Debug.print ( “ “ );
#endif

if ( ++bBufIdx > 3 ) break;
}

if ( bBufIdx == 4 && bBuffer[0] == ( bBuffer[1] >> 4 ) )
{
#ifdef DEBUG_MLINK
Debug.print ( „OK“ );
#endif

HandleSensor ();
}
}
}
else if ( bBuffer[0] == 0x5A )
{
#ifdef DEBUG_MLINK
Debug.print ( „!- Reset“ );
#endif
}
else if ( bBuffer[0] >= 0x80 && bBuffer[0] <= 0x8F )
{
#ifdef DEBUG_MLINK
Debug.print ( „!- System Function“ );
#endif

MLink.clear();
}
else
{
#ifdef DEBUG_MLINK
Debug.print ( „!- “ );
Debug.print ( bBuffer[0], HEX );
Debug.print ( “ “ );
while ( MLink.available() )
{
Debug.print ( MLink.read(), HEX );
Debug.print ( “ “ );
}
#else
MLink.clear();
#endif
}
#ifdef DEBUG_MLINK
Debug.println ();
#endif
}