#define FOSC 16000000
//#define MPLABSIM
#define PIC18F2620
//#define PIC18F8720
//#define PIC18F8722

#if defined(PIC18F2620) || defined(PIC18F8722)
	#define EUSART
#else
	#define AUSART
#endif

#if defined(PIC18F8720) || defined(PIC18F8722)
	#define DUALUART
#endif

// Set to 8 for Simulator (MPLAB v7.11) or 18F452 or 18F8720
// Set to 64 for 18F2620 or 18F8722
#if defined (PIC18F8720) || defined(MPLABSIM)
	#define FPM_PAGESIZE 8
#else
	#define FPM_PAGESIZE 64
#endif 

#if defined(DUALUART)
	#define TXREG TXREG1
	#define OERR OERR1
	#define CREN CREN1
	#define RCREG RCREG1
	#define SPEN SPEN1
	#define TX9 TX9_1
	#define TXEN TXEN1
	#define SYNC SYNC1
	#define BRGH BRGH1
	#define RX9 RX9_1
	#define ADDEN ADDEN1
	#define SPBRG SPBRG1
	#if defined(EUSART)
		#define SPBRGH SPBRGH1
		#define WUE WUE1
		#define ABDEN ABDEN1
		#define BRG16 BRG16_1
	#endif
	#if defined(PIC18F8722)
		#define TXIF TX1IF
		#define TXIE TX1IE
		#define RCIF RC1IF
		#define RCIP RC1IP
		#define TXIP TX1IP
		#define RCIE RC1IE
	#endif
#endif

/*******/
// Retest RTC timings
// Retest serial I/O - SWS TX seems to break SWS RX when initialised
// Test LCD
// Test ADC
// BTN UP in conjunction with LCD write causes corruption
// Use LATx instead of PORTx??
// LATB.7 as output for CTS? - yes set B.7 to 1
// 
// A/D routines
// Rotator routines
// Host mode
// Maidenhead locator routines


// GPS routines
// Example (signal not acquired):
// $GPRMC,235947.000,V,0000.0000,N,00000.0000,E,,,041299,,*1D
// Example (signal acquired):
// $GPRMC,092204.999,A,4250.5589,S,14718.5084,E,0.00,89.68,211200,,*25
// Example (ETREX)
// $GPRMC,090932,A,5130.1301,N,00010.4065,W,0.0,263.7,180305,2.6,W,A*13
// Example (RF Soln)
// $GPRMC,091113.00,A,5130.13535,N,00010.40224,W,000.0,215.2,180305,05.2,W,A*05



// LVB Tracker
// Copyright 2003-2005 Howard Long G6LVB
//
// PLAN13 algorithm:
// Copyright 1983-2005 James R. Miller G3RUH
//
// PLAN13 C port:
// Copyright 1997-2005 Edson Pereira N1VTN
//
// AOS/LOS algorithms: 
// Copyright 1992-2002 John Magliacane KD2BD

// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or (at 
// your option) any later version. This program is distributed in the
// hope that it will be useful, but WITHOUT ANY WARRANTY; without even 
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the GNU General Public License for more details. You 
// should have received a copy of the GNU General Public License along 
// with this program; if not, write to the Free Software Foundation, 
// Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

/********************************************************************************/
/* Declarations start here                                                      */
/********************************************************************************/

// rambank usage:
//  0
//  1 PRD, MNU, FPM, GPS, ROT, BTN
//  2 PRD
//  3 KEP
//  4 DEE, PRD (blacklist)
//  5 DEE, PRD (AOSLOSSTRUCTs), CC8E Math, HST
//  6
//  7
//  8 Plan13
//  9 Plan13
// 10 Plan13
// 11 
// 12 LCD, RTC, Serial I/O, SND, ADC
// 13 Serial I/O
// 14 Serial I/O
// 15

/********************************************/
/* Device configuration bits                */
/********************************************/

// See Microchip doc 39626b.pdf, pp249-256
// Ignore CC8E warning: "Data above address 0xFFFF is removed in the COD file"
#if defined(PIC18F2620)
	#pragma config[0x00]=0b.0000.0000
	#pragma config[0x01]=0b.0000.0110
	#pragma config[0x02]=0b.0001.1000
	#pragma config[0x03]=0b.0001.1110
	#pragma config[0x04]=0b.0000.0000
	#pragma config[0x05]=0b.1000.0001
	#pragma config[0x06]=0b.1000.0000
	#pragma config[0x07]=0b.0000.0000
	#pragma config[0x08]=0b.0000.1111
	#pragma config[0x09]=0b.1100.0000
	#pragma config[0x0A]=0b.0000.1111
	#pragma config[0x0B]=0b.1110.0000
	#pragma config[0x0C]=0b.0000.1111
	#pragma config[0x0D]=0b.0100.0000
#endif
#if defined(PIC18F8720)
	#pragma config[0x01]=0b.0010.0110
	#pragma config[0x02]=0b.0001.1000
	#pragma config[0x03]=0b.0001.1110
	#pragma config[0x04]=0b.1111.0011
	#pragma config[0x05]=0b.1000.0011
	#pragma config[0x06]=0b.1000.0000
	#pragma config[0x08]=0b.1111.1111
	#pragma config[0x09]=0b.1100.0000
	#pragma config[0x0A]=0b.1111.1111
	#pragma config[0x0B]=0b.1110.0000
	#pragma config[0x0C]=0b.1111.1111
	#pragma config[0x0D]=0b.0100.0000
#endif
#if defined(PIC18F8722)
	#pragma config[0x01]=0b.0000.0110
	#pragma config[0x02]=0b.0000.1100
	#pragma config[0x03]=0b.0000.1110
	#pragma config[0x04]=0b.1000.0011
	#pragma config[0x05]=0b.0000.0011
	#pragma config[0x06]=0b.1000.0000
	#pragma config[0x08]=0b.1111.1111
	#pragma config[0x09]=0b.1100.0000
	#pragma config[0x0A]=0b.1111.1111
	#pragma config[0x0B]=0b.1110.0000
	#pragma config[0x0C]=0b.1111.1111
	#pragma config[0x0D]=0b.0100.0000
#endif

/********************************************/
/* CC8E interrupt declarations              */
/* Note further CC8E declarations after ISR */
/********************************************/

#include <int18xxx.h>

/*******************************************/
/* Generic declarations                    */
/*******************************************/

typedef uns8 U8;
typedef uns16 U16;
typedef uns24 U24;
typedef int8 S8;
typedef int16 S16;
typedef int24 S24;
typedef int24 MYLONG;
typedef bit BOOL;
typedef float32 MYFLOAT;
#define F // Used to be able to port code to/from Microsoft C to enable definition of 32 bit literal floats

#define NULL 0
#define FALSE 0
#define TRUE 1

/********************************************/
/* Hardware not yet defined in CC8E headers */
/********************************************/

bit T0PS0 @ T0CON.0;
bit T0PS1 @ T0CON.1;
bit T0PS2 @ T0CON.2;
bit T2OUTPS0 @ T2CON.3;
bit T2OUTPS1 @ T2CON.4;
bit T2OUTPS2 @ T2CON.5;
bit T2OUTPS3 @ T2CON.6;
bit PCFG0 @ ADCON1.0;
bit PCFG1 @ ADCON1.1;
bit PCFG2 @ ADCON1.2;
bit PCFG3 @ ADCON1.3;
bit ACQT0 @ ADCON2.3;
bit ACQT1 @ ADCON2.4;
bit ACQT2 @ ADCON2.5;
U16 ADRES @ 0xFC3;

/*******************************************/
/* Floating point library declarations     */
/*******************************************/

#define M_PI 3.141592653589793238F

static const MYFLOAT sq2p1	 =2.414213562373095048802e0F;
static const MYFLOAT sq2m1	 = .414213562373095048802e0F;
static const MYFLOAT pio2	 =1.570796326794896619231e0F;
static const MYFLOAT pio4	 = .785398163397448309615e0F;
static const MYFLOAT p4	 = .161536412982230228262e2F;
static const MYFLOAT p3	 = .26842548195503973794141e3F;
static const MYFLOAT p2	 = .11530293515404850115428136e4F;
static const MYFLOAT p1	 = .178040631643319697105464587e4F;
static const MYFLOAT p0	 = .89678597403663861959987488e3F;
static const MYFLOAT q4	 = .5895697050844462222791e2F;
static const MYFLOAT q3	 = .536265374031215315104235e3F;
static const MYFLOAT q2	 = .16667838148816337184521798e4F;
static const MYFLOAT q1	 = .207933497444540981287275926e4F;
static const MYFLOAT q0	 = .89678597403663861962481162e3F;

/*******************************************/
/* Generic I/O declarations                */
/*******************************************/

typedef enum {PE_NUL,PE_LCD,PE_HWS,PE_SWS1,PE_SWS2,PE_MRS} PORTENUM;

/*******************************************/
/* Flash Program Memory declarations       */
/*******************************************/

#define FPM_DATA 0xF000
#define FPM_ERASEBLOCKSIZE 64
#define FPM_ERASEBLOCKMASK (FPM_ERASEBLOCKSIZE-1)
#define FPM_PAGEMASK (FPM_PAGESIZE-1)
#define FPM_MAXRETRIES 2 // Number of attempts to rewrite Flash before failure

/*******************************************/
/* Queue declarations                      */
/*******************************************/

typedef struct
{
	U8 u8In;
	U8 u8Out;
	U8 u8Size;
	char size2 *pac;
} QUEUESTRUCT;

/************************************************/
/* Floating point ASCII conversion declarations */
/************************************************/

#define FPA_MIDPOINT 12

static const MYFLOAT _affpa[26]=
{
	0.0000000000001F,
	0.000000000001F,
	0.00000000001F,
	0.0000000001F,
	0.000000001F,
	0.00000001F,
	0.0000001F,
	0.000001F,
	0.00001F,
	0.0001F,
	0.001F,
	0.01F,
	0.1F, // <-- FPA_MIDPOINT index
	1.0F,
	10.0F,
	100.0F,
	1000.0F,
	10000.0F,
	100000.0F,
	1000000.0F,
	10000000.0F,
	100000000.0F,
	1000000000.0F,
	10000000000.0F,
	100000000000.0F,
	1000000000000.0F
};

/*******************************************/
/* RTC declarations                        */
/*******************************************/

// TC= 65536 - (Fosc / 4  / tickspersec)
#define RTC_TIMER1RELOAD (FOSC/4/1000)

typedef struct
{
	U16 u16Timer1Reload;
	U16 u16Tick; // 1/1000th sec, continually increments
	U16 u16MSec; // 1/1000th sec, rolls over after 999
	U8 u8Sec;
	U8 u8Min;
	U8 u8Hour;
	U8 u8Day;
	U8 u8Month;
	U16 u16Year;
} RTCSTRUCT;

#pragma rambank 12
static RTCSTRUCT _rtcs;
#pragma rambank 0

/*******************************************/
/* ADC declarations                        */
/*******************************************/

#define ADC_NUMCHANNELS 2
#define ADC_LOG2NUMSAMPLES 4
#define ADC_NUMSAMPLES (1<<ADC_LOG2NUMSAMPLES) // Must be a power of 2
#define ADC_CHANNELAZ 0
#define ADC_CHANNELEL 1

typedef struct
{
	U8 u8Sample;
	U8 u8Channel;
	U16 u16Acc;
	U16 au16[ADC_NUMCHANNELS];
} ADCSTRUCT;

#pragma rambank 1
static ADCSTRUCT _adcs;
#pragma rambank 0

/*******************************************/
/* ROT declarations                        */
/*******************************************/

#define ROT_LEFT LATC.0
#define ROT_RIGHT LATC.1
#define ROT_DOWN LATC.2
#define ROT_UP LATC.3

#define ROT_LEFT_DIR TRISC.0
#define ROT_RIGHT_DIR TRISC.1
#define ROT_DOWN_DIR TRISC.2
#define ROT_UP_DIR TRISC.3

// Degrees = ( (S16) (ADC - u16Off) ) / fMul
// ADC = (U16) ( ( ( Degrees * fMul ) + u16Off )
typedef struct
{
	S16 s16AzTarget; // The target Az as an ADC value
	S16 s16ElTarget; // The target El as an ADC value
	BOOL bAzTrack;
	BOOL bElTrack;
} ROTSTRUCT;

typedef struct
{
	U16 u16AzOff;
	U16 u16ElOff;
	MYFLOAT fAzMul;
	MYFLOAT fElMul;
	bit bAz450; // True if it's a 450 degree rotator
	bit bSNS; // True if it's a South-North-South rotator
} ROTDEESTRUCT;

#pragma rambank 1
static ROTSTRUCT _rs;
#pragma rambank 0

/*******************************************/
/* SND declarations                        */
/*******************************************/

//************ To use the sound port with ICD 
//************ we need to select a different port!
#if defined(PIC18F8720) || defined(PIC18F8722)
#define SND_PORT LATD.0
#define SND_PORT_DIR TRISD.0
#else
#define SND_PORT LATB.6
#define SND_PORT_DIR TRISB.6
#endif

typedef struct
{
	U8 u8Count;
	BOOL bActive;
	BOOL bBuffer;
} SNDSTRUCT;

#pragma rambank 12
static SNDSTRUCT _ss;
#pragma rambank 0

#define SND_RELOAD 8 // Uses 9600 TMR2 as a timebase

/*******************************************/
/* Morse (MRS) declarations                */
/*******************************************/

// number of milliseconds for a dit 'on'
// 50ms is 12WPM, 120ms is 5WPM
#define MRS_RELOAD 50

typedef struct
{
	char c;
	U8 u8Len;
	U8 u8Symbol;
} MRSDATASTRUCT;

static const MRSDATASTRUCT _amds[]=
{
	{'A',2,0b.0000.0001},
	{'B',4,0b.0000.1000},
	{'C',4,0b.0000.1010},
	{'D',3,0b.0000.0100},
	{'E',1,0b.0000.0000},
	{'F',4,0b.0000.0010},
	{'G',3,0b.0000.0110},
	{'H',4,0b.0000.0000},
	{'I',2,0b.0000.0000},
	{'J',4,0b.0000.0111},
	{'K',3,0b.0000.0101},
	{'L',4,0b.0000.0100},
	{'M',2,0b.0000.0011},
	{'N',2,0b.0000.0010},
	{'O',3,0b.0000.0111},
	{'P',4,0b.0000.0110},
	{'Q',4,0b.0000.1101},
	{'R',3,0b.0000.0010},
	{'S',3,0b.0000.0000},
	{'T',1,0b.0000.0001},
	{'U',3,0b.0000.0001},
	{'V',4,0b.0000.0001},
	{'W',3,0b.0000.0011},
	{'X',4,0b.0000.1001},
	{'Y',4,0b.0000.1011},
	{'Z',4,0b.0000.1100},
	{'0',5,0b.0001.1111},
	{'1',5,0b.0000.1111},
	{'2',5,0b.0000.0111},
	{'3',5,0b.0000.0011},
	{'4',5,0b.0000.0001},
	{'5',5,0b.0000.0000},
	{'6',5,0b.0001.0000},
	{'7',5,0b.0001.1000},
	{'8',5,0b.0001.1100},
	{'9',5,0b.0001.1110},
	{'.',6,0b.0001.0101},
	{',',6,0b.0011.0011},
	{':',6,0b.0011.1000},
	{'?',6,0b.0000.1100},
	{'\'',6,0b.0001.1110},
	{'-',6,0b.0010.0001},
	{'/',5,0b.0001.0010},
	{'(',6,0b.0010.1101},
	{')',6,0b.0010.1101},
	{'"',6,0b.0001.0010},
	{'@',6,0b.0001.1010},
	{'=',5,0b.0001.0001},
	{'\0',0,0b.0000.0000}
};

#define MRS_QUEUESIZE 40

typedef struct
{
	U16 u16Count;
	U8 u8Mask;
	U8 u8Symbol;
} MRSSTRUCT;

#pragma rambank 12
static char _acmrs[MRS_QUEUESIZE];
QUEUESTRUCT _qsmrs;
MRSSTRUCT _ms;
#pragma rambank 0

/*******************************************/
/* LCD declarations                        */
/*******************************************/

// LVB Tracker LCD pin assignments
#define LCD_RS LATB.0
#define LCD_RW LATB.1
#define LCD_E LATB.2
#define LCD_RS_DIR TRISB.0
#define LCD_RW_DIR TRISB.1
#define LCD_E_DIR TRISB.2
#define LCD_DATA PORTA
#define LCD_DATA_DIR TRISA
#define LCD_DATA_SHIFT 2 // Number of bits LCD data bit 0 is shifted left from PORTx.0
#define LCD_DATA_MASK 0b.0011.1100
#define LCD_DATA_NOTMASK 0b.1100.0011

// PICDEM2 LCD Pin Assignments 
/*
#define LCD_RS LATA.3
#define LCD_RW LATA.2
#define LCD_E LATA.1
#define LCD_RS_DIR TRISA.3
#define LCD_RW_DIR TRISA.2
#define LCD_E_DIR TRISA.1
#define LCD_DATA PORTD
#define LCD_DATA_DIR TRISD
#define LCD_DATA_SHIFT 0
#define LCD_DATA_MASK 0b.0000.1111
#define LCD_DATA_NOTMASK 0b.1111.0000
#define LCD_TIMERWAIT 200 // Time to wait in ms to update LCD to stop flickering
*/

#pragma rambank 12
static BOOL _bLCDActive;
#pragma rambank 0

/*******************************************/
/* Button declarations                     */
/*******************************************/

#define BTN_ENABLE_ LATB.3
#define BTN_ENABLE_DIR TRISB.3
#define BTN_DATA PORTA
#define BTN_DATA_DIR TRISA
#define BTN_DATA_SHIFT 2 // Number of bits button data bit 0 is shifted left from PORTx.0
#define BTN_DATA_MASK 0b.0011.1100
#define BTN_DATA_NOTMASK 0b.1100.0011

#define BTN_BIT_LEFT 0
#define BTN_BIT_RIGHT 1
#define BTN_BIT_DOWN 2
#define BTN_BIT_UP 3

#define BTN_LEFT 1
#define BTN_RIGHT 2
#define BTN_DOWN 4
#define BTN_UP 8

#define BTN_ESC BTN_LEFT
#define BTN_OK BTN_RIGHT
#define BTN_PREV BTN_UP
#define BTN_NEXT BTN_DOWN

#pragma rambank 1
static U8 _u8btnLast;
#pragma rambank 0

/*******************************************/
/* Serial declarations                     */
/*******************************************/

/* Serial #defines and hardware allocation */

// The RX bits must be on upper four bits of port B
#define SWS_RX1BIT 4
#define SWS_RX2BIT 5

bit SWS_RX1D @ PORTB.SWS_RX1BIT;
bit SWS_RX1DIR @ TRISB.SWS_RX1BIT;
bit SWS_RX2D @ PORTB.SWS_RX2BIT;
bit SWS_RX2DIR @ TRISB.SWS_RX2BIT;

bit SWS_TX1D @LATC.4;
bit SWS_TX1DIR @TRISC.4;
bit SWS_TX2D @LATC.5;
bit SWS_TX2DIR @TRISC.5;

#define SWS_RX1QUEUESIZE 80
#define SWS_TX1QUEUESIZE 80
#define SWS_RX2QUEUESIZE 80
#define SWS_TX2QUEUESIZE 80
#define HWS_RXQUEUESIZE 80
#define HWS_TXQUEUESIZE 80

// 64 + 1:16 prescale for one bit time at 9600bps and 40MHz
// 103 + 1:4 prescale for one bit time at 9600bps and 16MHz
// TC = fosc/4 / prescale / 9600 - 1

#if FOSC <= 39000000
  #if FOSC <= 9000000
    #define SWS_TXTIMER2CONST (FOSC/4/9600-1)
    #define SWS_TXTIMER2PS0 0 // 1:1 prescaler
    #define SWS_TXTIMER2PS1 0
  #else
    #define SWS_TXTIMER2CONST (FOSC/4/4/9600-1)
    #define SWS_TXTIMER2PS0 1 // 1:4 prescaler
    #define SWS_TXTIMER2PS1 0
  #endif
#else
  #define SWS_TXTIMER2CONST (FOSC/4/16/9600-1)
  #define SWS_TXTIMER2PS0 0 // 1:16 prescaler
  #define SWS_TXTIMER2PS1 1
#endif

/* Serial typedefs */

typedef enum 
{
	SBRE_1200,
	SBRE_2400,
	SBRE_4800,
	SBRE_9600
} S_BAUDRATEENUM;

/* Software serial typedefs */

typedef struct
{
	U8 u8BitCount; // Bit countdown when shifting out data - 0 is resting, 1 is stop, 2-9 is bits, 10 is start
	U16 u16TimerReload; // 1 bit time minus fudge factor
	U16 u16TimerStart; // 5/4 bit times (was 1.5 but not reliable)
	char c; // Shift register for incoming character
} SWSRXSTRUCT;

typedef struct
{
	U8 u8BitCount;
	U8 u8Count; // countdown to each bit
	U8 u8CountReload; // reload value for u8Count
	char c; // character being shifted out
	bit bBuffer; // 1 bit buffer to reduce jitter
} SWSTXSTRUCT;

/* Software Serial constant data */

// 208 is one bit time at 9600bps and 16MHz, 1:2 prescale
// 521 is one bit time at 9600bps and 40MHz, 1:2 prescale
// TC = fosc/4 / prescale / 9600
static const U16 _au16swsrx1Timer0[4]=
{
	FOSC/4/2/1200,
	FOSC/4/2/2400,
	FOSC/4/2/4800,
	FOSC/4/2/9600
};

// 417 is one bit time at 9600bps and 16MHz xtal, 1:1 prescale
// 1042 is one bit time at 9600bps and 40MHz xtal, 1:1 prescale
// TC = fosc/4 / prescale / 9600
static const U16 _au16swsrx2Timer3[4]=
{
	FOSC/4/1/1200,
	FOSC/4/1/2400,
	FOSC/4/1/4800,
	FOSC/4/1/9600
};

static const U8 _au8swstxBaud[4]=
{
	8,
	4,
	2,
	1
};

/* Hardware Serial constant data */

// n = Fosc/BR/16-1
static const U16 _au16hwsBaud[4]=
{
#ifdef EUSART
	FOSC/1200/16-1,
	FOSC/2400/16-1,
	FOSC/4800/16-1,
	FOSC/9600/16-1
#else
	FOSC/1200/64-1,
	FOSC/2400/64-1,
	FOSC/4800/64-1,
	FOSC/9600/64-1
#endif
};

/* Serial static data allocation */

