/******************************************************************************
//
//   Ublox Neo5 GPS control
//   mcm 15 Sep 08
//   jts ported to Spray2008 controller board Nov2008
//   0902 jan2010, limit #sat seen output value to <16 (one nibble).
//   nov11, moved tAcc from static short to static long (value never used)
//	 dec11, add some hint that it is a bad packet
//   jan12, ignore leapseconds.
//   jan13, add a reset of the TPU if can't talk to UBX
//
//==== Future Expansion =======================================================
//		last update: Feb2009
//	1. downloading/uploading the almanac (just needs porting from mcm work)
//  2. understanding gpsFix - moving some globals elsewhere? define ubx struct?
//  3. Restructure to fewer packets?
//		List the desired data & then fewest packets to get that data.
//
//****************************************************************************/
#include	<cfxbios.h>		// Persistor BIOS and I/O Definitions
#include	<cfxpico.h>		// Persistor PicoDOS Definitions

#include	<assert.h>
#include	<ctype.h>
#include	<errno.h>
#include	<float.h>
#include	<limits.h>
#include	<locale.h>
#include	<math.h>
#include	<setjmp.h>
#include	<signal.h>
#include	<stdarg.h>
#include	<stddef.h>
#include	<stdio.h>
#include	<stdlib.h>
#include	<string.h>
#include	<time.h>

#include    "cf2_qsm332.h"
#include    "spi_cmd.h"
#include    "tcm2.h"
#include    "pitch_roll.h"
#include    "gpio_mux.h"
#include    "setup.h"

#include    "gps.h"
#include    "ublox.h"

#define NTRY 12 // = #tries to get command response from UBX
#define MAXSAT 30 // maximum number of satellites to store status of

// globals
extern char   Verbose;  // switch for displaying more info
extern char   IRQ_Flag; // interrupt flag: says what caused the interrupt
extern short  iparam [ MAX_PARAM ]; // global parameter settings
extern struct all_param all;



// global to ubx: used to allow diagnostic print-out AFTER data collection
unsigned char        sv_prn[8], nsat;
static UBXPKT        cmd;
static short         UBX_parse_state, ck_a, ck_b;
static char          *ppkt;
static long          iTOW, ttff, msss,Lon,Lat,height,hMSL,hAcc,tAcc, vAcc;
static unsigned char gpsFlags,diffStat;
static unsigned char gpsFix;

//static unsigned long Alm[32][10],ALMweek;
static short year, month, dayofM,hour,min,sec,week;
static short pDOP,tDOP,vDOP,hDOP,numSV;
static SVdata SatDat[MAXSAT];
//nov11, add #sat being used
static short N_Sat_Used;

// in gps structure, nsat is defined as:
// nsat is both # satellites (0..8) and health flags:
// bit 7 : 0=antenna ok, 1=either open or shorted
// bit 6 : 0=acquiring ok, 1=too few satellites
// bit 5 : 0=back-up battery ram ok, else =1
// bit 4 : 0=alamanc complete, else =1


/* replaced with swap2ch, swap4ch  nov2008
void *RevbPut(unsigned char *a, unsigned char *b, short n) //******************
{ // xfr n bytes of a to b, reversing the order
  short i;
  
  for (i=0;i<n;i++) b[n-1-i] = a[i];
  
  return b;
} //**************************************************************************/

//*****************************************************************************
void UBX_checksum( UBXPKT *cmd,unsigned char *chk_a, unsigned char *chk_b)
// calculates the checksum of a UBX message cmd.
// returns the checksum as two unsigned character ready to send or compare.
{
	unsigned short a,b,len;
	unsigned char *cbuf;
	unsigned char *cbufend;
	
	len = UBXLEN(cmd); // length of the command, decoding command bytes
	cbuf = &(cmd->classID); // point to first byte to start computing chksum
	
	// compute the last-byte position
	cbufend = &(cmd->classID); 
	cbufend +=len;
	cbufend += 4;
//	cbufend = cbuf + len +4 ; // lengthof(ID + len + payload) 

	for (a=b=0; cbuf < cbufend; cbuf++) 
	{ // accumulate the checksum
	    a+= *cbuf;
		b+= a;
	}
	
	*chk_a= a & 0xff;
	*chk_b= b & 0xff;   
	
	return; 
} //***************************************************************************


void  send_cmd ( TUPort  *tup, UBXPKT  *cmd ) //*******************************
{ //* formats a command and sends it to the receiver 
  // cmd must have Class ID and msgID, length and payload
  
	unsigned char *cbuf, *cbufend, chk_a, chk_b;
	short j, b;
	
    TURxFlush(tup);   //  flush the tpu port
    // FOLLOWING delay doesn't seem to make packet reception better =======
    // let any present message finish before proceeding ===================
    // get bytes until no more rcv'd ======================================
    //SCITxPutChar('<');
    j=0;
    do
    { // get next char, until buffer is full, or timed-out
      b = TURxGetByteWithTimeout( tup, 10); //get a byte, 10 ms max
    } while (b>=0 && j++< MAX_RPTBUF  );
    //SCITxPutChar('>');
    //=====================================================================
    //=====================================================================
    
	// send the new command -----------------------------------------------
	cmd->sync1=UBX_SYNC_CHAR1;
	cmd->sync2=UBX_SYNC_CHAR2;
	
    cbufend = (char *)cmd + UBXLEN(cmd) +UBXOVHD;

	for (cbuf = (char *)cmd; cbuf < cbufend; cbuf++) 
	{   // send the cmd
		TUTxPutByte( tup, *cbuf, 0 ); // 0=don't wait for a reply
	}
	
	UBX_checksum(cmd,&chk_a, &chk_b);
	TUTxPutByte( tup, chk_a, 0);

	TUTxPutByte( tup, chk_b, 0);
	
	if (Verbose) cprintf("cmd sent\n");
	return;
} //***************************************************************************


short  UBX_setport(  TUPort    *tup ) //***************************************
// Sets the serial port to use only the UBX format. This is the fastest/easiest
// way to turn off the constant stream of NMEA messages that are the default.
// Could also be done using the UBX CFG_MSG command to set the rate of each 
// of the NMEA messages.

// returns TRUE(1) if successful, else 0

{   // cmd is presently defined globally
    short done, ack_ok, good=0, i, nmea_found;
    
	cmd.classID = UBX_CFG;
	cmd.msgID   = 0x00;
	cmd.len_LOB = 20;
	cmd.len_HOB = 0;
	
	memset (cmd.buf, 0, 20); //clear the payload
	cmd.buf[0]  = 1;	//port 1
	cmd.buf[4]  = 0xd0; //charLen=8 + a stray bit

//	cmd.buf[5]=0x02; //Odd parity,1 stop bit USED FOR HC12
	cmd.buf[5]=0x08; //Noparity,1 stop bit
	cmd.buf[8]=0x80; //
	cmd.buf[9]=0x25; //9600 baud = 0x2580, 4800=12C0
	//cmd.buf[8]=0xC0; //
	//cmd.buf[9]=0x12; //9600 baud = 0x2580, 4800=12C0
	cmd.buf[12]=0x01 ;//ubx only mode on input
	cmd.buf[14]=0x01 ;//ubx only mode on output
	
	nmea_found = 1; // assume it is nmea-mode
	i = 0; // zero attempts so far
	while ( nmea_found && i++<3 ) //+++++++++++++++++++++++++++++
	{ // oct11 try up to 3x to turn off the nmea
	  nmea_found = UBX_flush_incoming( tup ); // look for gap
	  send_cmd ( tup, &cmd); // send cmd to turn off nmea
	} //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
	
	// now get the reply
	// done = UBX_get_reply( tup, &cmd); // true if valid packet
	done = UBX_get_reply( tup ); // oct11 true if valid packet
	if ( done ) //=========================================================
	{ // then we have a complete message
	  ack_ok = cmd.classID== UBX_ACK && cmd.msgID==0x01; // yes, ack is ok
	  if (ack_ok)
		   good = cmd.buf[0]== UBX_CFG && cmd.buf[1]==0x00;
	} //===================================================================
			
	return ( good );
} //***************************************************************************

short  UBX_flush_incoming ( TUPort    *tup  ) //******************************* 
{  // oct11 update
   // waits for the serial char stream to stop.
   // The biggest gap between NMEA messages is about 640 ms.
   //-----
   // -- return 1 if a character was flushed, else 0
   
   short b, j, k, rv;  

  TURxFlush(tup);   //  flush the tpu port
  b = TURxGetByteWithTimeout( tup, 800); //get a byte, wait up to 800ms
  // b=-1 if it timed out.
  if (b<0) return 0; //-- no bytes found, return now.
  rv = 1; // else set return flag that we found a char.
  
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+++ Byte was found: it is in NMEA mode; wait for a gap
  j=k=0;
  while (b>=0 && j++<800 )  //+++++++++++++++++++++++++++++++++++++
  {  //keep reading in bytes until no more
    TURxFlush(tup);   //  flush the tpu port
    b = TURxGetByteWithTimeout( tup, 50); //get the next byte
    
    /* oct11; called multiple times from UBX_init
       shouldn't need the following:
    //---following  skips over the next gaps
    //---or until 3s has passed.  
    if ( b<0 && k <60 ) //------------------------------
    { //--- wait up to 3 s. total
      i = 0;
      while (b<0 && i++<20 )
      { // wait up to 1 s for the next byte
        TURxFlush(tup);   //  flush the tpu port
        b = TURxGetByteWithTimeout( tup, 50); // wait up to 0.05 s
      }
      k += i; // 0.05s counter
    } //------------------------------------------------
    */
    
  } //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  
  return rv;
} //***************************************************************************

// Parse states
// UBX_parse_state is the last completed state, NOT the current state.

#define UBX_INIT      0
#define UBX_SYNC1     1
#define UBX_SYNC2     2
#define UBX_CLASS     3
#define UBX_ID        4
#define UBX_LEN1      5
#define UBX_LEN2      6
#define UBX_PAYLOAD   7
#define UBX_CHECKSUM1 8

