📄 gps.c
字号:
#define GPS_GLOBALS
#include <includes.h>
#if GPS_MODULE == DEF_ENABLED
#define MAXFIELD 25 // maximum field length
static OS_STK GPS_TaskStk[GPS_TASK_STK_SIZE];
static OS_EVENT *GPS_Sem;
NP_STATE m_nState; // Current state protocol parser is in
INT8U m_btChecksum; // Calculated NMEA sentence checksum
INT8U m_btReceivedChecksum; // Received NMEA sentence checksum (if exists)
INT16U m_wIndex; // Index used for command and data
INT8U m_pCommand[NP_MAX_CMD_LEN]; // NMEA command
INT8U m_pData[NP_MAX_DATA_LEN]; // NMEA data
static void GPS_InitOS(void);
static void GPS_Task(void *p_arg);
/*
*********************************************************************************************************
* INITIALIZE GPS
*********************************************************************************************************
*/
void GPS_Init (void)
{
m_nState = NP_STATE_SOM;
m_dwCommandCount = 0;
GPDataReset();
GPS_InitOS();
GPS_InitTarget(); /* Initialize target specific code */
}
/*
*********************************************************************************************************
* INITIALIZE THE uC/OS-View TASK AND OS OBJECTS
*********************************************************************************************************
*/
static void GPS_InitOS (void)
{
#if OS_TASK_NAME_SIZE > 7 || OS_EVENT_NAME_SIZE > 7
INT8U err;
#endif
GPS_Sem = OSSemCreate(0);
#if OS_EVENT_NAME_SIZE > 17
OSEventNameSet(GPS_Sem, (INT8U *)"GPS Signal", &err);
#else
#if OS_EVENT_NAME_SIZE > 7
OSEventNameSet(GPS_Sem, (INT8U *)"GPS", &err);
#endif
#endif
(void)OSTaskCreateExt(GPS_Task,
(void *)0, /* No arguments passed to OSView_Task() */
&GPS_TaskStk[GPS_TASK_STK_SIZE - 1], /* Set Top-Of-Stack */
GPS_TASK_PRIO, /* Lowest priority level */
GPS_TASK_PRIO,
&GPS_TaskStk[0], /* Set Bottom-Of-Stack */
GPS_TASK_STK_SIZE,
(void *)0, /* No TCB extension */
OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR); /* Enable stack checking + clear stack */
#if OS_TASK_NAME_SIZE > 10
OSTaskNameSet(GPS_TASK_PRIO, (INT8U *)"GPS Task", &err);
#else
#if OS_TASK_NAME_SIZE > 7
OSTaskNameSet(GPS_TASK_PRIO, (INT8U *)"GPS", &err);
#endif
#endif
}
/*
*********************************************************************************************************
* GPS TASK
*********************************************************************************************************
*/
static void GPS_Task (void *p_arg)
{
//INT8U err;
//INT8U s[100];
while(1) {
//(void)OSSemPend(GPS_Sem, 0, &err);
OSTimeDlyHMSM(0, 0, 0, 500);
//sprintf((char *)s, "\n时:%2d 分:%2d 秒:%2d 纬度:%f 经度:%f 高度:%f\n", GPGGAData.Hour, GPGGAData.Minute, GPGGAData.Second, GPGGAData.Latitude, GPGGAData.Longitude, GPGGAData.Altitude);
//OSView_TxStr(s, 1);
}
}
/*
*********************************************************************************************************
* Rx Handler
*
* Description: This routine is called from the Rx interrupt service handler.
*********************************************************************************************************
*/
void GPS_RxHandler (INT8U rx_data)
{
ProcessNMEA(rx_data);
}
/*
*********************************************************************************************************
* Tx Handler
*
* Description: This routine is called from the transmitter buffer empty interrupt service handler.
* It will send out the next byte in the buffer.
*
* Returns: none
*********************************************************************************************************
*/
void GPS_TxHandler (void)
{
}
///////////////////////////////////////////////////////////////////////////////
// Reset: Reset all NMEA data to start-up default values.
///////////////////////////////////////////////////////////////////////////////
void GPDataReset(void)
{
int i;
//
// GPGGA Data
//
GPGGAData.Hour = 0; //
GPGGAData.Minute = 0; //
GPGGAData.Second = 0; //
GPGGAData.Latitude = 0.0; // < 0 = South, > 0 = North
GPGGAData.Longitude = 0.0; // < 0 = West, > 0 = East
GPGGAData.GPSQuality = 0; // 0 = fix not available, 1 = GPS sps mode, 2 = Differential GPS, SPS mode, fix valid, 3 = GPS PPS mode, fix valid
GPGGAData.NumOfSatsInUse = 0; //
GPGGAData.HDOP = 0.0; //
GPGGAData.Altitude = 0.0; // Altitude: mean-sea-level (geoid) meters
GPGGAData.Count = 0; //
GPGGAData.OldVSpeedSeconds = 0; //
GPGGAData.OldVSpeedAlt = 0.0; //
GPGGAData.VertSpeed = 0.0; //
//
// GPGSA
//
GPGSAData.Mode = 'M'; // M = manual, A = automatic 2D/3D
GPGSAData.FixMode = 1; // 1 = fix not available, 2 = 2D, 3 = 3D
for(i = 0; i < NP_MAX_CHAN; i++)
{
GPGSAData.SatsInSolution[i] = 0; // ID of sats in solution
}
GPGSAData.PDOP = 0.0; //
GPGSAData.HDOP = 0.0; //
GPGSAData.VDOP = 0.0; //
GPGSAData.Count = 0; //
//
// GPGSV
//
GPGSVData.TotalNumOfMsg = 0; //
GPGSVData.TotalNumSatsInView = 0; //
for(i = 0; i < NP_MAX_CHAN; i++)
{
GPGSVData.SatInfo[i].Azimuth = 0;
GPGSVData.SatInfo[i].Elevation = 0;
GPGSVData.SatInfo[i].PRN = 0;
GPGSVData.SatInfo[i].SignalQuality = 0;
GPGSVData.SatInfo[i].UsedInSolution = FALSE;
}
GPGSVData.Count = 0;
//
// GPRMB
//
GPRMBData.DataStatus = 'V'; // A = data valid, V = navigation receiver warning
GPRMBData.CrosstrackError = 0.0; // nautical miles
GPRMBData.DirectionToSteer = '?'; // L/R
GPRMBData.OriginWaypoint[0] = '\0'; // Origin Waypoint ID
GPRMBData.DestWaypoint[0] = '\0'; // Destination waypoint ID
GPRMBData.DestLatitude = 0.0; // destination waypoint latitude
GPRMBData.DestLongitude = 0.0; // destination waypoint longitude
GPRMBData.RangeToDest = 0.0; // Range to destination nautical mi
GPRMBData.BearingToDest = 0.0; // Bearing to destination, degrees true
GPRMBData.DestClosingVelocity = 0.0; // Destination closing velocity, knots
GPRMBData.ArrivalStatus = 'V'; // A = arrival circle entered, V = not entered
GPRMBData.Count = 0; //
//
// GPRMC
//
GPRMCData.Hour = 0; //
GPRMCData.Minute = 0; //
GPRMCData.Second = 0; //
GPRMCData.DataValid = 'V'; // A = Data valid, V = navigation rx warning
GPRMCData.Latitude = 0.0; // current latitude
GPRMCData.Longitude = 0.0; // current longitude
GPRMCData.GroundSpeed = 0.0; // speed over ground, knots
GPRMCData.Course = 0.0; // course over ground, degrees true
GPRMCData.Day = 1; //
GPRMCData.Month = 1; //
GPRMCData.Year = 2000; //
GPRMCData.MagVar = 0.0; // magnitic variation, degrees East(+)/West(-)
GPRMCData.Count = 0; //
//
// GPZDA
//
GPZDAData.Hour = 0; //
GPZDAData.Minute = 0; //
GPZDAData.Second = 0; //
GPZDAData.Day = 1; // 1 - 31
GPZDAData.Month = 1; // 1 - 12
GPZDAData.Year = 2000; //
GPZDAData.LocalZoneHour = 0; // 0 to +/- 13
GPZDAData.LocalZoneMinute = 0; // 0 - 59
GPZDAData.Count = 0; //
}
///////////////////////////////////////////////////////////////////////////////
// ProcessNMEA: This method is the main state machine which processes individual
// bytes from the buffer and parses a NMEA sentence. A typical
// sentence is constructed as:
//
// $CMD,DDDD,DDDD,....DD*CS<CR><LF>
//
// Where:
// '$' HEX 24 Start of sentence
// 'CMD' Address/NMEA command
// ',DDDD' Zero or more data fields
// '*CS' Checksum field
// <CR><LF> Hex 0d 0A End of sentence
//
// When a valid sentence is received, this function sends the
// NMEA command and data to the ProcessCommand method for
// individual field parsing.
//
// NOTE:
//
///////////////////////////////////////////////////////////////////////////////
void ProcessNMEA(INT8U btData)
{
switch(m_nState)
{
///////////////////////////////////////////////////////////////////////
// Search for start of message '$'
case NP_STATE_SOM :
if(btData == '$')
{
m_btChecksum = 0; // reset checksum
m_wIndex = 0; // reset index
m_nState = NP_STATE_CMD;
}
break;
///////////////////////////////////////////////////////////////////////
// Retrieve command (NMEA Address)
case NP_STATE_CMD :
if(btData != ',' && btData != '*')
{
m_pCommand[m_wIndex++] = btData;
m_btChecksum ^= btData;
// Check for command overflow
if(m_wIndex >= NP_MAX_CMD_LEN)
{
m_nState = NP_STATE_SOM;
}
}
else
{
m_pCommand[m_wIndex] = '\0'; // terminate command
m_btChecksum ^= btData;
m_wIndex = 0;
m_nState = NP_STATE_DATA; // goto get data state
}
break;
///////////////////////////////////////////////////////////////////////
// Store data and check for end of sentence or checksum flag
case NP_STATE_DATA :
if(btData == '*') // checksum flag?
{
m_pData[m_wIndex] = '\0';
m_nState = NP_STATE_CHECKSUM_1;
}
else // no checksum flag, store data
{
//
// Check for end of sentence with no checksum
//
if(btData == '\r')
{
m_pData[m_wIndex] = '\0';
ProcessCommand(m_pCommand, m_pData);
m_nState = NP_STATE_SOM;
return;
}
//
// Store data and calculate checksum
//
m_btChecksum ^= btData;
m_pData[m_wIndex] = btData;
if(++m_wIndex >= NP_MAX_DATA_LEN) // Check for buffer overflow
{
m_nState = NP_STATE_SOM;
}
}
break;
///////////////////////////////////////////////////////////////////////
case NP_STATE_CHECKSUM_1 :
if( (btData - '0') <= 9)
{
m_btReceivedChecksum = (btData - '0') << 4;
}
else
{
m_btReceivedChecksum = (btData - 'A' + 10) << 4;
}
m_nState = NP_STATE_CHECKSUM_2;
break;
///////////////////////////////////////////////////////////////////////
case NP_STATE_CHECKSUM_2 :
if( (btData - '0') <= 9)
{
m_btReceivedChecksum |= (btData - '0');
}
else
{
m_btReceivedChecksum |= (btData - 'A' + 10);
}
if(m_btChecksum == m_btReceivedChecksum)
{
ProcessCommand(m_pCommand, m_pData);
}
m_nState = NP_STATE_SOM;
break;
///////////////////////////////////////////////////////////////////////
default : m_nState = NP_STATE_SOM;
}
}
///////////////////////////////////////////////////////////////////////////////
// Process NMEA sentence - Use the NMEA address (*pCommand) and call the
// appropriate sentense data prossor.
///////////////////////////////////////////////////////////////////////////////
BOOLEAN ProcessCommand(INT8U *pCommand, INT8U *pData)
{
//
// GPGGA
//
if( strcmp((char *)pCommand, "GPGGA") == NULL )
{
ProcessGPGGA(pData);
OSSemPost(GPS_Sem);
}
//
// GPGSA
//
else if( strcmp((char *)pCommand, "GPGSA") == NULL )
{
ProcessGPGSA(pData);
}
//
// GPGSV
//
else if( strcmp((char *)pCommand, "GPGSV") == NULL )
{
ProcessGPGSV(pData);
}
//
// GPRMB
//
else if( strcmp((char *)pCommand, "GPRMB") == NULL )
{
ProcessGPRMB(pData);
}
//
// GPRMC
//
else if( strcmp((char *)pCommand, "GPRMC") == NULL )
{
ProcessGPRMC(pData);
}
//
// GPZDA
//
else if( strcmp((char *)pCommand, "GPZDA") == NULL )
{
ProcessGPZDA(pData);
}
m_dwCommandCount++;
return TRUE;
}
///////////////////////////////////////////////////////////////////////////////
// Check to see if supplied satellite ID is used in the GPS solution.
// Retruned: BOOL - TRUE if satellate ID is used in solution
// FALSE if not used in solution.
///////////////////////////////////////////////////////////////////////////////
BOOLEAN IsSatUsedInSolution(INT16U wSatID)
{
INT32S i;
if(wSatID == 0) return FALSE;
for(i = 0; i < 12; i++)
{
if(wSatID == GPGSAData.SatsInSolution[i])
{
return TRUE;
}
}
return FALSE;
}
///////////////////////////////////////////////////////////////////////////////
// Name: GetField
//
// Description: This function will get the specified field in a NMEA string.
//
// Entry: INT8U *pData - Pointer to NMEA string
// INT8U *pField - pointer to returned field
// INT32S nfieldNum - Field offset to get
// INT32S nMaxFieldLen - Maximum of bytes pFiled can handle
///////////////////////////////////////////////////////////////////////////////
BOOLEAN GetField(INT8U *pData, INT8U *pField, INT32S nFieldNum, INT32S nMaxFieldLen)
{
INT32S i, i2, nField;
//
// Validate params
//
if(pData == NULL || pField == NULL || nMaxFieldLen <= 0)
{
return FALSE;
}
//
// Go to the beginning of the selected field
//
i = 0;
nField = 0;
while(nField != nFieldNum && pData[i])
{
if(pData[i] == ',')
{
nField++;
}
i++;
if(pData[i] == NULL)
{
pField[0] = '\0';
return FALSE;
}
}
if(pData[i] == ',' || pData[i] == '*')
{
pField[0] = '\0';
return FALSE;
}
//
// copy field from pData to Field
//
i2 = 0;
while(pData[i] != ',' && pData[i] != '*' && pData[i])
{
pField[i2] = pData[i];
i2++; i++;
//
// check if field is too big to fit on passed parameter. If it is,
// crop returned field to its max length.
//
if(i2 >= nMaxFieldLen)
{
i2 = nMaxFieldLen-1;
break;
}
}
pField[i2] = '\0';
return TRUE;
}
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
void ProcessGPZDA(INT8U *pData)
{
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';
GPZDAData.Hour = atoi(pBuff);
// minute
pBuff[0] = pField[2];
pBuff[1] = pField[3];
pBuff[2] = '\0';
GPZDAData.Minute = atoi(pBuff);
// Second
pBuff[0] = pField[4];
pBuff[1] = pField[5];
pBuff[2] = '\0';
GPZDAData.Second = atoi(pBuff);
}
//
// Day
//
if(GetField(pData, pField, 1, MAXFIELD))
{
GPZDAData.Day = atoi((char *)pField);
}
else
{
GPZDAData.Day = 1;
}
//
// Month
//
if(GetField(pData, pField, 2, MAXFIELD))
{
GPZDAData.Month = atoi((char *)pField);
}
else
{
GPZDAData.Month = 1;
}
//
// Year
//
if(GetField(pData, pField, 3, MAXFIELD))
{
GPZDAData.Year = atoi((char *)pField);
}
else
{
GPZDAData.Year = 1;
}
//
// Local zone hour
//
if(GetField(pData, pField, 4, MAXFIELD))
{
GPZDAData.LocalZoneHour = atoi((char *)pField);
}
else
{
GPZDAData.LocalZoneHour = 0;
}
//
// Local zone hour
//
if(GetField(pData, pField, 5, MAXFIELD))
{
GPZDAData.LocalZoneMinute = atoi((char *)pField);
}
else
{
GPZDAData.LocalZoneMinute = 0;
}
GPZDAData.Count++;
}
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
void ProcessGPRMC(INT8U *pData)
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -