/** * 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); } } }