short UBX_get_reply( TUPort *tup    ) //***************************************
{
// waits for a reply, parses the command, char by char, stopping on errors
// returns 1 if good message, 0 otherwise.

   short i=0, j,rv=0, b;  	

   //  wait up to X second for first char ===============================
   //  even at 800ms, get time-out failures
   //  double the time to 1.6s to hopefully guarantee all is ok
   //  This initial wait is the main time-consumer, as
   //  seven packets are sent per check_UBX call.
          SCITxPutChar('[');
   //oct11, increase from 1600 to 3200 for UBX version 6
   b = TURxGetByteWithTimeout( tup, 3200); //get a byte  
          SCITxPutChar(']');
   if (b<0) //============================================================
   {
     cprintf("timed out waiting for UBLOX reply\n");
     return 0;	 // no character available
   } //===================================================================
   
   UBX_parse_state = UBX_INIT; // set the global parse-status to initial
   //ppkt = (char*) cmd; // global pointer to the UBLOX cmd packet
   ppkt = (char*) &cmd; // global pointer to the UBLOX cmd packet
   
   
   
    // get bytes until end of packet ======================================
    j=0;
    do
    { // get next char, until buffer is full, or timed-out
       rv = UBX_parse( (uchar) b ); // parse the last byte
       if (rv==0) //not done, get the next byte
          b = TURxGetByteWithTimeout( tup, 10); //get a byte, 10 ms max

    } while (b>=0 && j++< MAX_RPTBUF && rv==0 );
    //=====================================================================
    if (Verbose) 
    {  cprintf("cmd rcvd %d\n",j);
       for (i=0; i<j; i++)
       {
         cprintf("%02x.", cmd.buf[i] );
         if (i%10==0) cprintf("\n");
       }
       cprintf("\n");
    }
   return rv;
} //***************************************************************************


short UBX_parse( unsigned char c )  //*****************************************
// checks a UBX command char by char.
// returns 1 if the command is complete
// returns 0 if not done, including errors
{
  static short payload_len,payload_cnt;
  short done,i;
  char *cp;

  done = 0;
  
// UBX_parse_state is the last completed state: it is NOT the current state.

  if (UBX_parse_state < UBX_PAYLOAD) //====================================
  { // don't include checksum in checksum
    ck_a += c;
    ck_b += ck_a;
    
     *(ppkt++) =c; //store the character (checksums aren't stored)
  } //=====================================================================
  
  switch (UBX_parse_state) //++++++++++++++++++++++++++++++++++++++++++++++++
  { // dependent upon the last state, take appropriate action
  
  case UBX_INIT: // just starting
    if (c == UBX_SYNC_CHAR1){ //beginning of message?
        UBX_parse_state++;
      }
    else					// nope, keep looking
    	ppkt--;// put-back (reset pointer) if not first sync char
    break;
    
  case UBX_SYNC1:  // last char was the first sync mark
    if (c != UBX_SYNC_CHAR2) // this one should be the 2nd
      goto error; // if not, goto exception
      
    ck_a = 0;   //resets checksum counters after both sync chars found
    ck_b = 0;
    UBX_parse_state++;
    break;
    
  case UBX_SYNC2: // last char was SYNC 2
    UBX_parse_state++;  // checksum updated and char stowed already, just inc state
    break;
    
  case UBX_CLASS:
    UBX_parse_state++;
    break; 
       
  case UBX_ID:
    UBX_parse_state++;
    break;
    
  case UBX_LEN1:
    payload_len = cmd.len_HOB*256 + cmd.len_LOB ;
    payload_cnt=0;   // init payload counter
    if ( payload_len> MAX_RPTBUF)
    {  //payload exceeds buffer length
	    printf("payload_len:%d\n",payload_len);
    	for(i=0;i<6;i++)printf(" %2x",*((unsigned char*)&cmd+i));
    	printf("\n");
        goto error;
    }
    UBX_parse_state++;
    break;
    
  case UBX_LEN2:	  // getting payload
    payload_cnt++;
    if (payload_cnt>= payload_len) 
    {  //check to see if have it all
      UBX_parse_state++;	  //got it, move on
    }
    break;
    
  case UBX_PAYLOAD:
    if (c != (ck_a & 0xff))
    {	// check the first checksum
      printf("checksum A error");
      goto error;
    }
    UBX_parse_state++;
    break;
    
  case UBX_CHECKSUM1:
    if ( c != (ck_b & 0xff)  )
    {		// check the first checksum
      printf("checksum b error");
      goto error;
    }
    done = 1;
    break;
  } //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  
  return done; // normal return for no error
  
  
 error:  // handle an error by resetting !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  cprintf("parsing error %d\n",UBX_parse_state);
     //!debug
     for(cp=(char*)(&cmd);cp<ppkt;cp++)
         	cprintf("0x%0x ", (short)*cp);
     cprintf("\n");
     /**/
  // reset to the beginning
  UBX_parse_state = UBX_INIT;
  ppkt=(char*)(&cmd);
  //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  
  return done;
} //***************************************************************************


