rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-1404 - PVT will provide SOL+VELNED+POSLLH+Time. DOP will provide all dop values

Browse Source
This commit is contained in:
Alessio Morale 2014-08-03 15:04:40 +02:00
parent 1dd05c24cc
commit c68dd3c00e
1 changed files with 30 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
flight/modules/GPS/UBX.c
View File

@ -255,43 +255,42 @@ void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPositi
{
GPSVelocitySensorData GpsVelocity;
if (check_msgtracker(pvt->iTOW, ALL_RECEIVED)) {
if ((pvt->fixType == PVT_FIX_TYPE_3D) ||
(pvt->fixType == PVT_FIX_TYPE_2D)) {
GpsVelocity.North = (float)pvt->velN / 100.0f;
GpsVelocity.East = (float)pvt->velE / 100.0f;
GpsVelocity.Down = (float)pvt->velD / 100.0f;
GPSVelocitySensorSet(&GpsVelocity);
msgtracker.currentTOW = pvt->iTOW;
msgtracker.msg_received = (SOL_RECEIVED | VELNED_RECEIVED | POSLLH_RECEIVED);
GpsPosition->Groundspeed = (float)pvt->gSpeed * 0.01f;
GpsPosition->Heading = (float)pvt->heading * 1.0e-5f;
GpsPosition->Altitude = (float)pvt->hMSL * 0.001f;
GpsPosition->GeoidSeparation = (float)(pvt->height - pvt->hMSL) * 0.001f;
GpsPosition->Latitude = pvt->lat;
GpsPosition->Longitude = pvt->lon;
GpsPosition->Status = pvt->fixType == PVT_FIX_TYPE_3D ?
GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D;
GpsVelocity.North = (float)pvt->velN / 100.0f;
GpsVelocity.East = (float)pvt->velE / 100.0f;
GpsVelocity.Down = (float)pvt->velD / 100.0f;
GPSVelocitySensorSet(&GpsVelocity);
GpsPosition->PDOP = (float)pvt->pDOP * 0.01f;
} else {
GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
}
GpsPosition->Groundspeed = (float)pvt->gSpeed * 0.01f;
GpsPosition->Heading = (float)pvt->heading * 1.0e-5f;
GpsPosition->Altitude = (float)pvt->hMSL * 0.001f;
GpsPosition->GeoidSeparation = (float)(pvt->height - pvt->hMSL) * 0.001f;
GpsPosition->Latitude = pvt->lat;
GpsPosition->Longitude = pvt->lon;
GpsPosition->Satellites = pvt->numSV;
if(pvt->flags & PVT_FLAGS_GNNSFIX_OK) {
GpsPosition->Status = pvt->fixType == PVT_FIX_TYPE_3D ?
GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D;
} else {
GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
}
#if !defined(PIOS_GPS_MINIMAL)
if (pvt->valid & PVT_VALID_VALIDTIME) {
// Time is valid, set GpsTime
GPSTimeData GpsTime;
if (pvt->valid & PVT_VALID_VALIDTIME) {
// Time is valid, set GpsTime
GPSTimeData GpsTime;
GpsTime.Year = pvt->year;
GpsTime.Month = pvt->month;
GpsTime.Day = pvt->day;
GpsTime.Hour = pvt->hour;
GpsTime.Minute = pvt->min;
GpsTime.Second = pvt->sec;
GpsTime.Year = pvt->year;
GpsTime.Month = pvt->month;
GpsTime.Day = pvt->day;
GpsTime.Hour = pvt->hour;
GpsTime.Minute = pvt->min;
GpsTime.Second = pvt->sec;
GPSTimeSet(&GpsTime);
}
GPSTimeSet(&GpsTime);
}
#endif
}
}
#if !defined(PIOS_GPS_MINIMAL)
void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc)