mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-11 19:24:10 +01:00
f2431b2868
This change does remove any alarm when no port is configured for gps. Things depending on GPS will continue to raise their own alarm when no gps data are feed.
371 lines
11 KiB
C
371 lines
11 KiB
C
/**
|
|
******************************************************************************
|
|
* @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 <openpilot.h>
|
|
|
|
#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;
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|