short  UBX_Rst  ( TUPort  *tup ) //********************************************
{
   // Resets the Ublox
   // Many options, experience will tell what is needed
   // Starting with a controlled hot reset
    short done, ack_ok, good=0;
   
	// cmd is global 
	cmd.classID  = UBX_CFG;
	cmd.msgID    = 0x04;
	cmd.len_LOB  = 4;
	cmd.len_HOB  = 0;
	memset (cmd.buf, 0, 4); //clear the payload
	 
//	cmd.buf[2]=0;// hardware reset without clearing GPS ephem,alm
	cmd.buf[2]=0x1;		//controlled reset
	
	send_cmd ( tup, &cmd); // send the command 
	
	// get the reply
	//done = UBX_get_reply( tup, &cmd); // true if valid packet
	done = UBX_get_reply( tup ); //oct11 true if valid packet
	if ( done ) //=========================================================
	{ // then we have a complete message
	  ack_ok = cmd.classID== UBX_ACK && cmd.msgID==0x01; // yes, ack is ok
	  if (ack_ok)
		   good = cmd.buf[0]== UBX_CFG && cmd.buf[1]==0x04;
	} //===================================================================

	return good;
} //***************************************************************************


short   UBX_cold  ( void ) //**************************************************
{ // cold reset ubx -- only used for testing
  // returns TRUE if acknowledged correctly, else FALSE
  short good;
  TUPort    *tup;

  tup = all.tu_port[TU_GPS].tup;  // correct tpu uart port pointer
  
    // set the command packet
 	cmd.classID= UBX_CFG;
	cmd.msgID=0x04;
	cmd.len_LOB=4;
	cmd.len_HOB=0;
	memset (cmd.buf, 0, 4); //clear the payload 
//	cmd.buf[3]=0;// hardware reset --like powering up
	cmd.buf[0]=0xff;		//cold start
	cmd.buf[1]=0xff;		//cold start
	cmd.buf[2]=2;           // OCT11 software reset (GPS only)

	send_cmd ( tup, &cmd); // send the command 
	memset ((char*)&cmd, 0, 40); //clear the header 
	
	// get the reply and parse ============================================
	//good = UBX_get_reply( tup, &cmd); // true if valid packet
	good = UBX_get_reply( tup ); //oct11 true if valid packet
	good = good && cmd.classID == UBX_ACK && cmd.msgID == 0x01; // correct ack
	good = good && cmd.buf[0]  == UBX_CFG && cmd.buf[1]== 0x04;// correct reply

	return good;
} //***************************************************************************

short   UBX_init (  void  ) //*************************************************
{  // initialize the UBLOX GPS 
   // returns TRUE if OK,
   // FALSE (0) if error
   
   short ok = 0, i=0;

   TUPort    *tup;
   
    tup = all.tu_port[TU_GPS].tup;  // correct tpu uart port pointer
    
    while (i++<10 && !ok ) //==============================================
    { // keep trying to initialize
      ok = UBX_setport( tup );
      DelayMilliSecs( 100 );
      // oct11, if having problems, try cycling power on the GPS
      if ( !ok &&  (i+1)%3 == 0 ) //-----------------------
      { //true when i==2, 5, 8
        gps_off( );
	    setup_one_tpu_uart( TU_GPS ); //jan13, re-init the uart
	    tup = all.tu_port[TU_GPS].tup;
        DelayMilliSecs(2000); // wait 2 s to power down  
        gps_on( );
        DelayMilliSecs(2000); // wait 2 s to power up  
      } //-------------------------------------------------
    } //===================================================================
    
  return ( ok );
} //***************************************************************************


short UBX_SVinfo ( TUPort  *tup ) //*******************************************
{
    // Gets Satillite information. Loads it into locally global variables
    // returns #channels if good, 0 if bad
	short nchan=0,i;
    short done, good;
	
	
	cmd.classID = UBX_NAV;
	cmd.msgID   = 0x30;
	cmd.len_LOB = cmd.len_HOB=0;
	
	send_cmd ( tup, &cmd); // send the command 
	memset ((char*)&cmd, 0, 40); //clear the header 
	
	// get the reply and parse
	// done = UBX_get_reply( tup, &cmd); // true if valid packet
	done = UBX_get_reply( tup ); // oct11 true if valid packet
	good = cmd.classID== UBX_NAV && cmd.msgID==0x30; // correct info
	if (done && good ) // =================================================
	{
		swap4ch(cmd.buf,(char*)&iTOW );   // put in big-endian format
		nchan = cmd.buf[4];  // = #satellite channels
		if(nchan>MAXSAT)  nchan=MAXSAT; // limit to max size
		if (Verbose)
		{
		  cprintf("chn  id flag qual cno  elev  azi\n");
		}
		
		for (i=0;i<nchan;i++) //---------------------------------------
		{ // fill in the satellite array
			SatDat[i].chn      =cmd.buf[i*12+ 8];
			SatDat[i].svid     =cmd.buf[i*12+ 9];
			SatDat[i].sv_flags =cmd.buf[i*12+10];
			SatDat[i].quality  =cmd.buf[i*12+11];
			SatDat[i].cno      =cmd.buf[i*12+12];
			SatDat[i].elev     =cmd.buf[i*12+13];
		    swap2ch(&cmd.buf[i*12+14],(char*)&SatDat[i].azim );   
		    swap4ch(&cmd.buf[i*12+16],(char*)&SatDat[i].prRes );  
		    
		    if (Verbose)
		    { // display more info
		      if ( SatDat[i].quality>1 ) // display
		      cprintf("%3d %3d %4x %4x %3d   %3d %3d\n",
		        SatDat[i].chn, SatDat[i].svid, SatDat[i].sv_flags,
		        SatDat[i].quality, SatDat[i].cno, 
		        SatDat[i].elev, SatDat[i].azim );
		    } 		
		} //-----------------------------------------------------------
		
	} //===================================================================
	
	return nchan;
} //***************************************************************************


