GPS Secrets

Ein sehr altes Projekt, für einen Z180 Controller.
Alles abstripen was man nicht braucht.
Darin enthalten:
GPS DNMEA Datenzerlegung
Grafik LCD
Zeichensatz
Elektronisches Kompassmodul
Funkprotokoll für 70cm
UND, die Gaus-Krüger Transformation mit Vektor Berechnung
---weiter unten..

http://calc.gknavigation.de/
http://de.wikipedia.org/wiki/Gau%C3%9F-Kr%C3%BCger-Koordinatensystem
http://pages.globetrotter.net/roule/utmgoogle.htm
Lese auch http://forum.zusi.de/viewtopic.php?f=18&t=8162

CODE
Xe = tx + ((1+s) - rz * ry) * Xa
Ye = ty + (rz * (1+s) - rx) * Ya
Ze = tz + (-ry * rx * (1+s)) * Za

tx = 591,28 ty = 81,35 tz = 396,39 alles [m]
s = 1,00000982
rx = -1,477 ry = 0,0736 rz = 1,458 in ["]
Wenn ich nun mit folgenden Werten rechne
Xa = 51,323372°
Ya = 8,353184°
bekomme ich folgendes raus
Xe = 688,419799°
Ye = 118,145657°

Richtig wäre allerdings etwas im 51,...° 8,...° Bereich
Habe ich noch irgendwas übersehen oder liege ich völlig falsch?

Die Umrechnung von Gauß-Krüger/Potsdam in geografisch/WGS84 besteht aus folgenden Schritten:

Rückprojektion Gauß-Krüger in geografisch
Transformation geografisch in geozentrisch
Helmert-Transformation für geodätisches Datum
Rücktransformation geozentrisch in geografisch

Habe in der Zwischenzeit zwar schon eine Molodensky Transformation hinbekommen (welche mich von der Genauigkeit der Ergebnisse leider nicht überzeugen kann), aber bei der Helmert Transformation habe ich immernoch so meine Probleme.

Genauer gesagt habe ich Probleme mit den Formeln für die umrechnung geografischer in geozentrische Koordinaten und entsprechend umgekehrt.
a = 6377397.155;
b = 6356078.96282;

f = (a - b) / a;
e2 = 2 * f - f^2
N = a / Sqrt(1 - e2 * sin^2(hw));

X = (N + h) * cos(hw) * cos(rw)
Y = (N + h) * cos(hw) * sin(rw)
Z = (b^2 / a^2) * N + h) * sin(hw)

Folgendes sind typische Eingangs- und Ausgabewerte
rw = 3520575,062 X = 4268789,637253819
hw = 5730679,045 Y = -4259561,0700920643
h = 0,0 Z = -1613276.7350094349

pafi10_8.c

/*
    Geschrieben für: Z180 Controller
    Autor: Christof Ermer
    Last change: 05.Juli 1999

    ** DELETE runwatch();    for Distribution **

Beachte das für eine Tastatur != DIN 0..3 = 4 Bit in Y
die Ausblendmaske für "Tasten gelöst = 00001111) angepasst werden muß
Angaben direkt an Zeilenanfang sind Testausgaben und könne abgeschaltet werden.
*/

//PREPARE BORD LP3100
/*
REMOVE R9 for long Wachtdog time
REMOVE R40 if use SRAM/RTC Backup Battery 3V
Change 3 Jumpers for TTL-Level of TX1/RX1
*/

//** WICHTIG ** BOARD FESTLEGEN *****
#define BOARD_TYPE LP31xx
//** WICHTIG ** BOARD FESTLEGEN *****

//#use xmem.lib
//#use eziolp31.lib
#use tl_lp31.lib
#use aascz0.lib        //use RS232
#use aascz1.lib        //use RS232/RS485
//#use tl.lib
#use string.lib
#use math.lib
//#use stdio.lib
//#use circbuf.lib
//#use drivers.lib
//#use bios.lib
//#use fvcom.lib
//#use fvsirf1.lib
//#use vdriver.lib    //COSTATEMENTS - Vertraegt sich nicht mit timer
//#use MODEM232.LIB //resetZ180Int

/***************/
/** DEFINES **/
/***************/

//#define DEVELOPMENTSTATUS_ENABLE

#define OCTALMOD    0x8        //8=Mathematischer REST MOD = Z„hler % MOD
#define NIPPLEMOD    0x10        //16=Mathematischer REST MOD = Z„hler % MOD
#define BYTEMOD    0x100        //256=Mathematischer REST MOD = Z„hler % MOD
#define WORDMOD    0x10000    //65536=Mathematischer REST MOD = Z„hler % MOD

#define BIT0    0x1
#define BIT1    0x2
#define BIT2    0x4
#define BIT3    0x8
#define BIT4    0x10
#define BIT5    0x20
#define BIT6    0x40
#define BIT7    0x80
#define BIT8    0x100
#define BIT9     0x200
#define BIT10    0x400
#define BIT11    0x800
#define BIT12    0x1000
#define BIT13    0x2000
#define BIT14    0x4000
#define BIT15    0x8000

#define ASCII_LF         0x0A
#define ASCII_CR         0x0D
#define ASCII_ETX        0x03
#define ASCII_SPACE    0x20

//Hardware defintion section
//ADRESSEN Shutdown
#define TX_RS232        0x4082
#define RX_RS232        0x4083
#define CTS_RS_RS232    0x4084
#define TX_RS485        0x40A1
#define RX_RS485        0x40A2
#define ADC_REF_ENABLE    0x40A5

//Adressen der Z_World Outarea
#define ADR_DS0 0x8000
#define ADR_DS1 0x8080
#define ADR_DS2 0x8100
#define ADR_DS3 0x8180
#define ADR_DS4 0x8200
#define ADR_DS5 0x8280
#define ADR_DS6 0x8300
#define ADR_DS7 0x8380

//Z_WORLD_1300-HARDWARE
#define DINOUT_OUTPORT 0x4040
#define DINOUT_INPORT 0x4020
#define DOUT_PORT 0x4060
#define DIN_PORT 0x4040        //Nur 4 Bit 0..3   obere 4 Bit Maskieren oder ignorieren
#define DINOUT_OUT_ENABLE 0x40A4 ///OE on 74574. 0=Freigabe 1=Sperren des 74574 on Z_CARD
#define MBR_ADR_OFFON_01    0x4081    //MasterBufferedReset_Enable --> Bufferdreset on /BRESET
#define ZW_LED_OFFON_01 0x40A0

//LLL
//LCD_HARDWARE
#define LCD_ROWS 8
#define LCD_COLS 21
#define LCDCHARMAX 0x5F
#define LCDCHARROWSUM (LCDCHARMAX * 6) //0x5F*6 ->6*8Font
#define LCD_CMD_PRT DOUT_PORT            //CS3W Z-WORLD

#define LCD_DATAOUT_PRT    DINOUT_OUTPORT    //CS2W Z-WORLD //Muss zum Lesen des LCD_DATAIN_PRT
#define LCD_DATAIN_PRT    DINOUT_INPORT        //CS1R Z-WORLD    // gesperrt = 0 gesetzt werden.
#define LCD_DI    7    //Bit7   DOUT
#define LCD_RW    6    //BIT6
#define LCD_E        3    //BIT3
//******SONDERBEHANDLUNG IM PROTOYPE***************
// WORK : ANPASSUNG IN REDESIGN
#define LCD_CS1    5    //Bit 5
#define LCD_CS2    2    //Bit 2
#define LCD_RES    4    //Bit 4
//*******************************
#define LCD_BUSY_FLAG        0x80
#define LCD_RESET_FLAG    0x10
#define LCD_ONOFF_FLAG    0x20
#define LCD_CMD_ON    0x3F
#define LCD_CMD_OFF    0x3E
#define LCD_CMD_STARTZEILE 0xC0 // + || MASKARDESSE Y 0..63
#define LCD_CMD_PAGE    0xB8             // + || MASKARDESSE X 0..7
#define LCD_CMD_SET_Y_ADR    0x40    // + || MASKARDESSE Y0..63

#define COMP_CTRL_OUT    ADR_DS2
#define COMP_CTRL_IN    ADR_DS2
//COMPASS-DEFINES
#define COPA_O_CI        0    //Calibrate indicater
#define COPA_O_EOC    1    //uses Bit/Pin Nr
#define COPA_O_SDO    2
#define COPA_I_RESET    0
#define COPA_I_SS        1
#define COPA_I_CAL    3
#define COPA_I_PC        4 //I= Input compass BIT 0 and so on
#define COPA_I_SCLK    5

#define COMPASSTIMEOUT 150 //ms

//Logische Festlegungen
#define GPS_COM_PORT            0             //Z0
#define PWR_CTRL_IN             ADR_DS2
#define GPS_CTRL_OUT             ADR_DS2
#define GPS_CTRL_In             ADR_DS2
#define GPS_PWR_OFFON_01    7    //Bit 7 on ADR_DS2
#define GPS_WakeUpN_Bit     5 // on ADR_DS2
#define LBO5V_PWR_CTRL_Bit    3    // on ADR_DS2
#define LBO3V_PWR_CTRL_Bit    4    // on ADR_DS2

#define MODEM_PORT    1    //Z1
#define TXBUFSIZE    0x100
#define RXBUFSIZE    0x400
#define STRSIZE        0x200

#define COM_MUX_ADR0         DOUT0    //DUAL MUX 4052 ADRbit
#define COM_MUX_ADR1         DOUT1    //DUAL MUX 4052 ADRbit
#define GPS_T0_COM_MUX    0    //DUAL MUX 4052 Channel
#define GPS_T1_COM_MUX    1    //DUAL MUX 4052 Channel
#define BCD__COM_MUX        3    //DUAL MUX 4052 Channel

/******************/
/** ENDE DEFINES **/
/******************/

/***************/
/** TYPEDEFS **/
/***************/
typedef unsigned char    BOOL;
typedef unsigned char    BYTE;
typedef unsigned int    UINT;
typedef unsigned int    WORD;
typedef unsigned long    ULONG;
typedef unsigned long LWORD;
//typedef double    DOUBLE;                //Weil 8-Bit-CPUs kein double kennen
typedef float    DOUBLE;                //Weil 8-Bit-CPUs kein double kennen

//***** FP-PROTKOLL *****
///********** FORM C++ FUER WINDIOWS *********
#define FP_STX        0x2
#define FP_ETX        0x3
#define FP_MASTER    0x10
#define MAX_FP_CH    28

#define TIME_AWAIT_RESPONSE            50   //Modem < 30 ms
#define TIMEOUT_AWAIT_RESPONSE    150 //150 ms
#define REBIRTH_SLAVE_ID                 0xFF

#define FP_HEADER_BYTE_MAX    8
#define FP_DATA_BYTE_MAX        0xFF
#define MODEM_STR_MAX                64

typedef struct complexType
                        {
                        DOUBLE real;
                        DOUBLE imag;
                        } COMPLEX;

typedef struct FP_Head
    {//12 HeaderBytes
    BYTE bFP_STX;    //0x2    //Einklinken und Correctness test
    BYTE bFP_From;    //Master ID 01
    BYTE bFP_To;    //ID->Kennungsnr: 0..MODEMMAX
    BYTE bFP_What;
    BYTE bFP_DatSeg;
    BYTE bFP_DatSum;
    BYTE bFP_DatState;    //Sollständig, Teile folgen mit nächsten Str.
    BYTE bFP_ChkSum;
    }stFP_HEAD_TYPE;


typedef union Funk_Prot_Header //UNION läßt
    {
    stFP_HEAD_TYPE stFP_HD;
    BYTE baFP_HD_Str[FP_HEADER_BYTE_MAX+1];//8+1
    }FUNK_PROT_HEADER_TYPE;

//MODEMSECION
#define MODEM_FREQUENCY_MIN_CHANGE_TIME    3    //in ms
#define MODEM_RX_TX_MIN_CHANGE_TIME            30        //in ms
#define MODEM_TX_TRANSMITER_TIME                30        //in ms
#define HEADERLENGTH    10                                            //in Bytes

#define MODEM_ID_STR_MAX    20    //etwas > 13
//!!!!!!!!!!!!! M BEI FUNKTION ERWEITERN ************
#define POSSIBLE_MODEMS_MAX    20   //Soviele Modem wie Kanäle ertsmal....
//!!!!!!!!!!!!! M BEI FUNKTION ERWEITERN ************

//Standard IDs
#define REBIRTH_SLAVE_ID    0xFF

//FUNK PROTOKOLL
#define FP_ID_OFFSET 0xA //=10
#define FP_STX    0x2
#define FP_ETX    0x3
#define FP_DELIMITER    ','
#define FP_MASTER    0x10    //ID or Name of Master Only

//DatStatus
#define STAT_COMPLETE 0x0
//Channel assign
#define MAX_FP_CH    28
#define ToAll_FP_CH 01
#define    DGPS_BROADCAST_FPCH    ToAll_FP_CH
#define REBIRTH_FP_CH    ToAll_FP_CH

//TO
#define TO_REBIRTH_SLAVES 0xFF
#define    TO_ALLSLAVES 0x10
#define    TO_MASTER 0x20
#define    TO_Act_SLAVE 0x30
#define    TO_DIFFERENCE_GPS 0x40

//WHAT_TO_DO
#define DO_TAKE_YOUR_NEW_ID    20    //Logical
#define DO_SetFPChannel            30
#define DO_DISPLAY_MSG            40
#define DO_GIVE_LIVESIGN        50        //Logical ID
#define DO_GIVE_MODEM_ID         60
#define DO_LIVESIGN_RESPONSE        70
#define DO_MODEM_ID_RESPONSE        80
#define DO_GIVE_COMPASSVAL    90
#define DO_TAKE_COMPASSVAL    100
///********** FORM C++ FUER WINDIOWS *********

/***************************************************************************/
/***************************      TYPE DEFS       **************************/
/***************************************************************************/

typedef struct stGGA
    {
    BYTE    bUTC_Hour;
    BYTE    bUTC_Min;
    DOUBLE dUTC_Sec;
    BYTE bLatitude_Grad;        //Latitude
    DOUBLE dLatitude_Min;
    char    cGlobus_NS;            //North_South
    BYTE     bLongitude_Grad;    //Longitude
    DOUBLE    dLongitude_Min;
    char    cGlobus_WE;            //West_East
    BYTE    bGPS_Quality;        //quality of GPS data
    BYTE    bSatellitsCount;    //number of satellites
    DOUBLE    dSat_DOP_Quality;    //DOP
    DOUBLE    dAltitude;            //Altitude
    char    cUnit_Altitude_MF; //meter or feet
/*
    DOUBLE    dGeodial_Sep    ;    //Geodial Separation
    char    cUnit_Geo_Sep_MF;    //meter or feet
    DOUBLE    dDGPS_Age;            //DGPS data: age in sec or 0 when unused
    WORD    wD_Station_ID;
*/
    }GPS_GGA_SRUCTTYPE;

