nmea.cpp

开源GPS源码

#include <time.h>
#include <stdio.h>
#include <float.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>

#include "NMEA.h"
#include "consts.h"
#include "structs.h"
#include "globals.h"
#include "FwInter.h"
#include "serport.h"

extern char gSimulate;

/***********************************************************************
// Clifford Kelley <[e-mail]>
// This program is licensed under GNU GENERAL PUBLIC LICENSE
    Version 0.7 3/13/04 
	1.) Added Geoidal Seperation to GGA. Hard coded to 0.0
	2.) Set Mode to A in GAS
    Version 0.6 2/26/04 Removed channels structure definition
	Version 0.5 2/22/04  Fixed bug in checksum routine 
	Version 0.4 Added buad rate selection and NMEA sentence selection capablity
	Version 0.3 Turned off hardware flow control
	Version 0.2 Fixed SV in view bug
	Version 0.1 Initial release
***********************************************************************/

void SendNMEA( void )
{
	if (GPGGA !=0)	NMEASendGPGGA();	// NMEA, use sentance options
	if (GPGSA !=0)	NMEASendGPGSA();
	if (GPGSV !=0)	NMEASendGPGSV();
	if (GPRMC !=0)	NMEASendGPRMC();
   if (GPZDA !=0)	NMEASendGPZDA();
}

/*     NMEA Notes  

	Longitude and latitude can be expressed in several different representations: 

	dms (deg,min,sec) - Degrees, minutes and seconds format (e.g. 5045'23.99") 

	degrees - Decimal degrees (e.g. 50.7567) 

	radians - Radians (e.g. 0.88587090) 

	degrees, minutes - Degrees and minutes format (e.g. 5045.3998') 

	Select the appropriate representation found in the string files

	Converting Between Decimal Degrees, Degrees, Minutes and Seconds, and Radians
	(dd + mm/60 +ss/3600) to Decimal degrees (dd.ff)

	dd = whole degrees, mm = minutes, ss = seconds

	dd.ff = dd + mm/60 + ss/3600

	Example: 30 degrees 15 minutes 22 seconds = 30 + 15/60 + 22/3600 = 30.2561

	Decimal degrees (dd.ff) to (dd + mm/60 +ss/3600)

	For the reverse conversion, we want to convert dd.ff to dd mm ss. Here ff = the fractional part of a decimal degree.

	mm = 60*ff

	ss = 60*(fractional part of mm)

	Use only the whole number part of mm in the final result.

	30.2561 degrees = 30 degrees

	.2561*60 = 15.366 minutes

	.366 minutes = 22 seconds, so the final result is 30 degrees 15 minutes 22 seconds

	Decimal degrees (dd.ff) to Radians

	Radians = (dd.ff)*pi/180

	Radians to Decimal degrees (dd.ff)

	(dd.ff) = Radians*180/pi

	Degrees, Minutes and Seconds to Distance

	A degree of longitude at the equator is 111.2 kilometers. A minute is 1853 meters.
	A second is 30.9 meters. For other latitudes multiply by cos(lat). Distances for degrees, 
	minutes and seconds in latitude are very similar and differ very slightly with latitude. 
	(Before satellites, observing those differences was a principal method for determining the exact shape of the earth.)


	D = Degrees
	M = Minutes
	S = Seconds
	.m = Decimal Minutes
	.s = Decimal Seconds

	DM.m = Degrees, Minutes, Decimal Minutes (eg. 45 22.6333)
	D.d  = Degrees, Decimal Degrees (eg. 45.3772)
	DMS  = Degrees, Minutes, Seconds (eg. 45 22'38")

	Process for Converting Latitude/Longitude Coordinates: 
	
	1) DMS --> DM.m  (45o22'38" --> 45o22.6333)

	 - Divide S by 60 to get .m (38/60=.6333)
	 - Add .m to M to get M.m (22+.6333=22.6333)

	2) DM.m --> D.d  (45o 22.6333 --> 45.3772)

	 - Divide M.m by 60 to get .d (22.6333/60=.3772)
	 - Add .d to D to get D.d (45+.3772=45.3772)

	3) D.d --> DM.m  ( 45.3772 --> 45 22.6333)

	 - Multiply .d by 60 to get M.m (.3772 * 60 = 22.6333)

	4) DM.m --> DMS  (45o22.6333 --> 45 22'38")

	 - Multiply .m by 60 to get S(.6333*60=38)


	Converting Degrees, Minutes, Seconds to Decimal Format

	latitude and longitude in a decimal format: 42.1361
	latitude and longitude in degree, minute, second format: 42deg, 08min, 10sec

	To convert coordinates from degrees, minutes,
	seconds format to decimal format, use this easy formula:

	degrees + (minutes/60) +
	(seconds/3600)

	The example coordinate above would be
	calculated as:
	42 + (8/60) + (10/3600) = 42.1361
	or
	42 + (.1333) + (.0028) = 42.1361




*/

