/**
* Initialise the gps module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t GPSInitialize(void)
{
gpsPort = PIOS_COM_GPS;
uint8_t gpsProtocol;
#ifdef MODULE_GPS_BUILTIN
module_enabled = true;
#else
uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_state);
if (module_state[MODULESETTINGS_ADMINSTATE_GPS] == MODULESETTINGS_ADMINSTATE_ENABLED) {
module_enabled = true;
} else {
module_enabled = false;
}
#endif
// These things are only conditional on small F1 targets.
// Expected to be always present otherwise.
#ifdef SMALLF1
if (gpsPort && module_enabled) {
#endif
GPSPositionInitialize();
GPSVelocityInitialize();
#if !defined(PIOS_GPS_MINIMAL)
GPSTimeInitialize();
GPSSatellitesInitialize();
HomeLocationInitialize();
UBloxInfoInitialize();
#endif
#if defined(PIOS_GPS_PROVIDES_AIRSPEED)
AirspeedActualInitialize();
#endif
updateSettings();
#ifdef SMALLF1
}
#endif
if (gpsPort && module_enabled) {
ModuleSettingsGPSDataProtocolGet(&gpsProtocol);
switch (gpsProtocol) {
case MODULESETTINGS_GPSDATAPROTOCOL_NMEA:
gps_rx_buffer = PIOS_malloc(NMEA_MAX_PACKET_LENGTH);
break;
case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
gps_rx_buffer = PIOS_malloc(sizeof(struct UBXPacket));
break;
default:
gps_rx_buffer = NULL;
}
PIOS_Assert(gps_rx_buffer);
return 0;
}
return -1;
}
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);
}
}
}
/**
* Initialise the gps module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t GPSInitialize(void)
{
gpsPort = PIOS_COM_GPS;
uint8_t gpsProtocol;
#ifdef MODULE_GPS_BUILTIN
module_enabled = true;
#else
uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_state);
if (module_state[MODULESETTINGS_ADMINSTATE_GPS] == MODULESETTINGS_ADMINSTATE_ENABLED) {
module_enabled = true;
} else {
module_enabled = false;
}
#endif
#if defined(REVOLUTION)
// These objects MUST be initialized for Revolution
// because the rest of the system expects to just
// attach to their queues
GPSPositionInitialize();
GPSVelocityInitialize();
GPSTimeInitialize();
GPSSatellitesInitialize();
HomeLocationInitialize();
UBloxInfoInitialize();
updateSettings();
#else
if (gpsPort && module_enabled) {
GPSPositionInitialize();
GPSVelocityInitialize();
#if !defined(PIOS_GPS_MINIMAL)
GPSTimeInitialize();
GPSSatellitesInitialize();
#endif
#if defined(PIOS_GPS_PROVIDES_AIRSPEED)
AirspeedActualInitialize();
#endif
updateSettings();
}
#endif
if (gpsPort && module_enabled) {
ModuleSettingsGPSDataProtocolGet(&gpsProtocol);
switch (gpsProtocol) {
case MODULESETTINGS_GPSDATAPROTOCOL_NMEA:
gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH);
break;
case MODULESETTINGS_GPSDATAPROTOCOL_UBX:
gps_rx_buffer = pvPortMalloc(sizeof(struct UBXPacket));
break;
default:
gps_rx_buffer = NULL;
}
PIOS_Assert(gps_rx_buffer);
return 0;
}
return -1;
}
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);
}
}
}
