/****************************************************************************/
/* Copyright 1991-2011 MBARI												*/
/****************************************************************************/
/* $Header: /home/cvs/oasis4/src/controller/drivers/airmarPB200.c,v 1.1 2011/10/25 22:39:21 bobh Exp $*/
/* Summary	: Driver Routines for AirMar PB200 Met Station					*/
/* Filename : airmarPB200.c													*/
/* Author	: Robert Herlien (rah)											*/
/* Project	: OASIS Mooring													*/
/* $Revision: 1.1 $															*/
/* Created	: 2/2/2011 from garmin.c										*/
/*																			*/
/* MBARI provides this documentation and code "as is", with no warranty,	*/
/* express or implied, of its quality or consistency. It is provided without*/
/* support and without obligation on the part of the Monterey Bay Aquarium	*/
/* Research Institute to assist in its use, correction, modification, or	*/
/* enhancement. This information should not be published or distributed to	*/
/* third parties without specific written permission from MBARI.			*/
/*																			*/
/****************************************************************************/
/* Modification History:													*/
/* 02feb2011 rah - created from garmin.c									*/
/* $Log: airmarPB200.c,v $
/* Revision 1.1	 2011/10/25 22:39:21  bobh
/* Initial oasis4 revision
/*
/* Revision 1.1	 2011/02/04 17:51:15  bobh
/* Added AirMar PB200 driver
/*
*/
/****************************************************************************/

#include <mbariTypes.h>					/* MBARI type definitions			*/
#include <mbariConst.h>					/* MBARI constants					*/
#include <oasis.h>						/* OASIS controller definitions		*/
#include <airmarPB200.h>				/* Defs for AirMar PB200			*/
#include <io.h>							/* OASIS I/O definitions			*/
#include <malloc.h>						/* OASIS malloc routines			*/
#include <log.h>						/* Log record definitions			*/
#include <task.h>						/* OASIS Multitasking definitions	*/
#include <drvr.h>						/* OASIS driver functions			*/
#include <utils.h>						/* OASIS Utility routines			*/
#include <debug.h>

#include <stdio.h>						/* Standard I/O functions			*/
#include <string.h>						/* For memcpy						*/

//#define DEBUG_AIRMAR	1


/********************************/
/*		External Data			*/
/********************************/

Extern TimeOfDay		tod;			/* Current time in TimeOfDay format */
										/* Following set by user via "parms"*/
Extern Parm_t			gpsInitAlt;		/* Initial GPS altitude				*/


/********************************/
/*		Module Local Data		*/
/********************************/

MLocal char		msgBuf[2*MSGBUF_SIZE];
MLocal char		lastGGA[MSGBUF_SIZE];
MLocal char		lastMDA[MSGBUF_SIZE];


/************************************************************************/
/* Function	   : pb200_wake												*/
/* Purpose	   : PB200 serial wakeup function, executed when user CONNECTs*/ 
/* Inputs	   : Driver ptr, MBool (TRUE for on, FALSE for off)			*/
/* Outputs	   : TRUE													*/
/************************************************************************/
MBool pb200_wake( Driver *dp, MBool on )
{
#pragma unused(dp)
	xprintf("$PAMTX,%c\r\n", on ? '0' : '1');
	return( TRUE );

} /* pb200_wake() */


/************************************************************************/
/* Function	   : pb200Init												*/
/* Purpose	   : Initialize Airmar PB200								*/
/* Inputs	   : None													*/
/* Outputs	   : None													*/
/************************************************************************/
MLocal Void pb200Init(MBool initAlt)
{
	xputs("$PAMTC,EN,ALL,0\r\n");		/* Turn off all messages		*/
	task_delay(TICKS_PER_SECOND/2);
	if (initAlt)						/* Init alt & 2D msging			*/
	{
		xprintf("$PAMTC,ALT,SET,%ld.0,1,2\r\n", gpsInitAlt);
		task_delay(TICKS_PER_SECOND/2);
	}
	xputs("$PAMTC,EN,GGA,1\r\n");		/* Turn on GGA and MDA msgs		*/
	task_delay(TICKS_PER_SECOND/2);
	xputs("$PAMTC,EN,MDA,1\r\n");
	xflush_ser(TICKS_PER_SECOND/2);		/* Flush any resulting output	*/

} /* pb200Init() */