typedef struct st_Ell_coor_dbl
    {
DOUBLE dLati_deg;           //Latitude
DOUBLE dLongi_deg;          //Longitude
} Ell_coor_dbl_STRUCTTYPE;

/*************/
/** GLOBALS **/
/*************/
/*//#INT_VEC SER0_VEC INT_HNDL_GPS     // Set interrupt vector*/
/*//#INT_VEC SER1_VEC INT_HNDL_MODEM // Set interrupt vector*/

//****** COM_LIB_SELECT ************
struct _Channel *gpst_Z0_Channel;
struct _Channel *gpst_Z1_Channel;

//PORTVALUES
BYTE gbDOUT_OutVal;

root char gcaStr[STRSIZE+1];
root char gca_Z0_TxBuf[TXBUFSIZE];
root char gca_Z1_TxBuf[TXBUFSIZE];
root char gca_Z1_RxBuf[RXBUFSIZE];
root char gca_Z1_RxStr[RXBUFSIZE];    //extra Str to manage the RX-MSGs
root char gca_Z0_RxStr[RXBUFSIZE+1];    //extra Str to manage the RX-MSGs
root char gca_Z0_RxBuf[RXBUFSIZE+1];

//** RADIO FP-SECTION ***
WORD gwMasterPollCnt;
FUNK_PROT_HEADER_TYPE gstunFP_HD;    //UnionStrcture to Radio-Header
BYTE gbaDelimitVal[FP_DATA_BYTE_MAX+1];//DELIMITERHANDLING
BYTE gbaFP_DATA[FP_DATA_BYTE_MAX+1];        //Modem-Tx-String mit Delimiters
WORD gwDataParser;
ULONG gulModem_ID;
BYTE gbMyFP_ID_LockNr;
BYTE gbActiveFPChannel;

static char gcaGPGGA[]={"$GPGGA"};
static char gcaRET[]={'\13','\0'};
static char gcaETX[]={'\3','\0'};
static char gcaDelimiter[]={","};    //Delimiter

//KKK
//KEYBORD_HARDWARE
#define KEY_RELEASE_MASK 0x0F // 00001111 for DIN0..3 must be 1
#define KEY_X_PORT   ADR_DS4

#define KEY_Y_PORT   DIN_PORT    //DINx = 0x4040
#define KEYINTENABLE_BIT 6
#define KEY_CTRL_OUTPORT    ADR_DS2
#define KEYMATRIX_X    8    //OUTPUT-Leitungen
#define KEYMATRIX_Y    4    //INPUT-LEITUNGEN

BYTE gbaKeyMatrix[KEYMATRIX_X][KEYMATRIX_Y]=            //7*3Tastatur
    {
//Y0 ,1 ,2 ,3 ,4

    'S',' ','R','V',    //X0

    '*','0','+','\0',    //X1

    '1','2','3','\0',    //X2

    '\1','\2','\0','\0', //X3

    '\0','\0','\0','G',    //X4

    '\3','\4','\5','\0', //X5    //F3

    '7','8','9','\0', //X6

    '4','5','6','\0' //X7
    };

//*****************************
// Ellipsenparameter und definitionen
//*****************************
//constants //other values seted in
//Ellipsenparameter
//gf_A = 6378388.0    //Bessel
//gf_B = gf_A-gf_A / 297.0;

//**Elipsoid*Section
static DOUBLE gfObjDist,gfObjDir;
//constants //other values seted in main
DOUBLE gf_A;
DOUBLE gf_PI;
DOUBLE gf_B,gf_E2,gf_E1,gf_M0,gf_D2R,gf_R2D;
DOUBLE gf_EPS;

/*************************************************************************/
/******************      GLOBALS AND DUMMIES     *************************/
/*************************************************************************/
//WORK Vorläufuges Target , spaeter von Datenbamk via Modem
static GPS_GGA_SRUCTTYPE gstGGA;
static Ell_coor_dbl_STRUCTTYPE gstEllCorDbl;
static DOUBLE gfaUserPos[2];
static DOUBLE gfaTargetPos[2];     //= {5452600.0, 4506000.0}; set in main //******* DUMMY FOR TARGET ****

//char gcaGPSDUMMY[]="$GPGGA,084116.999,5200.0000,N,03000.0000,E,1,05,1.8,501.6,M,,,,0000*09";

//********** TEXT GLOBALS ***********
xmem static char gcaEngFont6x8[] = {
/* char 0x20 of width 6 at 0x2da */
'\x0',
'\x0',
'\x0',
'\x0',
'\x0',
'\x0',
/* char 0x21 of width 6 at 0x2ea */
'\x0',
'\x0',
'\x2f',
'\x0',
'\x0',
'\x0',
/* char 0x22 of width 6 at 0x2fa */
'\x0',
'\x3',
'\x0',
'\x3',
'\x0',
'\x0',
/* char 0x23 of width 6 at 0x30a */
'\x14',
'\x7f',
'\x14',
'\x7f',
'\x14',
'\x0',
/* char 0x24 of width 6 at 0x31a */
'\x24',
'\x2a',
'\x7f',
'\x2a',
'\x12',
'\x0',
/* char 0x25 of width 6 at 0x32a */
'\x63',
'\x13',
'\x8',
'\x64',
'\x63',
'\x0',
/* char 0x26 of width 6 at 0x33a */
'\x3a',
'\x45',
'\x4a',
'\x30',
'\x48',
'\x0',
/* char 0x27 of width 6 at 0x34a */
'\x0',
'\x0',
'\x3',
'\x0',
'\x0',
'\x0',
/* char 0x28 of width 6 at 0x35a */
'\x0',
'\x1c',
'\x22',
'\x41',
'\x0',
'\x0',
/* char 0x29 of width 6 at 0x36a */
'\x0',
'\x41',
'\x22',
'\x1c',
'\x0',
'\x0',
/* char 0x2a of width 6 at 0x37a */
'\x0',
'\x2a',
'\x1c',
'\x2a',
'\x0',
'\x0',
/* char 0x2b of width 6 at 0x38a */
'\x0',
'\x8',
'\x1c',
'\x8',
'\x0',
'\x0',
/* char 0x2c of width 6 at 0x39a */
'\x0',
'\x0',
'\xa0',
'\x60',
'\x0',
'\x0',
/* char 0x2d of width 6 at 0x3aa */
'\x0',
'\x8',
'\x8',
'\x8',
'\x0',
'\x0',
/* char 0x2e of width 6 at 0x3ba */
'\x0',
'\x0',
'\x40',
'\x0',
'\x0',
'\x0',
/* char 0x2f of width 6 at 0x3ca */
'\x60',
'\x10',
'\x8',
'\x4',
'\x3',
'\x0',
/* char 0x30 of width 6 at 0x3da */
'\x1c',
'\x22',
'\x41',
'\x22',
'\x1c',
'\x0',
/* char 0x31 of width 6 at 0x3ea */
'\x0',
'\x2',
'\x7f',
'\x0',
'\x0',
'\x0',
/* char 0x32 of width 6 at 0x3fa */
'\x62',
'\x51',
'\x49',
'\x45',
'\x42',
'\x0',
/* char 0x33 of width 6 at 0x40a */
'\x22',
'\x41',
'\x49',
'\x49',
'\x36',
'\x0',
/* char 0x34 of width 6 at 0x41a */
'\x18',
'\x14',
'\x12',
'\x7f',
'\x10',
'\x0',
/* char 0x35 of width 6 at 0x42a */
'\x2f',
'\x49',
'\x49',
'\x49',
'\x31',
'\x0',
/* char 0x36 of width 6 at 0x43a */
'\x3c',
'\x4a',
'\x49',
'\x48',
'\x30',
'\x0',
/* char 0x37 of width 6 at 0x44a */
'\x1',
'\x71',
'\x9',
'\x5',
'\x3',
'\x0',
/* char 0x38 of width 6 at 0x45a */
'\x36',
'\x49',
'\x49',
'\x49',
'\x36',
'\x0',
/* char 0x39 of width 6 at 0x46a */
'\x6',
'\x9',
'\x49',
'\x29',
'\x1e',
'\x0',
/* char 0x3a of width 6 at 0x47a */
'\x0',
'\x0',
'\x14',
'\x0',
'\x0',
'\x0',
/* char 0x3b of width 6 at 0x48a */
'\x0',
'\x20',
'\x14',
'\x0',
'\x0',
'\x0',
/* char 0x3c of width 6 at 0x49a */
'\x8',
'\x14',
'\x22',
'\x41',
'\x0',
'\x0',
/* char 0x3d of width 6 at 0x4aa */
'\x0',
'\x14',
'\x14',
'\x14',
'\x0',
'\x0',
/* char 0x3e of width 6 at 0x4ba */
'\x0',
'\x41',
'\x22',
'\x14',
'\x8',
'\x0',
/* char 0x3f of width 6 at 0x4ca */
'\x2',
'\x1',
'\x59',
'\x5',
'\x2',
'\x0',
/* char 0x40 of width 6 at 0x4da */
'\x3e',
'\x41',
'\x5d',
'\x55',
'\x1e',
'\x0',
/* char 0x41 of width 6 at 0x4ea */
'\x7e',
'\x9',
'\x9',
'\x9',
'\x7e',
'\x0',
/* char 0x42 of width 6 at 0x4fa */
'\x7f',
'\x49',
'\x49',
'\x49',
'\x36',
'\x0',
/* char 0x43 of width 6 at 0x50a */
'\x3e',
'\x41',
'\x41',
'\x41',
'\x22',
'\x0',
/* char 0x44 of width 6 at 0x51a */
'\x7f',
'\x41',
'\x41',
'\x41',
'\x3e',
'\x0',
/* char 0x45 of width 6 at 0x52a */
'\x7f',
'\x49',
'\x49',
'\x49',
'\x41',
'\x0',
/* char 0x46 of width 6 at 0x53a */
'\x7f',
'\x9',
'\x9',
'\x9',
'\x1',
'\x0',
/* char 0x47 of width 6 at 0x54a */
'\x3e',
'\x41',
'\x49',
'\x49',
'\x38',
'\x0',
/* char 0x48 of width 6 at 0x55a */
'\x7f',
'\x8',
'\x8',
'\x8',
'\x7f',
'\x0',
/* char 0x49 of width 6 at 0x56a */
'\x0',
'\x0',
'\x7f',
'\x0',
'\x0',
'\x0',
/* char 0x4a of width 6 at 0x57a */
'\x30',
'\x40',
'\x40',
'\x40',
'\x3f',
'\x0',
/* char 0x4b of width 6 at 0x58a */
'\x7f',
'\x8',
'\x4',
'\x1a',
'\x61',
'\x0',
/* char 0x4c of width 6 at 0x59a */
'\x7f',
'\x40',
'\x40',
'\x40',
'\x40',
'\x0',
/* char 0x4d of width 6 at 0x5aa */
'\x7f',
'\x2',
'\xc',
'\x2',
'\x7f',
'\x0',
/* char 0x4e of width 6 at 0x5ba */
'\x7f',
'\x6',
'\x8',
'\x30',
'\x7f',
'\x0',
/* char 0x4f of width 6 at 0x5ca */
'\x3e',
'\x41',
'\x41',
'\x41',
'\x3e',
'\x0',
/* char 0x50 of width 6 at 0x5da */
'\x7f',
'\x9',
'\x9',
'\x9',
'\x6',
'\x0',
/* char 0x51 of width 6 at 0x5ea */
'\x3e',
'\x41',
'\x51',
'\x21',
'\x5e',
'\x0',
/* char 0x52 of width 6 at 0x5fa */
'\x7f',
'\x9',
'\x9',
'\x19',
'\x66',
'\x0',
/* char 0x53 of width 6 at 0x60a */
'\x26',
'\x49',
'\x49',
'\x49',
'\x32',
'\x0',
/* char 0x54 of width 6 at 0x61a */
'\x1',
'\x1',
'\x7f',
'\x1',
'\x1',
'\x0',
/* char 0x55 of width 6 at 0x62a */
'\x3f',
'\x40',
'\x40',
'\x40',
'\x3f',
'\x0',
/* char 0x56 of width 6 at 0x63a */
'\x3',
'\x1c',
'\x60',
'\x1c',
'\x3',
'\x0',
/* char 0x57 of width 6 at 0x64a */
'\x3f',
'\x40',
'\x38',
'\x40',
'\x3f',
'\x0',
/* char 0x58 of width 6 at 0x65a */
'\x63',
'\x14',
'\x8',
'\x14',
'\x63',
'\x0',
/* char 0x59 of width 6 at 0x66a */
'\x3',
'\x4',
'\x78',
'\x4',
'\x3',
'\x0',
/* char 0x5a of width 6 at 0x67a */
'\x61',
'\x51',
'\x49',
'\x45',
'\x43',
'\x0',
/* char 0x5b of width 6 at 0x68a */
'\x0',
'\x7f',
'\x41',
'\x41',
'\x0',
'\x0',
/* char 0x5c of width 6 at 0x69a */
'\x3',
'\x4',
'\x8',
'\x10',
'\x60',
'\x0',
/* char 0x5d of width 6 at 0x6aa */
'\x0',
'\x41',
'\x41',
'\x7f',
'\x0',
'\x0',
/* char 0x5e of width 6 at 0x6ba */
'\x0',
'\x2',
'\x1',
'\x2',
'\x0',
'\x0',
/* char 0x5f of width 6 at 0x6ca */
'\x80',
'\x80',
'\x80',
'\x80',
'\x80',
'\x0',
/* char 0x60 of width 6 at 0x6da */
'\x0',
'\x1',
'\x2',
'\x4',
'\x0',
'\x0',
/* char 0x61 of width 6 at 0x6ea */
'\x38',
'\x44',
'\x44',
'\x24',
'\x78',
'\x0',
/* char 0x62 of width 6 at 0x6fa */
'\x7f',
'\x44',
'\x44',
'\x44',
'\x38',
'\x0',
/* char 0x63 of width 6 at 0x70a */
'\x38',
'\x44',
'\x44',
'\x44',
'\x28',
'\x0',
/* char 0x64 of width 6 at 0x71a */
'\x38',
'\x44',
'\x44',
'\x44',
'\x7f',
'\x0',
/* char 0x65 of width 6 at 0x72a */
'\x38',
'\x54',
'\x54',
'\x54',
'\x18',
'\x0',
/* char 0x66 of width 6 at 0x73a */
'\x0',
'\x4',
'\x7e',
'\x5',
'\x0',
'\x0',
/* char 0x67 of width 6 at 0x74a */
'\x98',
'\xa4',
'\xa4',
'\xa4',
'\x7c',
'\x0',
/* char 0x68 of width 6 at 0x75a */
'\x7f',
'\x4',
'\x4',
'\x4',
'\x78',
'\x0',
/* char 0x69 of width 6 at 0x76a */
'\x0',
'\x0',
'\x7d',
'\x0',
'\x0',
'\x0',
/* char 0x6a of width 6 at 0x77a */
'\x0',
'\x40',
'\x80',
'\x7d',
'\x0',
'\x0',
/* char 0x6b of width 6 at 0x78a */
'\x7f',
'\x8',
'\x18',
'\x24',
'\x44',
'\x0',
/* char 0x6c of width 6 at 0x79a */
'\x0',
'\x1',
'\x3e',
'\x40',
'\x0',
'\x0',
/* char 0x6d of width 6 at 0x7aa */
'\x7c',
'\x4',
'\x78',
'\x4',
'\x78',
'\x0',
/* char 0x6e of width 6 at 0x7ba */
'\x7c',
'\x4',
'\x4',
'\x4',
'\x78',
'\x0',
/* char 0x6f of width 6 at 0x7ca */
'\x38',
'\x44',
'\x44',
'\x44',
'\x38',
'\x0',
/* char 0x70 of width 6 at 0x7da */
'\xfc',
'\x24',
'\x24',
'\x24',
'\x18',
'\x0',
/* char 0x71 of width 6 at 0x7ea */
'\x18',
'\x24',
'\x24',
'\x24',
'\xfc',
'\x0',
/* char 0x72 of width 6 at 0x7fa */
'\x7c',
'\x8',
'\x4',
'\x4',
'\x4',
'\x0',
/* char 0x73 of width 6 at 0x80a */
'\x48',
'\x54',
'\x54',
'\x54',
'\x24',
'\x0',
/* char 0x74 of width 6 at 0x81a */
'\x0',
'\x4',
'\x3f',
'\x44',
'\x0',
'\x0',
/* char 0x75 of width 6 at 0x82a */
'\x3c',
'\x40',
'\x40',
'\x40',
'\x7c',
'\x0',
/* char 0x76 of width 6 at 0x83a */
'\xc',
'\x30',
'\x40',
'\x30',
'\xc',
'\x0',
/* char 0x77 of width 6 at 0x84a */
'\x3c',
'\x40',
'\x38',
'\x40',
'\x3c',
'\x0',
/* char 0x78 of width 6 at 0x85a */
'\x44',
'\x28',
'\x10',
'\x28',
'\x44',
'\x0',
/* char 0x79 of width 6 at 0x86a */
'\x9c',
'\xa0',
'\xa0',
'\xa0',
'\x7c',
'\x0',
/* char 0x7a of width 6 at 0x87a */
'\x44',
'\x64',
'\x54',
'\x4c',
'\x44',
'\x0',
/* char 0x7b of width 6 at 0x88a */
'\x0',
'\x8',
'\x36',
'\x41',
'\x0',
'\x0',
/* char 0x7c of width 6 at 0x89a */
'\x0',
'\x0',
'\x7f',
'\x0',
'\x0',
'\x0',
/* char 0x7d of width 6 at 0x8aa */
'\x0',
'\x41',
'\x36',
'\x8',
'\x0',
'\x0',
/* char 0x7e of width 6 at 0x8ba */
'\x2',
'\x1',
'\x2',
'\x1',
'\x0',
'\x0'
};