#pragma rambank 13
static char _acswstx1[SWS_TX1QUEUESIZE];
static char _acswsrx1[SWS_RX1QUEUESIZE];
static char _acswstx2[SWS_TX2QUEUESIZE];
#pragma rambank 14
static char _acswsrx2[SWS_RX2QUEUESIZE];
static char _achwsrx[HWS_RXQUEUESIZE];
static char _achwstx[HWS_TXQUEUESIZE];

#pragma rambank 12
static QUEUESTRUCT _qsswstx1;
static QUEUESTRUCT _qsswsrx1;
static QUEUESTRUCT _qsswstx2;
static QUEUESTRUCT _qsswsrx2;
static QUEUESTRUCT _qshwstx;
static QUEUESTRUCT _qshwsrx;

static SWSRXSTRUCT _swsrx1;
static SWSRXSTRUCT _swsrx2;
static SWSTXSTRUCT _swstx1;
static SWSTXSTRUCT _swstx2;
#pragma rambank 0

/*******************************************/
/* Plan13 declarations                     */
/*******************************************/

#define ONEPPM 1.0e-6

#pragma rambank 8

typedef enum
{
	P13VE_SHADOWSIDE,
	P13VE_SUNNYSIDE,
	P13VE_ECLIPSED,
	P13VE_VISIBLE,
} P13_VISIBLEENUM;

MYFLOAT observer_lon;
MYFLOAT observer_lat;
MYFLOAT observer_height;

MYFLOAT   EL;			       /* Elevation                          */
MYFLOAT   TN;			       /*                                    */

MYFLOAT   E;
MYFLOAT   N;
MYFLOAT   AZ;
MYFLOAT   SLON;
MYFLOAT   SLAT;
MYFLOAT   RR;
MYFLOAT   CL;
MYFLOAT   CS;
MYFLOAT   SL;
MYFLOAT   CO;
MYFLOAT   SO;
MYFLOAT   RE;
MYFLOAT   FL;
MYFLOAT   RP;
MYFLOAT   RE2;
MYFLOAT   RP2;
MYFLOAT   D;
MYFLOAT   R;
MYFLOAT   Rx;
MYFLOAT   Ry;
MYFLOAT   Rz;
MYFLOAT   Ex;
MYFLOAT   Ey;
MYFLOAT   Ez;
MYFLOAT   Ny;
MYFLOAT   Nx;
MYFLOAT   Nz;
MYFLOAT   Ox;
MYFLOAT   Oy;
MYFLOAT   Oz;
MYFLOAT   U;
MYFLOAT   Ux;
MYFLOAT   Uy;
MYFLOAT   Uz;
MYFLOAT   YT;
MYFLOAT   WW;
MYFLOAT   WE;
MYFLOAT   W0;
MYFLOAT   VOx;
MYFLOAT   VOy;
MYFLOAT   VOz;
MYFLOAT   DE;
MYFLOAT   GM;
MYFLOAT   J2;
MYFLOAT   N0;
MYFLOAT   A0;
MYFLOAT   b0;
MYFLOAT   SI;
MYFLOAT   CI;
MYFLOAT   PC;
MYFLOAT   QD;
MYFLOAT   WD;
MYFLOAT   myDC;
U16   YG;
MYFLOAT   G0;

#pragma rambank 9

MYFLOAT   MAS0;
MYFLOAT   MASD;
MYFLOAT   INS;
MYFLOAT   CNS;
MYFLOAT   SNS;
MYFLOAT   EQC1;
MYFLOAT   EQC2;
MYFLOAT   TEG;
MYFLOAT   GHAE;
MYFLOAT   MRSE;
MYFLOAT   MASE;
MYFLOAT   ax;
MYFLOAT   ay;
MYFLOAT   az;
MYLONG      OLDRN;

MYFLOAT   T;
MYFLOAT   DT;
MYFLOAT   KD;
MYFLOAT   KDP;
MYFLOAT   M;
MYLONG      DR;
MYLONG     RN;
MYFLOAT   EA;
MYFLOAT   C;
MYFLOAT   myS;
MYFLOAT   DNOM;
MYFLOAT   A;
MYFLOAT   B;
MYFLOAT   RS;
MYFLOAT   Sx;
MYFLOAT   Sy;
MYFLOAT   Sz;
MYFLOAT   Vx;
MYFLOAT   Vy;
MYFLOAT   Vz;
MYFLOAT   AP;
MYFLOAT   CW;
MYFLOAT   SW;
MYFLOAT   RAAN;
MYFLOAT   CQ;
MYFLOAT   SQ;
MYFLOAT   CXx;
MYFLOAT   CXy;
MYFLOAT   CXz;
MYFLOAT   CYx;
MYFLOAT   CYy;
MYFLOAT   CYz;
MYFLOAT   CZx;
MYFLOAT   CZy;
MYFLOAT   CZz;
MYFLOAT   SATx;
MYFLOAT   SATy;
MYFLOAT   SATz;
MYFLOAT   ANTx;
MYFLOAT   ANTy;
MYFLOAT   ANTz;
MYFLOAT   VELx;
MYFLOAT   VELy;
MYFLOAT   VELz;
MYFLOAT   Ax;
MYFLOAT   Ay;
MYFLOAT   Az;

#pragma rambank 10

//MYFLOAT   Sz;
//MYFLOAT   Vz;
MYFLOAT   GHAA;

MYFLOAT   DS;
MYFLOAT   DF;

// SunVec...
MYFLOAT   MAS;
MYFLOAT   TAS;
MYFLOAT   SUNx;
MYFLOAT   SUNy;
MYFLOAT   SUNz;
MYFLOAT   SSA;
MYFLOAT   ILL;
MYFLOAT   CUA;
MYFLOAT   UMD;
MYFLOAT   Hx;
MYFLOAT   Hy;
MYFLOAT   Hz;
MYFLOAT   SEL;
MYFLOAT   SAZ;

P13_VISIBLEENUM ECL;

/* keplerians */
char     SAT[20];
MYLONG     SATNO;
MYFLOAT   YE;
MYFLOAT   TE;
MYFLOAT   myIN;
MYFLOAT   RA;
MYFLOAT   EC;
MYFLOAT   WP;
MYFLOAT   MA;
MYFLOAT   MM;
MYFLOAT   M2;
MYLONG     RV;
MYFLOAT   ALON;
MYFLOAT   ALAT;

/* location */
char     LOC[20];
MYFLOAT   LA;
MYFLOAT   LO;
MYFLOAT   HT;

MYFLOAT      DN;

#pragma rambank 0

/*******************************************/
/* Prediction declarations                 */
/* (These sit on top of Plan13)            */
/*******************************************/

#define PRD_MAXSATS 50 // 54 bytes per KEPUNION
#define PRD_KEPUNIONSIZE 54 // must be on an even byte boundary
#define PRD_KEPNAMESIZE 10

typedef struct
{
	U16 u16Year; /* eg, 2005 */
	U8 u8Month; /* 1-12 */
	U8 u8Day; /* 1-31 */
	U8 u8Hour;
	U8 u8Min;
	U8 u8Sec;
} TIMESTRUCT;

typedef struct // If you change the size of this struct, change PRD_KEPUNIONSIZE too
{
	char szName[PRD_KEPNAMESIZE];
	U24 u24CatalogNumber;
	U8 u8EpochYear;
	MYFLOAT fEpochTime;
	U16 u16ElementSet;
	MYFLOAT fInclination;
	MYFLOAT fRAOfNode;
	MYFLOAT fEccentricity;
	MYFLOAT fArgOfPerigee;
	MYFLOAT fMeanAnomaly;
	MYFLOAT fMeanMotion;
	MYFLOAT fDecayRate;
	U24 u24EpochRev;
	U8 u8ALon;
	U8 u8ALat;
	U8 u8Priority;
} KEPSTRUCT;

typedef union
{
	U8 au8[PRD_KEPUNIONSIZE];
	KEPSTRUCT ks;
} KEPUNION;

typedef struct
{
	MYFLOAT fAzimuth;
	MYFLOAT fElevation;
	MYFLOAT fRange;
	MYFLOAT fHeight;
	MYFLOAT fSSPLon;
	MYFLOAT fSSPLat;
	MYFLOAT fRangeRate;
	MYFLOAT fSquint;
	MYLONG lOrbit;
	MYFLOAT fMA;
} PREDICTSTRUCT;

typedef struct
{
	MYFLOAT fLon;
	MYFLOAT fLat;
	MYFLOAT fHeight;
} OBSERVERSTRUCT;

typedef struct
{
	U8 u8KepIndex;
	MYFLOAT fDayAOS;
	MYFLOAT fTimeAOS;
	MYFLOAT fAzAOS;
	MYFLOAT fMaxEl;
	MYFLOAT fDayLOS;
	MYFLOAT fTimeLOS;
	MYFLOAT fAzLOS;
	MYLONG lOrbit;
	BOOL bDecayed;
	BOOL bGeostationary;
	BOOL bAOSHappens;
} AOSLOSSTRUCT;

#pragma rambank 4
static U8 _au8prdBlacklist[(PRD_MAXSATS+7)/8];
#pragma rambank 0

/*******************************************/
/* GPS declarations                        */
/*******************************************/

#define GPS_LINESIZE 100

#pragma rambank 1
static char _acgps[GPS_LINESIZE];
static U8 _u8gpsPos;
static U8 _u8gpsNumSats;
#pragma rambank 0

static BOOL GPSPoll(void);

/*******************************************/
/* HST declarations                        */
/*******************************************/

#define HST_LINESIZE 80

#define HST_ECNONE 0 // EasyComm command decode numbers
#define HST_ECAZ 1
#define HST_ECEL 2
#define HST_ECUP 3
#define HST_ECDN 4

#pragma rambank 5
static char _achst[HST_LINESIZE];
static U8 _u8hstPos;
#pragma rambank 0

static void HSTPoll(void);

/*******************************************/
/* MNU declarations                        */
/*******************************************/

static BOOL MNUMsg(PORTENUM pe,const char size2 *psz);

/*******************************************/
/* Data EEPROM declarations                */
/*******************************************/

typedef struct
{
	OBSERVERSTRUCT os; // Last observer position saved
	ROTDEESTRUCT rds; // Saved rotator settings
	U8 u8Checksum;
} DEESTRUCT;

#pragma rambank 5
static DEESTRUCT _ds;
#pragma rambank 0

/********************************************************************************/
/* The code starts here                                                         */
/********************************************************************************/

/*******************************************/
/* Interrupt routines                      */
/*******************************************/

/*************************************************/
/* High priority ISR:                            */
/* RBIF   Software serial RX start bit detection */
/* TMR0IF Software serial RX1                    */
/* TMR2IF Software serial TX and SND             */
/* TMR3IF Software serial RX2                    */
/*                                               */
/* Low priority ISR:                             */
/* TXIF   Hardware serial                        */
/* RCIF   Hardware serial                        */
/* TMR1IF RTC and MRS                            */
/*************************************************/

static void ISRHigh2(void);

#pragma origin 0x08

interrupt ISRHigh(void)
{
	// shadow / fastMode has a bug - see 80224a silicon errata
	U8 u8BSR=BSR;
	ISRHigh2();
	BSR=u8BSR;
}

#pragma origin 0x18

interrupt ISRLow(void)
{
	U8 u8STATUS=STATUS;
	U8 u8BSR=BSR;
	U8 u8W=W;
//	U8 u8FSR0L=FSR0L;
	U16 u16FSR0=FSR0; // Ignore compiler warning about saving FSR0H register!

	// handle the interrupt

  /******************************************/
	/* Serial (Hardware) interrupts           */
  /******************************************/

	if (TXIF)
	{
		if (_qshwstx.u8Out!=_qshwstx.u8In)
		{
			TXREG=_achwstx[_qshwstx.u8Out++];
			if (_qshwstx.u8Out>=_qshwstx.u8Size)
			{
				_qshwstx.u8Out=0;
			}
		}
		else
		{
			TXIE=0;
		}
		TXIF=0;
	}

	if (RCIF)
	{
		if (OERR)
		{
      CREN=0; /* Must reset Rx logic on overrun */
      CREN=1;
		}
		else
		{
			char c=RCREG;
			// This is a 'copy' of QueuePut
			U8 u8InNew=_qshwsrx.u8In+1;
			
			if (u8InNew>=_qshwsrx.u8Size)
			{
				u8InNew=0;
			}
			if (u8InNew!=_qshwsrx.u8Out)
			{
				_achwsrx[_qshwsrx.u8In]=c;
				_qshwsrx.u8In=u8InNew;
			}
		}
		RCIF=0;
	}

  /******************************************/
	/* RTC interrupt                          */
  /******************************************/

  // handle the timer1 interrupt, every 1ms
  if (TMR1IF)
  {
	  /******************************************/
		/* RTC service                            */
	  /******************************************/

		// This is a smart bit of code to reload timer 1
		{
			U16 u16;
			BOOL b=GIEH;

			GIEH=0;
			u16.low8=TMR1L;
			u16.high8=TMR1H;
			u16-=_rtcs.u16Timer1Reload;
			TMR1H=u16.high8;
			TMR1L=u16.low8;
			GIEH=b;
		}

	  /******************************************/
		/* ADC service                            */
	  /******************************************/
		// Note that the ADC interrupt is not used
		// because it converts too quickly so would 
		// slow other stuff. Better to check on the 
		// RTC.
		if (!GO)
		{
			_adcs.u16Acc+=ADRES;
			_adcs.u8Sample++;
			if (_adcs.u8Sample>=ADC_NUMSAMPLES)
			{
				U16 u16Result=_adcs.u16Acc >> ADC_LOG2NUMSAMPLES;
				_adcs.u8Sample=0;
				_adcs.au16[_adcs.u8Channel]=u16Result;

				// We can only check the Rotator here if we use
				// simple integer arithmetic within the ISR

			  if (_rs.bAzTrack && _adcs.u8Channel==ADC_CHANNELAZ)
			  {
			    S16 s16Az=(S16)u16Result;
			
			    if ((_rs.s16AzTarget>=s16Az && ROT_LEFT) ||
			      (_rs.s16AzTarget<=s16Az && ROT_RIGHT))
			    {
			      ROT_LEFT=0;
			      ROT_RIGHT=0;
			      _rs.bAzTrack=FALSE;
			    }
			  }
			
			  if (_rs.bElTrack && _adcs.u8Channel==ADC_CHANNELEL)
			  {
			    S16 s16El=(S16)u16Result;
			
			    if ((_rs.s16ElTarget>=s16El && ROT_DOWN) ||
			      (_rs.s16ElTarget<=s16El && ROT_UP))
			    {
			      ROT_DOWN=0;
			      ROT_UP=0;
			      _rs.bElTrack=FALSE;
			    }
			  }

				_adcs.u16Acc=0;
				_adcs.u8Channel++;
				if (_adcs.u8Channel>=ADC_NUMCHANNELS)
				{
					_adcs.u8Channel=0;
				}
				CHS0=_adcs.u8Channel.0;
				CHS1=_adcs.u8Channel.1;
				CHS2=_adcs.u8Channel.2;
				CHS3=_adcs.u8Channel.3;
			}
			GO=1;
		}

	  /******************************************/
		/* Morse service                          */
	  /******************************************/
		_ms.u16Count--;
		if (_ms.u16Count==0)
		{
			if (_ss.bActive) // Were we sending a sound?
			{
				_ss.bActive=FALSE; // No sound now.
				if (_ms.u8Mask!=0)
				{
					// Send an intra symbol blank (one 'unit')
					_ms.u16Count=MRS_RELOAD;
				}
				else
				{
					// Send an inter symbol blank (three 'units')
					_ms.u16Count=(U16)MRS_RELOAD+MRS_RELOAD+MRS_RELOAD;
				}
			}
			else
			{
				U8 u8Mask=_ms.u8Mask;

				if (u8Mask==0)
				{
					// Load next character from queue (QueueGet)
					if (_qsmrs.u8Out!=_qsmrs.u8In)
					{
						char c=_acmrs[_qsmrs.u8Out++];

						if (_qsmrs.u8Out>=_qsmrs.u8Size)
						{
							_qsmrs.u8Out=0;
						}

						if (c==' ')
						{
							// For a space, add in another four 'units'
							_ms.u16Count=(U16)MRS_RELOAD+MRS_RELOAD+MRS_RELOAD+MRS_RELOAD;
						}
						else
						{
							// Now locate Morse symbol for the given character
							U24 u24TBLPTR=TBLPTR;
							U8 u8TABLAT=TABLAT;
							const MRSDATASTRUCT *pmds=_amds;
							BOOL bFinished=FALSE;
							char c2;

							if (c>='a' && c<='z')
							{
								// Capitalise if necessary
								c-='a'-'A';
							}

							// Search the const array
							do
							{
								c2=pmds->c;
								if (c==c2)
								{
									bFinished=TRUE;
								}
								else
								{
									if (c2=='\0')
									{
										bFinished=TRUE;
									}
									else
									{
										pmds++;
									}
								}
							} while (!bFinished);

							if (c2==c)
							{
								U8 u8Len=pmds->u8Len-1;

								_ms.u8Symbol=pmds->u8Symbol;
								u8Mask=1<<u8Len;
								_ms.u8Mask=u8Mask;
							}
							TABLAT=u8TABLAT;
							TBLPTR=u24TBLPTR;
						}
					}
				}

				if (u8Mask!=0)
				{
					_ss.bActive=TRUE; // Switch the sound on.
					if ((u8Mask & _ms.u8Symbol)!=0)
					{
						// Dash... three 'units'
						_ms.u16Count=(U16)MRS_RELOAD+MRS_RELOAD+MRS_RELOAD;
					}
					else
					{
						// Dit... one 'unit'
						_ms.u16Count=MRS_RELOAD;
					}
					u8Mask>>=1;
					_ms.u8Mask=u8Mask;
				}
			}
		}

	  /******************************************/
		/* Back to RTC service                    */
	  /******************************************/
    _rtcs.u16Tick++;
    _rtcs.u16MSec++;
		if (_rtcs.u16MSec>=1000)
		{
			_rtcs.u16MSec=0;
			_rtcs.u8Sec++;
			if (_rtcs.u8Sec>=60)
			{
				_rtcs.u8Sec=0;
				_rtcs.u8Min++;
				if (_rtcs.u8Min>=60)
				{
					_rtcs.u8Min=0;
					_rtcs.u8Hour++;
					if (_rtcs.u8Hour>=24)
					{
						BOOL b=FALSE;

						_rtcs.u8Hour=0;
						_rtcs.u8Day++;

						if (_rtcs.u8Day>31)
						{
							b=TRUE;
						}
						else
						{
							if (_rtcs.u8Day>30)
							{
								if (_rtcs.u8Month==4 || _rtcs.u8Month==6 || _rtcs.u8Month==9 || _rtcs.u8Month==11)
								{
									b=TRUE;
								}
							}
							else
							{
								if (_rtcs.u8Month==2 && _rtcs.u8Day>28)
								{
									if (_rtcs.u8Day>29)
									{
										b=TRUE;
									}
									else
									{
										U16 u16=_rtcs.u16Year%4;

										if (u16!=0)
										{
											b=TRUE;
										}
									}
								}
							}
						}

						if (b)
						{
							_rtcs.u8Day=1;
							_rtcs.u8Month++;
							if (_rtcs.u8Month>12)
							{
								_rtcs.u8Month=1;
								_rtcs.u16Year++;
							}
						}
					}
				}
			}
		}
    TMR1IF=0; /* Must remember to clear overflow or will continuously interrupt! */
  }

	FSR0=u16FSR0; // Ignore compiler warning about saving FSR0H register!
//	FSR0L=u8FSR0L;
	W=u8W;
	BSR=u8BSR;
	STATUS=u8STATUS;
}