short UBX_status ( TUPort *tup, struct gps_param *gps ) //*********************
{ // get the status of the fix
  // returns 0 if did not get a good packet
  // returns 1 if was a good packet
    short done, good;

	// set up the command packet
	cmd.classID  = UBX_NAV;
	cmd.msgID    = 0x03;
	cmd.len_LOB  = cmd.len_HOB =0;
	send_cmd ( tup, &cmd ); // send the command
	
	memset ((char *)&cmd, 0, 30); //clear the header
	// get the reply and parse
	//done = UBX_get_reply( tup, &cmd); // true if valid packet
	done = UBX_get_reply( tup ); // oct11, true if valid packet
	good = done && cmd.classID== UBX_NAV && cmd.msgID==0x03; // correct info
	
	if ( good ) // ========================================================
	{   // parse the information
	
	    swap4ch( cmd.buf, (char*) &iTOW );   
	    gpsFix     = cmd.buf[4]; // 2=2d, 3=3d, else not valid
	    gpsFlags   = cmd.buf[5]; //NOT used
	    diffStat   = cmd.buf[6]; //NOT used
	    swap4ch( &cmd.buf[8], (char*) &ttff );  //NOT used 
	    swap4ch( &cmd.buf[12],(char*) &msss );  //NOT used
	       
	    if ( gpsFix==2 || gpsFix==3 ) //-------------------------
	    {  // then the fix is valid
	    	gps->valid = TRUE; // set valid if fix is 2D or better.
	    	// gps->time  = ttff/10; //time to first fix...
	    	// we want time to reported fix, so we can use the gps time
	    	// to correctly compute timing.
	    	// this is set in get_gps
	    } // ----------------------------------------------------
	    else // not valid!
	    	gps->valid=FALSE; //---------------------------------

	} //===================================================================

	return good; 
} //***************************************************************************


short  UBX_GetTime ( TUPort *tup, struct gps_param *gps ) //*******************
{
// Gets UTCtime and stores it in local variables.
// If time is valid, then stores it in the GPS structure.

	short done, good, tv;
	uchar t_valid=0;
	
	// set up the command packet
	cmd.classID   = UBX_NAV;
	cmd.msgID     = 0x21;
	cmd.len_LOB   = cmd.len_HOB = 0;
	send_cmd ( tup, &cmd ); // send the command
	
	memset ((char *)&cmd, 0, 30); //clear the header
	// get the reply and parse
	// done = UBX_get_reply( tup, &cmd); // true if valid packet
	done = UBX_get_reply( tup ); // oct11 true if valid packet
	good = done && cmd.classID== UBX_NAV && cmd.msgID==0x21; // correct info
	
	if ( good ) // ========================================================
	{   // parse the information
	    swap4ch( &cmd.buf[ 0], (char*)&iTOW ); // time of week
	    swap4ch( &cmd.buf[ 4], (char*)&tAcc ); // time accuracy estimate
	    swap2ch( &cmd.buf[12], (char*)&year );// year
	    month   = cmd.buf[14];   // month
	    dayofM  = cmd.buf[15];   // actually date (day of month)	
	    hour    = cmd.buf[16];   // utc hr
	    min     = cmd.buf[17];   // utc min
	    sec     = cmd.buf[18];   // utc sec
	    t_valid = cmd.buf[19];   // time-valid flag
	    
	    //---jan12 update to not pay attn to leapseconds
	    //tv = ( t_valid & 0x07 ) == 0x07;  // true if first 3bits=1
	    tv = ( t_valid & 0x03 ) == 0x03;  // true if first 3bits=1
	    if (tv) //-----------------------------------------
	    { // if time is valid, store it
		    gps->utc_hr   = hour;
		    gps->utc_min  = min;
		    gps->utc_sec  = sec;
		    gps->gps_day  = (short)( iTOW/1000/86400 );  
		    // gps_day = day of week NOT day of month
		    cprintf("Valid time %2d/%02d/%04d %02d:%02d:%02d  Tflag=%02x\n",
		                month,dayofM,year, hour,min,sec, t_valid);
	    } //-----------------------------------------------
	    else cprintf("Time not valid yet; Tflag = %02x\n", t_valid );
		    // nov11 added t_valid print
	    
	} //===================================================================
	
	return (  tv );
} //***************************************************************************


