Exemple #1
0
int Compose_Pressure_POV_slow(char *sentence, float static_pressure, float dynamic_pressure)
{
	int length;
	int success = 1;

	// check static_pressure input value for validity
	if ((static_pressure < 0) || (static_pressure > 2000))
	{
		static_pressure = 9999;
		success = -10;
	}
	
	// check dynamic_pressure input value for validity
	/// @todo add useful range for dynamic_pressure vales !!
	if ((dynamic_pressure < -999.0) || (dynamic_pressure > 9998.0))
	{
		dynamic_pressure = 9999;
		success = -20;
	}

	// compose NMEA String
	length = sprintf(sentence, "$POV,P,%+07.2f,Q,%+05.2f", static_pressure, (dynamic_pressure)); 
	
	// Calculate NMEA checksum and add to string
	sprintf(sentence + length, "*%02X\n", NMEA_checksum(sentence));
	
	//print sentence for debug
	debug_print("POV slow NMEA sentence: %s\n", sentence);
	return (success);
}
Exemple #2
0
int Compose_Pressure_POV_fast(char *sentence, float te_vario)
{
	int length;
	int success = 1;

	// check te_vario input value for validity
	if ((te_vario < -50) || (te_vario > 50))
	{
		te_vario = 99;
		success = -10;
	}
	
	// compose NMEA String
	length = sprintf(sentence, "$POV,E,%+05.2f", te_vario); 
	
	// Calculate NMEA checksum and add to string
	sprintf(sentence + length, "*%02X\n", NMEA_checksum(sentence));
	
	//print sentence for debug
	debug_print("NMEA sentence: %s\n", sentence);
	return (success);
}
Exemple #3
0
int Compose_Voltage_POV(char *sentence, float voltage)
{
	int length;
	int success = 1;

	// check voltage input value for validity
	if ((voltage < 2.) || (voltage > 20.))
	{
		voltage = 0.;
		success = -10;
	}
	
	// compose NMEA String
	length = sprintf(sentence, "$POV,V,%+05.2f", voltage); 
	
	// Calculate NMEA checksum and add to string
	sprintf(sentence + length, "*%02X\n", NMEA_checksum(sentence));
	
	//print sentence for debug
	debug_print("NMEA sentence: %s\n", sentence);
	return (success);
}
Exemple #4
0
static void gpsTask(void *parameters)
{
	portTickType xDelay = 100 / portTICK_RATE_MS;
	uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
	GPSPositionData GpsData;
	
#ifdef ENABLE_GPS_BINARY_GTOP
	GTOP_BIN_init();
#else
	uint8_t rx_count = 0;
	bool start_flag = false;
	bool found_cr = false;
	int32_t gpsRxOverflow = 0;
#endif
	
#ifdef FULL_COLD_RESTART
	// tell the GPS to do a FULL COLD restart
	PIOS_COM_SendStringNonBlocking(gpsPort, "$PMTK104*37\r\n");
	timeOfLastCommandMs = timeNowMs;
	while (timeNowMs - timeOfLastCommandMs < 300)	// delay for 300ms to let the GPS sort itself out
	{
		vTaskDelay(xDelay);	// Block task until next update
		timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
	}
#endif

#ifdef DISABLE_GPS_TRESHOLD
	PIOS_COM_SendStringNonBlocking(gpsPort, "$PMTK397,0*23\r\n");
#endif

#ifdef ENABLE_GPS_BINARY_GTOP
	// switch to GTOP binary mode
	PIOS_COM_SendStringNonBlocking(gpsPort ,"$PGCMD,21,1*6F\r\n");
#endif
	
#ifdef ENABLE_GPS_ONESENTENCE_GTOP
	// switch to single sentence mode
	PIOS_COM_SendStringNonBlocking(gpsPort, "$PGCMD,21,2*6C\r\n");
#endif

#ifdef ENABLE_GPS_NMEA
	// switch to NMEA mode
	PIOS_COM_SendStringNonBlocking(gpsPort, "$PGCMD,21,3*6D\r\n");
#endif

	numUpdates = 0;
	numChecksumErrors = 0;
	numParsingErrors = 0;

	timeOfLastUpdateMs = timeNowMs;
	timeOfLastCommandMs = timeNowMs;

	// Loop forever
	while (1)
	{
		#ifdef ENABLE_GPS_BINARY_GTOP
			// GTOP BINARY GPS mode

			while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0)
			{
				int res = GTOP_BIN_update_position(PIOS_COM_ReceiveBuffer(gpsPort), &numChecksumErrors, &numParsingErrors);
				if (res >= 0)
				{
					numUpdates++;

					timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
					timeOfLastUpdateMs = timeNowMs;
					timeOfLastCommandMs = timeNowMs;
				}
			}

		#else
			// NMEA or SINGLE-SENTENCE GPS mode

			// This blocks the task until there is something on the buffer
			while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0)
			{
				char c = PIOS_COM_ReceiveBuffer(gpsPort);
			
				// detect start while acquiring stream
				if (!start_flag && (c == '$'))
				{
					start_flag = true;
					found_cr = false;
					rx_count = 0;
				}
				else
				if (!start_flag)
					continue;
			
				if (rx_count >= sizeof(gps_rx_buffer))
				{
					// The buffer is already full and we haven't found a valid NMEA sentence.
					// Flush the buffer and note the overflow event.
					gpsRxOverflow++;
					start_flag = false;
					found_cr = false;
					rx_count = 0;
				}
				else
				{
					gps_rx_buffer[rx_count] = c;
					rx_count++;
				}
			
				// look for ending '\r\n' sequence
				if (!found_cr && (c == '\r') )
					found_cr = true;
				else
				if (found_cr && (c != '\n') )
					found_cr = false;  // false end flag
				else
				if (found_cr && (c == '\n') )
				{
					// The NMEA functions require a zero-terminated string
					// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
					gps_rx_buffer[rx_count-2] = 0;

					// prepare to parse next sentence
					start_flag = false;
					found_cr = false;
					rx_count = 0;
					// Our rxBuffer must look like this now:
					//   [0]           = '$'
					//   ...           = zero or more bytes of sentence payload
					//   [end_pos - 1] = '\r'
					//   [end_pos]     = '\n'
					//
					// Prepare to consume the sentence from the buffer
				
					// Validate the checksum over the sentence
					if (!NMEA_checksum(&gps_rx_buffer[1]))
					{	// Invalid checksum.  May indicate dropped characters on Rx.
						PIOS_DEBUG_PinHigh(2);
						++numChecksumErrors;
						PIOS_DEBUG_PinLow(2);
					}
					else
					{	// Valid checksum, use this packet to update the GPS position
						if (!NMEA_update_position(&gps_rx_buffer[1])) {
							PIOS_DEBUG_PinHigh(2);
							++numParsingErrors;
							PIOS_DEBUG_PinLow(2);
						}
						else
							++numUpdates;

						timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
						timeOfLastUpdateMs = timeNowMs;
						timeOfLastCommandMs = timeNowMs;
					}
				}
			}
		#endif

		// Check for GPS timeout
		timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
		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.

			GPSPositionGet(&GpsData);
			GpsData.Status = GPSPOSITION_STATUS_NOGPS;
			GPSPositionSet(&GpsData);
			AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);

			if ((timeNowMs - timeOfLastCommandMs) >= GPS_COMMAND_RESEND_TIMEOUT_MS)
			{	// resend the command .. just incase the gps has only just been plugged in or the gps did not get our last command
				timeOfLastCommandMs = timeNowMs;

				#ifdef ENABLE_GPS_BINARY_GTOP
					GTOP_BIN_init();
					// switch to binary mode
					PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,1*6F\r\n");
				#endif

				#ifdef ENABLE_GPS_ONESENTENCE_GTOP
					// switch to single sentence mode
					PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,2*6C\r\n");
				#endif

				#ifdef ENABLE_GPS_NMEA
					// switch to NMEA mode
					PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,3*6D\r\n");
				#endif

				#ifdef DISABLE_GPS_TRESHOLD
					PIOS_COM_SendStringNonBlocking(gpsPort,"$PMTK397,0*23\r\n");
				#endif
			}
		}
		else
		{	// we appear to be receiving GPS sentences OK, we've had an update

			HomeLocationData home;
			HomeLocationGet(&home);

			GPSPositionGet(&GpsData);
			if ((GpsData.Status == GPSPOSITION_STATUS_FIX3D) && (home.Set == HOMELOCATION_SET_FALSE))
				setHomeLocation(&GpsData);

			//criteria for GPS-OK taken from this post...
			//http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
			if ((GpsData.PDOP < 3.5) && (GpsData.Satellites >= 7))
				AlarmsClear(SYSTEMALARMS_ALARM_GPS);
			else
			if (GpsData.Status == GPSPOSITION_STATUS_FIX3D)
				AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
			else
				AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
		}

		// Block task until next update
		vTaskDelay(xDelay);
	}
}