/************************************************************************/
/* Function	   : skip_field												*/
/* Purpose	   : skip over n commas of GPS message						*/
/* Inputs	   : String pointer, number of fields to skip				*/
/* Outputs	   : Pointer to first char after nth ','					*/
/************************************************************************/
MLocal char *skip_field( Reg char *p, Nat16 num )
{
	for ( ; num && *p; p++ )
	{
		if ( *p == ',' )
			num--;
	}

	return( p );

} /* skip_field() */


/****************************************************************************/
/* Function	   : getMsg														*/
/* Purpose	   : Get an NMEA Message										*/	
/* Inputs	   : Driver pointer, buffer, buffer size						*/
/* Outputs	   : OK (0) if got fix, SYNC_ERR (-7) if not position msg		*/
/*				 ERROR if timed out, NOT_AVAIL_ERR (-2) if GPS not available*/
/****************************************************************************/
MLocal MsgRtn getMsg(char *buffer, Nat16 bufsize, Nat16 tmout)
{
	Nat16		i;
	Int16		rtn;
	char		*p;

	for ( i = 0; i < 10; i++ )			/* Try 10 times to get a fix	*/
	{
		if (((rtn = xgets_tmout(buffer, bufsize, tmout)) > 0) &&
			(buffer[0] == '$'))
		{
			if ((p = strchr(buffer, '\r')) != NULL)
				*p = '\0';
#ifdef DEBUG_AIRMAR
			printf("NMEA Msg:  %s\n", buffer);
#endif
			if (strncmp(buffer, "$GPGGA", 6) == 0)
				return(MSG_GGA);
			else if (strncmp(buffer, "$WIMDA", 6) == 0)
				return(MSG_MDA);
			else
				return(MSG_MISC);
		}
		task_delay(TICKS_PER_SECOND/10); /* Allow someone else to run	*/
	}
	
	return(MSG_ERR);

} /* getMsg() */


/************************************************************************/
/* Function	   : pb200_drv												*/
/* Purpose	   : AirMar PB200 driver									*/
/* Inputs	   : Driver Pointer											*/
/* Outputs	   : None													*/
/************************************************************************/
Void pb200_drv( Driver *dp )
{
	TimeOfDay	start_time;
	char		*p;
	Flt32		windSpeed;
	MsgRtn		msgRtn;
	Int16		gpsMode, rtn;
	MBool		gotFix=FALSE, gotWind=FALSE;

	drv_ser_port(dp);					/* Get serial port, turn on power*/

	delay_secs(dp->drv_parms[PARM0]);	/* Wait warmup time				*/

	pb200Init((dp->drv_flags & DO_INIT) != 0);	/* Initialize device msgs*/

	start_time = tod;
	lastGGA[0] = lastMDA[0] = '\0';

	while (!(gotFix && gotWind) && ((tod - start_time) < dp->drv_parms[PARM1]))
	{
		msgRtn = getMsg(msgBuf, sizeof(msgBuf), (Nat16)(dp->drv_parms[TIMEOUT]));
		if (msgRtn == MSG_GGA)
		{
			gpsMode = 0;
			p = skipField(msgBuf, 6);
			rtn = sscanf(p, " %hd", &gpsMode);

			if ((rtn > 0) && (gpsMode >= 1) && (gpsMode <= 3))
			{
				gotFix = TRUE;
				strncpy(lastGGA, msgBuf, sizeof(lastGGA));
#ifdef DEBUG_AIRMAR
				printf("Got GPS Fix:  %s\n", msgBuf);
#endif
			}
		}
		else if (gotFix && (msgRtn == MSG_MDA))
		{
			windSpeed = 0.0;
			p = skipField(msgBuf, 17);
			if (sscanf(p, " %f", &windSpeed) > 0)
			{
				gotWind = TRUE;
				strncpy(lastMDA, msgBuf, sizeof(lastMDA));
#ifdef DEBUG_AIRMAR
				printf("Got Met Wind:  %s\n", msgBuf);
#endif
			} 
		}
	}
	
	drv_ser_release(dp);				/* Release ser port				  */
	drv_pwroff(dp);						/* Turn off GPS power			  */
	dp->drv_flags &= ~DO_INIT;

	sprintf(msgBuf, "%s\r\n%s\r\n", lastGGA, lastMDA);

	if (strlen(msgBuf) > 4)
		drvLog(dp, msgBuf, strlen(msgBuf));
	else
		drvLogError(dp, TMOUT_ERR);

} /* pb200_drv() */