short  UBX_SetRTC  ( void  ) //************************************************
{
// Gets GPS time.
// If time is valid, then updates the RTC.
// returns TRUE if successful

	short done, good, gps_wk;
	uchar t_valid=0, leap_sec;
	ulong ux_ti, gps_sec;
    TUPort    *tup;
   
    tup = all.tu_port[TU_GPS].tup;  // correct tpu uart port pointer


	// set up the command packet
	cmd.classID   = UBX_NAV;
	cmd.msgID     = 0x20; // NAV-TIMEGPS
	cmd.len_LOB   = cmd.len_HOB = 0;
	send_cmd ( tup, &cmd ); // send the command
	
	memset ((char *)&cmd, 0, 30); //clear the header
	// get the reply and parse
	//done = UBX_get_reply( tup, &cmd); // true if valid packet
	done = UBX_get_reply( tup ); // oct11 true if valid packet
	good = done && cmd.classID== UBX_NAV && cmd.msgID==0x20; // correct info
	
	if ( good ) // ========================================================
	{   // parse the information
	    swap4ch( &cmd.buf[ 0], (char*)&iTOW );   // time of week, ms
	    gps_sec = iTOW/1000; // time of week, seconds
	    swap2ch( &cmd.buf[ 8], (char*)&gps_wk ); // gps week
	    leap_sec = cmd.buf[10];   // GPS - UTC seconds
	    t_valid  = cmd.buf[11];   // time-valid flag
	    
	    t_valid = ( t_valid & 0x07 ) == 0x7;  // good data
	    if (t_valid) //------------------------------------
	    { // time is valid, set the RTC
	        ux_ti = TI_UX0 + gps_wk*S_PER_WK + gps_sec - leap_sec;
	        RTCSetTime ( ux_ti, 0 );
	        cprintf("RTC updated: gps_wk=%d, s=%ld = ux %lu \n", 
	                              gps_wk, gps_sec, ux_ti );
            //above verified correct against web unix clock
	    } //------------------------------------------------
	    
	} //===================================================================
	
	return (  (short) t_valid );
} //***************************************************************************


short UBX_NavSOL ( TUPort *tup, struct gps_param *gps ) //*********************
{ // gets navigation solution information
  // stores the GPS week, gpsFix, numSV
  // returns TRUE if a good fix
  
	short fix, good, done;
	
	// set up the command packet
	cmd.classID   = UBX_NAV;
	cmd.msgID     = 0x06;
	cmd.len_LOB   = cmd.len_HOB =0;
	send_cmd ( tup, &cmd ); // send the command
	
	memset ((char *)&cmd, 0, 52); //clear the header
	// get the reply and parse
	// done = UBX_get_reply( tup, &cmd); // true if valid packet
	done = UBX_get_reply( tup ); // oct11 true if valid packet
	good = done && cmd.classID== UBX_NAV && cmd.msgID==0x06; // correct info

	
	if ( good ) // ========================================================
	{   // parse the information

		swap4ch(  cmd.buf,    (char*)&iTOW ); // time-of-week   
	    swap2ch( &cmd.buf[8], (char*)&week ); // week-number
	    gpsFix  = cmd.buf[10];  // =2=2D, 3=3D, else not valid
	    fix     = cmd.buf[11];
	    numSV   = cmd.buf[47];
	      
	    if ( fix & 0x04 )	// true if week is valid
	    	gps->gps_week = week;
	    //jul09 v3, nsat is set to #seen in check_UBX
	    /*old  if( fix  & 0x01 )	// true if fix is valid
	    	   gps->nsat     = numSV; */

        if ( gpsFix==2 || gpsFix==3 ) //-------------------------
        { // then it is valid
  	       cprintf("NavSOL: valid %dd fix, using %d sats\n", gpsFix, numSV );
	       gps->valid = TRUE; // set valid if fix is 2D or better.
        } 
        else 
        {  
           cprintf("NavSOL has no valid fix yet\n");
           gps->valid = FALSE;
        } //-----------------------------------------------------
		    
	} //===================================================================
		
	return ( gps->valid );
} //***************************************************************************