/*****************/
/** END GLOBALS **/
/*****************/

/*************************************************************************/
/******************   PROTOTYPES             *****************************/
/*************************************************************************/
//KEY
interrupt reti void My_Int0_ISR(); //interrupt reti
BYTE GetKeyScan(void);

//***COM_PROTOTYPES ***
void Scan_GPS_Data(char* pcInStr);        // needs global GPS_GGA_SRUCTTYPE

//***GPS_PROTOTYPES ***
void GetUserPosition(DOUBLE* pdGPS_X,DOUBLE* pdGPS_Y);
void GetTargetGuidance(void);
int Ell_coor_dbl(void);    //needs GPS_GGA_SRUCTTYPE & Ell_coor_dbl_STRUCTTYPE
DOUBLE artanh(DOUBLE);
COMPLEX addc(COMPLEX, COMPLEX);
COMPLEX mult1c(DOUBLE, COMPLEX);
COMPLEX mult2c(COMPLEX, COMPLEX);
COMPLEX powc (COMPLEX, DOUBLE);
COMPLEX sinc(COMPLEX);
COMPLEX asinc(COMPLEX);
COMPLEX artanhc(COMPLEX);
COMPLEX tanhc(COMPLEX);
int GPS_EllipseToGausKrueger(DOUBLE* fpX_Ret,DOUBLE* fpY_Ret);    //returns CbR X,Y
float Distance (float* pfGK_Pos1,float* pfGK_Pos2); //Compute distance
float Direction(float* pfGK_Pos1,float* pfGK_Pos2); // compute direction

/*************************/
/** FUNCTION PROTOTYPES **/
/*************************/
int Round(float fIn);
xmem void TestPwr(void);
xmem BYTE GetPrellFree(WORD wPortNr);
xmem DOUBLE Divide(DOUBLE f_Z,DOUBLE f_N);
xmem void DoHandl_MSG(BYTE bMSG);
void CopyStrFromTo(char* pcstrDest,char* pcStrSrc,WORD wFrom, WORD wTo);
void CopyByteFromTo(char* pcstrDest,char* pcStrSrc,WORD wFrom, WORD wTo);//without wTo=12 -> 0..11
void Append_Ch_On_Str(char* pStr,char cCh);
void Strip_LastCh_Str(char* pStr);
BYTE GetPortBit(WORD wIO_Port,BYTE bBit);    //usage: GetPort(In_PORT,Bit);
void SetPortByteBit(BYTE* pbOutVal,WORD wIO_Port,BYTE bBit);    //usage:(& gBYTEVAL of Port,Out_PORT,Bit;
void ClearPortByteBit(BYTE* pbOutVal,WORD wIO_Port,BYTE bBit);    //usage:(& gBYTEVAL of Port,Out_PORT,Bit;

//PROTOKOLL-SECTION
BYTE FP_ChangeChannel(BYTE bNewCh);
ULONG GetMyModem_ID(void);    //-->gcaStr
char* Get_FP_Data_Str(char* caDest);    // ret 0 ist noch nicht vollständig + \0
char* PackAndTX_ModemData(BYTE bAction,char* pcTxData);    //-->pcDest
void Do_HNDL_FP(char* pcRxStr);
void FP_Head_Clear(void);
void FP_Head_Cat(void);
char* CatByteOnDelimiterStr(char* lpcDest,BYTE bInByte);    //ret--> lpcDest + ','
int TearLimiterStr(char* lpsInStr); //-->gbaDelimitVal[STRSIZE]   |.>Return counts

//*** LCD _PROTOTYPES ***
//GRAPHIC_LCD SECTION
void Entrance_Scr(void);
void Menue_Scr(void);
void LCD_Clr(void);        //NEEDS Block 1 or Block 2
void LCD_INIT_12864(void);
void LCD_StatChk(BYTE bCS);        //NEEDS Block 1 or Block 2
BYTE LCD_ClearCS(BYTE bCS);
void LCD_WriteData(BYTE bCS,BYTE bData);
BYTE LCD_ReadStatus(BYTE bCS);
void LCD_WriteCmd(BYTE bCS,BYTE bCmd);
void LCD_WrStr(BYTE bLn,char* pcStr);
BYTE LCD_ReadData(BYTE bCS);

//COMPASS-SECTION
void ResetCompass(void);
void CompassInit(void);
void CalibrateCompass(void);
WORD ReadCompass(void);
WORD MoveDirec(WORD wCom_Angle, float fAzi_Angle);
//COM-SECTION
xmem void OpenAndInitComPorts(BYTE bPrt);
int COM_Tx_Block_Zx(BYTE bComNr,char* pcStr,WORD wLength);
void ExecuteRadioTX(char *pcStr);
char* COM_Rx_Buff_Zx(BYTE bComNr,char cTerminator);
char* Scan_Modem_RX_MSG(void);
char* Scan_GPS_RX_MSG(void);

/******************************/
/** ENDE FUNCTION PROTOTYPES **/
/*****************************/

/*******COMPILERDIRECTIVES *****/
//#INT_VEC SER0_VEC COM_RxGPS_GGA
//#INT_VEC SER1_VEC COM_RxModem_GGA
/***END COMPILERDIRECTIVES ****/

/**************************************************************************/
/**************         FUNCTIONS            ******************************/
/**************************************************************************/

/*****************************/
xmem void TestPwr(void)
/*****************************/
{
if(GetPortBit(PWR_CTRL_IN,LBO5V_PWR_CTRL_Bit))
    {LCD_WrStr(6,"! Batterie 5V zu klein");};
if(GetPortBit(PWR_CTRL_IN,LBO3V_PWR_CTRL_Bit))
    {LCD_WrStr(7,"! Batterie 3,3V zu klein");};
};

/*****************************/
root void Debug(char* pcStr)        //only for develpment ... remove later
/*****************************/
{
auto char caStr[22];

hitwd();
if( (pcStr==NULL) || (!*pcStr ) ) return;
strncpy(caStr,pcStr,21);
caStr[21]=0;
LCD_WrStr(0,caStr);
hitwd();
/*
printf("\n");
hitwd();
printf(caStr);
hitwd();
*/
};

/******************************/
xmem void Pause(WORD wPT)
/******************************/
{
auto WORD wCC;
for(wCC=0;wCC<wPT;wCC++){hitwd();};
};

//*******************************************
xmem BYTE GetPrellFree(WORD wPortNr)//ACHTUNG OHNE OFFSET
//*******************************************
{
BYTE bReaded;//F�r unruhige Leitungen
do{//Wiederhole so so oft, bis das ergebniss immer gleich ist
    bReaded=inport(wPortNr);
hitwd();
    }while( bReaded != inport(wPortNr) );
return bReaded;
};

//DO
/******************************/
xmem void DoHandl_MSG(BYTE bMSG)
/******************************/
{
auto char* pcRx;
auto DOUBLE dGPS_X,dGPS_Y;
hitwd();
switch(bMSG)
    {
    case '\1' :    //Funktionstasten F1 usw.
        {
        LCD_WrStr(0,"Read Compass");
        sprintf(gcaStr,"%u Grad",ReadCompass());
        LCD_WrStr(1,gcaStr);
        break;
        };
    case '\2' :
        {
        LCD_WrStr(0,"Reset Compass");
        ResetCompass();
        break;
        };
    case '\3' :
        {
        LCD_WrStr(0,"CalibrateCompass");
        CalibrateCompass();
        break;
        };
    case '\4' :
        {
        GetMyModem_ID();
        LCD_WrStr(0,"GetMyModem_ID()");
        sprintf(gcaStr,"Lock-ID:%d",gbMyFP_ID_LockNr);
        LCD_WrStr(1,gcaStr);
        break;
        };
    case '\5' :
        {
        Entrance_Scr();
        break;
        };
    case '0' :
        {
        break;
        };
    case '1' :
        {
        sprintf(gcaStr,"STX: %d",gstunFP_HD.stFP_HD.bFP_STX);
        LCD_WrStr(0,gcaStr);
        break;
        };
    case '2' :
        {
        sprintf(gcaStr,"From %d",gstunFP_HD.stFP_HD.bFP_From);
        LCD_WrStr(0,gcaStr);
        break;
        };
    case '3' :
        {
        sprintf(gcaStr,"To %d",gstunFP_HD.stFP_HD.bFP_To);
        LCD_WrStr(0,gcaStr);
        break;
        };
    case '4' :
        {
        sprintf(gcaStr,"What %d",gstunFP_HD.stFP_HD.bFP_What);
        LCD_WrStr(0,gcaStr);
        break;
        };
    case '5' :
        {
        sprintf(gcaStr,"DatSeg %d",gstunFP_HD.stFP_HD.bFP_DatSeg);
        LCD_WrStr(0,gcaStr);
        break;
        };
    case '6' :
        {
        sprintf(gcaStr,"DatSum %d",gstunFP_HD.stFP_HD.bFP_DatSum);
        LCD_WrStr(0,gcaStr);
        break;
        };
    case '7' :
        {
        sprintf(gcaStr,"DatState %d",gstunFP_HD.stFP_HD.bFP_DatState);
        LCD_WrStr(0,gcaStr);
        break;
        };
    case '8' :
        {
        sprintf(gcaStr,"ChkSum %d",gstunFP_HD.stFP_HD.bFP_ChkSum);
        LCD_WrStr(0,gcaStr);
        break;
        };
    case '9' :
        {
LCD_WrStr(0,"GO");
strcpy(gcaStr,"Send to Master");
LCD_WrStr(1,gcaStr);
ExecuteRadioTX(gcaStr);
        break;
        };
    case ' ' :    //SPACE
        {
        break;
        };
    case 'S' :    //Shift
        {
        Menue_Scr();
        break;
        };
    case 'R' :
        {
        break;
        };
    case '*' :
        {
        break;
        };
    case '+' :
        {
        break;
        };
    case 'G' :    //START - GO
        {
        LCD_WrStr(0,"Berechne Position");
        outport(GPS_CTRL_OUT+GPS_PWR_OFFON_01,1);    //GPS PWR ON
        tdelay(100);
        do
            {
            hitwd();
            pcRx=Scan_GPS_RX_MSG();
            }while( (*pcRx==NULL) || (strncmp(pcRx,"$GPGGA",6)) );    //strcmp
        if( (pcRx != NULL) && (!strncmp(pcRx,"$GPGGA",6)) )    //strcmp
            {//Alles OK
            LCD_WrStr(0,pcRx);    //Anzeige GPS in LCD
            GetTargetGuidance();    //Erzeugt Suchrichtungsermittlung /
            GetUserPosition(&dGPS_X,&dGPS_Y);    //Needs Two DOUBLE Vars &Var1,&Var2
            };
//WIRKLICH ABSCHALTEN ???????????????????????
        outport(GPS_CTRL_OUT+GPS_PWR_OFFON_01,0);    // PWR OUT
        break;
        };
    case '+' :    //RETUN //ENTER
        {
        break;
        };
    };//switch
};//ENDE