static void ISRHigh2(void)
{
	U8 u8STATUS=STATUS;
	U8 u8W=W;
//	U8 u8FSR0L=FSR0L;
	U16 u16FSR0=FSR0; // Ignore compiler warning about saving FSR0H register!

	// in an attempt to reduce jitter, get and set i/o bits ASAP
	// and process later...
	U8 u8PortB;
	bit bswsrx1;
	bit bswsrx2;

	// Buffer the interrupt flags as we need to
	// read them twice in the ISR and need them
	// to be the same
	bit bTMR0IF=TMR0IF;
	bit bTMR2IF=TMR2IF;
	bit bTMR3IF=TMR3IF;
	bit bRBIF=RBIF;
	
	// Now for the 'quick' stuff
	if (bRBIF) // Software Serial Ports RX1/RX2
	{
		W=PORTB; // have to do it this way or RBIF will not reset
		u8PortB=W;
	}
	if (bTMR2IF)
	{
		// SND_PORT=_ss.bBuffer; // This doesn't work, so use 'if'
		//**************
		U8 __u8=PORTB;
		nop();
		if (_ss.bBuffer)
		{
			SND_PORT=TRUE;
		}
		else
		{
			SND_PORT=FALSE;
		}
		SWS_TX1D=_swstx1.bBuffer;
		SWS_TX2D=_swstx2.bBuffer;
	}
  if (bTMR0IF) // Software Serial Port RX1
	{
		bswsrx1=SWS_RX1D;
	}
  if (bTMR3IF) // Software Serial Port RX2
	{
		bswsrx2=SWS_RX2D;
	}

	if (bRBIF) // Software Serial Ports RX1/RX2
	{
		RBIF=0;
		if (_swsrx1.u8BitCount==0 && u8PortB.SWS_RX1BIT==0)
		{
			_swsrx1.u8BitCount=9;
			TMR0H=_swsrx1.u16TimerStart.high8;
			TMR0L=_swsrx1.u16TimerStart.low8;
			TMR0IF=0;
			TMR0IE=1;
			TMR0ON=1;
		}
		if (_swsrx2.u8BitCount==0 && u8PortB.SWS_RX2BIT==0)
		{
			_swsrx2.u8BitCount=9;
			TMR3H=_swsrx2.u16TimerStart.high8;
			TMR3L=_swsrx2.u16TimerStart.low8;
			TMR3IF=0;
			TMR3IE=1;
			TMR3ON=1;
		}
	}

  // handle the timer interrupt
  if (bTMR0IF) // Software Serial Port RX1
  {
		if (_swsrx1.u8BitCount==1 && bswsrx1==1)
		{
			// valid stop bit
			TMR0ON=0;
			TMR0IE=0;
			_swsrx1.u8BitCount=0;
			
			// This is a 'copy' of QueuePut
			{
				U8 u8InNew=_qsswsrx1.u8In+1;
			
				if (u8InNew>=_qsswsrx1.u8Size)
				{
					u8InNew=0;
				}
				if (u8InNew!=_qsswsrx1.u8Out)
				{
					_acswsrx1[_qsswsrx1.u8In]=_swsrx1.c;
					_qsswsrx1.u8In=u8InNew;
				}
			}
		}
		else
		{
			// This is a smart bit of code to reload timer 0
			U16 u16;
			u16.low8=TMR0L;
			u16.high8=TMR0H;
			u16-=_swsrx1.u16TimerReload;
			TMR0H=u16.high8;
			TMR0L=u16.low8;

			if (_swsrx1.u8BitCount>=2)
			{
				Carry=bswsrx1;
				_swsrx1.c=rr(_swsrx1.c);
				_swsrx1.u8BitCount--;
			}
		}
	  TMR0IF=0; /* Must remember to clear overflow or will continuously interrupt! */
  }

  if (bTMR3IF) // Software Serial Port RX2
  {
		if (_swsrx2.u8BitCount==1 && bswsrx2==1)
		{
			// valid stop bit
			TMR3ON=0;
			TMR3IE=0;
			_swsrx2.u8BitCount=0;

			// This is a 'copy' of QueuePut
			{
				U8 u8InNew=_qsswsrx2.u8In+1;
			
				if (u8InNew>=_qsswsrx2.u8Size)
				{
					u8InNew=0;
				}
				if (u8InNew!=_qsswsrx2.u8Out)
				{
					_acswsrx2[_qsswsrx2.u8In]=_swsrx2.c;
					_qsswsrx2.u8In=u8InNew;
				}
			}
		}
		else
		{
			// This is a smart bit of code to reload timer 0
			U16 u16;
			u16.low8=TMR3L;
			u16.high8=TMR3H;
			u16-=_swsrx2.u16TimerReload;
			TMR3H=u16.high8;
			TMR3L=u16.low8;

			if (_swsrx2.u8BitCount>=2)
			{
				Carry=bswsrx2;
				_swsrx2.c=rr(_swsrx2.c);
				_swsrx2.u8BitCount--;
			}
		}
	  TMR3IF=0; /* Must remember to clear overflow or will continuously interrupt! */
  }

	if (bTMR2IF) // Software serial TX
	{
		// SND part
		_ss.u8Count--;
		if (_ss.u8Count==0)
		{
			if (_ss.bActive)
			{
				_ss.bBuffer=!_ss.bBuffer;
			}
			else
			{
				_ss.bBuffer=0;
			}
			_ss.u8Count=SND_RELOAD;
		}

		// SWSTX1 part
		_swstx1.u8Count--;
		if (_swstx1.u8Count==0)
		{
			_swstx1.u8Count=_swstx1.u8CountReload;

			if (_swstx1.u8BitCount>0)
			{
				if (_swstx1.u8BitCount==10)
				{
					// Send start bit
					_swstx1.bBuffer=0;
					_swstx1.c=_acswstx1[_qsswstx1.u8Out++];
					if (_qsswstx1.u8Out>=_qsswstx1.u8Size)
					{
						_qsswstx1.u8Out=0;
					}
				}
				else
				{
					if (_swstx1.u8BitCount==1)
					{
						// Send stop bit
						_swstx1.bBuffer=1;
						// ...and prepare next character
						// This is a copy of the QueueGet routine
						if (_qsswstx1.u8Out!=_qsswstx1.u8In)
						{
							_swstx1.u8BitCount=11; // Note this is decremented soon
						}
					}
					else
					{
						_swstx1.bBuffer=_swstx1.c.0;
						_swstx1.c=rr(_swstx1.c); // This is one instruction less than using ">>="
					}
				}
				_swstx1.u8BitCount--;
			}
		}

		// SWSTX2 part
		_swstx2.u8Count--;
		if (_swstx2.u8Count==0)
		{
			_swstx2.u8Count=_swstx2.u8CountReload;

			if (_swstx2.u8BitCount>0)
			{
				if (_swstx2.u8BitCount==10)
				{
					// Send start bit
					_swstx2.bBuffer=0;
					_swstx2.c=_acswstx2[_qsswstx2.u8Out++];
					if (_qsswstx2.u8Out>=_qsswstx2.u8Size)
					{
						_qsswstx2.u8Out=0;
					}
				}
				else
				{
					if (_swstx2.u8BitCount==1)
					{
						// Send stop bit
						_swstx2.bBuffer=1;
						// ...and prepare next character
						// This is a copy of the QueueGet routine
						if (_qsswstx2.u8Out!=_qsswstx2.u8In)
						{
							_swstx2.u8BitCount=11; // Note this is decremented soon
						}
					}
					else
					{
						_swstx2.bBuffer=_swstx2.c.0;
						_swstx2.c=rr(_swstx2.c); // This is one instruction less than using ">>="
					}
				}
				_swstx2.u8BitCount--;
			}
		}
	  TMR2IF=0; /* Must remember to clear overflow or will continuously interrupt! */
	}

//	FSR0L=u8FSR0L;
	FSR0=u16FSR0; // Ignore compiler warning about saving FSR0H register!
	W=u8W;
	STATUS=u8STATUS;
}

/*******************************************/
/* CC8E Math liraries                      */
/*******************************************/

#pragma rambank 5
#include <math32f.h>
#include <math32lb.h>
#pragma rambank 0

/*******************************************/
/* Generic string/mem routines             */
/*******************************************/

static void strcpy(char size2 *pszDst,char const size2 *pszSrc)
{
	char c;

	do
	{
		c=*pszSrc++;
		*pszDst++=c;
	}
	while (c!='\0');
}

static void strncpy(char size2 *pszDst,char const size2 *pszSrc,U8 u8Size)
{
	char c;

	if (u8Size>0)
	{
		do
		{
			c=*pszSrc++;
			*pszDst++=c;
			u8Size--;
		}	while (u8Size>0 && c!='\0');
	}
}

static void strcat(char size2 *pszDst,char const size2 *pszSrc)
{
	char c;

	while (*pszDst!='\0')
	{
		pszDst++;
	}
	strcpy(pszDst,pszSrc);
}

static U8 strlen(char const size2 *psz)
{
	U8 u8=0;
	char c=*psz++;

	while (c!='\0')
	{
		u8++;
		c=*psz++;

	}
	return u8;
}

static const char size2 *strchr(const char size2 *psz,char c)
{
	BOOL bFinished=FALSE;
	const char size2 *pszResult=NULL;

	while (!bFinished)
	{
		char c2=*psz;

		if (c2=='\0')
		{
			bFinished=TRUE;
		}
		if (c2==c)
		{
			pszResult=psz;
			bFinished=TRUE;
		}
		psz++;
	}
	return pszResult;
}

static S8 strcmp(char const size2 *pszSrc,char const size2 *pszDst)
{
	S8 s8Result;
	char cSrc;
	char cDst;

	do
	{
		cSrc=*pszSrc++;
		cDst=*pszDst++;
		s8Result=cSrc-cDst;
	} while (s8Result==0 && cSrc!='\0');

	return s8Result;
}

static S8 strncmp(char const size2 *pszSrc,char const size2 *pszDst,U8 u8Size)
{
	BOOL bFinished=FALSE;
	BOOL bResult=TRUE;
	S8 s8Result=0;
	char cSrc=1;
	char cDst;

	while (u8Size>0 && s8Result==0 && cSrc!='\0')
	{
		char cSrc=*pszSrc++;
		char cDst=*pszDst++;

		s8Result=cSrc-cDst;
		u8Size--;
	}
	return s8Result;
}

static BOOL isdigit(char c)
{
	if (c>='0' && c<='9')
	{
		return TRUE;
	}
	return FALSE;
}

static char toupper(char c)
{
	if (c>='a' && c<='z')
	{
		return c-' ';
	}
	return c;
}

static void memcpy(U8 size2 *pu8Dst,const U8 size2 *pu8Src,U8 u8Size)
{
	if (pu8Src<pu8Dst)
	{
		pu8Dst+=u8Size;
		pu8Src+=u8Size;
		for (;u8Size>0;u8Size--)
		{
			U8 u8=*--pu8Src;
			*--pu8Dst=u8;
		}
	}
	else
	{
		for (;u8Size>0;u8Size--)
		{
			U8 u8=*pu8Src++;
			*pu8Dst++=u8;
		}
	}
}

static void memset(U8 size2 *pu8Dst,U8 u8Value,U8 u8Size)
{
	while (u8Size!=0)
	{
		*pu8Dst++=u8Value;
		u8Size--;
	}
}


static void ostostr(OBSERVERSTRUCT size2 *pos,char size2 *psz)
{
	// Converts the *pos to a six character string (plus terminating '\0')
	MYFLOAT f;
	MYFLOAT f2;
	U8 u8;

	f=pos->fLon/20.0F+9.0F;
	u8=(U8)f;
	psz[0]=u8+'A';
	f2=u8;
	f-=f2;
	f*=10.0F;
	u8=f;
	psz[2]=u8+'0';
	f2=u8;
	f-=f2;
	f*=24.0F;
	u8=f;
	psz[4]=u8+'a';

	f=pos->fLat/10.0F+9.0F;
	u8=(U8)f;
	psz[1]=u8+'A';
	f2=u8;
	f-=f2;
	f*=10.0F;
	u8=f;
	psz[3]=u8+'0';
	f2=u8;
	f-=f2;
	f*=24.0F;
	u8=f;
	psz[5]=u8+'a';

	psz[6]='\0';
}

static void strtoos(char size2 *psz,OBSERVERSTRUCT size2 *pos)
{
	// Convert a string to *pos
	MYFLOAT f=psz[4]-'a';
	f/=24.0F;
	f+=psz[2]-'0';
	f/=10.0F;
	f+=psz[0]-'A';
	f-=9.0F;
	f*=20.0F;
	f+=0.04165F;
	pos->fLon=f;

	f=psz[5]-'a';
	f/=24.0F;
	f+=psz[3]-'0';
	f/=10.0F;
	f+=psz[1]-'A';
	f-=9.0F;
	f*=10.0F;
	f+=0.020835F;
	pos->fLat=f;
}

/*******************************************/
/* Floating point library routines         */
/*******************************************/

/*
	floating-point arctangent

	myatan returns the value of the arctangent of its
	argument in the range [-pi/2,pi/2].

	atan2 returns the arctangent of arg1/arg2
	in the range [-pi,pi].

	there are no error returns.

	coefficients are #5077 from Hart & Cheney. (19.56D)
*/

/*
	xatan evaluates a series valid in the
	range [-0.414...,+0.414...].
*/

static MYFLOAT FPLxatan(MYFLOAT arg)
{
	MYFLOAT argsq;
	MYFLOAT value;
	MYFLOAT f,f2;

	argsq = arg*arg;

//	value = ((((p4*argsq + p3)*argsq + p2)*argsq + p1)*argsq + p0);
	f=p4;
	f*=argsq;
	f2=p3;
	f+=f2;
  f*=argsq;
	f2=p2;
	f+=f2;
	f*=argsq;
	f2=p1;
	f+=f2;
	f*=argsq;
	f2=p0;
	f+=f2;
	value=f;

//	value = value/(((((argsq + q4)*argsq + q3)*argsq + q2)*argsq + q1)*argsq + q0);
	f2=q4;
	f=argsq+f2;
	f*=argsq;
	f2=q3;
	f+=f2;
	f*=argsq;
	f2=q2;
	f+=f2;
	f*=argsq;
	f2=q1;
	f+=f2;
	f*=argsq;
	f2=q0;
	f+=f2;
	value=value/f;

	return(value*arg);
}

/*
	satan reduces its argument (known to be positive)
	to the range [0,0.414...] and calls xatan.
*/

static MYFLOAT FPLsatan(MYFLOAT arg)
{
	MYFLOAT f,f2;

	f=sq2m1;
	if(arg < f)
	{
		return(FPLxatan(arg));
	}
	else 
	{
		f=sq2p1;
		if(arg > f)
		{
			f=FPLxatan(1.0F/arg);
			f2=pio2;
			return(f2-f);
		}
		else
		{
			f=(arg-1.0F);
			f2=(arg+1.0F);
			f=f/f2;
			f=FPLxatan(f);
			f2=pio4;
			return(f2 + f);
		}
	}
}

/*
	myatan makes its argument positive and
	calls the inner routine satan.
*/

static MYFLOAT FPLatan(MYFLOAT arg)
{
	if(arg>0)
		return(FPLsatan(arg));
	else
		return(-FPLsatan(-arg));
}

static MYFLOAT FPLabs(MYFLOAT x)
{
  if (x>0.0F)
  {
    return x;
  }
  else
  {
    if (x<0.0F)
    {
      return -x;
    }
    else
    {
      return 0.0F;
    }
  }
}

static MYFLOAT FPLpow(MYFLOAT x,MYFLOAT y)
{
	/*return exp(y*log(x));*/
	MYFLOAT f=(MYFLOAT)log(x);
	f*=y;
	return (MYFLOAT)exp(f);
}

static MYFLOAT FPLasin(MYFLOAT x)
{
  MYFLOAT y;

	/*if ( x > 1.0F || -x >1.0F )*/
	if ( x > 1.0F || x <-1.0F )
	{
		return 0.0F;
	}

	/*y = myatan(x/(1.0F+sqrt(1.0F-(x*x))));*/
	y=x*x;
	y=1.0F-y;
	y=(MYFLOAT)sqrt(y);
	y+=1.0F;
	y=FPLatan(x/y);

	return y + y;
}

static MYFLOAT FPLacos(MYFLOAT x)
{
	MYFLOAT f=FPLasin(x);

	return M_PI/2.0F-f;
}

/*******************************************/
/* Flash Program Memory routines           */
/*******************************************/

static BOOL FPMCompare(U24 u24Addr,U8 size2 const *pu8,U8 u8Size)
{
	U8 u8;

	for (u8=0;u8<u8Size;u8++)
	{
		U8 u8Temp;
		U8 u8Temp2;

		TBLPTR=u24Addr;
		#asm
			TBLRD*
		#endasm
		u8Temp=TABLAT;
		u8Temp2=*pu8++;
		if (u8Temp!=u8Temp2)
		{
			return FALSE;
		}
		u24Addr++;
	}
	return TRUE;
}

static void FPMRead(U24 u24Addr,U8 size2 *pu8,U8 u8Size)
{
	TBLPTR=u24Addr;
	U8 u8;

	for (u8=0;u8<u8Size;u8++)
	{
		#asm
			TBLRD*+
		#endasm
		*pu8++=TABLAT;
	}	
}

static BOOL FPMWrite(U24 u24AddrStart,U8 const size2 *pu8,U8 u8Size)
{
	// Program the correct number of pages
	// of flash program memory 
	U24 u24AddrBlockStart=u24AddrStart & ~((U24)FPM_ERASEBLOCKMASK);
	U24 u24AddrEnd=u24AddrStart+u8Size;
	u24AddrEnd--;
	U24 u24AddrBlockEnd=u24AddrEnd+FPM_ERASEBLOCKSIZE;
	u24AddrBlockEnd &= ~((U24)FPM_ERASEBLOCKMASK);
	U24 u24Addr=u24AddrBlockStart;
	U8 u8Retries=0; // Number of retries on a given erase block

	while (u24Addr<u24AddrBlockEnd)
	{
		U8 u8;
		U24 u24AddrSave=u24Addr;
		U8 const size2 *pu8Save=pu8;

		// Read the block in first (39626b p80)
#pragma rambank 1
		U8 au8Buffer[FPM_ERASEBLOCKSIZE];
#pragma rambank 0
		U8 size2 *pu8Buffer=au8Buffer;

		for (u8=0;u8<FPM_ERASEBLOCKSIZE;u8++)
		{
			U8 u8Temp;

			TBLPTR=u24Addr;
			#asm
				TBLRD*;
			#endasm
			u8Temp=TABLAT;

			if (u24Addr>=u24AddrStart && u24Addr<=u24AddrEnd)
			{
				u8Temp=*pu8++;
			}
			*pu8Buffer++=u8Temp;
			u24Addr++;	
		}

		u24Addr=u24AddrSave;

		// Erase block procedure from 39626b p80
		TBLPTR=u24Addr;
		EEPGD=1;
		CFGS=0;
		WREN=1;
		FREE=1;
		GIE=0;
		EECON2=0x55;
		EECON2=0xAA;
		WR=1;
		GIE=1;
	
		pu8Buffer=au8Buffer;
		#asm
			TBLRD*-;
		#endasm

		for (u8=0;u8<FPM_ERASEBLOCKSIZE;u8++)
		{
			TABLAT=*pu8Buffer++;
			#asm
				TBLWT+*
			#endasm
			u24Addr++;

			if ((u24Addr & FPM_PAGEMASK)==0)
			{
				// Program memory procedure from 39626b p81
				EEPGD=1;
				CFGS=0;
				WREN=1;
				GIE=0;
				EECON2=0x55;
				EECON2=0xAA;
				WR=1;
				GIE=1;
				WREN=0;
			}
		}

		if (FPMCompare(u24AddrSave,au8Buffer,FPM_ERASEBLOCKSIZE))
		{
			u8Retries=0;
		}
		else
		{
			u8Retries++;
			if (u8Retries>FPM_MAXRETRIES)
			{
				return FALSE;
			}
			u24Addr=u24AddrSave;
			pu8=pu8Save;
		}

	}
	return TRUE;
}

/********************************************/
/* Floating point ASCII conversion routines */
/********************************************/

static MYFLOAT FPAatof(char size2 *psz)
{
	BOOL bNeg=FALSE;
	BOOL bFinished=FALSE;
	BOOL bDP=FALSE;
	BOOL bDigStarted=FALSE;
	BOOL bStarted=FALSE;
	MYFLOAT f=0.0;

	while (!bFinished)
	{
		char c=*psz++;
		U8 u8DPIdx;

		switch (c)
		{
			case '\0':
				bFinished=TRUE;
				break;
			case ' ':
			case '\t':
			case '\r':
			case '\n':
				if (bStarted)
				{
					bFinished=TRUE;
				}
				break;
			case '-':
				if (bDigStarted) // Already started reading digits, so stop
				{
					bFinished=TRUE;
				}
				bNeg=!bNeg;
				break;
			case '+':
				if (bDigStarted) // Already started reading digits, so stop
				{
					bFinished=TRUE;
				}
				break;
			case '.':
				if (bDP) // Already have a decimal point, so stop
				{
					bFinished=TRUE;
				}
				bDP=TRUE;
				u8DPIdx=FPA_MIDPOINT;
				bStarted=TRUE;
				break;
			case '0':
			case '1':
			case '2':
			case '3':
			case '4':
			case '5':
			case '6':
			case '7':
			case '8':
			case '9':
				c-='0';
				if (bDP)
				{
					MYFLOAT fTemp;
					
					fTemp=_affpa[u8DPIdx--];
					fTemp*=c;
					f+=fTemp;
				}
				else
				{
					f*=10.0F;
					f+=c;
				}
				bStarted=TRUE;
				break;
			default:
				bFinished=TRUE;
				break;
		}
	}
	if (bNeg)
	{
		return -f;
	}
	else
	{
		return f;
	}
}

static void FPAftoa(char size2 *psz,MYFLOAT f,U8 u8Leading,U8 u8Trailing,BOOL bSign,BOOL bZeroSuppress)
{
  MYFLOAT fTemp;
	char cSign=' ';

	if (f>0.0F)
	{
		cSign='+';
	}
	else
	{
		if (f<0.0F)
		{
			cSign='-';
			f=-f;
		}
	}
	if (bSign)
	{
		*psz++=cSign;
	}
  fTemp=_affpa[u8Leading+FPA_MIDPOINT+1];
	if (f>=fTemp)
	{
		*psz++='O';
		*psz++='V';
		*psz++='E';
		*psz++='R';
	}
	else
	{
		U8 u8;
		U8 u8Start=u8Leading+FPA_MIDPOINT;
		U8 u8End=FPA_MIDPOINT-u8Trailing;

		for (u8=u8Start;u8>u8End;u8--)
		{
      char c;
			char c2;

			fTemp=_affpa[u8];
			c=(char)(f/fTemp);

			if (c!=0 || u8<=FPA_MIDPOINT+1)
			{
				bZeroSuppress=FALSE;
			}
			if (u8==FPA_MIDPOINT)
			{
				*psz++='.';
			}

			if (bZeroSuppress && c==0)
			{
				c2=' ';
			}
			else
			{
				c2=c+'0';
			}
			*psz++=c2;
      fTemp*=c;
			f-=fTemp;
		}
	}
	*psz='\0';
}

/*******************************************/
/* Integer to/from ASCII routines          */
/*******************************************/

static BOOL atou24(char const size2 *psz,U24 *pu24)
{
  // Parse psz for a number
  BOOL bFinished=FALSE;
  BOOL bNumFound=FALSE; // at end of string
  char c;
  U24 u24=0;

  while (!bFinished)
  {
    c=*psz++;
    if (c<'0' || c>'9')
    {
      bFinished=TRUE;
    }
    else
    {
      bNumFound=TRUE;
      u24*=10;
      c-='0';
      u24+=c;
    }
  }
  if (bNumFound)
  {
    *pu24=u24;
  }
  return bNumFound;  
}

static BOOL atou16(char const size2 *psz,U16 *pu16)
{
	U24 u24;
	BOOL b=atou24(psz,&u24);

	*pu16=(U16)u24;
	return b;
}

static BOOL atou8(char const size2 *psz,U8 *pu8)
{
	U24 u24;
	BOOL b=atou24(psz,&u24);

	*pu8=(U8)u24;
	return b;
}

static void u24toa(char size2 *psz,U24 u24,U8 u8Len)
{
	static const U24 au24[8]=
	{
		10000000,
		1000000,
		100000,
		10000,
		1000,
		100,
		10,
		1
	};
	U8 u8;
	U24 u24Temp;

	u8=7-u8Len;
	u24Temp=au24[u8];
	if (u24>=u24Temp)
	{
		*psz++='O';
		*psz++='F';
		*psz++='L';
		*psz++='W';
	}
	else
	{
		for (u8++;u8<8;u8++)
		{
			U24 u24Temp=au24[u8];
			char c=(char)(u24/u24Temp);
			U24 u24Temp2=c*u24Temp;
	
			u24-=u24Temp2;
			c+='0';
			*psz++=c;
		}
	}
	*psz='\0';
}

static void u16toa(char size2 *psz,U16 u16,U8 u8Len)
{
	u24toa(psz,u16,u8Len);
}

static void u8toa(char size2 *psz,U8 u8,U8 u8Len)
{
	u24toa(psz,u8,u8Len);
}

/*******************************************/
/* RTC routines                            */
/*******************************************/

static void RTCSetTime(U16 u16Year,U8 u8Month,U8 u8Day,U8 u8Hour,U8 u8Min,U8 u8Sec,U16 u16MSec)
{
	BOOL b=GIEL;

	GIEL=0;
	_rtcs.u16MSec=u16MSec;
	_rtcs.u8Sec=u8Sec;
	_rtcs.u8Min=u8Min;
	_rtcs.u8Hour=u8Hour;
	_rtcs.u8Day=u8Day;
	_rtcs.u8Month=u8Month;
	_rtcs.u16Year=u16Year;
	GIEL=b;
}

