/** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup GSPModule GPS Module * @brief Process GPS information * @{ * * @file GPS.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief GPS module, handles GPS and NMEA stream * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. * * You should have received a copy of the GNU General Public License along * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ // **************** #include #include "gpsposition.h" #include "homelocation.h" #include "gpstime.h" #include "gpssatellites.h" #include "gpsvelocity.h" #include "systemsettings.h" #include "taskinfo.h" #include "hwsettings.h" #include "WorldMagModel.h" #include "CoordinateConversions.h" #include "GPS.h" #include "NMEA.h" #include "UBX.h" // **************** // Private functions static void gpsTask(void *parameters); static void updateSettings(); #ifdef PIOS_GPS_SETS_HOMELOCATION static void setHomeLocation(GPSPositionData *gpsData); static float GravityAccel(float latitude, float longitude, float altitude); #endif // **************** // Private constants #define GPS_TIMEOUT_MS 500 #ifdef PIOS_GPS_SETS_HOMELOCATION // Unfortunately need a good size stack for the WMM calculation #define STACK_SIZE_BYTES 850 #else #if defined(PIOS_GPS_MINIMAL) #define STACK_SIZE_BYTES 500 #else #define STACK_SIZE_BYTES 650 #endif // PIOS_GPS_MINIMAL #endif // PIOS_GPS_SETS_HOMELOCATION #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // **************** // Private variables static uint32_t gpsPort; static bool gpsEnabled = false; static xTaskHandle gpsTaskHandle; static char *gps_rx_buffer; static uint32_t timeOfLastCommandMs; static uint32_t timeOfLastUpdateMs; #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) static struct GPS_RX_STATS gpsRxStats; #endif // **************** /** * Initialise the gps module * \return -1 if initialisation failed * \return 0 on success */ int32_t GPSStart(void) { if (gpsEnabled) { if (gpsPort) { // Start gps task xTaskCreate(gpsTask, (signed char *)"GPS", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &gpsTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_GPS, gpsTaskHandle); return 0; } } return -1; } /** * 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 gpsEnabled = true; #else HwSettingsInitialize(); uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; HwSettingsOptionalModulesGet(optionalModules); if (optionalModules[HWSETTINGS_OPTIONALMODULES_GPS] == HWSETTINGS_OPTIONALMODULES_ENABLED) { gpsEnabled = true; } else { gpsEnabled = 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(); updateSettings(); #else if (gpsPort && gpsEnabled) { GPSPositionInitialize(); GPSVelocityInitialize(); #if !defined(PIOS_GPS_MINIMAL) GPSTimeInitialize(); GPSSatellitesInitialize(); #endif #ifdef PIOS_GPS_SETS_HOMELOCATION HomeLocationInitialize(); #endif updateSettings(); } #endif /* if defined(REVOLUTION) */ if (gpsPort && gpsEnabled) { SystemSettingsInitialize(); SystemSettingsGPSDataProtocolGet(&gpsProtocol); switch (gpsProtocol) { case SYSTEMSETTINGS_GPSDATAPROTOCOL_NMEA: gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); break; case SYSTEMSETTINGS_GPSDATAPROTOCOL_UBX: gps_rx_buffer = pvPortMalloc(sizeof(struct UBXPacket)); break; default: gps_rx_buffer = NULL; } PIOS_Assert(gps_rx_buffer); return 0; } return -1; } MODULE_INITCALL(GPSInitialize, GPSStart); // **************** /** * Main gps task. It does not return. */ static void gpsTask(__attribute__((unused)) void *parameters) { portTickType xDelay = 100 / portTICK_RATE_MS; uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; GPSPositionData gpsposition; uint8_t gpsProtocol; SystemSettingsGPSDataProtocolGet(&gpsProtocol); timeOfLastUpdateMs = timeNowMs; timeOfLastCommandMs = timeNowMs; 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 SYSTEMSETTINGS_GPSDATAPROTOCOL_NMEA: res = parse_nmea_stream(c, gps_rx_buffer, &gpsposition, &gpsRxStats); break; #endif #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) case SYSTEMSETTINGS_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 = xTaskGetTickCount() * portTICK_RATE_MS; timeOfLastUpdateMs = timeNowMs; timeOfLastCommandMs = timeNowMs; } } // 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. 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)) { 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) { AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); } } } } #ifdef PIOS_GPS_SETS_HOMELOCATION /* * Estimate the acceleration due to gravity for a particular location in LLA */ static float GravityAccel(float latitude, __attribute__((unused)) float longitude, float altitude) { /* WGS84 gravity model. The effect of gravity over latitude is strong * enough to change the estimated accelerometer bias in those apps. */ float sinsq = sinf(latitude); sinsq *= sinsq; /* Likewise, over the altitude range of a high-altitude balloon, the effect * due to change in altitude can also affect the model. */ return 9.7803267714f * (1.0f + 0.00193185138639f * sinsq) / sqrtf(1.0f - 0.00669437999013f * sinsq) - 3.086e-6f * altitude; } // **************** static void setHomeLocation(GPSPositionData *gpsData) { HomeLocationData home; HomeLocationGet(&home); GPSTimeData gps; GPSTimeGet(&gps); if (gps.Year >= 2000) { /* Store LLA */ home.Latitude = gpsData->Latitude; home.Longitude = gpsData->Longitude; home.Altitude = gpsData->Altitude + gpsData->GeoidSeparation; /* Compute home ECEF coordinates and the rotation matrix into NED * Note that floats are used here - they should give enough precision * for this application.*/ float LLA[3] = { (home.Latitude) / 10e6f, (home.Longitude) / 10e6f, (home.Altitude) }; /* Compute magnetic flux direction at home location */ if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) == 0) { /*Compute local acceleration due to gravity. Vehicles that span a very large * range of altitude (say, weather balloons) may need to update this during the * flight. */ home.g_e = GravityAccel(LLA[0], LLA[1], LLA[2]); home.Set = HOMELOCATION_SET_TRUE; HomeLocationSet(&home); } } } #endif /* ifdef PIOS_GPS_SETS_HOMELOCATION */ /** * Update the GPS settings, called on startup. * FIXME: This should be in the GPSSettings object. But objects have * too much overhead yet. Also the GPS has no any specific settings * like protocol, etc. Thus the HwSettings object which contains the * GPS port speed is used for now. */ static void updateSettings() { if (gpsPort) { // Retrieve settings uint8_t speed; HwSettingsGPSSpeedGet(&speed); // Set port speed switch (speed) { case HWSETTINGS_GPSSPEED_2400: PIOS_COM_ChangeBaud(gpsPort, 2400); break; case HWSETTINGS_GPSSPEED_4800: PIOS_COM_ChangeBaud(gpsPort, 4800); break; case HWSETTINGS_GPSSPEED_9600: PIOS_COM_ChangeBaud(gpsPort, 9600); break; case HWSETTINGS_GPSSPEED_19200: PIOS_COM_ChangeBaud(gpsPort, 19200); break; case HWSETTINGS_GPSSPEED_38400: PIOS_COM_ChangeBaud(gpsPort, 38400); break; case HWSETTINGS_GPSSPEED_57600: PIOS_COM_ChangeBaud(gpsPort, 57600); break; case HWSETTINGS_GPSSPEED_115200: PIOS_COM_ChangeBaud(gpsPort, 115200); break; } } } /** * @} * @} */