//*****HELPFUNKTIONS ***************************
//**********************************************
xmem int Round(float fIn)
//**********************************************
{
return (int)floor(fIn+0.5);
}

//**********************************************
xmem DOUBLE Divide(DOUBLE f_Z,DOUBLE f_N)    //prevents X/0
//**********************************************
{
if(f_N)
    {
    return f_Z/f_N;
    }
else
    {
    LCD_WrStr(0,"Wrong Division !");
    LCD_WrStr(1,"X/0 unvalid !");
    return f_Z;    //Gib Zaehler zurück.
    };
};

//**********************************************
root BYTE GetPortBit(WORD wIO_Port,BYTE bBit)    //usage: GetPort(In_PORT,Bit);
//**********************************************
{
wIO_Port=inport(wIO_Port);//ACHTUNG OHNE OFFSET
return ( wIO_Port & (1 << bBit) );// Bit ausblenden //ret 1=1 0=0
};

//**********************************************
root void SetPortByteBit(BYTE* pbOutVal,WORD wIO_Port,BYTE bBit)    //usage:SetBit(& gBYTEVAL of Port,Out_PORT,Bit;
//**********************************************
{
SET(pbOutVal,bBit);
//*pbOutVal |= (1 << bBit);
outport(wIO_Port,*pbOutVal);
};

//**********************************************
root void ClearPortByteBit(BYTE* pbOutVal,WORD wIO_Port,BYTE bBit)    //usage:SetBit(& gBYTEVAL of Port,Out_PORT,Bit;
//**********************************************
{
RES(pbOutVal,bBit);
//*pbOutVal &= ~(1 << bBit);
outport(wIO_Port,*pbOutVal);
};

//**********************************************
root void CopyStrFromTo(char* pcstrDest,char* pcStrSrc,WORD wFrom, WORD wTo)
//**********************************************
{//Example: copy char from (without)to
strncpy(pcstrDest,pcStrSrc+wFrom,wTo-wFrom);
pcstrDest[wTo-wFrom]=0;
};

//**********************************************
root void CopyByteFromTo(char* pcstrDest,char* pcStrSrc,WORD wFrom, WORD wTo)//without wTo=12 -> 0..11
//**********************************************
{//Example: copy char from (without)to
auto WORD wNN;
hitwd();
for(wNN=0;wNN<(wTo-wFrom);wNN++)
    {
    *pcstrDest++=*(pcStrSrc++ + wFrom);
    };
};

//**********************************************
root void Append_Ch_On_Str(char* pStr,char cCh)
//**********************************************
{
auto WORD wLen;
if( (pStr==NULL) ) {return;};
wLen=strlen(pStr);
pStr[wLen++]=cCh;
pStr[wLen]=0;
};

//**********************************************
root void Strip_LastCh_Str(char* pStr)
//**********************************************
{
if( (pStr==NULL) || (!*pStr) ) {return;};
pStr[strlen(pStr)-1]=0;
};

//COM
//==================================
//***** COM-SECTION *********//
//==================================
/*
#define COM_MUX_ADR0 DOUT0    //DUAL MUX 4052 ADRbit
#define COM_MUX_ADR0 DOUT1    //DUAL MUX 4052 ADRbit
#define GPS_T0_COM_MUX    0    //DUAL MUX 4052 Channel
#define GPS_T1_COM_MUX    1    //DUAL MUX 4052 Channel
#define BCD__COM_MUX        3    //DUAL MUX 4052 Channel
*/
xmem void ComMuxChannel(BYTE bPrt)
{

outport(DOUT_PORT,bPrt);
};

//**********************************************
xmem void OpenAndInitComPorts(BYTE bPrt)
//**********************************************
{
//Debug("OpenAndInitComPorts");
outport(0x4084,1);
outport(0x4083,1);
outport(0x4082,1);
switch(bPrt)
    {
    case 0:
        {
        if(gpst_Z0_Channel != NULL){aascClose(gpst_Z0_Channel);};
        if( (gpst_Z0_Channel = aascOpen(DEV_Z0,0,ASCI_PARAM_8N1 | ASCI_PARAM_1200*8,NULL))==NULL )
            {
            LCD_WrStr(0,"Z0 Error");
            }
        else
            {
            aascSetWriteBuf(gpst_Z0_Channel,gca_Z0_TxBuf,sizeof(gca_Z0_TxBuf));
            aascSetReadBuf(gpst_Z0_Channel,gca_Z0_RxBuf,sizeof(gca_Z0_RxBuf));
            aascFlush(gpst_Z0_Channel);
            aascTxSwitch(gpst_Z0_Channel,1);    //Einschalten
            aascRxSwitch(gpst_Z0_Channel,1);    //Einschalten
            };
        break;
        };
    case 1:
        {
        if(gpst_Z1_Channel != NULL){aascClose(gpst_Z1_Channel);};
        if( (gpst_Z1_Channel = aascOpen(DEV_Z1,0,ASCI_PARAM_8N1 | ASCI_PARAM_1200*8,NULL))==NULL )
            {
            LCD_WrStr(1,"Z1 Error");
            }
        else
            {
            aascSetWriteBuf(gpst_Z1_Channel,gca_Z1_TxBuf,sizeof(gca_Z1_TxBuf));
            aascSetReadBuf(gpst_Z1_Channel,gca_Z1_RxBuf,sizeof(gca_Z1_RxBuf));
            aascFlush(gpst_Z1_Channel);
            aascTxSwitch(gpst_Z1_Channel,1);    //Einschalten
            aascRxSwitch(gpst_Z1_Channel,1);    //Einschalten
            };
        break;
        };
    };
hitwd();
};

//**********************************************
root int COM_Tx_Block_Zx(BYTE bComNr,char* pcStr,WORD wLength) //no strlen(pcStr), ZERO tx too
//**********************************************
{// Return -1 == ERROR
hitwd();
//Debug("COM_Tx_Block_Zx()");
wLength%=TXBUFSIZE;
if( (pcStr==NULL) || (!*pcStr) )return -1;
switch(bComNr)
    {
    case 0:
        {
        if(gpst_Z0_Channel==NULL)return -1;
        aascFlushWrBuf(gpst_Z0_Channel);
        aascWriteBlk(gpst_Z0_Channel,pcStr,wLength,0);
        break;
        };
    case 1:
        {
        if(gpst_Z1_Channel==NULL)return -1;
        aascFlushWrBuf(gpst_Z1_Channel);
        aascWriteBlk(gpst_Z1_Channel,pcStr,wLength,0);
        break;
        };
    };
tdelay( floor(((wLength+4) * 8/4600)) + 30 );
hitwd();
return 0;
};

//**********************************************
root void ExecuteRadioTX(char *pcStr)
//**********************************************
{
Append_Ch_On_Str(pcStr,FP_ETX);
COM_Tx_Block_Zx(MODEM_PORT,pcStr,strlen(pcStr));
};

//RX
//**********************************************
root char* COM_Rx_Buff_Zx(BYTE bComNr,char cTerminator)
//**********************************************
{
auto UINT uiPacketSize;
auto UINT uiCharCount;

hitwd();
//Debug("COM_Rx_Buff_Zx");

switch(bComNr)
    {
    case 0:
        {
        *gca_Z0_RxStr=0;
        if(gpst_Z0_Channel==NULL)return gca_Z0_RxStr;
//        aascRxSwitch(gpst_Z0_Channel,1);    //Einschalten
        uiPacketSize=aascScanTerm(gpst_Z0_Channel,cTerminator);
        if( uiPacketSize )    //Ist eine CR da ? und wenn ja, wieviele Zeichen
            {
            uiCharCount=aascReadBlk(gpst_Z0_Channel,gca_Z0_RxStr,uiPacketSize % RXBUFSIZE,0); //uiRemainingChars-1
            hitwd();
            gca_Z0_RxStr[uiCharCount % RXBUFSIZE]=0;
            if( aascReadBufLeft(gpst_Z0_Channel) )
                {
                aascFlushRdBuf(gpst_Z0_Channel);    //Lösche GPS_Empfangspuffer, damit die Daten immer Frisch sind !
                };
            };
        return gca_Z0_RxStr;
        };
    case 1:
        {
        *gca_Z1_RxStr=0;
        if(gpst_Z1_Channel==NULL)return gca_Z1_RxStr;
        aascRxSwitch(gpst_Z1_Channel,1);    //Einschalten
        uiPacketSize=aascScanTerm(gpst_Z1_Channel,cTerminator);
        if( uiPacketSize )    //Ist eine CR da ? und wenn ja, wieviele Zeichen
            {
            uiCharCount=aascReadBlk(gpst_Z1_Channel,gca_Z1_RxStr,uiPacketSize % RXBUFSIZE,0); //uiRemainingChars-1
            hitwd();
            gca_Z1_RxStr[uiCharCount % RXBUFSIZE]=0;
            if( aascReadBufLeft(gpst_Z1_Channel) )
                {
                aascFlushRdBuf(gpst_Z1_Channel);
                };
            };
        return gca_Z1_RxStr;
        };    //case
    default:
        {
        *gcaStr=0;
        return gcaStr;
        };
    };//switch
};

//SCAN
//**********************************************
root char* Scan_GPS_RX_MSG(void)    //If "$GPGGA" in Rx return
//**********************************************
{//gca_Z0_RxStr
auto char* pcRx;
auto char* pcPosOK;
auto WORD wCount;
//LATER SWITCH FROM TRICKLE POWER MODE in RUNSTATE

//Debug("Scan_GPS_RX_MSG");
wCount=0;
pcRx=pcPosOK=NULL;
do{                //POLLING //Wait on NMEA-STR!
    hitwd();
    aascRxSwitch(gpst_Z0_Channel,1);    //Einschalten
    pcRx=COM_Rx_Buff_Zx(GPS_COM_PORT,0x0D);
    if( (pcRx != NULL) && (*pcRx) )        //Wenn nicht ""
        {
        hitwd();
        if(strchr(pcRx,ASCII_CR) != NULL)                                    //ENDE DA ?
            {
            hitwd();
            pcPosOK=strstr(pcRx,"$GPGGA");    // "$GPGGA" gcaGPGGA
            if(pcPosOK!=NULL)    //ANFANG DA ?        //returns pcPosOK=NULL;
                {
                if( !strncmp(pcPosOK,gcaGPGGA,6) )//strcmp
                    {
                    //Alles OK
                    aascRxSwitch(gpst_Z0_Channel,0);    //Ausschalten
                    Scan_GPS_Data(pcPosOK);
                    LCD_WrStr(0,pcPosOK);
                    };
                };    //"$GPGGA" DA
            };//ende da
        };
    }while( (wCount++ < 0xFFF) && (strncmp(pcPosOK,"$GPGGA",6)) );//ANFANG Korrekt //strcmp && TIMOUT
aascRxSwitch(gpst_Z0_Channel,0);    //Ausschalten
return pcPosOK;
};

//Data
#define GGA_STRSEG_MAX        11
#define GGA_SEGSTRSIZEMAX    17
//**********************************************
root void Scan_GPS_Data(char* pcInStr)// needs global GPS_GGA_SRUCTTYPE
//**********************************************
{
auto char caStrToken[GGA_STRSEG_MAX][GGA_SEGSTRSIZEMAX]; //[11][17]    //2Dim Für die Datenfelder
auto char* pcScanStr;
auto char* pcStrSeg;
auto BYTE bSeg;

//Debug("Scan_GPS_Data()");

//Lösche die Hauptstruktur
gstGGA.bUTC_Hour=gstGGA.bUTC_Min=gstGGA.dUTC_Sec=
gstGGA.bLatitude_Grad=gstGGA.dLatitude_Min=
gstGGA.cGlobus_NS=
gstGGA.bLongitude_Grad=gstGGA.dLongitude_Min=
gstGGA.cGlobus_WE=
gstGGA.bGPS_Quality=gstGGA.bSatellitsCount=gstGGA.dSat_DOP_Quality=
gstGGA.dAltitude=gstGGA.cUnit_Altitude_MF
=0;

hitwd();
pcScanStr=strstr(pcInStr,"$GPGGA");
if( (pcScanStr == NULL) || (strncmp(pcScanStr,"$GPGGA",6) )    )    //strcmp Sicherung
    {return;};                                     //Abbruch falls nichts passt.

strncpy(gcaStr,pcScanStr,STRSIZE);    //Transfer to save side
gcaStr[STRSIZE]=0;    //Transfer to save side
hitwd();

if( (pcStrSeg=strtok(gcaStr,gcaDelimiter)) == NULL )
     {return;}; //Nichts da !                       //Abbruch falls kein Delimiter
//Alles OK !!
bSeg=0;
strcpy(&caStrToken[bSeg++][0],pcStrSeg);    //bseg=0 Token gefunden - Points to the firts occurence
while( ((pcStrSeg=strtok(NULL,gcaDelimiter)) != NULL) && (bSeg < GGA_STRSEG_MAX) )
    {
    hitwd();
    strcpy(&caStrToken[bSeg++][0],pcStrSeg); //now ->bseg=1..10
    };

//Füge nun die getrennten Tokens in die richte Position in der typisierten Struktur ein
//UTC-PositionTime
bSeg=1;
CopyStrFromTo(gcaStr,&caStrToken[bSeg][0],0,2);       //hour
gstGGA.bUTC_Hour=(BYTE)atoi(gcaStr);
CopyStrFromTo(gcaStr,&caStrToken[bSeg][0],2,4);      //minute
gstGGA.bUTC_Min=(BYTE)atoi(gcaStr);
CopyStrFromTo(gcaStr,&caStrToken[bSeg][0],4,11);     //second
gstGGA.dUTC_Sec=(DOUBLE)atof(gcaStr);

//Latitude
bSeg=2;
CopyStrFromTo(gcaStr,&caStrToken[bSeg][0],0,2);      //degree
gstGGA.bLatitude_Grad=(BYTE)atoi(gcaStr);
CopyStrFromTo(gcaStr,&caStrToken[bSeg][0],2,10);     //minute
gstGGA.dLatitude_Min=(DOUBLE)atof(gcaStr);

//NORTH_SOUTH
bSeg=3;
gstGGA.cGlobus_NS=caStrToken[bSeg][0];

//Longitude
bSeg=4;
CopyStrFromTo(gcaStr,&caStrToken[bSeg][0],0,3);      //degree
gstGGA.bLongitude_Grad=(BYTE)atoi(gcaStr);
CopyStrFromTo(gcaStr,&caStrToken[bSeg][0],3,10);     //minute
gstGGA.dLongitude_Min=(DOUBLE)atof(gcaStr);

//WEST_EAST
bSeg=5;
gstGGA.cGlobus_WE=caStrToken[bSeg][0];

bSeg=6;
gstGGA.bGPS_Quality=(BYTE)atoi(&caStrToken[bSeg][0]);

bSeg=7;
gstGGA.bSatellitsCount=(BYTE)atoi(&caStrToken[bSeg][0]);

bSeg=8;
gstGGA.dSat_DOP_Quality=(DOUBLE)atof(&caStrToken[bSeg][0]);

bSeg=9;
gstGGA.dAltitude=(DOUBLE)atof(&caStrToken[bSeg][0]);

bSeg=10;
gstGGA.cUnit_Altitude_MF=caStrToken[bSeg][0];
};