short  UBX_DOP (  TUPort *tup, struct gps_param *gps ) //**********************
{
  // Dilation of Precision info
  // all values are scaled by 100.
  // returns TRUE if the received packet was valid
  // returns 0 if it was bad
   short good, done;

	// set up the command packet
	cmd.classID    = UBX_NAV;
	cmd.msgID      = 0x04;
	cmd.len_LOB    = cmd.len_HOB =0;
	send_cmd ( tup, &cmd ); // send the command
	
	memset ((char *)&cmd, 0, 30 ); //clear the header
	// get the reply and parse
	// done = UBX_get_reply( tup, &cmd); // true if valid packet
	done = UBX_get_reply( tup ); // oct11 true if valid packet
	good = done && cmd.classID== UBX_NAV && cmd.msgID==0x04; // correct info

	
	if ( good ) // ========================================================
	{   // parse the information
	
	    swap4ch(  cmd.buf,     (char*)&iTOW );  // time-of-week 
	    swap2ch( &cmd.buf[ 6], (char*)&pDOP );   
	    swap2ch( &cmd.buf[ 8], (char*)&tDOP );   
	    swap2ch( &cmd.buf[10], (char*)&vDOP );   
	    swap2ch( &cmd.buf[12], (char*)&hDOP );
	    
	    if ( hDOP>10 && hDOP <1200 ) //--------------------------
	    { // then it is valid
	        cprintf("hdop:%d\n",hDOP);
	    	gps->hdop = hDOP;	// ** note hDOP scaling is 100 not 10. 
	    	 
		}  //----------------------------------------------------
		else cprintf("Hdop is not valid yet...\n");
	} //===================================================================
	else //dec11 add some hint that it is a bad packet
	{ 
	   gps->hdop = 1220; // flag that packet ID is not good
	   if (!done) gps->hdop = 1240; // bad packet entirely
	   cprintf("Bad packet in DOP\n");
	} 

	return ( good );
} //***************************************************************************

short  UBX_PosLLH  ( TUPort *tup ) //******************************************
{  // send the LLH command and parse
   // return TRUE if received a valid packet, else false
    short done, good;
    
	// set up the command packet
	cmd.classID    = UBX_NAV;
	cmd.msgID      = 0x02;
	cmd.len_LOB    = cmd.len_HOB = 0;
	send_cmd ( tup, &cmd ); // send the command
	
	memset ((char *)&cmd, 0, 30 ); //clear the header
	// get the reply and parse
	// done = UBX_get_reply( tup, &cmd); // true if valid packet
	done = UBX_get_reply( tup ); // oct11 true if valid packet
	good = done && cmd.classID== UBX_NAV && cmd.msgID==0x02; // correct info

	
	if ( good ) // ========================================================
	{   // parse the information

			swap4ch(  cmd.buf,     (char*) &iTOW   );   
		    swap4ch( &cmd.buf[ 4], (char*) &Lon    );   
		    swap4ch( &cmd.buf[ 8], (char*) &Lat    );   
		    swap4ch( &cmd.buf[12], (char*) &height );   
		    swap4ch( &cmd.buf[16], (char*) &hMSL   );   
		    swap4ch( &cmd.buf[20], (char*) &hAcc   );   
		    swap4ch( &cmd.buf[24], (char*) &vAcc   );   

	} //===================================================================

	return ( good );
} //***************************************************************************



short UBX_LLA ( TUPort *tup, struct gps_param *gps ) //************************
{ // read in LLA and write to gps struct
  // returns 1 if fix is valid, else returns 0
    //oct13 change to display -0 correctly
	float xlat, xlon, xdlat, xdlon;
	short lat_deg, lon_deg ;
	double lat_min, lon_min;
	short lat_sign, lon_sign, good, k;
	struct ublox_param *ubx;
	
	ubx = all.ubx;
	k = ubx->index;
	if (k<0 || k>1 ) k=0; // keep valid
	ubx->index = k;       // keep the valid #
	ubx->valid[k] = 0;    // set = NOT valid
	
	good = UBX_PosLLH ( tup ); // get a report
	if (!good )
	{ //then no good ======================================
	    gps_on(); // make sure it is on!!!!
	    cprintf("resetting port in UBX_LLA...");
	    // oct11, just do port reset
		// good = UBX_Rst( tup); // most likely need to reset
		// DelayMilliSecs( 100);
		good = UBX_init();
		if (good) cprintf ("RESET\n");
		else      cprintf ("Failed\n");
		
		return 0;
	}  //==================================================
	// else cprintf("LLH is rcvd\n");
	
	/*jul09 v3, check_UBX has already gotten status + hdop
	good = UBX_status ( tup, gps);	//sets gps->valid if good fix.
	if (good) cprintf("Status rcv'd\n");
	else cprintf("Status failed\n");
	**/
	// jul09 v3, assume gps is valid if flag confirms fix+ hdop < 9.0
	gps->valid = ( gps->valid && hDOP < 900 );

	// compute latitude ===================================================
	xlat     = (float)Lat*1e-7;  // decimal degrees
 	lat_sign = +1;
	if (xlat < 0.0) 
	{  // keep track of sign
		lat_sign = -1;
		xlat = -xlat;
	}
	lat_deg  = (short)xlat; // abs degrees
	lat_min  = (xlat - lat_deg) * 60.0; //&minutes

	// repeat for longitude ===============================================
	xlon     = (float)Lon*1e-7;
	lon_sign = +1;
	if (xlon < 0.0) 
	{
		lon_sign = -1;
		xlon = -xlon;
	}
	lon_deg  = (short) xlon; // abs degrees
	lon_min  = (xlon - lon_deg) * 60.0; // and minutes
	
    //oct13, changed to floats, & not quite zero if on equator
	xdlat = ((float)lat_deg+.001)*lat_sign;  // signed latitude
	xdlon = ((float)lon_deg+.001)*lon_sign;  // signed longitude

	printf("LLA: %02d:%02d:%02d %+3.0f:%06.3f  %+4.0f:%06.3f hdop=%4.1f \n",	
	       hour,min,sec,xdlat,lat_min,xdlon,lon_min,(float)hDOP/100. );


	// update values to gps if valid
	if (gps->valid) //=====================================================
	{ // update the values if the fix is valid
	  printf("Valid fix, updating location\n"); 
	  gps->lat_sign = lat_sign;
	  gps->lon_sign = lon_sign;
	  gps->lat_deg  = lat_deg;
	  gps->lon_deg  = lon_deg;
	  gps->lat_min  = (float)lat_min;
	  gps->lon_min  = (float)lon_min;

	  gps->xlat     = xlat*lat_sign;
	  gps->xlon     = xlon*lon_sign;
	  
	  ubx->valid[k] = 1; // set TRUE
	  ubx->lat[k]   = Lat;
	  ubx->lon[k]   = Lon;
	  ubx->ti[k]    = RTCGetTime (0, 0);
	  ubx->n_used[k]= N_Sat_Used; //nov11 add for flash file processing
	  
	}  // end if valid ====================================================
	
	good = gps->valid;  // return true if valid flag set
	return ( good );
} //***************************************************************************