static void RTCInit(void)
{
	_rtcs.u16Timer1Reload=RTC_TIMER1RELOAD-14;
	_rtcs.u16Tick=0;
	RTCSetTime(2005,6,5,12,54,0,0);

	RD16=1;
	TMR1H=_rtcs.u16Timer1Reload.high8;
	TMR1L=_rtcs.u16Timer1Reload.low8;
//	T1RUN=0;
	T1CKPS0=0; // 1:1 prescaler
	T1CKPS1=0;
	T1OSCEN=0;
	T1SYNC_=0;
	TMR1CS=0; /* select crystal osc (FOSC/4) */
	TMR1IP=0; // Low priority interrupt
  TMR1IE=1; /* Enable interrupt */
	TMR1ON=1;
}

static U16 RTCTimerGet(void)
{
	U16 u16;
	BOOL b=GIEL;

	GIEL=0;
	u16=_rtcs.u16Tick;
	GIEL=b;
	return u16;
}

static void RTCDelay(U16 u16)
{
	// Delay at least u16 milliseconds
	U16 u16Start=RTCTimerGet();
	U16 u16Diff;

	u16++; // Make sure it's at least u16 milliseconds
	do
	{
		u16Diff=RTCTimerGet()-u16Start;
	} while (u16>u16Diff);
}

static void RTCGetDateTime(TIMESTRUCT size2 *pts)
{
	BOOL b=GIEL;

	GIEL=0;
	pts->u16Year=_rtcs.u16Year;
	pts->u8Month=_rtcs.u8Month;
	pts->u8Day=_rtcs.u8Day;
	pts->u8Hour=_rtcs.u8Hour;
	pts->u8Min=_rtcs.u8Min;
	pts->u8Sec=_rtcs.u8Sec;
	GIEL=b;
}

/******************************************/
/* ADC routines                           */
/******************************************/

static void ADCInit(void)
{
	TRISA.0=1; // AN0/AN1 inputs
	TRISA.1=1;

	VCFG1=0; // Vref-=Vss
	VCFG0=0; // Vref+=Vdd
	PCFG0=1; // AN0 and AN1 active
	PCFG1=0;
	PCFG2=1;
	PCFG2=1;

	ADFM=1; // Right justified

	ADCS0=0; // Fosc/32
	ADCS1=1;
	ADCS2=0;

	ACQT0=1; // Tad=20
	ACQT1=1;
	ACQT2=1;

	_adcs.u8Channel=0;
	_adcs.u8Sample=0;
	_adcs.u16Acc=0;
	{
		U8 u8;
		for (u8=0;u8<ADC_NUMCHANNELS;u8++)
		{
			_adcs.au16[u8]=0;
		}
	}

	ADIF=0;
	ADIE=0; // We don't actually use the ADC interrupt
	ADIP=0; // Low priority

	ADON=1;

	CHS0=0;
	CHS1=0;
	CHS2=0;
	CHS3=0;

	GO=1;	
}

static U16 ADCRead(U8 u8Channel)
{
	U16 u16;
	U16 *pu16=&_adcs.au16[u8Channel];
	BOOL b=GIEL;

	GIEL=0;
	u16=*pu16;
	GIEL=b;
	return u16;
}

/******************************************/
/* ROT routines                           */
/******************************************/

// Deg = (S16) { [ (S16) (Adc-Off) ] / fMul + 0.5 }
// Adc = (U16) [ ( Deg + 0.5 ) * fMul + u16Off ]
/*
typedef struct
{
	U16 u16AzOff;
	U16 u16ElOff;
	MYFLOAT fAzMul;
	MYFLOAT fElMul;
	S16 s16AzTarget; // The target Az as an ADC value
	S16 s16ElTarget; // The target El as an ADC value
	BOOL bAzTrack;
	BOOL bElTrack;
} ROTSTRUCT;
*/

static MYFLOAT ROTAzAdcToDeg(U16 u16)
{
	S16 s16=(S16)(((S16)(u16-_ds.rds.u16AzOff)) / _ds.rds.fAzMul + 0.5F);
	return (MYFLOAT)s16;
}

static MYFLOAT ROTElAdcToDeg(U16 u16)
{
	S16 s16=(S16)(((S16)(u16-_ds.rds.u16ElOff)) / _ds.rds.fElMul + 0.5F);
	return (MYFLOAT)s16;
}

static S16 ROTAzDegToAdc(MYFLOAT fDeg)
{
	MYFLOAT f=(fDeg + 0.5F) * _ds.rds.fAzMul;
	f+=_ds.rds.u16AzOff;
	return (S16)f;
}

static S16 ROTElDegToAdc(MYFLOAT fDeg)
{
	MYFLOAT f=(fDeg + 0.5F) * _ds.rds.fElMul;
	f+=_ds.rds.u16ElOff;
	return (S16)f;
}

static void ROTTargetInit(void)
{
	_rs.bAzTrack=FALSE;
	_rs.bElTrack=FALSE;

	{
		S16 s16;
		bit b;

		s16=ROTAzDegToAdc(0);
		b=GIEL;
		GIEL=0;
		_rs.s16AzTarget=s16;
		GIEL=b;
		s16=ROTElDegToAdc(0);
		b=GIEL;
		GIEL=0;
		_rs.s16ElTarget=s16;
		GIEL=b;
	}
}

static void ROTInit(void)
{
	// Data EEPROM must have been set and read first!
	ROT_LEFT=0;
	ROT_RIGHT=0;
	ROT_DOWN=0;
	ROT_UP=0;

	ROT_LEFT_DIR=0;
	ROT_RIGHT_DIR=0;
	ROT_DOWN_DIR=0;
	ROT_UP_DIR=0;

	ROTTargetInit();
}

static void ROTSet(S16 s16Az,S16 s16El)
{
  // Set the rotator position. 
  // If El or Az are 32767, then no attempt is made to move that axis
	// Az given may be negative indicating a South stopping rotator

  if (s16Az!=32767)
  {
    if (_ds.rds.bSNS) // Normalise so that we deal only with 0-360 degrees internally
    {
      if (s16Az<180)
      {
        s16Az+=180;
      }
      else
      {
        s16Az-=180;
      }
    }

		s16Az=ROTAzDegToAdc(s16Az);
		{
			BOOL b=GIEL;
			GIEL=0;
	    _rs.s16AzTarget=s16Az;
			GIEL=b;
		}

    s16Az=(S16)ADCRead(0);
    if (_rs.s16AzTarget>s16Az)
    {
      ROT_RIGHT=1;
      ROT_LEFT=0;
      _rs.bAzTrack=TRUE;
    }
    else
    {
      if (_rs.s16AzTarget<s16Az)
      {
        ROT_LEFT=1;
        ROT_RIGHT=0;
        _rs.bAzTrack=TRUE;
      }
      else
      {
        ROT_LEFT=0;
        ROT_RIGHT=0;
        _rs.bAzTrack=FALSE;
      }
    }
  }
  if (s16El!=32767)
  {
		s16El=ROTAzDegToAdc(s16El);
		{
			BOOL b=GIEL;
			GIEL=0;
	    _rs.s16ElTarget=s16El;
			GIEL=b;
		}

    s16El=(S16)ADCRead(1);

    if (_rs.s16ElTarget>s16El)
    {
      ROT_UP=1;
      ROT_DOWN=0;
      _rs.bElTrack=TRUE;
    }
    else
    {
      if (_rs.s16ElTarget<s16El)
      {
        ROT_DOWN=1;
        ROT_UP=0;
        _rs.bElTrack=TRUE;
      }
      else
      {
        ROT_DOWN=0;
        ROT_UP=0;
        _rs.bElTrack=FALSE;
      }
    }
  }
}

/*******************************************/
/* Data EEPROM routines                    */
/*******************************************/

static void DEERead(U16 u16Addr,U8 size2 *pu8,U8 u8Size)
{
	U8 u8;

	for (u8=0;u8<u8Size;u8++)
	{
		EEADRH=u16Addr.high8;
		EEADR=u16Addr.low8;
		EEPGD=0;
		CFGS=0;
		RD=1;
		*pu8++=EEDATA;
		u16Addr++;
	}	
}

static void DEEWrite(U16 u16Addr,const U8 size2 *pu8,U8 u8Size)
{
	U8 u8;

	for (u8=0;u8<u8Size;u8++)
	{
		EEADRH=u16Addr.high8;
		EEADR=u16Addr.low8;
		EEDATA=*pu8++;
		EEPGD=0;
		CFGS=0;
		WREN=1;
		GIE=0;
		EECON2=0x55;
		EECON2=0xAA;
		WR=1;
		GIE=1;
		while (WR)
		{
			nop();
		}	
		EEIF=0;
		WREN=0;
		u16Addr++;
	}	
}

static BOOL DEEVerify(U16 u16Addr,U8 size2 *pu8,U8 u8Size)
{
	U8 u8;

	for (u8=0;u8<u8Size;u8++)
	{
		U8 u8Data=*pu8++;

		EEADRH=u16Addr.high8;
		EEADR=u16Addr.low8;
		EEPGD=0;
		CFGS=0;
		RD=1;
		if (u8Data!=EEDATA)
		{
			return FALSE;
		}
		u16Addr++;
	}
	return TRUE;	
}

static void DEERefresh(void)
{
	U16 u16Addr;

	CFGS=0;
	EEPGD=0;
	GIE=0;
	WREN=1;
	for (u16Addr=0;u16Addr<1024;u16Addr++)
	{
		EEADRH=u16Addr.high8;
		EEADR=u16Addr.low8;
		RD=1;
		EECON2=0x55;
		EECON2=0xAA;
		WR=1;
		while (WR)
		{
			nop();
		}
		EEIF=0;
	}
	WREN=0;
	GIE=1;
}

static S8 DEEChecksum(U8 size2 *pu8,U8 u8Size)
{
  // Result should always be zero if the checksum byte 
	// is included. If the checksum byte is not included,
	// then the result is the checksum required to make
	// the total zero.
  U8 u8;
  S8 s8Sum=0;

  for (u8=0;u8<u8Size;u8++)
  {
		U8 u8Data;

    s8Sum+=(S8)*pu8++;
  }
  return -s8Sum;
}

static BOOL DEEUpdate(void)
{
	S8 s8=DEEChecksum(&_ds,sizeof(_ds)-1);

	_ds.u8Checksum=(U8)s8;
	DEEWrite(0,&_ds,sizeof(_ds));
	return DEEVerify(0,&_ds,sizeof(_ds));
}

static BOOL DEEInit(void)
{
	BOOL bResult=FALSE;

	DEERead(0,&_ds,sizeof(_ds));

	if (DEEChecksum(&_ds,sizeof(_ds))!=0)
	{
		// Load defaults...
		_ds.rds.fAzMul=1.8583;
		_ds.rds.u16AzOff=8;
		_ds.rds.fElMul=4.5555;
		_ds.rds.u16ElOff=13;
		_ds.rds.bAz450=FALSE;
		_ds.rds.bSNS=FALSE;
	
		_ds.os.fLon=-0.173581F;
		_ds.os.fLat=51.497591F;
		_ds.os.fHeight=20.0F;

		return FALSE;
	}
	return TRUE;	
}

/*******************************************/
/* SND routines                            */
/*******************************************/

static void SNDInit(void)
{
	SND_PORT=0;
	SND_PORT_DIR=0; // Output
	_ss.u8Count=SND_RELOAD;
	_ss.bActive=FALSE;
	_ss.bBuffer=FALSE;
}

/*******************************************/
/* Queue routines                          */
/*******************************************/

static void QueueInit(QUEUESTRUCT size2 *pqs,U8 u8Size,char size2 *pac)
{
	pqs->u8Size=u8Size;
	pqs->u8In=0;
	pqs->u8Out=0;
	pqs->pac=pac;
}

static BOOL QueuePut(QUEUESTRUCT size2 *pqs,char c)
{
	U8 u8In=pqs->u8In;
	U8 u8InNew=u8In+1;
	char *pac=pqs->pac;

	if (u8InNew>=pqs->u8Size)
	{
		u8InNew=0;
	}
	if (u8InNew==pqs->u8Out)
	{
		return FALSE;
	}
	pac[u8In]=c;
	pqs->u8In=u8InNew;
	return TRUE;
}

static BOOL QueueGet(QUEUESTRUCT size2 *pqs,char *pc)
{
	U8 u8Out=pqs->u8Out;
	char *pac=pqs->pac;
	char c;

	U8 u8O=pqs->u8Out;
	U8 u8I=pqs->u8In;
	if (u8O!=u8I)
	{
		nop();
	}

	if (u8Out==pqs->u8In)
	{
		return FALSE;
	}
	c=pac[u8Out];
	*pc=c;
	u8Out++;
	if (u8Out>=pqs->u8Size)
	{
		u8Out=0;
	}
	pqs->u8Out=u8Out;
	return TRUE;
}

static BOOL QueueReady(QUEUESTRUCT size2 *pqs)
{
	// Return true if data ready
	U8 u8Out=pqs->u8Out;

	if (u8Out==pqs->u8In)
	{
		return FALSE;
	}
	return TRUE;
}

/*******************************************/
/* Morse (MRS) routines                    */
/*******************************************/

static void MRSInit(void)
{
	QueueInit(&_qsmrs,MRS_QUEUESIZE,_acmrs);
	_ms.u8Mask=0;
	_ms.u8Symbol=0;
	_ms.u16Count=MRS_RELOAD;
}

static void MRSPut(char c)
{
	while (!QueuePut(&_qsmrs,c))
	{
	}
}

/*******************************************/
/* Serial (software SWS) routines          */
/*******************************************/

static void SWSRxInit(S_BAUDRATEENUM sbre1,S_BAUDRATEENUM sbre2)
{
	// Timer 0 setup
	T08BIT=0; // 1=8 bit, 0=16 bit
	T0SE=1; // Select edge to clock
  PSA=0; /* Prescaler assigned to TMR0 */
  T0PS0=0; /* 1:2 prescale on FOSC/4 */
  T0PS1=0;
  T0PS2=0; 
  T0CS=0; /* select crystal osc (FOSC/4) */
	TMR0IP=1; // High priority interrupt
  TMR0IE=0; /* Disable interrupt */
	{
		U16 u16=_au16swsrx1Timer0[sbre1];

		_swsrx1.u16TimerReload=u16-7; // fudge factor for time taken to reload/adjust 16 bit counter/timer
		u16*=5; // Make 5/4 bit times for start bit delay rather than 3/2 as 3/2 fails sometimes
		u16/=4;
		_swsrx1.u16TimerStart=65536-u16;
	}
	TMR0ON=0;
	SWS_RX1DIR=1; // Set to input bit
	_swsrx1.u8BitCount=0;
	QueueInit(&_qsswsrx1,SWS_RX1QUEUESIZE,_acswsrx1);

	// Timer 3 setup
	T3RD16=1; // T3RD16=1 -> buffer TMR3H
	T3CKPS0=0; /* 1:1 prescale on FOSC/4 */
	T3CKPS1=0;
  TMR3CS=0; /* select crystal osc (FOSC/4) */
	TMR3IP=1; // High priority interrupt
  TMR3IE=0; /* Disable interrupt */
	{
		U16 u16=_au16swsrx2Timer3[sbre2];

		_swsrx2.u16TimerReload=u16-11; // fudge factor for time taken to reload/adjust 16 bit counter/timer
		u16*=5; // Make 5/4 bit times for start bit delay rather than 3/2 as 3/2 fails sometimes
		u16/=4;
		_swsrx2.u16TimerStart=65536-u16;
	}
	TMR3ON=0;
	SWS_RX2DIR=1; // Set to input bit
	_swsrx2.u8BitCount=0;
	QueueInit(&_qsswsrx2,SWS_RX2QUEUESIZE,_acswsrx2);

	RBIF=0;
	RBPU=0;
	RBIP=1; // High priority interrupt
	RBIE=1;
}

static void SWSTxInit(S_BAUDRATEENUM sbre1,S_BAUDRATEENUM sbre2)
{
	SWS_TX1D=1; // Stop bit
	SWS_TX1DIR=0;
	_swstx1.u8BitCount=0;
	_swstx1.u8CountReload=_au8swstxBaud[sbre1];
	_swstx1.u8Count=_swstx1.u8CountReload;
	_swstx1.bBuffer=1; // Stop bit
	QueueInit(&_qsswstx1,SWS_TX1QUEUESIZE,_acswstx1);

	SWS_TX2D=1; // Stop bit
	SWS_TX2DIR=0;
	_swstx2.u8BitCount=0;
	_swstx2.u8CountReload=_au8swstxBaud[sbre2];
	_swstx2.u8Count=_swstx2.u8CountReload;
	_swstx2.bBuffer=1; // Stop bit
	QueueInit(&_qsswstx2,SWS_TX2QUEUESIZE,_acswstx2);

	T2CKPS0=SWS_TXTIMER2PS0;
	T2CKPS1=SWS_TXTIMER2PS1;
	PR2=SWS_TXTIMER2CONST;
	TMR2=SWS_TXTIMER2CONST;
	TMR2IP=1; // High priority interrupt
	TMR2IE=1;
	TMR2ON=1;
}

static void SWSTx1Put(char c)
{
	while (!QueuePut(&_qsswstx1,c))
	{
	}
	if (_swstx1.u8BitCount==0)
	{
		_swstx1.u8BitCount=10; // Tickle the transmitter into action
	}
}

static void SWSTx2Put(char c)
{
	while (!QueuePut(&_qsswstx2,c))
	{
	}
	if (_swstx2.u8BitCount==0)
	{
		_swstx2.u8BitCount=10; // Tickle the transmitter into action
	}
}

static BOOL SWSRx1Get(char size2 *pc)
{
	return QueueGet(&_qsswsrx1,pc);
}

static BOOL SWSRx2Get(char size2 *pc)
{
	return QueueGet(&_qsswsrx2,pc);
}

static BOOL SWSRx1Ready(void)
{
	return QueueReady(&_qsswsrx1);
}

static BOOL SWSRx2Ready(void)
{
	return QueueReady(&_qsswsrx2);
}

/*******************************************/
/* Serial (hardware HWS) routines          */
/*******************************************/

static void HWSInit(S_BAUDRATEENUM sbre)
{
	U16 u16=_au16hwsBaud[sbre];

	QueueInit(&_qshwsrx,HWS_RXQUEUESIZE,_achwsrx);
	QueueInit(&_qshwstx,HWS_TXQUEUESIZE,_achwstx);

	SPEN=1;
	TRISC.7=1;
	TRISC.6=1;
	TX9=0;
	TXEN=1;
	SYNC=0;
	BRGH=0;
	RX9=0;
	CREN=1;
	ADDEN=0;
#ifdef EUSART
	BRG16=1;
	WUE=0;
	ABDEN=0;
	SPBRGH=u16.high8;
#endif
	SPBRG=u16.low8;
	RCIP=0; // Low priority interrupt
	TXIP=0; // Low priority interrupt
	RCIE=1;
	TXIE=0;
}

static void HWSTxPut(char c)
{
	while (!QueuePut(&_qshwstx,c))
	{
		nop();
	}
	if (!TXIE)
	{
		TXIF=1; // Frighten the ISR into action!
		TXIE=1;
	}
}

static BOOL HWSRxGet(char size2 *pc)
{
	return QueueGet(&_qshwsrx,pc);
}

static BOOL HWSRxReady(void)
{
	return QueueReady(&_qshwsrx);
}

/*******************************************/
/* LCD routines                            */
/*******************************************/

static void LCDWriteNibble(unsigned char uc) /* RS must be set/reset before calling */
{
  uc<<=LCD_DATA_SHIFT; /* Align with bits 0-3 */
  LCD_RW=0;
	LCD_DATA_DIR &= LCD_DATA_NOTMASK; /* Set to output bits 0-3 */
//  TRISD=0b.1111.0000; /* Set to output bits 0-3 */
	LCD_DATA=(LCD_DATA & LCD_DATA_NOTMASK) | uc;
//  PORTD=uc;
  LCD_E=1;
  nop();
  nop();
  LCD_E=0;
  LCD_RW=1;
	LCD_DATA_DIR |= LCD_DATA_MASK; /* Set to input bits 0-3 */
//	TRISD=0b.1111.1111; /* Set to input bits 0-3 */  
}

static unsigned char LCDReadByte(void) /* RS must be set/reset before calling */
{
  unsigned char uc,uc2;

  LCD_RW=1;
  LCD_E=1;
  nop();
  nop();
  uc=LCD_DATA;
  LCD_E=0;
	uc<<=4-LCD_DATA_SHIFT;
  uc&=0xF0;
  LCD_E=1;
  nop();
  nop();
  uc2=LCD_DATA;
  LCD_E=0;
  uc2>>=LCD_DATA_SHIFT;
  uc2&=0x0F;
  uc|=uc2;
  return uc;
}

static BOOL LCDWaitReady(void)
{
  if (_bLCDActive)
  {
    U16 u16=RTCTimerGet();

    while (LCDReadByte() & 0x80)
    {
      U16 u16Diff=RTCTimerGet()-u16;

      if (u16Diff>5) // Timeout
      {
        _bLCDActive=FALSE;
        return FALSE;
      }
    }
  }
  return TRUE;
}

static void LCDPut(unsigned char uc)
{
  if (_bLCDActive)
  {
    LCD_RS=0;
    LCDWaitReady();
    LCD_RS=1;

    LCDWriteNibble(uc>>4);
    LCDWriteNibble(uc);
  }
}

static unsigned char LCDGet(void)
{
  if (_bLCDActive)
  {
    LCD_RS=0;
    LCDWaitReady();
    LCD_RS=1;
    return LCDReadByte();
  }
  else
  {
    return 0;
  }
}

static void LCDCommand(unsigned char uc)
{
  if (_bLCDActive)
  {
    LCD_RS=0; /* Instruction mode */
    LCDWaitReady();

    LCDWriteNibble(uc>>4);
    LCDWriteNibble(uc);
  }
}

static void LCDSetCursor(unsigned char ucPos)
{
  if (_bLCDActive)
  {
    LCDCommand(0x80 | ucPos);
  }
}

static void LCDClear(void)
{
  if (_bLCDActive)
  {
    LCDCommand(0x01);
  }
}

