示例#1
0
文件: GPS.c 项目: EvalZero/TauLabs
static void gpsConfigure(uint8_t gpsProtocol)
{
	ModuleSettingsGPSAutoConfigureOptions gpsAutoConfigure;
	ModuleSettingsGPSAutoConfigureGet(&gpsAutoConfigure);

	if (gpsAutoConfigure != MODULESETTINGS_GPSAUTOCONFIGURE_TRUE) {
		return;
	}

#if !defined(PIOS_GPS_MINIMAL)
	switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
		case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
		{
			// 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;
			ModuleSettingsGPSConstellationOptions constellation;
			ModuleSettingsGPSSBASConstellationOptions sbas_const;
			ModuleSettingsGPSDynamicsModeOptions dyn_mode;

			ModuleSettingsGPSSpeedGet(&baud_rate);
			ModuleSettingsGPSConstellationGet(&constellation);
			ModuleSettingsGPSSBASConstellationGet(&sbas_const);
			ModuleSettingsGPSDynamicsModeGet(&dyn_mode);

			ubx_cfg_set_baudrate(gpsPort, baud_rate);

			PIOS_Thread_Sleep(1000);

			ubx_cfg_send_configuration(gpsPort, gps_rx_buffer,
					constellation, sbas_const, dyn_mode);
		}
		break;
#endif
	}
#endif /* PIOS_GPS_MINIMAL */
}
示例#2
0
文件: GPS.c 项目: 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);
		}

	}
}