//**********************************************
root char* Scan_Modem_RX_MSG(void)
//**********************************************
{
//gca_Z1_RxStr
auto char* pcRx;
hitwd();
pcRx=COM_Rx_Buff_Zx(MODEM_PORT,ASCII_ETX);

//LOOK of str is compltete --> with ETX and valid
// VORSICHT dieserSubstringsearch liefert NULL ZUrück
hitwd();
if( (strchr(pcRx,'\x2') == NULL) || (strchr(pcRx,ASCII_ETX)==NULL) )    //Setze diesen wieder uaf eien Leeren Str.
    {
    *gca_Z1_RxStr=0;
    pcRx=gca_Z1_RxStr;    //WATCH Save (NOT NULL POINTER) assigne and compare
    };

if( (*pcRx) && (pcRx != NULL) )
    {
    //Alles OK
    Do_HNDL_FP(pcRx);        //action
    };
return pcRx;
};

//==================================
//******** COMPASS SECTION ********
//==================================

//**********************************************
xmem void CompassInit(void)    //Rest if Hang or newStart
//**********************************************
{
outport(COMP_CTRL_OUT+COPA_I_PC,1);    // /Poll-Continue
outport(COMP_CTRL_OUT+COPA_I_SS,1);    // Slave Select
outport(COMP_CTRL_OUT+COPA_I_CAL,1);//Calibrate
outport(COMP_CTRL_OUT+COPA_I_SCLK,1);
outport(COMP_CTRL_OUT+COPA_I_RESET,1);
};

//**********************************************
xmem void ResetCompass(void)
//**********************************************
{
hitwd();
CompassInit();
outport(COMP_CTRL_OUT+COPA_I_RESET,0);
tdelay(10);
outport(COMP_CTRL_OUT+COPA_I_RESET,1);
tdelay(400);
hitwd();
};

//CCC
//**********************************************
xmem void CalibrateCompass(void)
//**********************************************
{
auto BYTE bMsg;
outport(COMP_CTRL_OUT+COPA_I_PC,1);    // /Poll-Continue
outport(COMP_CTRL_OUT+COPA_I_CAL,1);
tdelay(1);
LCD_WrStr(0,"Calibrate Compass");
LCD_WrStr(1,"Halte 1. Pos");
LCD_WrStr(2,"..Press any KEY");
do
    {
    hitwd();
    bMsg=GetKeyScan();    //Scan the state of the keypad
    }while( !bMsg );    //Taste gedrückt ist !=0
//Erster Calib_PULSE
outport(COMP_CTRL_OUT+COPA_I_CAL,0);
tdelay(11);
outport(COMP_CTRL_OUT+COPA_I_CAL,1);
tdelay(11);
if( GetPortBit(COMP_CTRL_IN,COPA_O_CI) )
    {
    LCD_WrStr(1,"Erste Pos. erfasst");
    }
else
    {
    LCD_WrStr(1,"Calibration error");
    return;
    };
LCD_WrStr(2,"Turn 180 Degree");
LCD_WrStr(3,"..Press any KEY");

do
    {
    hitwd();
    bMsg=GetKeyScan();    //Scan the state of the keypad
    }while( !bMsg );    //Taste gedrückt ist !=0
//Zweiter Calib_PULSE
outport(COMP_CTRL_OUT+COPA_I_CAL,0);
tdelay(11);
outport(COMP_CTRL_OUT+COPA_I_CAL,1);
tdelay(90);    //vorgeschrieben mindesten 80 ms

if( GetPortBit(COMP_CTRL_IN,COPA_O_CI) )
    {
    LCD_WrStr(1,"Calibrate not Valid");
    return;
    }
else
    {
    LCD_WrStr(1,"Calibrate Ok");
    };
};

//**********************************************
xmem WORD ReadCompass(void)
//**********************************************
{
auto int iNN;
auto WORD wVal;
outport(COMP_CTRL_OUT+COPA_I_SCLK,1);
outport(COMP_CTRL_OUT+COPA_I_PC,0);    // /Poll-Continue
tdelay(10);
hitwd();
outport(COMP_CTRL_OUT+COPA_I_PC,1);// /Poll-Continue
iNN=0;//Timeoutcounter
tdelay(1);
while( (!(GetPortBit(COMP_CTRL_IN,COPA_O_EOC))) && (iNN++ < COMPASSTIMEOUT) )    //wait, Bis wieder eins
    {
    tdelay(1);
    hitwd();
    }; //Wait to EOC of Compass 80-100mSec !To Long ??

if(iNN >= COMPASSTIMEOUT)
    {
    LCD_WrStr(6,"No Compass EOC");
    return 0xFFFF;    //Fehelrcode zurück ! Abfangen in Berechnung !
    };
tdelay(10);
hitwd();
outport(COMP_CTRL_OUT+COPA_I_SS,0);    // /Slave Select
tdelay(10);
hitwd();
wVal=0;    //Lösche Var !! WICHTIG !!
for(iNN=15;iNN >= 0 ;iNN--)//16 Bit von MSB nach LSB schieben
    {
    outport(COMP_CTRL_OUT+COPA_I_SCLK,0);
    tdelay(1);
    if( GetPortBit(COMP_CTRL_IN,COPA_O_SDO) )//if 1 dann setze bit
        {
        wVal |= (1<<iNN);    //Or Bit setzen, wenn 1};
        //0 braucht nicht gelöscht zu werden, ist ja schon 0
        };
    outport(COMP_CTRL_OUT+COPA_I_SCLK,1);
    hitwd();
    };//for
outport(COMP_CTRL_OUT+COPA_I_SS,1);
outport(COMP_CTRL_OUT+COPA_I_CAL,1);
hitwd();
return wVal;
};

//*** MODEM SECTION ***
//**********************************************
xmem ULONG GetMyModem_ID(void)    //ret ULONG |-->gcaStr
//**********************************************
{//returns also in gcaStr the strtype ID
//*** !!!! Nicht verwqechseln mit logischer Nr !!!-->BYTE gbMyFP_ID_LockNr;
//aascFlushRdBuf(gpst_Z1_Channel);//lösche Rx-Buf
strcpy(gcaStr,"00#V");
ExecuteRadioTX(gcaStr);
tdelay(10); //await Rx Answer
CopyStrFromTo(gcaStr,COM_Rx_Buff_Zx(MODEM_PORT,FP_ETX),0,6);//DigadesModem has a 6Count ID.
return atoi(gcaStr);
};

//**********************************************
root char* Get_FP_Data_Str(char* caDest)    // ret 0 ist noch nicht vollständig + \0
//**********************************************
{
auto WORD wParser;
wParser=0;
while( wParser < gstunFP_HD.stFP_HD.bFP_DatSum )
    {
    *(caDest + wParser )=gbaDelimitVal[wParser + FP_HEADER_BYTE_MAX];    //+Offset Header
    wParser++;
    };
*(caDest + wParser)=0;
return caDest;
};

//**********************************************
root int TearLimiterStr(char* lpsInStr) //-->gbaDelimitVal[STRSIZE]   |.>Return counts
//**********************************************
{
auto char* pcStrSeg;
auto WORD wSeg;
for(wSeg=0;wSeg < FP_DATA_BYTE_MAX;wSeg++)//Clear Target
    {
    gbaDelimitVal[wSeg] = 0;
    };
//1. Aufruf
wSeg=0; //WICHTIG
if( (pcStrSeg=strtok(lpsInStr,gcaDelimiter)) == NULL ){return wSeg;};//Abbruch falls kein Delimiter
do
    { //Test first Char==# and second char not 0
    if( (*pcStrSeg=='#') && (*(pcStrSeg+1)!=0) )    // "#0", "#3" and not {'#',0 };
        {
        switch(*(pcStrSeg+1))// "#0" oder "#3"
            {              //Für dieses Zechen gilt eine Sonderregelung bei der Übertragung
            case '0' :      //da die 0 + 3 Binär nicht übertragen werden darf
                {
                gbaDelimitVal[wSeg]=0;
                break;
                };
            case '3':
                {
                gbaDelimitVal[wSeg]=3;
                break;
                };
            case '1':
                {
                gbaDelimitVal[wSeg]=FP_DELIMITER;    //','
                break;
                };
            };//Switch
        }
    else
        {
        gbaDelimitVal[wSeg]=*pcStrSeg;
        };//if         //n+1 Aufruf of strtok
    }while( ((pcStrSeg=strtok(NULL,gcaDelimiter)) != NULL) && (wSeg++ < FP_DATA_BYTE_MAX) );
gwDataParser=FP_HEADER_BYTE_MAX; //1. DataPosition Vorbereitend für AbfrageAbtastung;
*lpsInStr=0;    //Gebe Leeren Str zurück weil rest 0x2 stehen bleibt
return wSeg;
};

//**********************************************
root char* CatByteOnDelimiterStr(char* lpcDest,BYTE bInByte)    //ret--> lpcDest + ','
//**********************************************
{
auto BYTE baC[3];
switch(bInByte)//Für dieses Zechen gilt eine Sonderregelung bei der Übertragung
    {             //da die 0 + 3 Binär nicht übertragen werden darf
    case 0 :
        {
        *baC='#'; //# ist Kennmarke für 0      -- "#0"
        *(baC+1)='0';
        break;
        };
    case FP_ETX    : //0x3
        {
        *baC='#'; //# ist Kennmarke für 3    -- "#3"
        *(baC+1)='3';
        break;
        };
    case FP_DELIMITER: //ßx3
        {
    *baC='#'; //# ist Kennmarke für ','   --"#1"
    *(baC+1)='1';
        break;
        };
    default :
        {
        *baC=bInByte;
        *(baC+1)=0;
        };
    }; //switch
*(baC+2)=0;
strcat(lpcDest,baC);
strcat(lpcDest,gcaDelimiter);
return lpcDest; // + ','    //DELIMITER
};

//**********************************************
xmem void FP_Head_Clear(void)
//**********************************************
{
*gbaFP_DATA=0;    //Clear Transverstr
gstunFP_HD.stFP_HD.bFP_STX=FP_STX;
gstunFP_HD.stFP_HD.bFP_From=gbMyFP_ID_LockNr;
gstunFP_HD.stFP_HD.bFP_To=FP_MASTER;
gstunFP_HD.stFP_HD.bFP_What=
gstunFP_HD.stFP_HD.bFP_DatSeg=
gstunFP_HD.stFP_HD.bFP_DatSum=
gstunFP_HD.stFP_HD.bFP_DatState=
gstunFP_HD.stFP_HD.bFP_ChkSum=
0;
};

//#define FP_HEADER_BYTE_MAX 255
//**********************************************
void FP_Head_Cat(void)
//**********************************************
{
auto WORD wNN;
*gbaFP_DATA=0;
for(wNN=0;wNN < FP_HEADER_BYTE_MAX;wNN++)
    {
    CatByteOnDelimiterStr( gbaFP_DATA ,*(gstunFP_HD.baFP_HD_Str + wNN) );
    }
};

/*
#define STAT_COMPLETE 0x00
*/
//**********************************************
root char* PackAndTX_ModemData(BYTE bAction,char* pcTxData)    //-->pcDest,gbActiveModemScanCnt;
//**********************************************
{
auto BYTE bTxDatLen;
auto BYTE bTxDatParse;
auto BYTE bSeg;

bTxDatParse=0;
bTxDatLen=strlen(pcTxData);    //lohnt sich aus Zeitersparniss

FP_Head_Clear();        //*gbaFP_DATA=0;
gstunFP_HD.stFP_HD.bFP_What=bAction;
gstunFP_HD.stFP_HD.bFP_DatSum=bTxDatLen;

//FP_HEADER_BYTE_MAX
//MODEM_STR_MAX
if(bTxDatLen)    // !=0 -->**Vorsicht !! Daten sind zu übertragen**
    {//Noch Daten Da
    while(bTxDatParse < bTxDatLen)
        {//Fülle Sendestring auf
        hitwd();
        gstunFP_HD.stFP_HD.bFP_DatSeg=++bSeg;    //1..nn
        gstunFP_HD.stFP_HD.bFP_DatState=bTxDatParse;        // Von 0... Bis
        FP_Head_Cat();        //build head
        while( ((strlen(gbaFP_DATA) + 4) < MODEM_STR_MAX) && (bTxDatParse < bTxDatLen) )
            {
            hitwd();
LCD_WrStr(2,pcTxData);
            *gcaStr=pcTxData[bTxDatParse];
LCD_WrStr(3,gcaStr);
            CatByteOnDelimiterStr( gbaFP_DATA,*(pcTxData + bTxDatParse++ ) ); //PARSER++
            };
        ExecuteRadioTX(gbaFP_DATA);
        };//while
    }//if
else    //**Keine weiteren Daten da..
    {
    gstunFP_HD.stFP_HD.bFP_DatSeg=0;//0         wenn 0=Kein Seg
    gstunFP_HD.stFP_HD.bFP_DatState=STAT_COMPLETE;
    FP_Head_Cat();        //build head
    ExecuteRadioTX(gbaFP_DATA);
    };
return gstunFP_HD.baFP_HD_Str;
};