static void LCDInit(void)
{
	// Disable A/D on RA2-5...
	ADCON1=0b.0000.1101; // 
	LCD_RS_DIR=0;
	LCD_RW_DIR=0;
	LCD_E_DIR=0;

  _bLCDActive=TRUE; // Let's be optimistic

  LCD_E=0;
  LCD_RS=0;
  RTCDelay(15);
  LCDWriteNibble(3);
  RTCDelay(5);
  LCDWriteNibble(3);
  RTCDelay(1);
  LCDWriteNibble(3);
  RTCDelay(1);
  LCDWriteNibble(2);
  RTCDelay(1);
  LCDCommand(0b.0010.1000);
  LCDCommand(0b.0000.1000);
  LCDCommand(0b.0000.0001);
  LCDCommand(0b.0000.0110);
  LCDCommand(0b.0000.1111);
}

/*******************************************/
/* Button (BTN) routines                   */
/*******************************************/

static void BTNInit(void)
{
	BTN_ENABLE_=1; // Disable buttons
	BTN_ENABLE_DIR=0; // Set to output
	_u8btnLast=0;
}

static U8 BTNGetRaw(void)
{
  // Not debounced.
  U8 u8;

  BTN_ENABLE_=0; // Enable buttons
  nop(); // Need this delay!!! 16F876A does not settle in time otherwise.
  u8=BTN_DATA; // Read buttons
  BTN_ENABLE_=1; // Disable buttons
  u8>>=BTN_DATA_SHIFT;
  u8&=0x0F;
  u8^=0x0F;
  return u8;
}

static U8 BTNGet(void) // Debounced result
{
  // Wait for 5ms of completely stable buttons to debounce
  U8 u8Org=BTNGetRaw();
  U16 u16Org=RTCTimerGet();
  U8 u8New=u8Org;
  U16 u16New=u16Org;
  U16 u16Diff;

  do
  {
    u8New=BTNGetRaw();
    u16New=RTCTimerGet();

    if (u8New!=u8Org)
    {
      u8Org=u8New;
      u16Org=u16New; // reset timer
    }
    u16Diff=u16New-u16Org;
  } while (u16Diff<5); // Let's have 5ms of the same please

  return u8New;
}

static U8 BTNRead(BOOL bPoll) // Only indicates new presses
{
	// If bPoll is TRUE, perform polls for GPS and Host etc
	U8 u8=BTNGet();
	U8 u8Result=0;

	if (u8.BTN_BIT_LEFT && !_u8btnLast.BTN_BIT_LEFT)
	{
		u8Result.BTN_BIT_LEFT=1;
	}

	if (u8.BTN_BIT_RIGHT && !_u8btnLast.BTN_BIT_RIGHT)
	{
		u8Result.BTN_BIT_RIGHT=1;
	}

	if (u8.BTN_BIT_DOWN && !_u8btnLast.BTN_BIT_DOWN)
	{
		u8Result.BTN_BIT_DOWN=1;
	}

	if (u8.BTN_BIT_UP && !_u8btnLast.BTN_BIT_UP)
	{
		u8Result.BTN_BIT_UP=1;
	}
	_u8btnLast=u8;

	if (bPoll)
	{
		GPSPoll();
		HSTPoll();
	}
	return u8Result;
}

/*******************************************/
/* Generic I/O routines                    */
/*******************************************/

static void WriteC(PORTENUM pe,char c)
{
	switch(pe)
	{
		case PE_NUL:
			break;
		case PE_LCD:
			LCDPut((unsigned char)c);
			break;
		case PE_HWS:
			HWSTxPut(c);
			break;
		case PE_SWS1:
			SWSTx1Put(c);
			break;
		case PE_SWS2:
			SWSTx2Put(c);
			break;
		case PE_MRS:
			MRSPut(c);
			break;
	}
}

static void WriteStr(PORTENUM pe,const char size2 *psz)
{
	char c=*psz++;

	while (c!='\0')
	{
		WriteC(pe,c);
		c=*psz++;
	}
}

static void WriteClear(PORTENUM pe)
{
	switch(pe)
	{
		case PE_NUL:
			break;
		case PE_LCD:
			LCDClear();
			break;
		case PE_HWS:
		case PE_SWS1:
		case PE_SWS2:
			WriteStr(pe,"\r\n\n");
			break;
		case PE_MRS:
			WriteStr(pe,"    ");
			break;
	}
}

static void WriteHome(PORTENUM pe)
{
	switch(pe)
	{
		case PE_NUL:
			break;
		case PE_LCD:
			LCDSetCursor(0);
			break;
		case PE_HWS:
		case PE_SWS1:
		case PE_SWS2:
			WriteC(pe,'\r');
			break;
		case PE_MRS:
			WriteStr(pe,"  ");
			break;
	}
}

static void WriteLF(PORTENUM pe)
{
	switch(pe)
	{
		case PE_NUL:
			break;
		case PE_LCD:
			LCDSetCursor(0x40);
			break;
		case PE_HWS:
		case PE_SWS1:
		case PE_SWS2:
			WriteC(pe,'\n');
			break;
		case PE_MRS:
			WriteC(pe,' ');
			break;
	}
}

static void WriteCursor(PORTENUM pe,U8 u8)
{
	// Set the cursor location...
	switch(pe)
	{
		case PE_NUL:
			break;
		case PE_LCD:
			LCDSetCursor(u8);
			break;
		case PE_HWS:
		case PE_SWS1:
		case PE_SWS2:
			break;
		case PE_MRS:
			break;
	}
}

static void WriteLocator(PORTENUM pe,OBSERVERSTRUCT size2 *pos)
{
	char sz[7];

	ostostr(pos,sz);
	WriteStr(pe,sz);
}

static void WriteU8(PORTENUM pe,U8 u8,U8 u8Len)
{
	char sz[10];
	
	u8toa(sz,u8,u8Len);
	WriteStr(pe,sz);
}

static void WriteU16(PORTENUM pe,U16 u16,U8 u8Len)
{
	char sz[10];
	
	u16toa(sz,u16,u8Len);
	WriteStr(pe,sz);
}

static void WriteS16(PORTENUM pe,S16 s16,U8 u8Len)
{
	char c='+';

	if (s16<0)
	{
		s16=-s16;
		c='-';
	}

	WriteC(pe,c);
	WriteU16(pe,(U16)s16,u8Len);
}

static void WriteU24(PORTENUM pe,U24 u24,U8 u8Len)
{
	char sz[10];
	
	u24toa(sz,u24,u8Len);
	WriteStr(pe,sz);
}

static void WriteF(PORTENUM pe,MYFLOAT f,U8 u8Leading,U8 u8Trailing,BOOL bSign,BOOL bZeroSuppress)
{
	char sz[20];

	FPAftoa(sz,f,u8Leading,u8Trailing,bSign,bZeroSuppress);
	WriteStr(pe,sz);
}

static void WriteDate(PORTENUM pe,TIMESTRUCT size2 *pts,BOOL bFullCentury)
{
	static const char szMonth[]="JanFebMarAprMayJunJulAugSepOctNovDec";
	char sz[4];
	U8 u8Month3=(pts->u8Month-1)*3;
	const char size2 *pszMonth=szMonth+u8Month3;

	strncpy(sz,szMonth+u8Month3,3);
	sz[3]='\0';

	WriteU8(pe,pts->u8Day,2);
	WriteStr(pe,sz);
	if (bFullCentury)
	{
		WriteU16(pe,pts->u16Year,4);
	}
	else
	{
		U8 u8=(U8)(pts->u16Year-2000);
		WriteU8(pe,u8,2);
	}
}

static void WriteTime(PORTENUM pe,TIMESTRUCT size2 *pts,BOOL bSeparators,BOOL bSeconds)
{
	WriteU8(pe,pts->u8Hour,2);
	if (bSeparators)
	{
		WriteC(pe,':');
	}
	WriteU8(pe,pts->u8Min,2);
	if (bSeconds)
	{
		if (bSeparators)
		{
			WriteC(pe,':');
		}
		WriteU8(pe,pts->u8Sec,2);
	}
}

static void WriteDateTime(PORTENUM pe,TIMESTRUCT size2 *pts,BOOL bSeparators,BOOL bSeconds,BOOL bFullCentury)
{
	WriteDate(pe,pts,bFullCentury);
	WriteC(pe,' ');
	WriteTime(pe,pts,bSeparators,bSeconds);
}

static BOOL ReadC(PORTENUM pe,char size2 *pc)
{
	BOOL bResult=FALSE;
	char c;

	switch(pe)
	{
		case PE_HWS:
			bResult=HWSRxGet(pc);
			break;
		case PE_SWS1:
			bResult=SWSRx1Get(pc);
			break;
		case PE_SWS2:
			bResult=SWSRx2Get(pc);
			break;
		default:
			c='\0';
			break;
	}
	return bResult;
}

static BOOL ReadStr(PORTENUM pe,char size2 *psz,U8 u8Size)
{
	// Read in a line of text up to length u8Size
	// Return TRUE on EOF or button press
	BOOL bFinished=FALSE;
	BOOL bResult=FALSE;

	while (!bFinished)
	{
		char c;

		while (!bFinished && u8Size!=0 && !ReadC(pe,&c))
		{
			if (BTNRead(FALSE)!=0)
			{
				bFinished=TRUE;
				bResult=TRUE;
			}
		}
		if (!bFinished)
		{
			if (c=='\r' || c=='\n')
			{
				bFinished=TRUE;
			}
			else
			{
				if (c==26) // ^Z
				{
					bFinished=TRUE;
					bResult=TRUE;
				}
				else
				{
					*psz++=c;
					u8Size--;
				}
			}
		}
	}
	*psz='\0';
	return bResult;
}

/*******************************************/
/* Plan13 routines                         */
/*******************************************/

#pragma rambank 10

static MYFLOAT P13Rad(MYFLOAT deg)
{
	// Convert degrees to radians
  return (MYFLOAT)(M_PI / 180.0F * deg);
}

static MYFLOAT P13Deg(MYFLOAT myrad)
{
	// Convert radians to degrees
  return (MYFLOAT)(myrad * 180.0F / M_PI);
}

static MYFLOAT P13Atan(MYFLOAT y, MYFLOAT x)
{
	// The converted G3RUH version of Atan
  MYFLOAT a;
    
  if (x != 0)
  {
		a = FPLatan(y / x);
  }
  else
  {
		/*a = M_PI / 2.0 * sin(y);*/
		//a=(MYFLOAT)sin(y);
		//a*=(MYFLOAT)(M_PI/2.0F);
		a=M_PI/2.0F*y;
		if (a!=0.0F)
		{
			if (y>0.0F)
			{
				a=1.0F;
			}
			else
			{
				a=-1.0F;
			}
		}
  }
    
  if (x < 0.0F)
  {
		a = a + M_PI;
  }
    
  if (a < 0.0F)
  {
		a = a + 2.0F * M_PI;
  }
    
  return (a);
}

/* Convert date to day number 
 * 
 * Function returns a general day number from year, month, and day.
 * Value is (JulianDay - 1721409.5) or (AmsatDay + 722100)
 * 
 */
static MYFLOAT P13DayToDayNum(U16 year, U8 month, U8 day)
{
	// Convert separate YMD to DayNum day
  MYFLOAT JulianDate;
	MYFLOAT f;
    
  if (month <= 2)
  { 
		year -= 1;
		month += 12;    
  }
    
  /*JulianDate = (MYLONG)((MYFLOAT)year * YM) + (MYLONG)((MYFLOAT)(month + 1) * 30.6) + (MYFLOAT)((MYLONG)day - (MYLONG)428);*/
	JulianDate=(MYFLOAT)year;
	JulianDate*=365.25F;
	JulianDate=(MYFLOAT)((MYLONG)JulianDate);

	f=(MYFLOAT)month;
	f+=1.0F;
	f*=30.6F;
	f=(MYFLOAT)((MYLONG)f);
	JulianDate+=f;

	f=(MYFLOAT)day;
  JulianDate+=f;
	JulianDate-=428.0F;
     
  return (JulianDate);
}

static MYFLOAT P13TimeToDayNum(U8 u8Hour, U8 u8Min, U8 u8Sec)
{
	// Convert separate HMS to a DayNum time
	// return ((u8Sec/60.0F+u8Min)/60.0F+u8Hour)/24.0F;

	MYFLOAT f=u8Sec/60.0F;

	f+=u8Min;
	f/=60.0F;
	f+=u8Hour;
	f/=24.0F;

	return f;
}

static void P13DayNumToTS(TIMESTRUCT size2 *pts,MYFLOAT fDay,MYFLOAT fTime)
{
	// Convert separate DayNum day and time to a TIMESTRUCT
	{
		MYFLOAT f=fTime;
		MYFLOAT f2;
		U8 u8;

		f*=24.0F;
		u8=((U8)f);
		f2=(MYFLOAT)u8;
		f-=f2;
		pts->u8Hour=u8;

		f*=60.0F;
		u8=((U8)f);
		f2=(MYFLOAT)u8;
		f-=f2;
		pts->u8Min=u8;

		f*=60.0F;
		u8=(U8)f;
		pts->u8Sec=u8;

	}
	
	{
		U24 u24i;
		U24 u24j;
		U24 u24k;
		U24 u24l;
		U24 u24n;
		MYFLOAT f=fTime;

		u24j = (U24)fDay + 1722825;

//	u24k = (u24j-1)/1461;
		u24k = u24j-1;
		u24k/=1461;

//	u24l = u24j - 1461*u24k;
		u24l = 1461*u24k;
		u24l = u24j - u24l;

//	u24n = (u24l-1)/365 - u24l/1461;
		{
			U24 u24=u24l/1451;

			u24n=u24l-1;
			u24n/=365;
			u24n-=u24;
		}

//	u24i = u24l - 365*u24n + 30;
		u24i=365*u24n;
		u24i=u24l-u24i;
		u24i+=30; //*******

//	u24j = (80*u24i)/2447;
		u24j=80*u24i;
		u24j/=2447;

//	pts->u8Day = (U8)(u24i - (2447*u24j)/80);
		{
			U24 u24=2447*u24j;

			u24/=80;
			pts->u8Day=(U8)(u24i-u24);
		}


		u24i = u24j/11;

//	pts->u8Month = (U8)(u24j + 2 - 12*u24i);
		{
			U24 u24=12*u24i;

			u24=2-u24;
			pts->u8Month=(U8)(u24j+u24);
		}

//	pts->u16Year = (U16)(4*u24k + u24n + u24i - 4716);
		{
			U24 u24=4*u24k;

			u24+=u24n;
			u24+=u24i;
			u24-=4716;
			pts->u16Year=(U16)(u24);
		}
	}
}

static MYFLOAT P13DayNumDiff(MYFLOAT fDay1,MYFLOAT fTime1,MYFLOAT fDay2,MYFLOAT fTime2)
{
	MYFLOAT f=fDay1-fDay2;

//f+=(fTime1-fTime2);
	f+=fTime1;
	f-=fTime2;
	return f;
}

static MYFLOAT P13TSDiff(TIMESTRUCT size2 *pts1,TIMESTRUCT size2 *pts2)
{
	// Calculate the difference between two TIMESTRUCTs, returning the result as
	// a single scalar DayNum
	U16 u16Year=pts1->u16Year;
	U8 u8Month=pts1->u8Month;
	U8 u8Day=pts1->u8Day;
	U8 u8Hour=pts1->u8Hour;
	U8 u8Min=pts1->u8Min;
	U8 u8Sec=pts1->u8Sec;
	MYFLOAT fDay1=P13DayToDayNum(u16Year,u8Month,u8Day);
	MYFLOAT fTime1=P13TimeToDayNum(u8Hour,u8Min,u8Sec);
	MYFLOAT fDay2;
	MYFLOAT fTime2;

	u16Year=pts2->u16Year;
	u8Month=pts2->u8Month;
	u8Day=pts2->u8Day;
	u8Hour=pts2->u8Hour;
	u8Min=pts2->u8Min;
	u8Sec=pts2->u8Sec;

	fDay2=P13DayToDayNum(u16Year,u8Month,u8Day);
	fTime2=P13TimeToDayNum(u8Hour,u8Min,u8Sec);

	return P13DayNumDiff(fDay1,fTime1,fDay2,fTime2);
}

static void P13DayNumAdd(MYFLOAT f)
{
	// Due to inaccuracies of 32 bit floating point,need to add time and date separately.
	TN+=f;
	while (TN<0.0F)
	{
		TN+=1.0F;
		DN-=1.0F;
	}
	while (TN>=1.0F)
	{
		TN-=1.0F;
		DN+=1.0F;
	}
}

static void P13Init(KEPSTRUCT size2 *pks,OBSERVERSTRUCT size2 *pos)
{
	// Prepares relatively constant values based on the observer's
	// and satellite keps for future prediction using P13Calc
#ifdef MYDEBUG2
		char sz[30];
#endif

	strcpy(SAT,pks->szName);
	SATNO=pks->u24CatalogNumber;
	RV=pks->u24EpochRev;
	YE=(MYFLOAT)pks->u8EpochYear+2000.0F;
	TE=pks->fEpochTime;
	myIN=pks->fInclination;
	RA=pks->fRAOfNode;
	EC=pks->fEccentricity;
	WP=pks->fArgOfPerigee;
	MA=pks->fMeanAnomaly;
	MM=pks->fMeanMotion;
	M2=pks->fDecayRate;
	ALON=pks->u8ALon;
	ALAT=pks->u8ALat;

  /* Observer's location */
  LA = P13Rad( pos->fLat );
#ifdef MYDEBUG2
	myputcs("Lat = ");
	myftoa(sz,LA,4,6);
	myputs(sz);
	myputcs("\r\n");
#endif

  LO = P13Rad( pos->fLon );
#ifdef MYDEBUG2
	myputcs("Lon = ");
	myftoa(sz,LO,4,6);
	myputs(sz);
	myputcs("\r\n");
#endif

  HT = pos->fHeight / 1000.0F;
    
  CL = (MYFLOAT)cos(LA);
  SL = (MYFLOAT)sin(LA);
  CO = (MYFLOAT)cos(LO);
  SO = (MYFLOAT)sin(LO);
  
  /* WGS-84 Earth ellipsoid */
  RE = 6378.137F;
  FL = 1.0F / 298.257222F;

  
  /*RP = RE * (1.0 - FL);*/
	RP=1.0F-FL;
	RP*=RE;

  RE2 = RE * RE;
  RP2 = RP * RP;
  
  /*D = sqrt( RE2 * CL * CL + RP2 * SL * SL );*/
	{
		MYFLOAT f=RE2 * CL * CL;

		D=RP2 * SL * SL;
		D+=f;
		D=(MYFLOAT)sqrt(D);
  }

  Rx = RE2 / D + HT;
  Rz = RP2 / D + HT;
  
  /* Observer's unit vectors Up EAST and NORTH in geocentric coordinates */
  Ux = CL * CO;
  Ex = -SO;
  Nx = -SL * CO;
  
  Uy = CL * SO;
  Ey = CO;
  Ny = -SL * SO;
  
  Uz = SL;
  Ez = 0;
  Nz = CL;
  
  /* Observer's XYZ coordinates at earth's surface */
  Ox = Rx * Ux;
  Oy = Rx * Uy;
  Oz = Rz * Uz;
  
  /* Convert angles to radians, etc. */
  RA = P13Rad(RA);
  myIN = P13Rad(myIN);
  WP = P13Rad(WP);
  MA = P13Rad(MA);
  MM = MM * 2.0F * M_PI;
  M2 = M2 * 2.0F * M_PI;
  
  
  YT = 365.2421970F;		/* Tropical year, days                       */
  WW = 2.0F * M_PI / YT;		/* Earth's rotation rate, rads/whole day     */
  WE = 2.0F * M_PI + WW;		/* Earth's rotation rate, rads/day           */
  W0 = WE / 86400.0F;		/* Earth's rotation rate, rads/sec           */
  
  /* Observer's velocity, geocentric coordinates */
  VOx = -Oy * W0;
  VOy = Ox * W0;
  
  /* Convert satellite epoch to Day No. and fraction of a day */
  /*DE = FNday(YE, 1, 0) + (MYLONG)TE;*/
	DE=P13DayToDayNum((U16)YE, 1, 0);
	DE+=(MYLONG)TE;

  /*TE = TE - (MYLONG)TE;*/
	{
		MYFLOAT f=(MYFLOAT)((MYLONG)TE);
		TE-=f;
	}
  
  /* Average Precession rates */
  GM = 3.986E5F;               /* Earth's gravitational constant km^3/s^2   */
  J2 = 1.08263E-3F;            /* 2nd Zonal coeff, Earth's gravity Field    */
  N0 = MM / 86400.0F;                    /* Mean motion rads/s                */
  A0 = FPLpow(GM / N0 / N0, 1.0F / 3.0F);  /* Semi major axis km                */

  /*b0 = A0 * sqrt(1.0 - EC * EC);*/      /* Semi minor axis km                */
	b0=EC * EC;
	b0=1.0F - b0;
	b0=(MYFLOAT)sqrt(b0);
	b0*=A0;

  SI = (MYFLOAT)sin(myIN);
  CI = (MYFLOAT)cos(myIN);

  /* PC = RE * A0 / (b0 * b0);*/
	PC=(b0 * b0);
	PC=RE * A0 / PC;

  PC = 1.5F * J2 * PC * PC * MM;       /* Precession const, rad/day         */
  QD = -PC * CI;                      /* Node Precession rate, rad/day     */

  /*WD = PC *(5.0 * CI*CI - 1.0) / 2.0;*/ /* Perigee Precession rate, rad/day  */
	WD=(5.0F * CI*CI - 1.0F)/2.0F;
	WD*=PC;

  myDC = -2.0F * M2 / MM / 3.0F;          /* Drag coeff                        */

  /* Sidereal and Solar data. NEVER needs changing. Valid to year 2015+ */

  /* GHAA, Year YG, Jan 0.0 */
  YG = 2005;
  G0 = 99.7599F;
  /* MA Sun and rate, deg, deg/day */
  MAS0 = 356.7424F;
  MASD = 0.98560028F;
  /* Sun's inclination */
  INS = P13Rad(23.4386F);
  CNS = (MYFLOAT)cos(INS);
  SNS = (MYFLOAT)sin(INS);
  /* Sun's equation of center terms */
  EQC1 = 0.03341F;
  EQC2 = 0.00035F;
  
  /* Bring Sun data to satellite epoch */
  /*TEG = (DE - FNday(YG, 1, 0)) + TE;*/  /* Elapsed Time: Epoch - YG          */
	TEG=P13DayToDayNum(YG, 1, 0);
	TEG=DE-TEG+TE;

  /*GHAE = rad(G0) + TEG * WE;*/          /* GHA Aries, epoch                  */
	{
		MYFLOAT f=P13Rad(G0);
		GHAE=TEG*WE+f;
	}

  /*MRSE = rad(G0) + (TEG * WW) + M_PI;*/   /* Mean RA Sun at Sat Epoch          */
	{
		MYFLOAT f=P13Rad(G0);
		MRSE=(TEG * WW) + M_PI + f;
	}

  /*MASE = rad(MAS0 + MASD * TEG);*/      /* Mean MA Sun                       */
  MASE = P13Rad(MASD * TEG + MAS0);      /* Mean MA Sun                       */
  
  /* Antenna unit vector in orbit plane coordinates */
  CO = (MYFLOAT)cos(P13Rad(ALON));
  SO = (MYFLOAT)sin(P13Rad(ALON));
  CL = (MYFLOAT)cos(P13Rad(ALAT));
  SL = (MYFLOAT)sin(P13Rad(ALAT));
  ax = -CL * CO;
  ay = -CL * SO;
  az = -SL;
  
  /* Miscellaneous */
  OLDRN = -99999;

#ifdef MYDEBUG
  mydebug("HT",HT);
  mydebug("CL",CL);
  mydebug("SL",SL);
  mydebug("CO",CO);
  mydebug("SO",SO);
  mydebug("RE",RE);
  mydebug("FL",FL);
  mydebug("RP",RP);
  mydebug("RE2",RE2);
  mydebug("RP2",RP2);
  mydebug("D",D);
  mydebug("Rx",Rx);
  mydebug("Rz",Rz);
  mydebug("Ux",Ux);
  mydebug("Ex",Ex);
  mydebug("Nx",Nx);
  mydebug("Uy",Uy);
  mydebug("Ey",Ey);
  mydebug("Ny",Ny);
  mydebug("Uz",Uz);
  mydebug("Ez",Ez);
  mydebug("Nz",Nz);
  mydebug("Ox",Ox);
  mydebug("Oy",Oy);
  mydebug("Oz",Oz);
  mydebug("RA",RA);
  mydebug("myIN",myIN);
  mydebug("WP",WP);
  mydebug("MA",MA);
  mydebug("MM",MM);
  mydebug("M2",M2);
  mydebug("YT",YT);
  mydebug("WW",WW);
  mydebug("WE",WE);
  mydebug("W0",W0);
  mydebug("VOx",VOx);
  mydebug("VOy",VOy);
	mydebug("DE",DE);
	mydebug("TE",TE);
	mydebug("GM",GM);
	mydebug("J2",J2);
	mydebug("N0",N0);
	mydebug("A0",A0);
	mydebug("b0",b0);
	mydebug("SI",SI);
	mydebug("CI",CI);
	mydebug("PC",PC);
	mydebug("QD",QD);
	mydebug("WD",WD);
	mydebug("DC",myDC);
	{
		MYFLOAT f=(MYFLOAT)YG;
		mydebug("YG",f);
	}
	mydebug("G0",G0);
	mydebug("MAS0",MAS0);
	mydebug("MASD",MASD);
	mydebug("INS",INS);
	mydebug("CNS",CNS);
	mydebug("SNS",SNS);
	mydebug("EQC1",EQC1);
	mydebug("EQC2",EQC2);
	mydebug("TEG",TEG);
	mydebug("GHAE",GHAE);
	mydebug("MRSE",MRSE);
	mydebug("MASE",MASE);
	mydebug("SO",SO);
	mydebug("CL",CL);
	mydebug("SL",SL);
	mydebug("ax",ax);
	mydebug("ay",ay);
	mydebug("az",az);
#endif

#ifdef MYDEBUG2
	myputcs("Initialization complete.\n");      
#endif
}