inline void NMEAAddCRLF( char* TransmitBuffer, char* TXBufferIndex )
{
	char* TempPtr = TransmitBuffer + *TXBufferIndex;
	
	*TempPtr++ = 0x0D;
	*TempPtr++ = 0x0A;
	//
	// Update the index.
	//
	*TXBufferIndex += 2;
	*TempPtr++ = '\0';  
}



inline void NMEAAddField( char* TransmitBuffer, char* TXBufferIndex, char* StringToAdd )
{
	int NumberOfChars = 0;
	NumberOfChars     = strlen( StringToAdd  );

	//
	// Copy the data.
	//
	memcpy( &TransmitBuffer[ *TXBufferIndex ], StringToAdd, NumberOfChars  );	

	//
	// Update the index.
	//
	*TXBufferIndex += NumberOfChars;
}

double ConvertLatLonFromDegreestoDegreesMinutes( double PositionDegrees )
{
	double Degrees          = ( int )PositionDegrees;
	double Minutes			= PositionDegrees - Degrees;

	double DegreesMinutes	= Minutes * 60 + ( Degrees * 10 * 10 );
	return DegreesMinutes;
}

//
// The checksum is the 8-bit exclusive OR (no start or stop bits) of all 
// characters in the sentence, including the "," delimiters, between -- but not including -- the 
// "$" and "*" delimiters. 
// The hexadecimal value of the most significant and least significant 4 bits of the result are converted to two 
// ASCII characters (0-9, A-F) for transmission. The most significant character is transmitted first. 
//
inline unsigned char CalculateNmeaCheckSum( char* NmeaSentence )
{
	int Index;
	unsigned char NmeaCheckSum = 0;
	
	for( Index = 1; NmeaSentence[ Index ] != '*'; Index++ )
	{
		NmeaCheckSum ^= NmeaSentence[ Index ];
	}

	return NmeaCheckSum;
}

inline void NMEAAddCheckSum( char* TransmitBuffer, char* TXBufferIndex )
{
	unsigned char CheckSum;
	unsigned char TXCheckSum;
	char          TempBuf[ 10 ];
	

	
	NMEAAddField( TransmitBuffer, TXBufferIndex, NMEA_SENTENCE_ASTERISK );

	CheckSum = CalculateNmeaCheckSum( TransmitBuffer );
	
    TXCheckSum = CheckSum & 0xf0;
	itoa(  TXCheckSum >> 4 ,TempBuf, 16 );
	TempBuf[ 1 ] = '\0';

	if( TempBuf[ 0 ] >= 'a' )
	{
	
		TempBuf[ 0 ]  = ( char )toupper( ( char )TempBuf[0] );
		TempBuf[ 1 ] = '\0';
	}


	NMEAAddField( TransmitBuffer, TXBufferIndex, TempBuf  );
	TXCheckSum = CheckSum & 0x0f;
	itoa(   TXCheckSum , TempBuf, 16 );
	TempBuf[ 1 ] = '\0';

    if( TempBuf[ 0 ] >= 'a' )
	{
	
		TempBuf[ 0 ]  = ( char )toupper( ( char )TempBuf[0] );
		TempBuf[ 1 ] = '\0';
	}


	NMEAAddField( TransmitBuffer, TXBufferIndex, TempBuf );
}

NMEA_STATUS NMEAGetGPGGAData( pNMEA_GGA_DATA pNmeaGGAData )
{
	
	//
	// Need to convert from degrees to degrees and minutes format.
	//
	pNmeaGGAData->TimeOfFixUTC       = GetTimeOfFixUTC();
	pNmeaGGAData->Lat			     = ConvertLatLonFromDegreestoDegreesMinutes( GetLatitudeAbs() );
	pNmeaGGAData->NorthSouth         = GetNorthSouth();
	pNmeaGGAData->Long			     = ConvertLatLonFromDegreestoDegreesMinutes( GetLongitudeAbs() );
	pNmeaGGAData->EastWest           = GetEastWest();
	pNmeaGGAData->FixQuality	     = GetFixQuality();
	pNmeaGGAData->NumberOfTrackingSV = GetNumberOfTrackingSVs();
	pNmeaGGAData->HDOP			     = GetHDOP();
	pNmeaGGAData->Altitude		     = GetAltitude();
	pNmeaGGAData->HeightOfGeoid      = GetHeightOfGeoid();

	return NMEA_SUCCESS;
}

/*
	GGA - essential fix data which provide 3D location and accuracy data. 

	 $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47

	Where:
		 GGA          Global Positioning System Fix Data
		 123519       Fix taken at 12:35:19 UTC
		 4807.038,N   Latitude 48 deg 07.038' N
		 01131.000,E  Longitude 11 deg 31.000' E
		 1            Fix quality: 0 = invalid
								   1 = GPS fix (SPS)
								   2 = DGPS fix
								   3 = PPS fix
								   4 = Real Time Kinematic
								   5 = Float RTK
								   6 = estimated (dead reckoning) (2.3 feature)
								   7 = Manual input mode
								   8 = Simulation mode
		 08           Number of satellites being tracked
		 0.9          Horizontal dilution of position
		 545.4,M      Altitude, Meters, above mean sea level
		 46.9,M       Height of geoid (mean sea level) above WGS84
						  ellipsoid
		 (empty field) time in seconds since last DGPS update
		 (empty field) DGPS station ID number
		 *47          the checksum data, always begins with *

	If the height of geoid is missing then the altitude should be suspect. 
	Some non-standard implementations report altitude with respect to the ellipsoid 
	rather than geoid altitude. Some units do not report negative altitudes at all. 
	This is the only sentence that reports altitude.
	
*/

NMEA_STATUS NMEASendGPGGA( void )
{
	
	NMEA_GGA_DATA NmeaGGAData;
	char          pFieldBuffer[ MAX_NMEA_FIELD ];
    char          TXBufferIndex = 0;
	char          TransmitBuffer[ MAX_NMEA_BUFFER ];


	memset( TransmitBuffer,0, MAX_NMEA_BUFFER ); 
	
	
	//
	// Get the necessary data from the FW.
	//
	NMEAGetGPGGAData( &NmeaGGAData );

    //
	// Add the header
	//
	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_START );
	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_GPGGA );
	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_COMMA );

	//
	// Field 0 Time   
	//
	// The time field must be 6 digits long. If the hour is less than 10 you 
	// must pad it with an extra zero.
	//
	sprintf( pFieldBuffer, "%06ld.00", ( long )NmeaGGAData.TimeOfFixUTC );
//	ftod( NmeaGGAData.TimeOfFixUTC, pFieldBuffer  );
	NMEAAddField( TransmitBuffer, &TXBufferIndex, pFieldBuffer );
	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_COMMA );


	
	//
	// Field 1 Latitude
	//
    ftod4( NmeaGGAData.Lat, pFieldBuffer );  // 4 decimal places
	NMEAAddField( TransmitBuffer, &TXBufferIndex, pFieldBuffer );
	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_COMMA );

	//
	// Field 2 North/South 
	//
	if( NmeaGGAData.NorthSouth == NMEA_SOUTH )
	{
		NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_SOUTH );
	}
	else
	{
		NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_NORTH );
	}

	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_COMMA );

	//
	// Field 3 Longitude
	//
	ftod4( NmeaGGAData.Long, pFieldBuffer ); // 4 decimal places
	NMEAAddField( TransmitBuffer, &TXBufferIndex, pFieldBuffer );
	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_COMMA );


	//
	// Field 4 East/West
	//
	if( NmeaGGAData.EastWest == NMEA_WEST )
	{
		NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_WEST );
	}
	else
	{
		NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_EAST );
	}

	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_COMMA );
	
	//
	// Field 5 Fix Quality
	//
	itoa(  NmeaGGAData.FixQuality, pFieldBuffer, 10 );
	// ftod( NmeaGGAData.FixQuality, pFieldBuffer );
	NMEAAddField( TransmitBuffer, &TXBufferIndex, pFieldBuffer );
	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_COMMA );
	
	//
	// Field 6 Number of SV's used.
	//
	sprintf( pFieldBuffer, "%02d", NmeaGGAData.NumberOfTrackingSV );
	//itoa(  NmeaGGAData.NumberOfTrackingSV, pFieldBuffer, 10 );
	NMEAAddField( TransmitBuffer, &TXBufferIndex, pFieldBuffer );
	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_COMMA );
	
	//
	// Field 7 HDOP
	//
	ftodPrecision1( NmeaGGAData.HDOP, pFieldBuffer );
	//ftod( NmeaGGAData.HDOP, pFieldBuffer );
	NMEAAddField( TransmitBuffer, &TXBufferIndex, pFieldBuffer );
	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_COMMA );

	//
	// Field 8 Altitude
	//
	ftodPrecision1( NmeaGGAData.Altitude, pFieldBuffer );
	// ftod( NmeaGGAData.Altitude, pFieldBuffer );
	NMEAAddField( TransmitBuffer, &TXBufferIndex, pFieldBuffer  );
	
	
	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_COMMA );
	//
	// Field 9 Altitude Units
	//
	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_UNITS_METERS );
	
	
	
	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_COMMA );
	//
	// Field 10 Geoid Seperation ( Height of geoid above WGS84 ellipsoid )
	//
	
	ftodPrecision1( 0.0, pFieldBuffer );
	NMEAAddField( TransmitBuffer, &TXBufferIndex, pFieldBuffer ); 

	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_COMMA );
	//
	// Feild 11 Geoid Seperation units
	//
	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_UNITS_METERS );

	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_COMMA );
	
	//
	// Field 12  DGPS Age
	//

	NMEAAddField( TransmitBuffer, &TXBufferIndex, NMEA_SENTENCE_COMMA );
	//
	// Field 12  DGPS StationID
	//
		
     
	//
	// Check Sum 
	//
	NMEAAddCheckSum( TransmitBuffer, &TXBufferIndex );

	//
	// CR/LF
	//
	NMEAAddCRLF( TransmitBuffer, &TXBufferIndex );




	//
	// Now send the sentence.
	//
	if( !ComPortWrite( ( unsigned char *)&TransmitBuffer[0],  TXBufferIndex ) )
	{
		return NMEA_FAILED;
	}
	else
	{
		return NMEA_SUCCESS;
	}