//**********************************************
root void Do_HNDL_FP(char* pcRxStr)    //FunkProtokoll
//**********************************************
{
//FP_HEADER_BYTE_MAX
auto BYTE bCC;
auto char caBuf[22];
hitwd();
if((pcRxStr=strchr(pcRxStr,FP_STX)) != NULL)    //search to 0x2=STX in Header
    {
    TearLimiterStr(pcRxStr);    //-->gbaDelimitVal[]
    for(bCC=0;bCC<FP_HEADER_BYTE_MAX;bCC++)    //Fülle headerstruct
        {
        *(gstunFP_HD.baFP_HD_Str+bCC)=(BYTE)gbaDelimitVal[bCC];
        };
    *(gstunFP_HD.baFP_HD_Str+bCC);
    }
else
    {
    return ;
    };

if(gstunFP_HD.stFP_HD.bFP_STX==FP_STX)    //STX on Correct Position ?
    {
LCD_WrStr(1,"FP_STX OK");
    if(gstunFP_HD.stFP_HD.bFP_From==FP_MASTER)
        {
LCD_WrStr(1,"FP_MASTER OK");
        if(gstunFP_HD.stFP_HD.bFP_To==gbMyFP_ID_LockNr)
            {
LCD_WrStr(1,"bFP_To OK");
            gwMasterPollCnt=0; //MasterTIMEOUT RESET
            switch(gstunFP_HD.stFP_HD.bFP_What)
                {
//!!!!!!!!!!! MSG MSG MSG MSG MSG MSG MSG MSG MSG MSG MSG MSG !!!!!!!
                case 0:
                    {
                    break;
                    };//Case
                case DO_DISPLAY_MSG :
                    {
                    Get_FP_Data_Str(gcaStr);
                    LCD_WrStr(1,gcaStr);
                    tdelay(500);
                    break;
                    };//Case
                case    DO_GIVE_MODEM_ID :
                    {
                    GetMyModem_ID();    //-->gcaStr
                    LCD_WrStr(0,"DO_GIVE_MODEM_ID");
                    PackAndTX_ModemData(DO_MODEM_ID_RESPONSE,gcaStr);    //-->pcDest
                    LCD_WrStr(0,"ID Done");
                    break;
                    };//Case
                case DO_TAKE_YOUR_NEW_ID :
                    {
                    *gcaStr=0;
                    LCD_WrStr(0,"DO_TAKE_YOUR_NEW_ID");
                    Get_FP_Data_Str(gcaStr);
                    gbMyFP_ID_LockNr=atoi(gcaStr);
                    LCD_WrStr(0,"Lock log ID:");
                    LCD_WrStr(1,gcaStr);
                    break;
                    };
                case DO_GIVE_LIVESIGN :
                    {
                    PackAndTX_ModemData(DO_LIVESIGN_RESPONSE,"");    //-->pcDest
                    LCD_WrStr(0,"Livesign");
                    break;
                    };
                case DO_SetFPChannel :
                    {
                    strcpy(gcaStr,"00#F");
                    Get_FP_Data_Str(caBuf);
                    strcat(gcaStr,caBuf);
                    ExecuteRadioTX(gcaStr);
                    LCD_WrStr(0,"DO_SetFPChannel");
                    break;
                    };//Case
                case DO_GIVE_COMPASSVAL :
                    {
                    sprintf(gcaStr,"Compass:%u Grad",ReadCompass());
                    PackAndTX_ModemData(DO_TAKE_COMPASSVAL,gcaStr);    //-->pcDest
                    LCD_WrStr(0,"DO_GIVE_COMPASSVAL");
                    break;
                    };//Case
                };//Switch
            };//if
        };//if
    };//if
};

//**********************************************
xmem BYTE FP_ChangeChannel(BYTE bNewCh)
//**********************************************
{
sprintf(gcaStr,"00#F%002d",bNewCh);
ExecuteRadioTX(gcaStr);
tdelay(MODEM_FREQUENCY_MIN_CHANGE_TIME);
hitwd();
gbActiveFPChannel=bNewCh;
};

/*

        . .
       /
            O
        ||
    /------\
                ^
*/

/************************************************************************************/
/*@@WORK ** WORK ** WORK ** WORK ** WORK ** WORK ** WORK ** WORK ** WORK ** WORK ****/
/************************************************************************************/

/*****************************************************************************
***************    calculation with COMPLEX data type     ********************
*****************************************************************************/

//----------------------------------------------------------------------------
// functions with COMPLEX data type
//----------------------------------------------------------------------------

//**********************************************
// tanh COMPLEX: tanhc = sinhc/coshc
xmem COMPLEX tanhc (COMPLEX xc)
//**********************************************
{
auto    COMPLEX yc1, yc2, yc3;                      // COMPLEX variable
yc2.real = sinh(xc.real)*cos(xc.imag);      // sinhc
yc2.imag = cosh(xc.real)*sin(xc.imag);

yc3.real = cosh(xc.real)*cos(xc.imag);      // coshc
yc3.imag = sinh(xc.real)*sin(xc.imag);

// division of COMPLEX data
yc1.real = Divide( (yc2.real*yc3.real + yc2.imag*yc3.imag) ,
                                                (yc3.real*yc3.real + yc3.imag*yc3.imag)
                                        );
yc1.imag = Divide( (yc2.imag*yc3.real - yc2.real*yc3.imag) ,
                                                (yc3.real*yc3.real + yc3.imag*yc3.imag)
                                    );
return yc1;
};
//-----------------------------------------------------------------------------

//**********************************************
// artanh COMPLEX: artanhc = 0.5*ln((1 + w)/(1 - w))
xmem COMPLEX artanhc (COMPLEX xc)
//**********************************************
{
auto    COMPLEX yc1, yc2, yc3;               // COMPLEX variable
    // addition of COMPLEX data
    yc1.real = xc.real + 1.0;
    yc2.real = 1.0 - xc.real;

    yc1.imag = xc.imag;
    yc2.imag = -xc.imag;

    // division of COMPLEX data
        yc3.real = Divide( (yc1.real*yc2.real + yc2.imag*yc1.imag),
                                                (yc2.real*yc2.real + yc2.imag*yc2.imag)
                                            );
        yc3.imag = Divide( (yc1.imag*yc2.real - yc1.real*yc2.imag),
                                            (yc2.real*yc2.real + yc2.imag*yc2.imag)
                                            );

        // logarithm of COMPLEX data
        yc1.real = 0.5*(log(yc3.real*yc3.real + yc3.imag*yc3.imag));
        yc1.imag = atan(Divide(yc3.imag,yc3.real));

        // multiplication of COMPLEX data
        yc1.real = 0.5*yc1.real;
        yc1.imag = 0.5*yc1.imag;

        return yc1;
}
//----------------------------------------------------------------------------

//**********************************************
// sin COMPLEX: sinc = sin(x)*cosh(y) + i * cos(x)*sinh(y)
xmem COMPLEX sinc (COMPLEX xc)
//**********************************************
{
auto COMPLEX yc1;                    // COMPLEX variable
yc1.real = sin(xc.real)*cosh(xc.imag);
yc1.imag = cos(xc.real)*sinh(xc.imag);
return yc1;
}
//----------------------------------------------------------------------------

//**********************************************
// asin COMPLEX: asinc = -i*ln(i*w + sqrt(1 - w*w))
xmem COMPLEX asinc (COMPLEX xc)
//**********************************************
{
auto        COMPLEX yc1, yc2,yc3;               // COMPLEX variable

// multiplication of COMPLEX data
yc1.real = xc.real*xc.real - xc.imag*xc.imag;
yc1.imag = 2*xc.real*xc.imag;

yc1.real = 1 - yc1.real;
yc1.imag = -yc1.imag;

// square root of COMPLEX data
yc3.real = sqrt((yc1.real + sqrt(yc1.real*yc1.real + yc1.imag*yc1.imag)) / 2.0);
yc3.imag = -sqrt((-yc1.real + sqrt(yc1.real*yc1.real + yc1.imag*yc1.imag)) / 2.0);

// change real, imag
yc1.real = -xc.imag + yc3.real;
yc1.imag = xc.real + yc3.imag;

// logarithm of COMPLEX data
yc2.real = -0.5*(log(yc1.real*yc1.real + yc1.imag*yc1.imag));
yc2.imag = -atan( Divide(yc1.imag,yc1.real) );

// change real, imag
yc1.real = -yc2.imag;
yc1.imag = yc2.real;                                        // ?????????

return yc1;
};
//-----------------------------------------------------------------------------

//**********************************************
// multiplication real*COMPLEX
xmem COMPLEX mult1c (DOUBLE x, COMPLEX xc)
//**********************************************
{
auto        COMPLEX yc1;                    // COMPLEX variable
yc1.real = x * xc.real;
yc1.imag = x * xc.imag;
return yc1;
}
//-----------------------------------------------------------------------------

//**********************************************
// multiplication COMPLEX*COMPLEX
xmem COMPLEX mult2c (COMPLEX xc1, COMPLEX xc2)
//**********************************************
{
auto        COMPLEX yc1;                    // COMPLEX variable
yc1.real = (xc1.real*xc2.real) - (xc1.imag*xc2.imag);
yc1.imag = (xc1.real*xc2.imag) + (xc1.imag*xc2.real);
return yc1;
}
//-----------------------------------------------------------------------------

//**********************************************
// addition COMPLEX+COMPLEX
xmem COMPLEX addc (COMPLEX xc1, COMPLEX xc2)
//**********************************************
{
auto COMPLEX yc1;    // COMPLEX variable
yc1.real = xc1.real + xc2.real;
yc1.imag = xc1.imag + xc2.imag;
return yc1;
}
//-----------------------------------------------------------------------------

//**********************************************
// power (COMPLEX, DOUBLE)
xmem COMPLEX powc (COMPLEX xc, DOUBLE x)
//**********************************************
{
auto         DOUBLE absc, phi;
auto         COMPLEX yc1;                      // COMPLEX variable

absc = sqrt(xc.real*xc.real + xc.imag*xc.imag);
phi = atan( Divide(xc.imag,xc.real) );

yc1.real = pow(absc, x)*cos(x*phi);
yc1.imag = pow(absc, x)*sin(x*phi);

return yc1;
};
//-----------------------------------------------------------------------------

/****************************************************************************
Name:        NMEA2Dbl.cpp

Input:        NMEA (gcaGPSDUMMY[])

Output:     X, Y of Gauss-Krueger coordinate system

Descrip.:   Read a NMEA-Dummie and convert into X, Y
*****************************************************************************/

// converts degree and minutes to degree
//**********************************************
xmem DOUBLE dezimal(BYTE bDeg, DOUBLE dMinute)
//**********************************************
{
return (bDeg + ((10.0*dMinute/6.0) / 100.0));
}

//coor
//**********************************************
xmem int Ell_coor_dbl(void)    //needs GPS_GGA_SRUCTTYPE & Ell_coor_dbl_STRUCTTYPE
//**********************************************
{
gstEllCorDbl.dLati_deg = dezimal(gstGGA.bLatitude_Grad,gstGGA.dLatitude_Min);
gstEllCorDbl.dLongi_deg = dezimal(gstGGA.bLongitude_Grad,gstGGA.dLongitude_Min);
if (gstGGA.cGlobus_NS=='S')
    {
gstEllCorDbl.dLati_deg = (-1)*gstEllCorDbl.dLati_deg;
};
if (gstGGA.cGlobus_WE=='W')
    {
    gstEllCorDbl.dLongi_deg = (-1)*gstEllCorDbl.dLongi_deg;
    };
return 0;
}

/****************************************************************************/
// artanh
//**********************************************
xmem DOUBLE artanh(DOUBLE x)
//**********************************************
{
return(0.5 * log( Divide((1.0 + x),(1.0 - x)) ));
}

//Elli
//**********************************************
root int GPS_EllipseToGausKrueger(DOUBLE* fpX_Ret,DOUBLE* fpY_Ret)    //returns CbR X,Y
//**********************************************
{
auto DOUBLE fPHI,fDeltaLAM,fLAM_0;
auto int iLL, iNN;
auto DOUBLE fXV,fYV;
auto DOUBLE fDD, fEE, KK, N0, fXX, fYY;
auto COMPLEX xBI, xBI1, xEB, xSS, xWW, xZZ;
auto float fNr;

sprintf(gcaStr,"Satcnt:%d",gstGGA.bSatellitsCount);
LCD_WrStr(1,gcaStr);
sprintf(gcaStr,"DOP_Qu:%f",gstGGA.dSat_DOP_Quality);
LCD_WrStr(2,gcaStr);
sprintf(gcaStr,"ObjDst:%f",gfObjDist);
LCD_WrStr(3,gcaStr);

Ell_coor_dbl();

fYV =fXV = 0;
fNr=floor( (gstEllCorDbl.dLongi_deg + 1.5) / 3.0 );
fPHI = gstEllCorDbl.dLati_deg * gf_D2R;
fLAM_0 = fNr * 3.0;
fDeltaLAM = (gstEllCorDbl.dLongi_deg - fLAM_0) * gf_D2R;
xWW.real = artanh(sin(fPHI)) - gf_E1*artanh(gf_E1*sin(fPHI));
xWW.imag = fDeltaLAM;
xBI = asinc(tanhc(xWW));
fDD = 0.75*gf_E2;
fEE = fDD;
xEB.real = fDD;
xEB.imag = 0;
xBI1 = asinc(tanhc(addc(xWW, mult1c(gf_E1, artanhc(mult1c(gf_E1, sinc(xBI)))))));
xBI = xBI1;
for(iNN=2; iNN<=10; iNN++)
    {
    hitwd();
fDD = 0.75*gf_E2;
fEE = fDD;
xEB.real = fDD;
xEB.imag = 0;
KK = 1.0;
xSS.real = KK;
xSS.imag = 0;
for (iLL=2; iLL<=iNN; iLL++)
    {
        hitwd();
    N0 = iLL - 1.0;
        fDD = fDD*(2.0*N0 + 1.0)*(2.0*N0 + 3.0)/((2.0*N0 + 2.0)*(2.0*N0 + 2.0))*gf_E2;
    fEE = fEE + fDD;
    N0 = iLL - 2.0;
    KK = KK*(2.0*N0 + 2.0)/(2.0*N0 + 3.0);
    xSS = addc(xSS, mult1c(KK, powc(sinc(xBI), (2.0*(iLL - 1.0)))));
    xEB = addc(xEB, mult1c(fDD, xSS));
    };
    xBI1 = asinc(tanhc(addc(xWW, mult1c(gf_E1, artanhc(mult1c(gf_E1, sinc(xBI)))))));
xZZ = addc((mult1c((gf_M0*(1.0 + fEE)), xBI1)),
                        mult1c((-1.0),(mult2c((mult1c(gf_M0/2.0, xEB)),
                        (sinc(mult1c(2.0, xBI1)))))));
fXX = xZZ.real;
fYY = xZZ.imag;
    if ( (fabs(fXX-fXV) < gf_EPS) && (fabs(fYY-fYV) < gf_EPS) )
    {
    break;
    };
fXV = fXX;
fYV = fYY;
    xBI = xBI1;
    };
fYY = fNr * 1000000.0 + 500000.0 + fYY;
*fpX_Ret=fXX;
*fpY_Ret=fYY;

hitwd();
sprintf(gcaStr,"X:%f",fXX);
LCD_WrStr(4,gcaStr);

hitwd();
sprintf(gcaStr,"Y:%f",fYY);
LCD_WrStr(5,gcaStr);

return 0;
};