/*
 * Calculate satellite position at DN, TN
 */ 
static void P13SatVec(void)
{
  /*T = (DN - DE) + (TN - TE);*/          /* Elapsed T since epoch             */
	T=DN - DE + TN - TE;

  DT = myDC * T / 2.0F;                  /* Linear drag terms                 */
  KD = 1.0F + 4.0F * DT;

  /*KDP = 1.0 - 7.0 * DT;*/
	KDP=7.0F*DT;
	KDP=1.0F-KDP;

  /*M = MA + MM * T * (1.0 - 3.0 * DT);*/ /* Mean anomaly at YR,/ TN           */
	M=3.0F * DT;
	M=1.00F-M;

	/*M*=MM * T;*/
	M=M * MM * T;

	M+=MA;

  DR = (MYLONG)(M / (2.0F * M_PI));         /* Strip out whole no of revs        */

  /*M = M - DR * 2.0 * M_PI*/;              /* M now in range 0 - 2PI            */
	{
		MYFLOAT f=2.0F * M_PI * DR;
		M=M-f;

		if (M<0.0F) // If 'prediction' is prior to Epoch, need to add pi/2
		{
			M+=2.0F*M_PI;
		}
	}

  /*RN = RV + DR + 1;*/                   /* Current orbit number              */
	RN=RV+DR;
	// RN+=1; // Not sure if we need to add one here - Nova & IT don't

  /* Solve M = EA - EC * sin(EA) for EA given M, by Newton's method        */

  EA = M;  
                          /* Initail solution                  */
  do 
  {
		C = (MYFLOAT)cos(EA);
		myS = (MYFLOAT)sin(EA);

		/*DNOM = 1.0 - EC * C;*/
		DNOM=EC*C;
		DNOM=1.0F-DNOM;

		/*D = (EA - EC * myS - M) / DNOM;*/ /* Change EA to better resolution    */
		D=EC*myS;
		D=(EA-D-M)/DNOM;

		EA = EA - D;                    /* by this amount until converged    */
  }
  while (FPLabs(D) > 1.0E-5);

  /* Distances */
  A = A0 * KD;
  B = b0 * KD;
  RS = A * DNOM;

  /* Calculate satellite position and velocity in plane of ellipse */
  /*Sx = A * (C - EC);*/
	Sx=(C-EC)*A;

  Vx = -A * myS / DNOM * N0;
  Sy = B * myS;
  Vy = B * C / DNOM * N0;
  
  /*AP = WP + WD * T * KDP;*/
	AP=WD * T * KDP + WP;

  CW = (MYFLOAT)cos(AP);
  SW = (MYFLOAT)sin(AP);

  /*RAAN = RA + QD * T * KDP;*/
	RAAN = QD * T * KDP + RA;

  CQ = (MYFLOAT)cos(RAAN);
  SQ = (MYFLOAT)sin(RAAN);
  
  /* Plane -> celestial coordinate transformation, [C] = [RAAN]*[myIN]*[AP] */
  /*CXx = CW * CQ - SW * CI * SQ;*/
	CXx=SW * CI * SQ;
	CXx=CW * CQ - CXx;

  /*CXy = -SW * CQ - CW * CI * SQ;*/
	CXy=CW * CI * SQ;
	CXy=-SW * CQ - CXy;

  CXz = SI * SQ;

  /*CYx = CW * SQ + SW * CI * CQ;*/
	CYx=SW * CI * CQ;
	CYx=CW * SQ + CYx;

  /*CYy = -SW * SQ + CW * CI * CQ;*/
	CYy =CW * CI * CQ;
	CYy =-SW * SQ + CYy;

  CYz = -SI * CQ;
  CZx = SW * SI;
  CZy = CW * SI;
  CZz = CI;
  
  /* Compute satellite's position vector, ANTenna axis unit vector    */
  /*   and velocity  in celestial coordinates. (Note: Sz = 0, Vz = 0) */
  /*SATx = Sx * CXx + Sy * CXy;*/
  SATx =Sy * CXy;
  SATx =Sx * CXx + SATx;

  /*ANTx = ax * CXx + ay * CXy + az * CXz;*/
	{
		MYFLOAT f;
		ANTx=ax * CXx;
		f=ay * CXy;
		ANTx+=f;
		f=az * CXz;
		ANTx+=f;
	}

  /*VELx = Vx * CXx + Vy * CXy;*/
  VELx =Vy * CXy;
  VELx =Vx * CXx + VELx;

  /*SATy = Sx * CYx + Sy * CYy;*/
  SATy =Sy * CYy;
  SATy =Sx * CYx + SATy;

  /*ANTy = ax * CYx + ay * CYy + az * CYz;*/
	{
		MYFLOAT f;
		ANTy=ax * CYx;
		f=ay * CYy;
		ANTy+=f;
		f=az * CYz;
		ANTy+=f;
	}

  /*VELy = Vx * CYx + Vy * CYy;*/
  VELy =Vy * CYy;
  VELy =Vx * CYx + VELy;

  /*SATz = Sx * CZx + Sy * CZy;*/
  SATz =Sy * CZy;
  SATz =Sx * CZx + SATz;

  /*ANTz = ax * CZx + ay * CZy + az * CZz;*/
	{
		MYFLOAT f;
		ANTz=ax * CZx;
		f=ay * CZy;
		ANTz+=f;
		f=az * CZz;
		ANTz+=f;
	}

  /*VELz = Vx * CZx + Vy * CZy;*/
  VELz =Vy * CZy;
  VELz =Vx * CZx + VELz;
  
  /* Also express SAT, ANT, and VEL in ECEF coordinates */
  /*GHAA = GHAE + WE * T;*/       /* GHA Aries at elaprsed time T */
  GHAA = WE * T + GHAE;

  C = (MYFLOAT)cos(-GHAA);
  myS = (MYFLOAT)sin(-GHAA);

  /*Sx = SATx * C - SATy * myS;*/
  Sx =SATy * myS;
  Sx =SATx * C - Sx;

  /*Ax = ANTx * C - ANTy * myS;*/
  Ax =ANTy * myS;
  Ax =ANTx * C - Ax;

  /*Vx = VELx * C - VELy * myS;*/
  Vx =VELy * myS;
  Vx =VELx * C - Vx;

  /*Sy = SATx * myS + SATy * C;*/
  Sy =SATy * C;
  Sy =SATx * myS + Sy;

  /*Ay = ANTx * myS + ANTy * C;*/
  Ay =ANTy * C;
  Ay =ANTx * myS + Ay;

  /*Vy = VELx * myS + VELy * C;*/
  Vy =VELy * C;
  Vy =VELx * myS + Vy;

  Sz = SATz;
  Az = ANTz;
  Vz = VELz;
}

/*
 * Compute and manipulate range/velocity/antenna vectors
 */
static void P13RangeVec(void)
{
  /* Range vector = sat vector - observer vector */
  Rx = Sx - Ox;
  Ry = Sy - Oy;
  Rz = Sz - Oz;    
  
  /*R = sqrt(Rx * Rx + Ry * Ry + Rz * Rz);*/    /* Range Magnitute */
	{
		MYFLOAT f;
		R=Rx * Rx;
		f=Ry * Ry;
		R+=f;
		f=Rz * Rz;
		R+=f;
		R=(MYFLOAT)sqrt(R);
	}			

  /* Normalize range vector */
  Rx = Rx / R;
  Ry = Ry / R;
  Rz = Rz / R;

  /*U = Rx * Ux + Ry * Uy + Rz * Uz;*/
	{
		MYFLOAT f;
		U=Rx * Ux;
		f=Ry * Uy;
		U+=f;
		f=Rz * Uz;
		U+=f;
	}

  /*E = Rx * Ex + */
	{
		MYFLOAT f;
		E=Rx * Ex;
		f=Ry * Ey;
		E+=f;			
	}

  /*N = Rx * Nx + Ry * Ny + Rz * Nz;*/
	{
		MYFLOAT f;
		N=Rx * Nx;
		f=Ry * Ny;
		N+=f;
		f=Rz * Nz;
		N+=f;
	}

  AZ = P13Deg(P13Atan(E, N));
  EL = P13Deg(FPLasin(U));
 
  /* Solve antenna vector along unit range vector, -r.a = cos(SQ) */

//SQ = P13Deg(myacos(-(Ax * Rx + Ay * Ry + Az * Rz)));

	{
	  MYFLOAT f;

		SQ=Ax*Rx;
		f=Ay*Ry;
		SQ+=f;
		f=Az*Rz;
		SQ+=f;
		SQ=P13Deg(FPLacos(-SQ));
	}

  /* Calculate sub-satellite Lat/Lon */
  SLON = P13Deg(P13Atan(Sy, Sx));   /* Lon, + East  */
  SLAT = P13Deg(FPLasin(Sz / RS));   /* Lat, + North */
  
  /* Resolve Sat-Obs velocity vector along unit range vector. (VOz = 0) */
  /*RR = (Vx - VOx) * Rx + (Vy - VOy) * Ry + Vz * Rz;*/ /* Range rate, km/sec */
	{
		MYFLOAT f;
		RR=(Vx - VOx) * Rx;
		f=(Vy - VOy) * Ry;
		RR+=f;
		f=Vz * Rz;
		RR+=f;
	}

  /*FR = rxFrequency * (1 - RR / 299792);*/
}

static void P13SunVec(void)
{
//	MAS = MASE + P13Rad(MASD*T);       /* MA of Sun round its orbit */
	MAS = P13Rad(MASD*T) + MASE;       /* MA of Sun round its orbit */	

//	TAS = MRSE + WW*T + EQC1*sin(MAS) + EQC2*sin(2*MAS);
	{
		MYFLOAT f;

		TAS=sin(2.0F*MAS)*EQC2+MRSE;
		f=WW*T;
		TAS+=f;
		f=sin(MAS)*EQC1;
		TAS+=f;
	}

	C=cos(TAS);
	myS=sin(TAS);       /* Sin/Cos Sun's true anomaly */

	SUNx=C; 
	SUNy=myS*CNS; 
	SUNz=myS*SNS; /* Sun unit vector - CELESTIAL coords */
	
	/* Find Solar angle, illumination, and eclipse status. */
//	SSA = -(ANTx*SUNx + ANTy*SUNy + ANTz*SUNz); /* Sin of Sun angle -a.h    */
	SSA=ANTx*SUNx;
	SSA=ANTy*SUNy+SSA;
	SSA=ANTz*SUNz+SSA;
	SSA=-SSA;

//	ILL = sqrt(1-SSA*SSA);                      /* Illumination             */
	ILL=SSA*SSA;
	ILL=1.0F-ILL;

//	CUA = -(SATx*SUNx+SATy*SUNy+SATz*SUNz)/RS;  /* Cos of umbral angle -h.s */
	CUA=SATx*SUNx;
	CUA=SATy*SUNy+CUA;
	CUA=SATz*SUNz+CUA;
	CUA=-CUA/RS;

//	UMD = RS*sqrt(1-CUA*CUA)/RE;                /* Umbral dist, Earth radii */
	UMD=CUA*CUA;
	UMD=sqrt(1-UMD)*RS/RE;

	if ( CUA >= 0 )
	{
	   ECL=P13VE_SHADOWSIDE;                   /* + for shadow side        */ 
	}
	else
	{
	   ECL=P13VE_SUNNYSIDE;                   /* - for sunny side         */
	}
	
	if ( UMD <= 1 && CUA>=0 )
	{
	   ECL=P13VE_ECLIPSED;                   /* ECL for eclipsed         */  
	}
	
	/* Obtain SUN unit vector in GEOCENTRIC coordinates */
	C = cos(-GHAA);
	myS = sin(-GHAA);

//	Hx=SUNx*C - SUNy*myS;
	Hx=-SUNy*myS;
	Hx=SUNx*C+Hx;

//	Hy=SUNx*myS + SUNy*C;  /* If Sun more than 10 deg below horizon */
	Hy=SUNy*C;
	Hy=SUNx*myS+Hy;

	Hz=SUNz;             /* satellite possibly visible            */
//	if ( (Hx*Ux+Hy*Uy+Hz*Uz < -0.17F) && ECL=='E' )
	{
		MYFLOAT f=Hx*Ux;

		f=Hy*Uy+f;
		f=Hz*Uz+f;

		if (f < -0.17F && ECL=='E')
		{
			ECL=P13VE_VISIBLE;
		}
	}
	
	/* Obtain Sun unit vector in ORBIT coordinates */
//	Hx =  SUNx*CXx + SUNy*CYx + SUNz*CZx;
	Hx=SUNx*CXx;
	Hx=SUNy*CYx+Hx;
	Hx=SUNz*CZx+Hx;

//	Hy =  SUNx*CXy + SUNy*CYy + SUNz*CZy;
	Hy=SUNx*CXy;
	Hy=SUNy*CYy+Hy;
	Hy=SUNz*CZy+Hy;

//	Hz =  SUNx*CXz + SUNy*CYz + SUNz*CZz;
	Hz=SUNx*CXz;
	Hz=SUNy*CYz+Hz;
	Hz=SUNz*CZz+Hz;

	SEL=FPLasin(Hz); 
	SAZ=P13Atan(Hy,Hx);
}

static void P13Calc(BOOL bCalcSunVec,BOOL bPoll)
{
	P13SatVec();
	P13RangeVec();
	if (bCalcSunVec)
	{
		P13SunVec();
	}
	if (bPoll)
	{
		GPSPoll();
		HSTPoll();
	}
}

static void P13TimeStructToDayNum(TIMESTRUCT size2 *pts)
{
	U16 u16Year=pts->u16Year;
	U8 u8Month=pts->u8Month;
	U8 u8Day=pts->u8Day;
	U8 u8Hour=pts->u8Hour;
	U8 u8Min=pts->u8Min;
	U8 u8Sec=pts->u8Sec;

	DN=P13DayToDayNum(u16Year,u8Month,u8Day);
	TN=P13TimeToDayNum(u8Hour,u8Min,u8Sec);
}

static void P13Predict(TIMESTRUCT size2 *pts,PREDICTSTRUCT size2 *pps)
{
	// Given the time in *pts, return Az/El etc in *pps
	// P13Init must have been called before to load keps and observer position
	P13TimeStructToDayNum(pts);

	P13Calc(TRUE,TRUE);

	pps->fAzimuth=AZ;
	pps->fElevation=EL;
	pps->fSSPLon=SLON;
	pps->fSSPLat=SLAT;

//pps->fHeight=RS-RE;
	{
		MYFLOAT f=RS-RE;

		pps->fHeight=f;
	}

	pps->fRange=R;
	pps->fRangeRate=RR;
	pps->lOrbit=RN;

//pps->fMA=M/2.0F/M_PI*256.0F;
	{
		MYFLOAT f=M/2.0F/M_PI*256.0F;
		pps->fMA=f;
	}
	pps->fSquint=SQ;
}

static BOOL P13AOSHappens(OBSERVERSTRUCT size2 *pos,KEPSTRUCT size2 *pks)
{
	// Based on John A. Magliacane's, KD2BD, Predict program

	MYFLOAT fLin,fSma,fApogee;
	MYFLOAT fMeanMotion=pks->fMeanMotion;
	MYFLOAT fEccentricity=pks->fEccentricity;

	if (fMeanMotion==0.0F)
	{
		return FALSE;
	}
	fLin=pks->fInclination;
	if (fLin>=90.0F)
	{
		fLin=180.0F-fLin;
	}

//fSma=331.25F*(MYFLOAT)exp(log(1440.0F/fMeanMotion)*(2.0F/3.0F));
	fSma=log(1440.0F/fMeanMotion)*(2.0F/3.0F);
	fSma=((MYFLOAT)exp(fSma))*331.25F;

//fApogee=fSma*(1.0F+fEccentricity)-RE;
	fApogee=1.0F+fEccentricity;
	fApogee*=fSma;
	fApogee-=RE;

//if (myacos(RE/(fApogee+RE))+P13Rad(fLin) > P13Rad(myabs(pos->fLat)))
	{
		MYFLOAT f1=fApogee+RE;
		MYFLOAT f2=P13Rad(FPLabs(pos->fLat));
		MYFLOAT f3=P13Rad(fLin);

		f1=FPLacos(RE/f1);
		f1+=f3;

		if (f1 > f2)
		{
			return TRUE;
		}
	}
	return FALSE;
}


static BOOL P13IsDecayed(KEPSTRUCT size2 *pks)
{
	// Based on John A. Magliacane's, KD2BD, Predict program
	MYFLOAT fTime=DN+TN;
//MYFLOAT fSatEpoch=P13DayToDayNum((U16)(pks->u8EpochYear+2000),1,0)+pks->fEpochTime;
	MYFLOAT fSatEpoch;
	{
		MYFLOAT f=pks->fEpochTime;
		fSatEpoch=P13DayToDayNum((U16)(pks->u8EpochYear+2000),1,0);
		fSatEpoch+=f;
	}

//if (fSatEpoch+( (16.666666F-pks->fMeanMotion) / (10.0F*myabs(pks->fDecayRate)) ) < fTime)
	{
		MYFLOAT f1=pks->fMeanMotion;
		MYFLOAT f2=16.666666F-f1;
		MYFLOAT f3=FPLabs(pks->fDecayRate)*10.0F;

		f2/=f3;
		f2+=fSatEpoch;
		if (f2 < fTime)
		{
			return TRUE;
		}
	}
	return FALSE;
}

static BOOL P13IsGeostationary(KEPSTRUCT size2 *pks)
// Based on John A. Magliacane's, KD2BD, Predict program
{
	if (FPLabs(pks->fMeanMotion-1.0027F)<0.0002F) 
	{
		return TRUE;
	}
	return FALSE;
}

static void P13AOSFind(OBSERVERSTRUCT size2 *pos,KEPSTRUCT size2 *pks)
// Based on John A. Magliacane's, KD2BD, Predict program
{
	/* This function finds and returns the time of AOS (aostime). */
	// If the satellite is already above the horizon, it will track back to AOS
	// It does NOT check if AOS is possible, or if Decayed , or if Geostationary
	BOOL bFinished=FALSE;
                             
	P13Calc(FALSE,TRUE);

	/* Get the satellite in range */

	while (EL < -1.0F)
	{
//	P13DayNumAdd(-0.00035F*(EL*((((RS-RE)/8400.0F)+0.46F))-2.0F));
		{
			MYFLOAT f=((RS-RE)/8400.0F)+0.46F;
			f*=EL;
			f-=2.0F;
			f*=-0.00035F;
			P13DayNumAdd(f);
		}

		/* Technically, this should be:

			 daynum-=0.0007*(sat_ele*(((sat_alt/8400.0)+0.46))-2.0);

			 but it sometimes skipped passes for
			 satellites in highly elliptical orbits. */

		P13Calc(FALSE,TRUE);
	}

	/* Find AOS */

	/** Users using Keplerian data to track the Sun MAY find
		  this section goes into an infinite loop when tracking
		  the Sun if their QTH is below 30 deg N! **/

	while (!bFinished)
	{
		if (FPLabs(EL) < 0.03)
		{
			bFinished=TRUE;
		}
		else
		{
//		P13DayNumAdd(-EL*(MYFLOAT)sqrt(RS-RE)/530000.0F);
			{
				MYFLOAT f=(MYFLOAT)sqrt(RS-RE)/530000.0F;
				f*=-EL;
				P13DayNumAdd(f);
			}

			P13Calc(FALSE,TRUE);
		}
	}
}