//*****************************************************************************
short  check_UBX ( void  )  //*************************************************
{  // get a full report from UBX
   // OLD: returns TRUE if LLA was a successful read
   // nov11 returns #satellites used in the fix if the LLA is valid (else 0)
   
  short status=0, i,n_used, lla_status;
  short min_sig, max_sig, avg_sig, nchan, nseen, sig;
  TUPort    *tup;
  struct gps_param *gps;
   
  tup = all.tu_port[TU_GPS].tup;  // correct tpu uart port pointer

  gps = all.gps; // point to the gps structure
  
  gps->valid = FALSE; // is updated by UBX_NavSol
  
  // read in # of satellites and signal strength ++++++++++++++++++++++++++
  // keep track of stats of snr +++++++++++++++++++++++++++++++++++++++++++
  nchan   = UBX_SVinfo( tup ); // get the info
  // init the variables
  n_used  = 0;     // # of satellites used
  nseen   = 0;     // # of satellites seen
  min_sig = 4000;  // minimum snr
  max_sig = 0;     // maximum snr
  avg_sig = 0;     // average snr
  
  if(nchan>0) //=======================================================
  { // do the stats for all the channels
  	for(i=0;i<nchan;i++) //--------------------------------------
  	{ // for each satellite channel
  	    // jul09 switch from being used to being seen
  		if ( SatDat[i].quality>1 ) //..................
  		{	// true if the satellite is seen
  		    // (may NOT be locked, or NOT being used)
  			nseen++;
  			sig  =  SatDat[i].cno; // snr
  			if ( sig>max_sig ) max_sig=sig;
  			if ( sig<min_sig ) min_sig=sig;
  			avg_sig += sig;
  		} //...............................................
  		//jul09 v3, accum #of satellites being used
  		if ( SatDat[i].sv_flags &0x1 ) 
  		{ // then it is being used for the fix
  		  n_used++;
  		}
  	} //---------------------------------------------------------
  } //=================================================================
  N_Sat_Used = n_used; //nov11 save a global copy
  
  if(nseen>0)  //---------------------------
  { // save satellite stats
      avg_sig /= nseen; // avg snr
  	  gps->smin  = min_sig;
  	  gps->savg  = avg_sig;
  	  gps->smax  = max_sig;
  	  if (nseen>15) nseen = 15; //0902 make fit into 1 nibble
  	  // this helps out with old code parsing, which expects nsat in one nibble.
  	  gps->nsat  = nseen;  
  } //--------------------------------------
  
  // this gives a visual clue to the user on #sats, signal strength
  cprintf("sees nsat=%2d, uses %2d max_sig,min_sig,avg_sig %d %d %d \n",
  	      nseen, n_used, max_sig,min_sig,avg_sig );


  // get the time +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  status = UBX_GetTime ( tup, gps );  // get time/date

  // get the HDOP info ++++++++++++++++++++++++++++++++++++++++++++++++++++
  UBX_DOP     ( tup, gps );  // get hdop

  status = UBX_NavSOL  ( tup, gps );  // gets gps week & day info


  //  get the LatLon info ++++++++++++++++++++++++++++++++++++++++++++++
  //  this updates lat, lon if gps is valid
  lla_status  = UBX_LLA( tup, gps );
  if (!lla_status) n_used=0; //nov11, make sure that n_used=0 if bad fix

	
  return ( n_used );
} //***************************************************************************

 //***** END ******************************************************************