//DIR
//**********************************************
root float Direction(float* pfGK_Pos1,float* pfGK_Pos2) // compute direction
//**********************************************
{
auto float fDirec;
auto float fDifOfLat_2min1, fDifOfLon_2min1;
auto float fDifOfLat_1min2, fDifOfLon_1min2;

fDifOfLat_2min1 = pfGK_Pos2[0]-pfGK_Pos1[0];
fDifOfLon_2min1 = pfGK_Pos2[1]-pfGK_Pos1[1];

fDifOfLat_1min2 = pfGK_Pos1[0]-pfGK_Pos2[0];
fDifOfLon_1min2 = pfGK_Pos1[1]-pfGK_Pos2[1];

if (pfGK_Pos1[0] <= pfGK_Pos2[0])
    {
if ((pfGK_Pos1[0] == pfGK_Pos2[0]) && (pfGK_Pos1[1] <= pfGK_Pos2[1]))
    {
    fDirec = gf_PI/2;
    }
else
        {
    if ((pfGK_Pos1[0] == pfGK_Pos2[0]) && (pfGK_Pos1[1] > pfGK_Pos2[1]))
        {
        fDirec = -gf_PI/2;
        }
        else
            {
            fDirec = atan( Divide(fDifOfLon_2min1,fDifOfLat_2min1) );
        };
    };
};

if ((pfGK_Pos1[0] > pfGK_Pos2[0]) && (pfGK_Pos1[1] <= pfGK_Pos2[1]))
    {
if (pfGK_Pos1[1] == pfGK_Pos2[1])
    {
    fDirec = gf_PI;
    }
else
    {
    fDirec = gf_PI/2 + atan( Divide(fDifOfLat_1min2,fDifOfLon_2min1) );
        };
    };

if ((pfGK_Pos1[0] > pfGK_Pos2[0]) && (pfGK_Pos1[1] > pfGK_Pos2[1]))
    {
     fDirec = -gf_PI/2 - atan( Divide(fDifOfLat_1min2,fDifOfLon_1min2) );
     };

fDirec = fDirec * gf_R2D;
return fDirec;
};

//DIS
//**********************************************
root float Distance(float* pfGK_Pos1,float* pfGK_Pos2) //Compute distance
//**********************************************
{
auto float fX_Dist, fY_Dist;
fX_Dist = pfGK_Pos2[0] - pfGK_Pos1[0];
fY_Dist = pfGK_Pos2[1] - pfGK_Pos1[1];
return sqrt( (fX_Dist*fX_Dist) + (fY_Dist*fY_Dist) );
};

//GTG
//**********************************************
root void GetTargetGuidance(void)
//**********************************************
{
auto WORD wSearchDir;
auto DOUBLE dGPS_X,dGPS_Y;
gfObjDist=gfObjDir=0;    // 0 Stellen

//1.Search actually Position;
GetUserPosition(&dGPS_X,&dGPS_Y);

if(dGPS_X==0 || dGPS_Y==0){return;};    //Fehler

gfaUserPos[0]=floor(dGPS_X);
gfaUserPos[1]=floor(dGPS_Y);
//2.Compare with Target

//2a-Distance
//!!! HIER KOMMEN DIE DATEN VOM MASTER FUER TARGET!!!!
gfObjDist = Distance(gfaUserPos, gfaTargetPos); //!!! >TARGET POS DUMMMY

//2b-Direction
gfObjDir = Direction(gfaUserPos, gfaTargetPos); //!!! >TARGET POS

//3. investigate Compassdirection
wSearchDir=MoveDirec(ReadCompass(),gfObjDir); //?? Eventuell unötiger Global

sprintf(gcaStr,"Go:%d Dst:%f",wSearchDir,gfObjDist);
LCD_WrStr(3,gcaStr);
};

//UPS
//**********************************************
#define GET_GPS_GGA_WAITITME 0xFFF
root void GetUserPosition(DOUBLE* pdGPS_X,DOUBLE* pdGPS_Y)    //Needs Two DOUBLE Vars &Var1,&Var2
//**********************************************
{
auto char* pczNMEA;
//Debug("GetUserPosition");
*pdGPS_X=0;
*pdGPS_Y=0;
//aascRxSwitch(gpst_Z0_Channel,1);    //Einschalten
pczNMEA=Scan_GPS_RX_MSG();
if( (pczNMEA != NULL) && (!strncmp(pczNMEA,gcaGPGGA,6)) )    //strcmp        //= "$GPGGA"
    {
    GPS_EllipseToGausKrueger(pdGPS_X,pdGPS_Y);    //returns X,Y
    };
};

//**********************************************
xmem WORD MoveDirec(WORD wCom_Angle, float fAzi_Angle)
//**********************************************
{
//LCD_WrStr(0,"MoveDirec");
if (fAzi_Angle < 0)
    {
    fAzi_Angle += 360.0;
    };
        // magn. Nord - geogr. Nord
return (360 - (Round(wCom_Angle - fAzi_Angle + 1.0) % 360)); //+Offset NPol
}

/***************************************************************************
    End of Functions
***************************************************************************/

//GRAPHIC_SECTION

//XXX
//****************************************************
xmem void Entrance_Scr(void)
//****************************************************
{
auto BYTE bMsg;
LCD_Clr();
LCD_WrStr(0,"* SOCRATEC GmbH *");
LCD_WrStr(1,"* Maxhuetter Str.16 *");
LCD_WrStr(2,"* 93158 Teublitz *");
LCD_WrStr(3,"* www.socratec.de *");
LCD_WrStr(4,"*T:09471 - 3100200 *");
LCD_WrStr(5,"*F:09471 - 3100209 *");
LCD_WrStr(6,"*** PARK AND FIND ***");
LCD_WrStr(7,"....Press any KEY");
outport(KEY_CTRL_OUTPORT+KEYINTENABLE_BIT,0);    //Disable Interrupt
do
    {
    hitwd();
    bMsg=GetKeyScan();    //Scan the state of the keypad
    }while( !bMsg );    //Taste gedrückt ist !=0
outport(KEY_CTRL_OUTPORT+KEYINTENABLE_BIT,1);    //Enable Interrupt
};

//****************************************************
xmem void Menue_Scr(void)
//****************************************************
{
auto BYTE bMsg;
LCD_Clr();
LCD_WrStr(0,"Start : GPS Position");
LCD_WrStr(1,"F1=Read Compass");
LCD_WrStr(2,"F2=Reset Comp.");
LCD_WrStr(3,"F3=Calibrate Comp.");
LCD_WrStr(4,"F4=Modem-ID");
LCD_WrStr(5,"F5=Entrance Scr");
LCD_WrStr(6,"9=Send Msg");
LCD_WrStr(7,"Shift=Menue");
do
    {
    hitwd();
    bMsg=GetKeyScan();    //Scan the state of the keypad
    }while( !bMsg );    //Taste gedrückt ist !=0
DoHandl_MSG(bMsg);    //gbKeyMsg ====> Key_switch_funktion
};

//****************************************************
xmem void LCD_INIT_12864(void)
//****************************************************
{//gbDOUT_OutVal
outport(DINOUT_OUT_ENABLE,1);//Sperre OUTPORT FUER Z_WORLD_CARD ONLINE ************
RES(&gbDOUT_OutVal,LCD_RES);outport(LCD_CMD_PRT,gbDOUT_OutVal);
hitwd();
SET(&gbDOUT_OutVal,LCD_RES);outport(LCD_CMD_PRT,gbDOUT_OutVal);
LCD_StatChk(1);
LCD_StatChk(2);
LCD_WriteCmd(1,LCD_CMD_ON);
LCD_WriteCmd(2,LCD_CMD_ON);
LCD_ClearCS(1);    //Felder Löschen .
LCD_ClearCS(2);
};

//****************************************************
root BYTE LCD_ReadStatus(BYTE bCS)
//****************************************************
{
auto BYTE bRet;
outport(DINOUT_OUT_ENABLE,1);    //Sperre OUTPORT FUER Z_WORLD_CARD ************

if(bCS==1)    //Select Chip /CS1
    {
    RES(&gbDOUT_OutVal,LCD_CS2);
    SET(&gbDOUT_OutVal,LCD_CS1);
    }
else    //Select Chip /CS2
    {
    RES(&gbDOUT_OutVal,LCD_CS1);
    SET(&gbDOUT_OutVal,LCD_CS2);
    };
outport(LCD_CMD_PRT,gbDOUT_OutVal);

SET(&gbDOUT_OutVal,LCD_RW);        //, da ich ja lesen möchte
RES(&gbDOUT_OutVal,LCD_DI);
SET(&gbDOUT_OutVal,LCD_E);
outport(LCD_CMD_PRT,gbDOUT_OutVal);
hitwd();
bRet=inport(LCD_DATAIN_PRT);
RES(&gbDOUT_OutVal,LCD_E);
outport(LCD_CMD_PRT,gbDOUT_OutVal);
hitwd();
return bRet;
};

//****************************************************
xmem void LCD_Clr(void)        //NEEDS Block 1 or Block 2
//****************************************************
{
auto BYTE bNN;
for(bNN=0;bNN<LCD_ROWS;bNN++)
    {
    LCD_WrStr(bNN," ");
    };
};

//****************************************************
root void LCD_StatChk(BYTE bCS)        //NEEDS Block 1 or Block 2
//****************************************************
{
auto BYTE bSt;    //Hier finded die Kanalumschaltung statt.
auto WORD wTT;
wTT=0;
do{
    hitwd();
    bSt=LCD_ReadStatus(bCS);
    }while( ((bSt & LCD_BUSY_FLAG) || (bSt & LCD_RESET_FLAG)) && (wTT++< 0xFFF) );    //Ist eins der Bits gesetzt ?
};

//****************************************************
root void LCD_WriteCmd(BYTE bCS,BYTE bCmd)
//****************************************************
{
LCD_StatChk(bCS); //Mit CS Auswahl
RES(&gbDOUT_OutVal,LCD_RW);    //Write
RES(&gbDOUT_OutVal,LCD_DI);    //Instruction
outport(LCD_CMD_PRT,gbDOUT_OutVal);
SET(&gbDOUT_OutVal,LCD_E);
outport(LCD_CMD_PRT,gbDOUT_OutVal);

outport(DINOUT_OUT_ENABLE,0);    //Freigabe DINOUT OUT FUER Z_WORLD_CARD ************

outport(LCD_DATAOUT_PRT,bCmd);    //Daten anlegen
RES(&gbDOUT_OutVal,LCD_E);
outport(LCD_CMD_PRT,gbDOUT_OutVal);
};

//****************************************************
root void LCD_WriteData(BYTE bCS,BYTE bData)
//****************************************************
{
LCD_StatChk(bCS); //Mit CS Auswahl
RES(&gbDOUT_OutVal,LCD_RW);    //Write
SET(&gbDOUT_OutVal,LCD_DI);    //Data
outport(LCD_CMD_PRT,gbDOUT_OutVal);

SET(&gbDOUT_OutVal,LCD_E);
outport(LCD_CMD_PRT,gbDOUT_OutVal);

outport(DINOUT_OUT_ENABLE,0);    //FUER Z_WORLD_CARD ONLINE ************
outport(LCD_DATAOUT_PRT,bData);

RES(&gbDOUT_OutVal,LCD_E);
outport(LCD_CMD_PRT,gbDOUT_OutVal);
};

//****************************************************
xmem BYTE LCD_ReadData(BYTE bCS)
//****************************************************
{
auto BYTE bRet;
LCD_StatChk(bCS);
outport(DINOUT_OUT_ENABLE,1);    //Sperre DINOUT-OUT FUER Z_WORLD_CARD ONLINE ************
SET(&gbDOUT_OutVal,LCD_RW);    //Read
SET(&gbDOUT_OutVal,LCD_DI);    //Data
outport(LCD_CMD_PRT,gbDOUT_OutVal);

SET(&gbDOUT_OutVal,LCD_E);
outport(LCD_CMD_PRT,gbDOUT_OutVal);

hitwd();
bRet=inport(LCD_DATAIN_PRT);
RES(&gbDOUT_OutVal,LCD_E);
outport(LCD_CMD_PRT,gbDOUT_OutVal);
return bRet;
};

//XXX
//****************************************************
root void LCD_WrStr(BYTE bLn,char* pcStr)
//****************************************************
{//ZEICHEN von 0x20 bis 0x7E
auto BYTE caBuff21[LCD_COLS+1];
auto BYTE bBl,bYPos,bScan,bCh,bStrLen;
auto WORD wChScnLn;

if( !*pcStr ){return;};

bBl=1;    //Immer links anfangen mit CS1
strncpy(caBuff21,pcStr,LCD_COLS); //IN Zeichenspeicher pcStr,21); //21 --> 128/6=21,3
caBuff21[LCD_COLS]=0;    ///Auf alle Fälle beenden

bStrLen=strlen(caBuff21);

//Fill Str with Space
for(bCh=bStrLen;bCh < LCD_COLS;) //CH for ch;
    {
    caBuff21[bCh++]=32; //Space
    };
caBuff21[bCh]=bYPos=bCh=0;

LCD_WriteCmd(bBl,LCD_CMD_STARTZEILE);        //Startposition
LCD_WriteCmd(bBl,LCD_CMD_PAGE | (bLn % LCD_ROWS) );
LCD_WriteCmd(bBl,LCD_CMD_SET_Y_ADR); //LCD_WriteCmd(bBl,LCD_CMD_SET_Y_ADR | bYPos );

while( caBuff21[bCh] && (bCh < LCD_COLS) )    //
    {
    if( (caBuff21[bCh] < 0x20) || (caBuff21[bCh] > 0x7E) )//Überschreibe mit SPACE falls nicht in Liste
        {
        caBuff21[bCh]=0x20;    //dann halt Space
        };
    for(bScan=0;bScan < 6;)    //Zeilenscan fuer die Pixelzusammensetzung eines Chars
        {
        if(bYPos > 63)
            {
            if(bBl==2){return;};    //Fertig
            bBl=2;
            bYPos=0;
            LCD_WriteCmd(bBl,LCD_CMD_STARTZEILE | 0 );                //Startposition
            LCD_WriteCmd(bBl,LCD_CMD_PAGE | (bLn % LCD_ROWS) );
            LCD_WriteCmd(bBl,LCD_CMD_SET_Y_ADR | bYPos );    //Setze an Anfang Vom CS2 Block
            }
//LCD_WriteCmd(bBl,LCD_CMD_SET_Y_ADR | bYPos++);    //Automatisch ?
        bYPos++;
        wChScnLn=abs( (caBuff21[bCh] - 0x20) * 6 + bScan++);     //!!Hier Increment bCh++ + bScan++
        LCD_WriteData(bBl,gcaEngFont6x8[wChScnLn % LCDCHARROWSUM]);
        };    //for
    bCh++;        //Next Char
    }; //while
outport(LCD_CMD_PRT,gbDOUT_OutVal);
RES(&gbDOUT_OutVal,LCD_CS1);
RES(&gbDOUT_OutVal,LCD_CS2);
};