static void P13LOSFind(OBSERVERSTRUCT size2 *pos,KEPSTRUCT size2 *pks)
// Based on John A. Magliacane's, KD2BD, Predict program
{
	// It does NOT check if AOS is possible, or if Decayed , or if Geostationary
	BOOL bFinished=FALSE;

	P13Calc(FALSE,TRUE);

	do
	{
//	P13DayNumAdd(EL*(MYFLOAT)sqrt(RS-RE)/502500.0F);
		{
			MYFLOAT f=(MYFLOAT)sqrt(RS-RE)/502500.0F;
			f*=EL;
			P13DayNumAdd(f);
		}

		P13Calc(FALSE,TRUE);

		if (FPLabs(EL) < 0.03F)
		{
			bFinished=TRUE;
		}					
	} while (!bFinished);
}

static void P13LOSFind2(OBSERVERSTRUCT size2 *pos,KEPSTRUCT size2 *pks)
// Based on John A. Magliacane's, KD2BD, Predict program
{
	/* This function steps through the pass to find LOS.
	   P13LOSFind() is called to "fine tune" and return the result. */
	// It does NOT check if AOS is possible, or if Decayed , or if Geostationary
	do
	{
//	P13DayNumAdd((MYFLOAT)cos(P13Rad(EL-1.0F))*(MYFLOAT)sqrt(RS-RE)/25000.0F);
		{
			MYFLOAT f1=(MYFLOAT)cos(P13Rad(EL-1.0F));
			MYFLOAT f2=(MYFLOAT)sqrt(RS-RE);
			f1*=f2;
			f1/=25000.0F;
			P13DayNumAdd(f1);
		}

		P13Calc(FALSE,TRUE);
	} while (EL>=0.0F);

	P13LOSFind(pos,pks);
}

static void P13AOSFindNext(OBSERVERSTRUCT size2 *pos,KEPSTRUCT size2 *pks)
// Based on John A. Magliacane's, KD2BD, Predict program
{
	/* This function finds and returns the time of the next
	   AOS for a satellite that is currently in range. */
	// It does NOT check if AOS is possible, or if Decayed , or if Geostationary

	P13LOSFind2(pos,pks);
	P13DayNumAdd(0.014F); // Plus 20 minutes
	P13AOSFind(pos,pks);
}

static BOOL P13GetAOS(OBSERVERSTRUCT size2 *pos,KEPSTRUCT size2 *pks,AOSLOSSTRUCT size2 *pals)
{
	// Get the current or next AOS time and AZ
//pals->bAOSHappens=P13AOSHappens(pos,pks);
	{
		BOOL b=P13AOSHappens(pos,pks);
		pals->bAOSHappens=b;
	}

//pals->bGeostationary=P13IsGeostationary(pks);
	{
		BOOL b=P13IsGeostationary(pks);
		pals->bGeostationary=b;
	}

//pals->bDecayed=P13IsDecayed(pks);
	{
		BOOL b=P13IsDecayed(pks);
		pals->bDecayed=b;
	}

	if (pals->bAOSHappens && !pals->bGeostationary && !pals->bDecayed)
	{
		P13AOSFind(pos,pks);
		pals->fDayAOS=DN;
		pals->fTimeAOS=TN;
		pals->fAzAOS=AZ;
		pals->lOrbit=RN; // Saved for blacklist




		return TRUE;
	}
	else
	{
		return FALSE;
	}
}

static void P13GetLOS(OBSERVERSTRUCT size2 *pos,KEPSTRUCT size2 *pks,AOSLOSSTRUCT size2 *pals)
{
	// We already have the AOS fromP13GetAOS, with DN/TN set up to AOS
	P13LOSFind2(pos,pks); // Warning: I do not check for geostationary or decayed etc here!

	pals->fDayLOS=DN;
	pals->fTimeLOS=TN;
	pals->fAzLOS=AZ;
}	

static void P13GetMaxEl(OBSERVERSTRUCT size2 *pos,KEPSTRUCT size2 *pks,AOSLOSSTRUCT size2 *pals)
{
	// We already have the AOS/LOS from P13GetAOS and P13GetLOS
	MYFLOAT fEl;
//	MYFLOAT fTimeDiff=P13DayNumDiff(pals->fDayLOS,pals->fTimeLOS,pals->fDayAOS,pals->fTimeAOS)/2.0F;
	MYFLOAT fTimeDiff;
	{
		MYFLOAT fDayLOS=pals->fDayLOS;
		MYFLOAT fTimeLOS=pals->fTimeLOS;
		MYFLOAT fDayAOS=pals->fDayAOS;
		MYFLOAT fTimeAOS=pals->fTimeAOS;

		fTimeDiff=P13DayNumDiff(fDayLOS,fTimeLOS,fDayAOS,fTimeAOS)/2.0F;
	}

	P13DayNumAdd(-fTimeDiff); // Set to middle of pass

	do 
	{
		P13Calc(FALSE,TRUE);
		fEl=EL;
		P13DayNumAdd(1.0F/86400.0F); // One second later
		P13Calc(FALSE,TRUE);
		fTimeDiff/=2.0F;
		if (EL>fEl)
		{
			P13DayNumAdd(-1.0F/86400.0F+fTimeDiff);
		}
		else
		{
			P13DayNumAdd(-1.0F/86400.0F-fTimeDiff);
		}
	} while (fTimeDiff>1.0F/86400.0F);
	pals->fMaxEl=EL;
}

static BOOL P13GetAOSLOS(OBSERVERSTRUCT size2 *pos,KEPSTRUCT size2 *pks,AOSLOSSTRUCT size2 *pals)
{
	BOOL b=P13GetAOS(pos,pks,pals);

	if (b)
	{	
		P13GetLOS(pos,pks,pals);
		P13GetMaxEl(pos,pks,pals);
		return TRUE;
	}
	else
	{
		return FALSE;
	}
}

#pragma rambank 0

/*******************************************/
/* KEP routines                            */
/*******************************************/

#define KEP_MAXCHARS 80

static BOOL KEPSet(U8 u8Idx,KEPUNION size2 *pku,BOOL bWriteAll)
{
	// Writes the given Keps to Flash program memory
	U24 u24Addr=(U24)u8Idx*sizeof(*pku);
	U8 u8Size=sizeof(*pku);

	u24Addr+=FPM_DATA;

	if (!bWriteAll)
	{
		U8 u8a=(U8)(&pku->ks.u8ALon);
		U8 u8b=(U8)(&pku->ks.szName[0]);

		u8Size=u8a-u8b;
	}
	
	return FPMWrite(u24Addr,pku->au8,u8Size);
}

static BOOL KEPGet(U8 u8Idx,KEPUNION size2 *pku)
{
	// Retreives the given Keps from Flash program memory
	// Return TRUE if populated
	U24 u24Addr=(U24)u8Idx*sizeof(*pku);

	u24Addr+=FPM_DATA;

	FPMRead(u24Addr,pku->au8,sizeof(*pku));

	return pku->au8[0]!=0xFF;
}

static BOOL KEPNext(U8 size2 *pu8Idx,KEPUNION size2 *pku)
{
	// Finds the next Keps after the current index in Flash program memory
	U8 u8Idx=*pu8Idx;
	BOOL bFound=FALSE;

	while (!bFound && u8Idx<PRD_MAXSATS-1)
	{
		u8Idx++;
		bFound=KEPGet(u8Idx,pku);
	}
	if (bFound)
	{
		*pu8Idx=u8Idx;
	}
	return bFound;
}

static BOOL KEPFirst(U8 size2 *pu8Idx,KEPUNION size2 *pku)
{
	// Finds the Keps including the current index in Flash program memory
	U8 u8Idx=*pu8Idx;
	BOOL bFound=KEPGet(u8Idx,pku);

	if (!bFound)
	{
		bFound=KEPNext(pu8Idx,pku);
	}
	return bFound;
}

static BOOL KEPPrev(U8 size2 *pu8Idx,KEPUNION size2 *pku)
{
	// Finds the previous Keps in Flash program memory
	U8 u8Idx=*pu8Idx;
	BOOL bFound=FALSE;

	while (!bFound && u8Idx>0)
	{
		u8Idx--;
		bFound=KEPGet(u8Idx,pku);
	}
	if (bFound)
	{
		*pu8Idx=u8Idx;
	}
	return bFound;
}

static BOOL KEPFindName(char size2 *pszName,U8 *pu8Idx)
{
	#pragma rambank 3
	KEPUNION ku;
	#pragma rambank 0
	BOOL bFinished=FALSE;
	BOOL bFound=FALSE;

	*pu8Idx=0;
	
	bFinished=!KEPFirst(pu8Idx,&ku);
	
	while (!bFinished)
	{
		if (strcmp(ku.ks.szName,pszName)==0)
		{
			bFound=TRUE;
		}
		if (bFound)
		{
			bFinished=TRUE;
		}
		else
		{
			bFinished=!KEPNext(pu8Idx,&ku);
		}
	}
	return bFound;
}

static BOOL KEPFindCatalog(U24 u24CatalogNumber,U8 *pu8Idx)
{
	#pragma rambank 3
	KEPUNION ku;
	#pragma rambank 0
	BOOL bFinished=FALSE;
	BOOL bFound=FALSE;

	*pu8Idx=0;
	
	bFinished=!KEPFirst(pu8Idx,&ku);
	
	while (!bFinished)
	{
		if (u24CatalogNumber==ku.ks.u24CatalogNumber)
		{
			bFound=TRUE;
			bFinished=TRUE;
		}
		if (!bFound)
		{
			bFinished=!KEPNext(pu8Idx,&ku);
		}
	}
	return bFound;
}

static BOOL KEPFindFirstFree(U8 *pu8Idx)
{
	BOOL bFound=FALSE;
	U8 u8Idx=0;

	while (u8Idx<PRD_MAXSATS && !bFound)
	{
		#pragma rambank 3
		KEPUNION ku;
		#pragma rambank 0

		if (!KEPGet(u8Idx,&ku))
		{
			*pu8Idx=u8Idx;
			bFound=TRUE;
		}
		else
		{
			u8Idx++;			
		}
	}
	return bFound;
}

static MYFLOAT KEPGetElement(char *psz, U8 u8Start, U8 u8Stop)
{
  U8 u8;
	U8 u8Len;
	#pragma rambank 3
  char   sz[15];
	#pragma rambank 0

  u8Len=u8Stop-u8Start+1;

  for (u8=0;u8<=u8Len;u8++)
  {
//	gestr[k] = gstr[gstart+k-1];
		char c=psz[u8Start+u8-1];
		sz[u8]=c;
  }

  sz[u8Len]='\0';
  return FPAatof(sz);
}

static BOOL KEPChecksum(char *psz)
{
	// Check that the checksum of the line is right...
	U8 u8Sum=0;
	U8 u8;
	char c;
	
	for (u8=0;u8<68;u8++)
	{
		U8 u8Val=0;

		c=*psz++;
		switch (c)
		{
			case '\0':
				return FALSE;
			case '-':
				u8Val=1;
				break;
			case '+':
				u8Val=2;
				break;
			default:
				if (c>='0' && c<='9')
				{
					u8Val+=c-'0';
				}
				break;
		}
		u8Sum+=u8Val;
	}

	u8Sum%=10;

	if (u8Sum==*psz-'0')
	{
		return TRUE;
	}
	return FALSE;
}


static void KEPBulkLoad(void)
{
	// Bulk loads TLE's
	#pragma rambank 3
	char szLine0[KEP_MAXCHARS+1];
	char szLine1[KEP_MAXCHARS+1];
	char szLine2[KEP_MAXCHARS+1];
	char size2 *psz0=szLine0;
	char size2 *psz1=szLine1;
	char size2 *psz2=szLine2;
	#pragma rambank 0
	BOOL bFinished=FALSE;

	WriteStr(PE_HWS,"Ready for TLE Keps...\r\n");
	
	szLine0[0]='\0';
	szLine1[0]='\0';
	szLine2[0]='\0';

	while (!bFinished)
	{
		bFinished=ReadStr(PE_HWS,psz2,KEP_MAXCHARS);

		if (strlen(psz2)>0)
		{
			if (strncmp(psz1,"1 ",2)==0 && strncmp(psz2,"2 ",2)==0 && strncmp(psz1+2,psz2+2,5)==0)
			{
				WriteStr(PE_HWS,"Located keps for ");
				WriteStr(PE_HWS,psz0);

				if (!KEPChecksum(psz1) || !KEPChecksum(psz2))
				{
					WriteStr(PE_HWS," update failed: bad checksum.\r\n");
				}
				else
				{

					#pragma rambank 3
					KEPUNION ku;
					#pragma rambank 0
					U24 u24;
					U8 u8Len=strlen(psz0);
	
					memset(ku.au8,0xFF,PRD_KEPUNIONSIZE);
	
					if (u8Len>0 && psz0[u8Len-1]=='\n')
					{
						psz0[u8Len-1]='\0';
					}
	
					strncpy(ku.ks.szName,psz0,PRD_KEPNAMESIZE-1);
					ku.ks.szName[PRD_KEPNAMESIZE-1]='\0';
	
					u24=(U24)(KEPGetElement(psz1,3,7)+ONEPPM);
					ku.ks.u24CatalogNumber=u24;
	
					u24=(U24)(KEPGetElement(psz2,64,68)+ONEPPM);
					ku.ks.u24EpochRev=u24;
	
					ku.ks.u8EpochYear=(U8)KEPGetElement(psz1,19,20);
					ku.ks.fEpochTime=KEPGetElement(psz1,21,32);
					ku.ks.fInclination=KEPGetElement(psz2,9,16);
					ku.ks.fRAOfNode=KEPGetElement(psz2,18,25);
					ku.ks.fEccentricity=KEPGetElement(psz2,27,33)*1.0e-7F;
					ku.ks.fArgOfPerigee=KEPGetElement(psz2,35,42);
					ku.ks.fMeanAnomaly=KEPGetElement(psz2,44,51);
					ku.ks.fMeanMotion=KEPGetElement(psz2,53,63);
					ku.ks.fDecayRate=KEPGetElement(psz1,34,43);
					ku.ks.u16ElementSet=(U16)(KEPGetElement(psz1,65,68)+ONEPPM);
	
					// Now write to Flash program memory...
					{
						U8 u8Idx;
	
						if (KEPFindCatalog(ku.ks.u24CatalogNumber,&u8Idx))
						{
							if (!KEPSet(u8Idx,&ku,FALSE))
							{
								WriteStr(PE_HWS," keps update failed (bad flash write)\r\n");
							}
							else
							{
								WriteStr(PE_HWS," keps updated\r\n");
							}
						}
						else
						{
							// Now insert...
							if (!KEPFindFirstFree(&u8Idx))
							{
								WriteStr(PE_HWS," keps insert failed (no free space)\r\n");
							}
							else
							{
								ku.ks.u8ALon=0;
								ku.ks.u8ALat=0;
								ku.ks.u8Priority=0;
				
								if (!KEPSet(u8Idx,&ku,TRUE))
								{
									WriteStr(PE_HWS," keps insert failed (bad flash write)\r\n");
								}
								else
								{
									WriteStr(PE_HWS," keps inserted\r\n");
								}
							}
						}
					}
				}
			}

			{
				char size2 *psz=psz0;
		
				psz0=psz1; // Rotate the strings
				psz1=psz2;
				psz2=psz;
			}
		}
	}
}

/*******************************************/
/* Prediction routines                     */
/* (These sit on top of Plan13)            */
/* Note that default rambank is 1!         */
/*******************************************/

#define PRD_MAXSORTSATS 5

#pragma rambank 1

static void PRDSetDayNumFromRTC(void)
{
	TIMESTRUCT ts;
	
	RTCGetDateTime(&ts);
	P13TimeStructToDayNum(&ts);
}

static BOOL PRDBlacklistGet(U8 u8KepIndex)
{
	U8 u8Idx=u8KepIndex >> 3;
	U8 u8BitCount=u8KepIndex & 0x07;
	U8 u8BitMask=1 << u8BitCount;
	U8 *pu8=_au8prdBlacklist+u8Idx;
	U8 u8=*pu8;

	if ((u8 & u8BitMask)!=0)
	{
		return TRUE;
	}
	return FALSE;
}

static void PRDBlacklistSet(U8 u8KepIndex,BOOL b)
{
	U8 u8Idx=u8KepIndex >> 3;
	U8 u8BitCount=u8KepIndex & 0x07;
	U8 u8BitMask=1 << u8BitCount;
	U8 *pu8=_au8prdBlacklist+u8Idx;
	U8 u8=*pu8;

	if (b)
	{
		u8|=u8BitMask;
	}
	else
	{
		u8&=~u8BitMask;
	}
}

static void PRDBlacklistInit()
{
	U8 u8;

	for (u8=0;u8<PRD_MAXSATS;u8++)
	{
		PRDBlacklistSet(u8,FALSE);
	}
}

static BOOL PRDTrack(PORTENUM pe,OBSERVERSTRUCT size2 *pos,U8 u8KepIndex,BOOL bInit,U8 size2 *pu8Detail)
{
	// Return TRUE if satellite has just dropped from the horizon (ie, do we want to recalc AOS)?
	#pragma rambank 2
	PREDICTSTRUCT ps;
	TIMESTRUCT ts;
	AOSLOSSTRUCT als;
	char sz[9];
	static MYFLOAT fLastEl; // If the last elevation was positive and new one negative, need to recalc AOS etc
	BOOL bResult=FALSE;
	#pragma rambank 1

	if (bInit)
	{
		// Get keps & AOS/LOS/MaxEl etc
		#pragma rambank 2
		KEPUNION ku;
		#pragma rambank 1

		fLastEl=-0.1F; // No need to recalc AOS etc after this...

		KEPGet(u8KepIndex,&ku);

		WriteClear(pe);
		WriteStr(pe,ku.ks.szName);
		WriteHome(pe);
		WriteLF(pe);
		WriteStr(pe,"Calc AOS/LOS...");

		P13Init(&ku.ks,pos);

		// Figure out AOS/LOS etc
		PRDSetDayNumFromRTC();
		P13Calc(FALSE,TRUE);
		if (P13GetAOS(pos,&ku.ks,&als))
		{
			P13GetLOS(pos,&ku.ks,&als);
			P13GetMaxEl(pos,&ku.ks,&als);
		}
	}
	RTCGetDateTime(&ts);
	P13Predict(&ts,&ps);

	// Save our elevation between calls to allow to see if we need to recalc AOS etc
	if (fLastEl>=0.0F && ps.fElevation<0.0F)
	{
		bResult=TRUE;
	}
	fLastEl=ps.fElevation;

	WriteClear(pe);
	strncpy(sz,SAT,8);
	{
		U8 u8=strlen(SAT);

		while (u8<8)
		{
			sz[u8++]=' ';
		}
	}
	sz[8]='\0';
	WriteStr(pe,sz);
	WriteF(pe,ps.fAzimuth,3,0,TRUE,TRUE);
	WriteC(pe,'/');
	WriteF(pe,ps.fElevation,2,0,TRUE,TRUE);
	WriteHome(pe);
	WriteLF(pe);

	if (*pu8Detail==255)
	{
		*pu8Detail=9;
	}

	if (*pu8Detail>=10)
	{
		*pu8Detail=0;
	}

	switch (*pu8Detail)
	{
		case 0:
			WriteDateTime(pe,&ts,TRUE,TRUE,FALSE);
			break;
		case 1:
			WriteStr(pe,"Locator   ");
			WriteLocator(pe,pos);
			break;
		case 2:
			if (als.bAOSHappens && !als.bGeostationary && !als.bDecayed)
			{
				TIMESTRUCT ts2;
				P13DayNumToTS(&ts2,als.fDayAOS,als.fTimeAOS);
				WriteStr(pe,"AOS ");
				WriteDateTime(pe,&ts2,FALSE,FALSE,FALSE);
			}
			else
			{
				WriteStr(pe,"No AOS/LOS");
			}
			break;
		case 3:
			if (als.bAOSHappens && !als.bGeostationary && !als.bDecayed)
			{
				TIMESTRUCT ts2;
				P13DayNumToTS(&ts2,als.fDayLOS,als.fTimeLOS);
				WriteStr(pe,"LOS ");
				WriteDateTime(pe,&ts2,FALSE,FALSE,FALSE);
				break;
			}
			(*pu8Detail)++; // No break!
		case 4:
			if (als.bAOSHappens && !als.bGeostationary && !als.bDecayed)
			{
				WriteStr(pe,"AOSLOS ");
				
				WriteF(pe,als.fAzAOS,3,0,TRUE,TRUE);
				WriteC(pe,'/');
				WriteF(pe,als.fAzLOS,3,0,TRUE,TRUE);
				break;
			}
			(*pu8Detail)++; // No break!
		case 5:
			if (als.bAOSHappens && !als.bGeostationary && !als.bDecayed)
			{
				MYFLOAT f;
				TIMESTRUCT ts2;

				if (ps.fElevation>=0.0F)
				{
					f=P13DayNumDiff(als.fDayLOS,als.fTimeLOS,DN,TN);
				}
				else
				{
					f=P13DayNumDiff(als.fDayAOS,als.fTimeAOS,DN,TN);
				}
				WriteStr(pe,"Until");
				WriteF(pe,f,2,0,FALSE,TRUE);
				WriteC(pe,'d');
				{
					MYFLOAT f2=(U24)f;

					f-=f2;
				}
				P13DayNumToTS(&ts2,0,f);
				WriteTime(pe,&ts2,TRUE,TRUE);
				break;
			}
			(*pu8Detail)++; // No break!

		case 6:
			if (als.bAOSHappens && !als.bGeostationary && !als.bDecayed)
			{
				MYFLOAT f=P13DayNumDiff(als.fDayLOS,als.fTimeLOS,als.fDayAOS,als.fTimeAOS);
				TIMESTRUCT ts2;

				WriteStr(pe,"Dur");
				WriteF(pe,f,4,0,FALSE,TRUE);
				WriteC(pe,'d');
				{
					MYFLOAT f2=(U24)f;

					f-=f2;
				}
				P13DayNumToTS(&ts2,0,f);
				WriteTime(pe,&ts2,TRUE,TRUE);
				break;
			}
			(*pu8Detail)++; // No break!

		case 7:
			WriteStr(pe,"Sq/MaxEl  ");
			WriteF(pe,ps.fSquint,3,0,FALSE,TRUE);
			WriteC(pe,'/');
			if (als.bAOSHappens && !als.bGeostationary && !als.bDecayed)
			{
				WriteF(pe,als.fMaxEl,2,0,FALSE,TRUE);
			}
			else
			{
				WriteStr(pe,"NA");
			}
			break;

		case 8:
			WriteStr(pe,"Or/MA ");
				
			{
				MYFLOAT f=(MYFLOAT)ps.lOrbit;

				WriteF(pe,f,6,0,FALSE,TRUE);
			}
			WriteC(pe,'/');
			WriteF(pe,ps.fMA,3,0,FALSE,TRUE);
			break;

		case 9:
			WriteStr(pe,"RH ");
			WriteF(pe,ps.fRange,6,0,FALSE,TRUE);
			WriteC(pe,'/');
			WriteF(pe,ps.fHeight,6,0,FALSE,TRUE);
			break;
	}
	return bResult;
}

