C++ (Cpp) parse_nmea_stream Examples

Example #1

File: GPS.c Project: 1heinz/TauLabs

static void gpsTask(void *parameters)
{
	portTickType xDelay = MS2TICKS(GPS_COM_TIMEOUT_MS);
	uint32_t timeNowMs = TICKS2MS(xTaskGetTickCount());

	GPSPositionData gpsposition;
	uint8_t	gpsProtocol;

	ModuleSettingsGPSDataProtocolGet(&gpsProtocol);

#if defined(PIOS_GPS_PROVIDES_AIRSPEED)
	gps_airspeed_initialize();
#endif

	timeOfLastUpdateMs = timeNowMs;
	timeOfLastCommandMs = timeNowMs;


#if !defined(PIOS_GPS_MINIMAL)
	switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
		case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
		{
			uint8_t gpsAutoConfigure;
			ModuleSettingsGPSAutoConfigureGet(&gpsAutoConfigure);

			if (gpsAutoConfigure == MODULESETTINGS_GPSAUTOCONFIGURE_TRUE) {

				// Wait for power to stabilize before talking to external devices
				vTaskDelay(MS2TICKS(1000));

				// Runs through a number of possible GPS baud rates to
				// configure the ublox baud rate. This uses a NMEA string
				// so could work for either UBX or NMEA actually. This is
				// somewhat redundant with updateSettings below, but that
				// is only called on startup and is not an issue.
				ModuleSettingsGPSSpeedOptions baud_rate;
				ModuleSettingsGPSSpeedGet(&baud_rate);
				ubx_cfg_set_baudrate(gpsPort, baud_rate);

				vTaskDelay(MS2TICKS(1000));

				ubx_cfg_send_configuration(gpsPort, gps_rx_buffer);
			}
		}
			break;
#endif
	}
#endif /* PIOS_GPS_MINIMAL */

	GPSPositionGet(&gpsposition);
	// Loop forever
	while (1)
	{
		uint8_t c;

		// This blocks the task until there is something on the buffer
		while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0)
		{
			int res;
			switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
				case MODULESETTINGS_GPSDATAPROTOCOL_NMEA:
					res = parse_nmea_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
					break;
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
				case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
					res = parse_ubx_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
					break;
#endif
				default:
					res = NO_PARSER; // this should not happen
					break;
			}

			if (res == PARSER_COMPLETE) {
				timeNowMs = TICKS2MS(xTaskGetTickCount());
				timeOfLastUpdateMs = timeNowMs;
				timeOfLastCommandMs = timeNowMs;
			}
		}

		// Check for GPS timeout
		timeNowMs = TICKS2MS(xTaskGetTickCount());
		if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) {
			// we have not received any valid GPS sentences for a while.
			// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
			uint8_t status = GPSPOSITION_STATUS_NOGPS;
			GPSPositionStatusSet(&status);
			AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
		} else {
			// we appear to be receiving GPS sentences OK, we've had an update
			//criteria for GPS-OK taken from this post
			if (gpsposition.PDOP < 3.5f && 
			    gpsposition.Satellites >= 7 &&
			    (gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
			         gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)) {
				AlarmsClear(SYSTEMALARMS_ALARM_GPS);
			} else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
			           gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)
						AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
					else
						AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
		}

	}
}

Example #2

File: GPS.c Project: EvalZero/TauLabs

static void gpsTask(void *parameters)
{
	GPSPositionData gpsposition;

	uint32_t timeOfLastUpdateMs = 0;
	uint32_t timeOfConfigAttemptMs = 0;

	uint8_t	gpsProtocol;

#ifdef PIOS_GPS_PROVIDES_AIRSPEED
	gps_airspeed_initialize();
#endif

	GPSPositionGet(&gpsposition);

	// Wait for power to stabilize before talking to external devices
	PIOS_Thread_Sleep(1000);

	// Loop forever
	while (1) {
		uint32_t xDelay = GPS_COM_TIMEOUT_MS;

		uint32_t loopTimeMs = PIOS_Thread_Systime();

		// XXX TODO: also on modulesettings change..
		if (!timeOfConfigAttemptMs) {
			ModuleSettingsGPSDataProtocolGet(&gpsProtocol);

			gpsConfigure(gpsProtocol);
			timeOfConfigAttemptMs = PIOS_Thread_Systime();

			continue;
		}

		uint8_t c;

		// This blocks the task until there is something on the buffer
		while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0)
		{
			int res;
			switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
				case MODULESETTINGS_GPSDATAPROTOCOL_NMEA:
					res = parse_nmea_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
					break;
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
				case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
					res = parse_ubx_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
					break;
#endif
				default:
					res = NO_PARSER; // this should not happen
					break;
			}

			if (res == PARSER_COMPLETE) {
				timeOfLastUpdateMs = loopTimeMs;
			}

			xDelay = 0;	// For now on, don't block / wait,
					// but consume what we can from the fifo
		}

		// Check for GPS timeout
		if ((loopTimeMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) {
			// we have not received any valid GPS sentences for a while.
			// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
			uint8_t status = GPSPOSITION_STATUS_NOGPS;
			GPSPositionStatusSet(&status);
			AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
			/* Don't reinitialize too often. */
			if ((loopTimeMs - timeOfConfigAttemptMs) >= GPS_TIMEOUT_MS) {
				timeOfConfigAttemptMs = 0; // reinit next loop
			}
		} else {
			// we appear to be receiving GPS sentences OK, we've had an update
			//criteria for GPS-OK taken from this post
			if (gpsposition.PDOP < 3.5f && 
			    gpsposition.Satellites >= 7 &&
			    (gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
			         gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)) {
				AlarmsClear(SYSTEMALARMS_ALARM_GPS);
			} else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
			           gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)
						AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
					else
						AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
		}

	}
}