gps.c

ARM_CORTEX-M3应用实例开发详解光盘

{
	char pBuff[10];
	INT8U pField[MAXFIELD];

	//
	// Time
	//
	if(GetField(pData, pField, 0, MAXFIELD))
	{
		// Hour
		pBuff[0] = pField[0];
		pBuff[1] = pField[1];
		pBuff[2] = '\0';
		GPRMCData.Hour = atoi(pBuff);

		// minute
		pBuff[0] = pField[2];
		pBuff[1] = pField[3];
		pBuff[2] = '\0';
		GPRMCData.Minute = atoi(pBuff);

		// Second
		pBuff[0] = pField[4];
		pBuff[1] = pField[5];
		pBuff[2] = '\0';
		GPRMCData.Second = atoi(pBuff);
	}

	//
	// Data valid
	//
	if(GetField(pData, pField, 1, MAXFIELD))
	{
		GPRMCData.DataValid = pField[0];
	}
	else
	{
		GPRMCData.DataValid = 'V';
	}

	//
	// latitude
	//
	if(GetField(pData, pField, 2, MAXFIELD))
	{
		GPRMCData.Latitude = atof((char *)pField+2) / 60.0;
		pField[2] = '\0';
		GPRMCData.Latitude += atof((char *)pField);

	}
	if(GetField(pData, pField, 3, MAXFIELD))
	{
		if(pField[0] == 'S')
		{
			GPRMCData.Latitude = -GPRMCData.Latitude;
		}
	}

	//
	// Longitude
	//
	if(GetField(pData, pField, 4, MAXFIELD))
	{
		GPRMCData.Longitude = atof((char *)pField+3) / 60.0;
		pField[3] = '\0';
		GPRMCData.Longitude += atof((char *)pField);
	}
	if(GetField(pData, pField, 5, MAXFIELD))
	{
		if(pField[0] == 'W')
		{
			GPRMCData.Longitude = -GPRMCData.Longitude;
		}
	}

	//
	// Ground speed
	//
	if(GetField(pData, pField, 6, MAXFIELD))
	{
		GPRMCData.GroundSpeed = atof((char *)pField);
	}
	else
	{
		GPRMCData.GroundSpeed = 0.0;
	}

	//
	// course over ground, degrees true
	//
	if(GetField(pData, pField, 7, MAXFIELD))
	{
		GPRMCData.Course = atof((char *)pField);
	}
	else
	{
		GPRMCData.Course = 0.0;
	}

	//
	// Date
	//
	if(GetField(pData, pField, 8, MAXFIELD))
	{
		// Day
		pBuff[0] = pField[0];
		pBuff[1] = pField[1];
		pBuff[2] = '\0';
		GPRMCData.Day = atoi(pBuff);

		// Month
		pBuff[0] = pField[2];
		pBuff[1] = pField[3];
		pBuff[2] = '\0';
		GPRMCData.Month = atoi(pBuff);

		// Year (Only two digits. I wonder why?)
		pBuff[0] = pField[4];
		pBuff[1] = pField[5];
		pBuff[2] = '\0';
		GPRMCData.Year = atoi(pBuff);
		GPRMCData.Year += 2000;				// make 4 digit date -- What assumptions should be made here?
	}

	//
	// course over ground, degrees true
	//
	if(GetField(pData, pField, 9, MAXFIELD))
	{
		GPRMCData.MagVar = atof((char *)pField);
	}
	else
	{
		GPRMCData.MagVar = 0.0;
	}
	if(GetField(pData, pField, 10, MAXFIELD))
	{
		if(pField[0] == 'W')
		{
			GPRMCData.MagVar = -GPRMCData.MagVar;
		}
	}

	GPRMCData.Count++;
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
void ProcessGPRMB(INT8U *pData)
{
	INT8U pField[MAXFIELD];

	//
	// Data status
	//
	if(GetField(pData, pField, 0, MAXFIELD))
	{
		GPRMBData.DataStatus = pField[0];
	}
	else
	{
		GPRMBData.DataStatus = 'V';
	}

	//
	// Cross track error
	//
	if(GetField(pData, pField, 1, MAXFIELD))
	{
		GPRMBData.CrosstrackError = atof((char *)pField);
	}
	else
	{
		GPRMBData.CrosstrackError = 0.0;
	}

	//
	// Direction to steer
	//
	if(GetField(pData, pField, 2, MAXFIELD))
	{
		GPRMBData.DirectionToSteer = pField[0];
	}
	else
	{
		GPRMBData.DirectionToSteer = '?';
	}

	//
	// Orgin waypoint ID
	//
	if(GetField(pData, pField, 3, MAXFIELD))
	{
		strcpy(GPRMBData.OriginWaypoint, (char *)pField);
	}
	else
	{
		GPRMBData.OriginWaypoint[0] = '\0';
	}

	//
	// Destination waypoint ID
	//
	if(GetField(pData, pField, 4, MAXFIELD))
	{
		strcpy(GPRMBData.DestWaypoint, (char *)pField);
	}
	else
	{
		GPRMBData.DestWaypoint[0] = '\0';
	}

	//
	// Destination latitude
	//
	if(GetField(pData, pField, 5, MAXFIELD))
	{
		GPRMBData.DestLatitude = atof((char *)pField+2) / 60.0;
		pField[2] = '\0';
		GPRMBData.DestLatitude += atof((char *)pField);

	}
	if(GetField(pData, pField, 6, MAXFIELD))
	{
		if(pField[0] == 'S')
		{
			GPRMBData.DestLatitude = -GPRMBData.DestLatitude;
		}
	}

	//
	// Destination Longitude
	//
	if(GetField(pData, pField, 7, MAXFIELD))
	{
		GPRMBData.DestLongitude = atof((char *)pField+3) / 60.0;
		pField[3] = '\0';
		GPRMBData.DestLongitude += atof((char *)pField);
	}
	if(GetField(pData, pField, 8, MAXFIELD))
	{
		if(pField[0] == 'W')
		{
			GPRMBData.DestLongitude = -GPRMBData.DestLongitude;
		}
	}

	//
	// Range to destination nautical mi
	//
	if(GetField(pData, pField, 9, MAXFIELD))
	{
		GPRMBData.RangeToDest = atof((char *)pField);
	}
	else
	{
		GPRMBData.CrosstrackError = 0.0;
	}

	//
	// Bearing to destination degrees true
	//
	if(GetField(pData, pField, 10, MAXFIELD))
	{
		GPRMBData.BearingToDest = atof((char *)pField);
	}
	else
	{
		GPRMBData.BearingToDest = 0.0;
	}

	//
	// Closing velocity
	//
	if(GetField(pData, pField, 11, MAXFIELD))
	{
		GPRMBData.DestClosingVelocity = atof((char *)pField);
	}
	else
	{
		GPRMBData.DestClosingVelocity = 0.0;
	}

	//
	// Arrival status
	//
	if(GetField(pData, pField, 12, MAXFIELD))
	{
		GPRMBData.ArrivalStatus = pField[0];
	}
	else
	{
		GPRMBData.DestClosingVelocity = 'V';
	}

	GPRMBData.Count++;
}

 ///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
void ProcessGPGSV(INT8U *pData)
{
	INT32S i, nTotalNumOfMsg = 0, nMsgNum = 0;
	INT8U pField[MAXFIELD];

	//
	// Total number of messages
	//
	if(GetField(pData, pField, 0, MAXFIELD))
	{
		nTotalNumOfMsg = atoi((char *)pField);

		//
		// Make sure that the nTotalNumOfMsg is valid. This is used to
		// calculate indexes into an array. I've seen corrept NMEA strings
		// with no checksum set this to large values.
		//
		if(nTotalNumOfMsg > 9 || nTotalNumOfMsg < 0) return; 
	}
	if(nTotalNumOfMsg < 1 || nTotalNumOfMsg*4 >= NP_MAX_CHAN)
	{
		return;
	}

	//
	// message number
	//
	if(GetField(pData, pField, 1, MAXFIELD))
	{
		nMsgNum = atoi((char *)pField);

		//
		// Make sure that the message number is valid. This is used to
		// calculate indexes into an array
		//
		if(nMsgNum > 9 || nMsgNum < 0) return; 
	}

	//
	// Total satellites in view
	//
	if(GetField(pData, pField, 2, MAXFIELD))
	{
		GPGSVData.TotalNumSatsInView = atoi((char *)pField);
	}

	//
	// Satelite data
	//
	for(i = 0; i < 4; i++)
	{
		// Satellite ID
		if(GetField(pData, pField, 3 + 4*i, MAXFIELD))
		{
			GPGSVData.SatInfo[i+(nMsgNum-1)*4].PRN = atoi((char *)pField);
		}
		else
		{
			GPGSVData.SatInfo[i+(nMsgNum-1)*4].PRN = 0;
		}

		// Elevarion
		if(GetField(pData, pField, 4 + 4*i, MAXFIELD))
		{
			GPGSVData.SatInfo[i+(nMsgNum-1)*4].Elevation = atoi((char *)pField);
		}
		else
		{
			GPGSVData.SatInfo[i+(nMsgNum-1)*4].Elevation = 0;
		}

		// Azimuth
		if(GetField(pData, pField, 5 + 4*i, MAXFIELD))
		{
			GPGSVData.SatInfo[i+(nMsgNum-1)*4].Azimuth = atoi((char *)pField);
		}
		else
		{
			GPGSVData.SatInfo[i+(nMsgNum-1)*4].Azimuth = 0;
		}

		// SNR
		if(GetField(pData, pField, 6 + 4*i, MAXFIELD))
		{
			GPGSVData.SatInfo[i+(nMsgNum-1)*4].SignalQuality = atoi((char *)pField);
		}
		else
		{
			GPGSVData.SatInfo[i+(nMsgNum-1)*4].SignalQuality = 0;
		}

		//
		// Update "used in solution" (m_bUsedInSolution) flag. This is base
		// on the GSA message and is an added convenience for post processing
		//
		GPGSVData.SatInfo[i+(nMsgNum-1)*4].UsedInSolution = IsSatUsedInSolution(GPGSVData.SatInfo[i+(nMsgNum-1)*4].PRN);
	}

	GPGSVData.Count++;
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
void ProcessGPGSA(INT8U *pData)
{
	INT8U pField[MAXFIELD];
	char pBuff[10];
	INT32S i;

	//
	// Mode
	//
	if(GetField(pData, pField, 0, MAXFIELD))
	{
		GPGSAData.Mode = pField[0];
	}

	//
	// Fix Mode
	//
	if(GetField(pData, pField, 1, MAXFIELD))
	{
		GPGSAData.FixMode = pField[0] - '0';
	}

	//
	// Active satellites
	//
	for(i = 0; i < 12; i++)
	{
		if(GetField(pData, pField, 2 + i, MAXFIELD))
		{
			pBuff[0] = pField[0];
			pBuff[1] = pField[1];
			pBuff[2] = '\0';
			GPGSAData.SatsInSolution[i] = atoi(pBuff);
		}
		else
		{
			GPGSAData.SatsInSolution[i] = 0;
		}
	}

	//
	// PDOP
	//
	if(GetField(pData, pField, 14, MAXFIELD))
	{
		GPGSAData.PDOP = atof((char *)pField);
	}
	else
	{
		GPGSAData.PDOP = 0.0;
	}

	//
	// HDOP
	//
	if(GetField(pData, pField, 15, MAXFIELD))
	{
		GPGSAData.HDOP = atof((char *)pField);
	}
	else
	{
		GPGSAData.HDOP = 0.0;
	}

	//
	// VDOP
	//
	if(GetField(pData, pField, 16, MAXFIELD))
	{
		GPGSAData.VDOP = atof((char *)pField);
	}
	else
	{
		GPGSAData.VDOP = 0.0;
	}

	GPGSAData.Count++;
}

///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
void ProcessGPGGA(INT8U *pData)
{
	INT8U pField[MAXFIELD];
	char pBuff[10];
	INT32S nSeconds;

	//
	// Time
	//
	if(GetField(pData, pField, 0, MAXFIELD))
	{
		// Hour
		pBuff[0] = pField[0];
		pBuff[1] = pField[1];
		pBuff[2] = '\0';
		GPGGAData.Hour = atoi(pBuff);

		// minute
		pBuff[0] = pField[2];
		pBuff[1] = pField[3];
		pBuff[2] = '\0';
		GPGGAData.Minute = atoi(pBuff);

		// Second
		pBuff[0] = pField[4];
		pBuff[1] = pField[5];
		pBuff[2] = '\0';
		GPGGAData.Second = atoi(pBuff);
	}

	//
	// Latitude
	//
	if(GetField(pData, pField, 1, MAXFIELD))
	{
		GPGGAData.Latitude = atof((char *)pField+2) / 60.0;
		pField[2] = '\0';
		GPGGAData.Latitude += atof((char *)pField);

	}
	if(GetField(pData, pField, 2, MAXFIELD))
	{
		if(pField[0] == 'S')
		{
			GPGGAData.Latitude = -GPGGAData.Latitude;
		}
	}

	//
	// Longitude
	//
	if(GetField(pData, pField, 3, MAXFIELD))
	{
		GPGGAData.Longitude = atof((char *)pField+3) / 60.0;
		pField[3] = '\0';
		GPGGAData.Longitude += atof((char *)pField);
	}
	if(GetField(pData, pField, 4, MAXFIELD))
	{
		if(pField[0] == 'W')
		{
			GPGGAData.Longitude = -GPGGAData.Longitude;
		}
	}

	//
	// GPS quality
	//
	if(GetField(pData, pField, 5, MAXFIELD))
	{
		GPGGAData.GPSQuality = pField[0] - '0';
	}

	//
	// Satellites in use
	//
	if(GetField(pData, pField, 6, MAXFIELD))
	{
		pBuff[0] = pField[0];
		pBuff[1] = pField[1];
		pBuff[2] = '\0';
		GPGGAData.NumOfSatsInUse = atoi(pBuff);
	}

	//
	// HDOP
	//
	if(GetField(pData, pField, 7, MAXFIELD))
	{
		GPGGAData.HDOP = atof((char *)pField);
	}
	
	//
	// Altitude
	//
	if(GetField(pData, pField, 8, MAXFIELD))
	{
		GPGGAData.Altitude = atof((char *)pField);
	}

	//
	// Durive vertical speed (bonus)
	//
	nSeconds = (INT32S)GPGGAData.Minute * 60 + (INT32S)GPGGAData.Second;
	if(nSeconds > GPGGAData.OldVSpeedSeconds)
	{
		double dDiff = (double)(GPGGAData.OldVSpeedSeconds-nSeconds);
		double dVal = dDiff/60.0;
		if(dVal != 0.0)
		{
			GPGGAData.VertSpeed = (GPGGAData.OldVSpeedAlt - GPGGAData.Altitude) / dVal;
		}
	}
	GPGGAData.OldVSpeedAlt = GPGGAData.Altitude;
	GPGGAData.OldVSpeedSeconds = nSeconds;

	GPGGAData.Count++;
}
#endif