rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

OP-1404 - Add support for UBlox PVT sentence

Browse Source
This commit is contained in:
Alessio Morale 2014-07-17 18:30:47 +02:00
parent 99f09d588f
commit 0976fb6e1b
2 changed files with 109 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -251,6 +251,47 @@ void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionSensorData *
}
}
void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPosition)
{
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);
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;
GpsPosition->PDOP = (float)pvt->pDOP * 0.01f;
} else {
GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
}
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;
GPSTimeSet(&GpsTime);
}
}
}
#if !defined(PIOS_GPS_MINIMAL)
void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc)
{
@ -324,6 +365,9 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
case UBX_ID_VELNED:
parse_ubx_nav_velned(&ubx->payload.nav_velned, GpsPosition);
break;
case UBX_ID_PVT:
parse_ubx_nav_pvt(&ubx->payload.nav_pvt, GpsPosition);
break;
#if !defined(PIOS_GPS_MINIMAL)
case UBX_ID_TIMEUTC:
parse_ubx_nav_timeutc(&ubx->payload.nav_timeutc);

75
flight/modules/GPS/inc/UBX.h
View File

@ -35,22 +35,23 @@
#include "GPS.h"
#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters
#define UBX_SYNC2 0x62
#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters
#define UBX_SYNC2 0x62
// From u-blox6 receiver protocol specification
// Messages classes
#define UBX_CLASS_NAV 0x01
#define UBX_CLASS_NAV 0x01
// Message IDs
#define UBX_ID_POSLLH 0x02
#define UBX_ID_STATUS 0x03
#define UBX_ID_DOP 0x04
#define UBX_ID_SOL 0x06
#define UBX_ID_VELNED 0x12
#define UBX_ID_TIMEUTC 0x21
#define UBX_ID_SVINFO 0x30
#define UBX_ID_POSLLH 0x02
#define UBX_ID_STATUS 0x03
#define UBX_ID_DOP 0x04
#define UBX_ID_SOL 0x06
#define UBX_ID_VELNED 0x12
#define UBX_ID_TIMEUTC 0x21
#define UBX_ID_SVINFO 0x30
#define UBX_ID_PVT 0x07
// private structures
@ -156,6 +157,59 @@ struct UBX_NAV_TIMEUTC {
uint8_t valid; // Validity Flags
};
#define PVT_VALID_VALIDDATE 0x01
#define PVT_VALID_VALIDTIME 0x02
#define PVT_VALID_FULLYRESOLVED 0x04
#define PVT_FIX_TYPE_NO_FIX 0
#define PVT_FIX_TYPE_DEAD_RECKON 0x01 // Dead Reckoning only
#define PVT_FIX_TYPE_2D 0x02 // 2D-Fix
#define PVT_FIX_TYPE_3D 0x03 // 3D-Fix
#define PVT_FIX_TYPE_GNSS_DEAD_RECKON 0x04 // GNSS + dead reckoning combined
#define PVT_FIX_TYPE_TIME_ONLY 0x05 // Time only fix
#define PVT_FLAGS_GNNSFIX_OK (1 << 0)
#define PVT_FLAGS_DIFFSOLN (1 << 1)
#define PVT_FLAGS_PSMSTATE_ENABLED (1 << 2)
#define PVT_FLAGS_PSMSTATE_ACQUISITION (2 << 2)
#define PVT_FLAGS_PSMSTATE_TRACKING (3 << 2)
#define PVT_FLAGS_PSMSTATE_PO_TRACKING (4 << 2)
#define PVT_FLAGS_PSMSTATE_INACTIVE (5 << 2)
// PVT Navigation Position Velocity Time Solution
struct UBX_NAV_PVT {
uint32_t iTOW;
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t min;
uint8_t sec;
uint8_t valid;
uint32_t tAcc;
int32_t nano;
uint8_t fixType;
uint8_t flags;
uint8_t reserved1;
uint8_t numSV;
int32_t lon;
int32_t lat;
int32_t height;
int32_t hMSL;
uint32_t hAcc;
uint32_t vAcc;
int32_t velN;
int32_t velE;
int32_t velD;
int32_t gSpeed;
int32_t heading;
uint32_t sAcc;
uint32_t headingAcc;
uint32_t pDOP;
uint16_t reserved2;
uint32_t reserved3;
};
// Space Vehicle (SV) Information
// Single SV information block
@ -198,6 +252,7 @@ typedef union {
struct UBX_NAV_DOP nav_dop;
struct UBX_NAV_SOL nav_sol;
struct UBX_NAV_VELNED nav_velned;
struct UBX_NAV_PVT nav_pvt;
#if !defined(PIOS_GPS_MINIMAL)
struct UBX_NAV_TIMEUTC nav_timeutc;
struct UBX_NAV_SVINFO nav_svinfo;