//****************************************************
xmem BYTE LCD_ClearCS(BYTE bCS)
//****************************************************
{
auto BYTE bXX,bYY;
LCD_WriteCmd(bCS,LCD_CMD_SET_Y_ADR | 0);
for(bXX=0;bXX < LCD_ROWS;)
    {
    LCD_WriteCmd(bCS,LCD_CMD_PAGE | bXX++ );
    for(bYY=0;bYY < 64;bYY++)
        {
        LCD_WriteData(bCS,0);            //bYY-->Auto Increment
        };
    };
RES(&gbDOUT_OutVal,LCD_CS1);
RES(&gbDOUT_OutVal,LCD_CS2);
outport(LCD_CMD_PRT,gbDOUT_OutVal);
};

//main
//**********************************************
//** MAIN ** ** MAIN ** ** MAIN ** ** MAIN **
//**********************************************
root void main(void)
{
//**VARS*
auto DOUBLE dGPS_X;
auto DOUBLE dGPS_Y;
auto char *pcRx;
auto WORD wReads;
auto WORD wCount;
auto BYTE bErrorWDTO;
auto BYTE bErrorSuperReset;
auto struct tm t;

_GLOBAL_INIT(); //WICHTIG !! MUST BE . START INITS
bErrorWDTO=_sysIsWDTO();
bErrorSuperReset=_sysIsSuperReset();
/*INITS*/
hitwd();
lp31Clk6MHz();    //lp31Clk3MHz(); //3MHZ
/*
#define GPS_CTRL_OUT             ADR_DS2
#define GPS_CTRL_In             ADR_DS2
#define GPS_PWR_OFFON_01    7    //Bit 7
*/
outport(MBR_ADR_OFFON_01,1);    //Schaltet/BRESET auf Freigabe
outport(GPS_CTRL_OUT + GPS_PWR_OFFON_01,0);    //GPS PWR OUT //GPS 3.3V aus
gbDOUT_OutVal=0;
outport(LCD_CMD_PRT,gbDOUT_OutVal); // = outport(DOUT_PORT,0);
outport(DINOUT_OUT_ENABLE,1);    //Ausschalten

eioBrdInit(0);
gpst_Z0_Channel=gpst_Z1_Channel=NULL;

//**Ellipsenparameter
//gf_A = 6378388.0                    //Bessel
//gf_B = gf_A-gf_A / 297.0;
gf_PI = 3.14159265358979324;
gf_A =     6378137.0;                        /* (m) */        //WGS 84
gf_B        = 6356752.314245191;    /* (m) */
gf_E2   = 1.0 - ( (gf_B*gf_B) / (gf_A*gf_A) );
gf_E1   = sqrt(gf_E2);
gf_M0   = gf_A * (1.0 - gf_E2);
gf_D2R = gf_PI / 180.0;
gf_R2D = 180.0 / gf_PI;
gf_EPS = 0.1;                            //accuracy

//******TIMER*********
//clockspeed *1200.0 / 20.0* 750.0 HZ für 1 ms Delays
init_timer1( (WORD)floor( (sysclock()*1200.0)/(20.0*750.0) ) );    //750Hz=1mS erwünscht

#ifdef DEVELOPMENTSTATUS_ENABLE
    runwatch();
#endif

//III
//***Z180-Interuppt-Section ***
#define ITC_INT0_MASK_ADR    0x34 //Bit 0 reset = Mask bzw.DISABLE Int0
rtcIRQ(0);    // INT0 disable, !! important by set up interrupt !!

//RTC**** RTC **********
//Registers A of RTC same adress how DINOUT_IN CS1R
//out    0x4020,    //out    0x4000, RTC_DATAPORT    //

#JUMP_VEC RST38_VEC My_Int0_ISR    //interruptroutine
rtcInit();

    //    set all alarm fields to don't care, you can
    //    set specific values in a field to indicate
    //    alarm "only when the value of RTC matches this
    //    value". This way, you can generate intervals at
    //    a second, a minute, an hour, a day or every
    //    month.
    t.tm_sec = 0xc0;
    t.tm_min = 0xc0;
    t.tm_hour = 0xc0;
    t.tm_mday = 0xc0;

    rtcSetAlmTime(&t);
    rtcClrIRQ();    //clears all the interrupt flags that can cause

    rtcSwAIE(0); //for alarm interrupt enable
    rtcSwPIE(0); //for periodic interrupt enable
    rtcSwUIE(0); //for update ended interrupt enable
    rtcSwWIE(0); //for wakeup interrupt enable
    rtcSwKSE(1); //for kickstart interrupt enable
    rtcSwRIE(0); //for ram clear interrupt enable

    rtcIRQ(1);        //!! enable INT0 to receive RTC IRQ !!


//******KEYbord*********
// WIRD INTERRUPT GESTEUTERT

//******* LCD *********
LCD_INIT_12864();

gfaTargetPos[0]=5452600.0; //******* DUMMY FOR TARGET ****
gfaTargetPos[1]=4506000.0; //******* DUMMY FOR TARGET ****

//OUT_PORTVALUES
gwMasterPollCnt=0;

//********** COM-initialisierungen ************
OpenAndInitComPorts(0);
OpenAndInitComPorts(1);
aascFlushRdBuf(gpst_Z0_Channel);    //lösche Rx-Buf
aascFlushRdBuf(gpst_Z1_Channel);    //lösche Rx-Buf
aascRxSwitch(gpst_Z0_Channel,1);    //Einschalten
aascRxSwitch(gpst_Z1_Channel,1);    //Einschalten

//**** Other Hardware ****
//****Compass Init
ResetCompass();    //Includes CompassInit() Timer must enabled before this can work

//INIT Modem to Channel 28
//1.Anmelden //Enrol by Master
gbMyFP_ID_LockNr=REBIRTH_SLAVE_ID;    //0xFF
FP_Head_Clear();
FP_ChangeChannel(REBIRTH_FP_CH);    //-->gbActiveFPChannel
hitwd();
//Zeige ERRORS vor Neustart an
if(bErrorSuperReset)
    {LCD_WrStr(0,"ErrorSuperReset");};
if(bErrorWDTO)
    {LCD_WrStr(1,"ErrorWDTO");};
//Zeige CPUSpeed an
sprintf(gcaStr,"CPU:%f Hz",sysclock()*1200.0);
LCD_WrStr(7,gcaStr);

outport(ZW_LED_OFFON_01,0);    //onbordled aus
outport(KEY_X_PORT,0);    //Stelle Scan an den Anfang
outport(KEY_CTRL_OUTPORT+KEYINTENABLE_BIT,1);    //Enable Interrupt

wCount=0;    //MAINSchleifenzähler TESTZweck

Entrance_Scr();
Menue_Scr();

//*** RUN_LOOP ***
while(1)
    {
    hitwd();    //Watchdog
    TestPwr();
//MODEM
//***MODEM_SECTION    POOLING *****
    if(gwMasterPollCnt++ > 100) // Timeout with MasterConnection ??
        {
        gbMyFP_ID_LockNr=REBIRTH_SLAVE_ID;
        };
    pcRx=Scan_Modem_RX_MSG();    //POLLING
    if( (pcRx != NULL) && (*pcRx) )
        {
        LCD_WrStr(1,pcRx);    //Anzeige Modem in LCD
        };

//GPS
//***GPS_SECTION    POOLING *****
    outport(GPS_CTRL_OUT+GPS_PWR_OFFON_01,1);    //GPS PWR ON
    tdelay(1);
    pcRx=Scan_GPS_RX_MSG();
LCD_WrStr(0,pcRx);    //Anzeige GPS in LCD
/*
    if( (pcRx != NULL) && (!strncmp(pcRx,"$GPGGA",6)) )    //strcmp
        {//Alles OK
        LCD_WrStr(0,pcRx);    //Anzeige GPS in LCD
//WIRKLICH ABSCHALTEN ???????????????????????
        outport(GPS_CTRL_OUT+GPS_PWR_OFFON_01,0);    //GPS PWR OUT
        GetTargetGuidance();    //Erzeugt Suchrichtungsermittlung /
        GetUserPosition(&dGPS_X,&dGPS_Y);    //Needs Two DOUBLE Vars &Var1,&Var2
        };
*/
//WIRKLICH ABSCHALTEN ???????????????????????
outport(GPS_CTRL_OUT+GPS_PWR_OFFON_01,0);    //GPS PWR OUT

//***Compass POOLING *****
    sprintf(gcaStr,"Compass: %u Grad",ReadCompass());
    LCD_WrStr(5,gcaStr);
    sprintf(gcaStr,"%u Run Loops",wCount++);
    LCD_WrStr(6,gcaStr);
    };//while

aascClose(gpst_Z0_Channel);
aascClose(gpst_Z1_Channel);
sysStandby();
};//main
/*******************************************/
/********** ENDE MAIN **********************/
/*******************************************/

//KKK
/*******************************************/
xmem BYTE GetKeyScan(void)
/*******************************************/
{
auto BYTE bYInVal;
auto BYTE bBitTastY;
auto BYTE bBitTastX;
//auto BYTE baShowCh[2];

outport(KEY_CTRL_OUTPORT+KEYINTENABLE_BIT,0);    //Disable Ínterrupt
outport(KEY_X_PORT,0);    //Stelle Scan an den Anfang
bYInVal=GetPrellFree(KEY_Y_PORT);    //Lade Wert am UND-Gater

//Lese anstehhenden Wert an Y
//ermittle dir Y-BitNummer     gbBitTastY startet mit 0
bBitTastX=bBitTastY=0;    //set to Start=NULL. Erstes Bit ~0x1 == 0xFE
while( ((bYInVal & (1<<bBitTastY)) != 0x00 ) && (bBitTastY < KEYMATRIX_Y) ) //Suche nach ertser NULL
    {
    bBitTastY++;
    };

if(bBitTastY >= KEYMATRIX_Y)    //Fehler if greater than Matrix
    {
    outport(KEY_X_PORT,0);    //Stelle Scan an den Anfang
    return 0;
    };    //in this case, goto allowed

//ermittele die X Bi tposition
outport(KEY_X_PORT,0xFE);    //bBitTastX=0;
while( ((bYInVal=GetPrellFree(KEY_Y_PORT) & (1<<bBitTastY)) != 0x00 ) && (bBitTastX < KEYMATRIX_X) )//Gefunden wenn Bitpos stimmt
    {
    bBitTastX++;
    outport(KEY_X_PORT,~(1<<bBitTastX)); //1.=11111101 then bit for bit to 0
    };
    //Gefunden wenn Bitpos stimmt
if(bBitTastX >= KEYMATRIX_X)
    {
    outport(KEY_X_PORT,0);    //Stelle Scan an den Anfang
    return 0;
    };    //in this case, goto allowed
//Generate Msg
bYInVal=gbaKeyMatrix[bBitTastX % KEYMATRIX_X][bBitTastY % KEYMATRIX_Y];

//baShowCh[0]=bYInVal;
//baShowCh[1]=0;
//sprintf(gcaStr,"TASTE:%s X:%d Y:%d",baShowCh,bBitTastX,bBitTastY);
//LCD_WrStr(6,gcaStr);

//if(bYInVa != 0) {Beep(1,BEEPDAUER); };    //Kein Beeper momentan da.

outport(KEY_X_PORT,0);    //Stelle Scan an den Anfang
return bYInVal;
};

//#JUMP_VEC RST28_VEC int0ISR//interruptroutine
//*******************************************
xmem interrupt reti void My_Int0_ISR() //interrupt reti
//*******************************************
{//rtcIRQ
//ACHTUNG wenn innerhalb eiens Interrrupt ein Tdelay() benutz wird, steht die Kiste ??
auto BYTE bKeyVal;
//auto BYTE baShowCh[2];
hitwd();
bKeyVal=rtcChkKF();
if( rtcChkKF() )    //If Kickstart/int interrupt Flag seted
    {
    outport(KEY_CTRL_OUTPORT+KEYINTENABLE_BIT,0);    //Disable Ínterrupt    on AND-GATE
//rtcIRQ(0);    //!! disable INT0 to receive RTC IRQ !!
    bKeyVal=GetKeyScan();    //!! NOW- Scan the state of the keypad
    if(bKeyVal)    //test Keyevent-case and Process it
        {
//baShowCh[0]=bKeyVal;
//baShowCh[1]=0;
//sprintf(gcaStr,"KEY:%s INT->DOHndl(%x)",baShowCh,bKeyVal);
//LCD_WrStr(4,gcaStr);
    rtcSwKF(0); //Turn of KickstartFlag
        EI();    //!!!!!! SONST STÜRTZ CPU ab if tdelay(x) is used;
        DoHandl_MSG(bKeyVal);    //gbKeyMsg ====> Key_switch_funktion
        };
//Wait on KeyRelease !! ALl is WAIT !!! IS THIS OK ??????
    while( (GetPrellFree(KEY_Y_PORT) & KEY_RELEASE_MASK) != KEY_RELEASE_MASK )
        {
        hitwd();
        LCD_WrStr(7,"Taste loslassen");
        };
    LCD_WrStr(7,"Tasten bereit");
    rtcSwKF(0); //Turn of KickstartFlag
//rtcSwKSE(1); //for kickstart interrupt enable
//rtcIRQ(1);    //!! enable INT0 to receive RTC IRQ !!
    outport(KEY_CTRL_OUTPORT+KEYINTENABLE_BIT,1);    //Enable Interrupt
    };//if
};