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);
}
}
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...
//http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
if (gpsposition.PDOP < 3.5f &&
gpsposition.Satellites >= 7 &&
(gpsposition.Status == GPSPOSITION_STATUS_FIX3D ||
gpsposition.Status == GPSPOSITION_STATUS_DIFF3D)) {
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
#ifdef PIOS_GPS_SETS_HOMELOCATION
HomeLocationData home;
HomeLocationGet(&home);
if (home.Set == HOMELOCATION_SET_FALSE)
setHomeLocation(&gpsposition);
#endif
} 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);
}
}
}