static void PRDAOSLOSInsert(AOSLOSSTRUCT size2 *paals,AOSLOSSTRUCT size2 *pals)
{
	// Insert, in increasing order, pals into paals
	U8 u8=0;
	U8 u8Idx=0xFF;
	AOSLOSSTRUCT size2 *paals2=paals;
	MYFLOAT fDayAOS=pals->fDayAOS;
	MYFLOAT fTimeAOS=pals->fTimeAOS;
	BOOL b=FALSE;

	// Find next
	while (!b && u8<PRD_MAXSORTSATS)
	{
		if (fDayAOS<paals2->fDayAOS)
		{
			b=TRUE;
		}
		else
		{
			MYFLOAT f=paals2->fDayAOS;

			if (fDayAOS==f)
			{
				if (fTimeAOS<paals2->fTimeAOS)
				{
					b=TRUE;
				}
			}
		}
		u8++;
		paals2++;
	}

	if (b)
	{
		// Move down paas items below pas
		memcpy(paals2,paals2-1,sizeof(AOSLOSSTRUCT)*(PRD_MAXSORTSATS-u8));
		memcpy(paals2-1,pals,sizeof(AOSLOSSTRUCT));
	}
}

static BOOL PRDAOSOrder(PORTENUM pe,OBSERVERSTRUCT size2 *pos,AOSLOSSTRUCT size2 *paals)
{
	// Order the next PRD_MAXSORTSATS AOS's
	#pragma rambank 2
	AOSLOSSTRUCT als;
	KEPUNION ku;
	MYFLOAT fDay;
	MYFLOAT fTime;
	#pragma rambank 1
	U8 u8;
	U24 u24Addr=FPM_DATA;
	AOSLOSSTRUCT size2 *pals=paals;

	for (u8=0;u8<PRD_MAXSORTSATS;u8++)
	{
		pals->u8KepIndex=0xFF;
		pals->fDayAOS=1000000.0F; // A day very far in the future, about the year 2738
		pals->fTimeAOS=0.0F;
	}
	pals=paals;

	PRDSetDayNumFromRTC();
	fDay=DN;
	fTime=TN;

	// Get the soonest five satellites...
	for (u8=0;u8<PRD_MAXSATS;u8++)
	{
		if (KEPGet(u8,&ku))
		{
			if (BTNRead(TRUE))
			{
				return FALSE;
			}
			WriteClear(pe);
			WriteStr(pe,"Finding AOS for");
			WriteHome(pe);
			WriteLF(pe);
			WriteStr(pe,ku.ks.szName);
			WriteStr(pe,"...");

			P13Init(&ku.ks,pos);
			DN=fDay;
			TN=fTime;
			P13Calc(FALSE,TRUE);
			if (P13GetAOS(pos,&ku.ks,&als))
			{
				if (BTNRead(TRUE))
				{
					return FALSE;
				}
				if (!PRDBlacklistGet(u8))
				{
					als.u8KepIndex=u8;
					PRDAOSLOSInsert(paals,&als);
				}
			}
		}
	}
	return TRUE;
}

static BOOL PRDTrackWait(PORTENUM pe,U8 u8Idx)
{
	BOOL bFinished=FALSE;
	BOOL bResult=FALSE;

	BOOL bInit=TRUE;
	U8 u8Detail=0;
	
	while (!bFinished)
	{
		U16 u16Start=RTCTimerGet();
		U16 u16Diff=0;
		U8 u8Btn=0;

		bInit=PRDTrack(pe,&_ds.os,u8Idx,bInit,&u8Detail);

		while (u8Btn==0 && u16Diff<1000)
		{
			u16Diff=RTCTimerGet()-u16Start;
			u8Btn=BTNRead(TRUE);
		}
		if (u8Btn.BTN_BIT_LEFT)
		{
			bResult=FALSE;
			bFinished=TRUE;
		}
		if (u8Btn.BTN_BIT_RIGHT)
		{
			// We could potentially use this button to change satellite modes
			bResult=TRUE;
			bFinished=TRUE;
		}
		if (u8Btn.BTN_BIT_DOWN)
		{
			u8Detail++;
		}
		if (u8Btn.BTN_BIT_UP)
		{
			u8Detail--;
		}
	}
	return bResult;
}


static BOOL PRDAuto(PORTENUM pe,OBSERVERSTRUCT size2 *pos)
{
	#pragma rambank 5
	AOSLOSSTRUCT aals[PRD_MAXSORTSATS];
	#pragma rambank 1
	BOOL bFinished=FALSE;

	while (!bFinished)
	{
		U8 u8;
		AOSLOSSTRUCT size2 *pals=aals;

		// Go through finding next AOS of all sats...
		if (!PRDAOSOrder(pe,pos,aals))
		{
			return FALSE;
		}

		for (u8=0;!bFinished && u8<PRD_MAXSORTSATS;u8++)
		{
			U8 u8KepIndex=pals->u8KepIndex;

			if (u8KepIndex==0xFF)
			{
				if (u8==0)
				{
					return FALSE;
				}
				bFinished=TRUE;
			}
			else
			{
				U8 u8Detail=0;

				if (!PRDTrackWait(pe,pals->u8KepIndex))
				{
					PRDBlacklistSet(u8KepIndex,TRUE);
				}
			}
		}
		bFinished=FALSE;
	}
	return TRUE;
}

static BOOL PRDTrackWhatsUp(PORTENUM pe,OBSERVERSTRUCT size2 *pos,const char size2 *pszHeader)
{
	// What sats are already up?
	#pragma rambank 2
	KEPUNION ku;
	#pragma rambank 1
	U8 u8;
	BOOL bFinished=FALSE;

	WriteClear(pe);
	WriteStr(pe,pszHeader);

	PRDBlacklistInit();

	while (!bFinished)
	{
		BOOL bFound=FALSE;

		for (u8=0;u8<PRD_MAXSATS;u8++)
		{
			if (KEPGet(u8,&ku))
			{
				if (BTNRead(TRUE))
				{
					return FALSE;
				}
				
				WriteClear(pe);
				WriteStr(pe,pszHeader);
				WriteHome(pe);
				WriteLF(pe);
				WriteStr(pe,SAT);
				WriteStr(pe,"...");
	
				P13Init(&ku.ks,pos);
				PRDSetDayNumFromRTC();
				P13Calc(FALSE,TRUE);
				if (EL>0.0F)
				{
					char sz[17];
					U8 u8Detail=0;
	
					strcpy(sz,"Track ");
					strcat(sz,SAT);
					strcat(sz,"?");
	
					if (MNUMsg(pe,sz))
					{
						bFound=TRUE;

						if (!PRDTrackWait(pe,u8))
						{
							PRDBlacklistSet(u8,TRUE);
						}
					}
					else
					{
						PRDBlacklistSet(u8,TRUE);
					}
				}
			}
		}

		if (!bFound)
		{
			bFinished=TRUE;
		}
	}
	return TRUE;
}

#pragma rambank 0

/*******************************************/
/* GPS routines                            */
/* Note that default rambank is 1!         */
/*******************************************/

#pragma rambank 1

static void GPSInit(void)
{
	// Reset the GPS input
	_u8gpsPos=0;
	_u8gpsNumSats=0;
}

static BOOL GPSGetParm(U8 u8Num,char size2 *pszResult,U8 u8Size)
{
	// Find parameter number u8Num (comma separated values)
	BOOL bFound=u8Num==0;
	BOOL bFinished=u8Num==0;
	char *psz=_acgps;

	while (!bFinished)
	{
		char c=*psz++;

		if (c=='\0' || c=='\r' || c=='\n')
		{
			bFinished=TRUE;
		}
		else
		{
			if (c==',')
			{
				u8Num--;
				if (u8Num==0)
				{
					bFinished=TRUE;
					bFound=TRUE;
				}
			}
		}
	}
	if (!bFound)
	{
		return FALSE;
	}

	bFinished=FALSE;
	while (!bFinished)
	{
		char c=*psz++;

		if (c=='\0' || c==',' || c=='\r' || c=='\n')
		{
			*pszResult='\0';
			bFinished=TRUE;
		}
		else
		{
			if (u8Size!=0)
			{
				*pszResult++=c;
				u8Size--;
			}
			else
			{
				return FALSE; // Not enough space in the destination string
			}
		}
	}
	return TRUE;
}

static BOOL GPSHexNibbleToU8(char c,char *pu8)
{
	U8 u8Result=0;

	c=toupper(c);

	if (isdigit(c))
	{
		u8Result=(U8)(c-'0');
	}
	else
	{
		if (c>='A' && c<='F')
		{
			u8Result=(U8)(c-0x40+9);
		}
		else
		{
			return FALSE;
		}
	}
	*pu8=u8Result;
	return TRUE;
}

static BOOL GPSChecksum(void)
{
	// Check that the GPS Line checksum is correct
	char *psz=_acgps;
	char c=*psz++;
	U8 u8Sum=0;

	while (c!='\0' && c!='$')
	{
		c=*psz++;
	}
	if (c=='\0')
	{
		return FALSE;
	}
	c=*psz++;
	while (c!='\0' && c!='*')
	{
		u8Sum^=(U8)c;
		c=*psz++;
	}
	if (c=='\0')
	{
		return FALSE;
	}
	
	{
		U8 u8;
		U8 u8Sum2;

		if (GPSHexNibbleToU8(*psz,&u8Sum2))
		{
			u8Sum2<<=4;
			psz++;

			if (GPSHexNibbleToU8(*psz,&u8))
			{
				u8Sum2+=u8;

				if (u8Sum2==u8Sum)
				{
					return TRUE;
				}
			}
		}
	}
	return FALSE;
}

static BOOL GPSParseLine(void)
{
	// Parse a line of GPS. Sniffs out the location and 
	// the date/time.
	BOOL bResult=FALSE; // Return TRUE if a good sentence was decoded
	char szParm[15];

	if (GPSChecksum())
	{
		if (GPSGetParm(0,szParm,sizeof(szParm)))
		{
			// Parse a GPS string
			if (strcmp(szParm,"$GPRMC")==0)
			{
				U16 u16Year;
				U8 u8Month;
				U8 u8Day;
				U8 u8Hour;
				U8 u8Min;
				U8 u8Sec;
	
				// Set date/time
				// Should we check parameter 2= 'A'?
				if (GPSGetParm(1,szParm,sizeof(szParm)))
				{
					if (strlen(szParm)>=6)
					{
						char szNum[4];
		
						// First six chars are HHMMSS
						szNum[0]=szParm[0];
						szNum[1]=szParm[1];
						szNum[2]='\0';
						if (atou8(szNum,&u8Hour))
						{
							szNum[0]=szParm[2];
							szNum[1]=szParm[3];
							if (atou8(szNum,&u8Min))
							{
								szNum[0]=szParm[4];
								szNum[1]=szParm[5];
								if (atou8(szNum,&u8Sec))
								{
									if (GPSGetParm(9,szParm,sizeof(szParm)))
									{
										// First six chars are DDMMYY
										szNum[0]=szParm[0];
										szNum[1]=szParm[1];
										szNum[2]='\0';
										if (atou8(szNum,&u8Day))
										{
											szNum[0]=szParm[2];
											szNum[1]=szParm[3];
											if (atou8(szNum,&u8Month))
											{
												szNum[0]=szParm[4];
												szNum[1]=szParm[5];
												if (atou16(szNum,&u16Year))
												{
													// Phew...
													u16Year+=2000;
													if (u16Year>=2000 && u16Year<2100 && u8Month>=1 && u8Month<=12 && u8Day>=1 && u8Day<=31 && u8Hour<=23 && u8Min<=59 && u8Sec<=59)
													{
														RTCSetTime(u16Year,u8Month,u8Day,u8Hour,u8Min,u8Sec,0);
														bResult=TRUE;
													}
												}
											}
										}
									}
								}
							}
						}
					}
				}
			}
			if (strcmp(szParm,"$GPGGA")==0)
			{
				// Set position
				if (GPSGetParm(6,szParm,sizeof(szParm)))
				{
					if (szParm[0]>='1' && szParm[0]<='3')
					{
						// We are valid
						if (GPSGetParm(2,szParm,sizeof(szParm)))
						{
							char szNum[4];
							U8 u8Lat;
							MYFLOAT fLat=FPAatof(&szParm[2])/60.0F;
	
							szNum[0]=szParm[0];
							szNum[1]=szParm[1];
							szNum[2]='\0';
							if (atou8(szNum,&u8Lat))
							{
								{
									MYFLOAT f=(MYFLOAT)u8Lat;
									fLat+=f;
								}
	
								if (GPSGetParm(3,szParm,sizeof(szParm)))
								{
									if (szParm[0]=='S')
									{
										fLat=-fLat;
									}
		
									if (GPSGetParm(4,szParm,sizeof(szParm)))
									{
										U8 u8Lon;
										MYFLOAT fLon=FPAatof(&szParm[3])/60.0F;
	
										szNum[0]=szParm[0];
										szNum[1]=szParm[1];
										szNum[2]=szParm[2];
										szNum[3]='\0';
										if (atou8(szNum,&u8Lon))
										{
											{
												MYFLOAT f=(MYFLOAT)u8Lon;
												fLon+=f;
											}
	
											if (GPSGetParm(5,szParm,sizeof(szParm)))
											{
												if (szParm[0]=='W')
												{
													fLon=-fLon;
												}
												if (GPSGetParm(9,szParm,sizeof(szParm)))
												{
													MYFLOAT fHeight=FPAatof(szParm);
		
													_ds.os.fLon=fLon;
													_ds.os.fLat=fLat;
													_ds.os.fHeight=fHeight;

													if (GPSGetParm(7,szParm,sizeof(szParm)))
													{
														szNum[0]=szParm[0];
														szNum[1]=szParm[1];
														szNum[2]='\0';
														atou8(szNum,&_u8gpsNumSats);
													}
													bResult=TRUE;
												}
											}
										}
									}
								}
							}
						}
					}
				}
			}
		}
	}
	return bResult;
}

static BOOL GPSPoll(void)
{
	BOOL bResult=FALSE; // Return TRUE if a sentence was parsed
	char c;

	// The GPS polling routine...
	while (SWSRx2Get(&c))
	{
		if (c=='\r' || c=='\n')
		{
			_acgps[_u8gpsPos]='\0';
			bResult=GPSParseLine();
			_u8gpsPos=0;
		}
		else
		{
			if (_u8gpsPos<GPS_LINESIZE-1)
			{
				_acgps[_u8gpsPos]=c;
				_u8gpsPos++;
			}
		}
	}
	return bResult;
}

#pragma rambank 0

/*******************************************/
/* HST (Host) routines                     */
/*******************************************/

static void HSTInit(void)
{
	// Reset the Host input line
	_u8hstPos=0;
}

static BOOL HSTParseWhite(U8 *pu8Pos)
{
  // Ignore white space in _achst. Return FALSE if no white space...
  BOOL bFinished=FALSE;
  BOOL bWhiteFound=FALSE; // at end of string
  U8 u8Pos=*pu8Pos;
  char c;

  while (!bFinished)
  {
    if (u8Pos<_u8hstPos)
    {
      c=_achst[u8Pos];
      if (c!=' ' && c!='\t')
      {
        bFinished=TRUE;
      }
      else
      {
        bWhiteFound=TRUE;
        u8Pos++;
      }
    }
    else
    {
      bFinished=TRUE;
    }
  }
  *pu8Pos=u8Pos;
  return bWhiteFound;  
}

static BOOL HSTParseU16(U8 *pu8Pos,U16 *pu16)
{
  // Parse _achst from *pu8Pos for a number. Return FALSE if error...
  BOOL bFinished=FALSE;
  BOOL bNumFound=FALSE; // at end of string
  U8 u8Pos=*pu8Pos;
  char c;
  U16 u16=0;

  while (!bFinished)
  {
    if (u8Pos<_u8hstPos)
    {
      c=_achst[u8Pos];
      if (c<'0' || c>'9')
      {
        bFinished=TRUE;
      }
      else
      {
        bNumFound=TRUE;
        u16*=10;
        c-='0';
        u16+=c;
        u8Pos++;
      }
    }
    else
    {
      bFinished=TRUE;
    }
  }
  *pu8Pos=u8Pos;
  if (bNumFound)
  {
    *pu16=u16;
  }
  return bNumFound;  
}

static BOOL HSTParseFloat(U8 *pu8Pos,MYFLOAT *pf)
{
  U16 u16Int;
  U16 u16Frac=0;
  MYFLOAT f;
  U8 u8;

  if (!HSTParseU16(pu8Pos,&u16Int))
  {
    return FALSE;
  }
  f=u16Int;
  u8=*pu8Pos;
  if (_achst[u8]=='.')
  {
    U8 u8Pos;

    (*pu8Pos)++;

    u8Pos=*pu8Pos; // So we know how many digits in the fractional part

    if (HSTParseU16(pu8Pos,&u16Frac))
    {
      MYFLOAT fFrac=u16Frac;

      u8Pos=*pu8Pos-u8Pos;
      while (u8Pos)
      {
        fFrac/=10;
        u8Pos--;
      }
      f+=fFrac;
    }
  }
  *pf=f;
  return TRUE;
}

static U8 HSTEasyCommCommand(U8 *pu8Pos)
{
  // Look for a two character Easycomm command...
  char c1='\0';
  char c2='\0';
  U8 u8Pos=*pu8Pos;
  U8 u8Command=HST_ECNONE;

  HSTParseWhite(&u8Pos); // ignore white space

  if (u8Pos<=_u8hstPos+1) // make sure there's at least two more characters
  {
    c1=toupper(_achst[u8Pos++]);
    c2=toupper(_achst[u8Pos++]);

    if (c1=='A' && c2=='Z')
    {
      u8Command=HST_ECAZ; 
    }
    else if (c1=='E' && c2=='L')
    {
      u8Command=HST_ECEL;
    }
    else if (c1=='U' && c2=='P')
    {
      u8Command=HST_ECUP;
    }
    else if (c1=='D' && c2=='N')
    {
      u8Command=HST_ECDN;
    }
    if (u8Command!=HST_ECNONE)
    {
      *pu8Pos=u8Pos;
    }
  }
  return u8Command;
}

static void HSTPutError(void)
{
	WriteStr(PE_HWS,"?>\r\n");
}

static void HSTPoll(void)
{
	while (HWSRxReady())
	{
		char c;

		HWSRxGet(&c);

		if (c=='\n' || c=='\r')
		{
      U8 u8Pos=0;
      U8 u8EasyCommCommand=HSTEasyCommCommand(&u8Pos);

      if (u8EasyCommCommand!=HST_ECNONE)
      {
        while (u8EasyCommCommand!=HST_ECNONE)
        {
          BOOL bError=FALSE;

          switch (u8EasyCommCommand)
          {
            case HST_ECAZ:
            case HST_ECEL:
              {
                MYFLOAT f;
                S16 s16;

                HSTParseWhite(&u8Pos);
                if (!HSTParseFloat(&u8Pos,&f))
                {
                  bError=TRUE;
                  break;
                }
                s16=(S16)f;
                if (u8EasyCommCommand==HST_ECAZ)
                {
                  ROTSet(s16,32767);
                }
                else
                {
                  ROTSet(32767,s16);
                }
              }
              break;
            case HST_ECUP:
            case HST_ECDN:
              // Parsed but ignored...
              {
                MYFLOAT f;
                char acMode[3];
                U8 u8=0;

                HSTParseWhite(&u8Pos);
                if (!HSTParseFloat(&u8Pos,&f))
                {
                  bError=TRUE;
                  break;
                }
                HSTParseWhite(&u8Pos);
                while (u8Pos<_u8hstPos && u8<3)
                {
                  char c=_achst[u8Pos++];

                  acMode[u8++]=c;
                }
                while (u8<3)
                {
                  acMode[u8++]=' ';
                }
              }
              break;
            default:
              bError=TRUE;
              break;
          }
          if (bError)
          {
            HSTPutError();
            u8EasyCommCommand=HST_ECNONE;
          }
          else
          {
            u8EasyCommCommand=HSTEasyCommCommand(&u8Pos);
          }
        }
      }
      else
      { // This is a GS232 command
        // Ignore leading spaces...
        while (u8Pos<_u8hstPos)
        {
          c=toupper(_achst[u8Pos]);
          u8Pos++;
          if (c!=' ' && c!='\t')
          {
            break;
          }
        };
        if (u8Pos<=_u8hstPos)
        {
          // Get command char
          switch (c)
          {
            case 'F':
              if (u8Pos<_u8hstPos)
              {
                char cAzEl=toupper(_achst[u8Pos++]);
  
                if (cAzEl=='W')
                {
                  if (DEEUpdate())
                  {
                    WriteStr(PE_HWS,"EEPROM write OK\r\n");
                  }
                  else
                  {
                    WriteStr(PE_HWS,"EEPROM failed\r\n");
                  }
                  break;
                }
  
                if (cAzEl=='S') // Set South stop rotator
                {
                  _ds.rds.bSNS=TRUE;
                  WriteStr(PE_HWS,"South stop\r\n");
                  break;
                }
  
                if (cAzEl=='N') // Set 0 - 360/450 rotator
                {
                  _ds.rds.bSNS=FALSE;
                  WriteStr(PE_HWS,"North stop\r\n");
                  break;
                }
  
                if (u8Pos<_u8hstPos)
                {
                  char cEndStart=toupper(_achst[u8Pos++]);
  
                  if (cAzEl=='A' && cEndStart=='O') // Set Az Offset directly
                  {
                    U16 u16;
  
                    HSTParseWhite(&u8Pos);
                    if (HSTParseU16(&u8Pos,&u16))
                    {
                      _ds.rds.u16AzOff=u16;
            					ROTTargetInit();
                      WriteStr(PE_HWS,"Az off=");
                      WriteU16(PE_HWS,_ds.rds.u16AzOff,3);
                      WriteStr(PE_HWS,"\r\n");
                      break;
                    }
                  }
                  if (cAzEl=='A' && cEndStart=='M') // Set Az Mul directly
                  {
                    MYFLOAT f;
  
                    HSTParseWhite(&u8Pos);
                    if (HSTParseFloat(&u8Pos,&f))
                    {
                      _ds.rds.fAzMul=f;
            					ROTTargetInit();
                      WriteStr(PE_HWS,"Az mul=");
                      WriteF(PE_HWS,_ds.rd