rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/amorale/OP-1404_ublox_pvt_support' into next

Browse Source
This commit is contained in:
Alessio Morale 2014-08-13 15:27:10 +02:00
commit 5dbbda3adc
4 changed files with 145 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flight/modules/GPS/NMEA.c
View File

@ -687,8 +687,8 @@ static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionSensorData *GpsD
// Get sat info // Get sat info
gsv_partial.PRN[sat_index] = atoi(param[parIdx++]); gsv_partial.PRN[sat_index] = atoi(param[parIdx++]);
gsv_partial.Elevation[sat_index] = NMEA_real_to_float(param[parIdx++]); gsv_partial.Elevation[sat_index] = atoi(param[parIdx++]);
gsv_partial.Azimuth[sat_index] = NMEA_real_to_float(param[parIdx++]); gsv_partial.Azimuth[sat_index] = atoi(param[parIdx++]);
gsv_partial.SNR[sat_index] = atoi(param[parIdx++]); gsv_partial.SNR[sat_index] = atoi(param[parIdx++]);
#ifdef NMEA_DEBUG_GSV #ifdef NMEA_DEBUG_GSV
DEBUG_MSG(" %d", gsv_partial.PRN[sat_index]); DEBUG_MSG(" %d", gsv_partial.PRN[sat_index]);

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

@ -32,10 +32,11 @@
#include "pios.h" #include "pios.h"
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
#include "inc/UBX.h"
#include "inc/GPS.h"
#include "UBX.h" // If a PVT sentence is received in the last UBX_PVT_TIMEOUT (ms) timeframe it disables VELNED/POSLLH/SOL/TIMEUTC
#include "GPS.h" #define UBX_PVT_TIMEOUT (1000)
// parse incoming character stream for messages in UBX binary format // parse incoming character stream for messages in UBX binary format
int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats) int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
@ -251,6 +252,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;
check_msgtracker(pvt->iTOW, (ALL_RECEIVED));
GpsVelocity.North = (float)pvt->velN * 0.001f;
GpsVelocity.East = (float)pvt->velE * 0.001f;
GpsVelocity.Down = (float)pvt->velD * 0.001f;
GPSVelocitySensorSet(&GpsVelocity);
GpsPosition->Groundspeed = (float)pvt->gSpeed * 0.001f;
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;
GpsPosition->PDOP = pvt->pDOP * 0.01f;
if (pvt->flags & PVT_FLAGS_GNSSFIX_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;
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);
}
#endif
}
#if !defined(PIOS_GPS_MINIMAL) #if !defined(PIOS_GPS_MINIMAL)
void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc) void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc)
{ {
@ -283,8 +325,8 @@ void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo)
svdata.SatsInView = 0; svdata.SatsInView = 0;
for (chan = 0; chan < svinfo->numCh; chan++) { for (chan = 0; chan < svinfo->numCh; chan++) {
if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM) { if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM) {
svdata.Azimuth[svdata.SatsInView] = (float)svinfo->sv[chan].azim; svdata.Azimuth[svdata.SatsInView] = svinfo->sv[chan].azim;
svdata.Elevation[svdata.SatsInView] = (float)svinfo->sv[chan].elev; svdata.Elevation[svdata.SatsInView] = svinfo->sv[chan].elev;
svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid; svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid;
svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno; svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno;
svdata.SatsInView++; svdata.SatsInView++;
@ -292,8 +334,8 @@ void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo)
} }
// fill remaining slots (if any) // fill remaining slots (if any)
for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) { for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) {
svdata.Azimuth[chan] = (float)0.0f; svdata.Azimuth[chan] = 0;
svdata.Elevation[chan] = (float)0.0f; svdata.Elevation[chan] = 0;
svdata.PRN[chan] = 0; svdata.PRN[chan] = 0;
svdata.SNR[chan] = 0; svdata.SNR[chan] = 0;
} }
@ -308,26 +350,51 @@ void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo)
uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition) uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
{ {
uint32_t id = 0; uint32_t id = 0;
static uint32_t lastPvtTime = 0;
static bool ubxInitialized = false;
if (!ubxInitialized) {
// initialize dop values. If no DOP sentence is received it is safer to initialize them to a high value rather than 0.
GpsPosition->HDOP = 99.99f;
GpsPosition->PDOP = 99.99f;
GpsPosition->VDOP = 99.99f;
ubxInitialized = true;
}
// is it using PVT?
bool usePvt = (lastPvtTime) && (PIOS_DELAY_GetuSSince(lastPvtTime) < UBX_PVT_TIMEOUT * 1000);
switch (ubx->header.class) { switch (ubx->header.class) {
case UBX_CLASS_NAV: case UBX_CLASS_NAV:
if (!usePvt) {
// Set of messages to be decoded when not using pvt
switch (ubx->header.id) {
case UBX_ID_POSLLH:
parse_ubx_nav_posllh(&ubx->payload.nav_posllh, GpsPosition);
break;
case UBX_ID_SOL:
parse_ubx_nav_sol(&ubx->payload.nav_sol, GpsPosition);
break;
case UBX_ID_VELNED:
parse_ubx_nav_velned(&ubx->payload.nav_velned, GpsPosition);
break;
#if !defined(PIOS_GPS_MINIMAL)
case UBX_ID_TIMEUTC:
parse_ubx_nav_timeutc(&ubx->payload.nav_timeutc);
break;
#endif
}
}
// messages used always
switch (ubx->header.id) { switch (ubx->header.id) {
case UBX_ID_POSLLH:
parse_ubx_nav_posllh(&ubx->payload.nav_posllh, GpsPosition);
break;
case UBX_ID_DOP: case UBX_ID_DOP:
parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition); parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition);
break; break;
case UBX_ID_SOL:
parse_ubx_nav_sol(&ubx->payload.nav_sol, GpsPosition); case UBX_ID_PVT:
break; parse_ubx_nav_pvt(&ubx->payload.nav_pvt, GpsPosition);
case UBX_ID_VELNED: lastPvtTime = PIOS_DELAY_GetuS();
parse_ubx_nav_velned(&ubx->payload.nav_velned, GpsPosition);
break; break;
#if !defined(PIOS_GPS_MINIMAL) #if !defined(PIOS_GPS_MINIMAL)
case UBX_ID_TIMEUTC:
parse_ubx_nav_timeutc(&ubx->payload.nav_timeutc);
break;
case UBX_ID_SVINFO: case UBX_ID_SVINFO:
parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo); parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo);
break; break;
@ -335,6 +402,7 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
} }
break; break;
} }
if (msgtracker.msg_received == ALL_RECEIVED) { if (msgtracker.msg_received == ALL_RECEIVED) {
GPSPositionSensorSet(GpsPosition); GPSPositionSensorSet(GpsPosition);
msgtracker.msg_received = NONE_RECEIVED; msgtracker.msg_received = NONE_RECEIVED;

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

@ -51,6 +51,7 @@
#define UBX_ID_VELNED 0x12 #define UBX_ID_VELNED 0x12
#define UBX_ID_TIMEUTC 0x21 #define UBX_ID_TIMEUTC 0x21
#define UBX_ID_SVINFO 0x30 #define UBX_ID_SVINFO 0x30
#define UBX_ID_PVT 0x07
// private structures // private structures
@ -156,6 +157,59 @@ struct UBX_NAV_TIMEUTC {
uint8_t valid; // Validity Flags 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_GNSSFIX_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;
uint16_t pDOP;
uint16_t reserved2;
uint32_t reserved3;
} __attribute__((packed));
// Space Vehicle (SV) Information // Space Vehicle (SV) Information
// Single SV information block // Single SV information block
@ -198,6 +252,7 @@ typedef union {
struct UBX_NAV_DOP nav_dop; struct UBX_NAV_DOP nav_dop;
struct UBX_NAV_SOL nav_sol; struct UBX_NAV_SOL nav_sol;
struct UBX_NAV_VELNED nav_velned; struct UBX_NAV_VELNED nav_velned;
struct UBX_NAV_PVT nav_pvt;
#if !defined(PIOS_GPS_MINIMAL) #if !defined(PIOS_GPS_MINIMAL)
struct UBX_NAV_TIMEUTC nav_timeutc; struct UBX_NAV_TIMEUTC nav_timeutc;
struct UBX_NAV_SVINFO nav_svinfo; struct UBX_NAV_SVINFO nav_svinfo;

4
shared/uavobjectdefinition/gpssatellites.xml
View File

@ -3,8 +3,8 @@
<description>Contains information about the GPS satellites in view from @ref GPSModule.</description> <description>Contains information about the GPS satellites in view from @ref GPSModule.</description>
<field name="SatsInView" units="" type="int8" elements="1"/> <field name="SatsInView" units="" type="int8" elements="1"/>
<field name="PRN" units="" type="int8" elements="16"/> <field name="PRN" units="" type="int8" elements="16"/>
<field name="Elevation" units="degrees" type="float" elements="16"/> <field name="Elevation" units="degrees" type="int8" elements="16"/>
<field name="Azimuth" units="degrees" type="float" elements="16"/> <field name="Azimuth" units="degrees" type="int16" elements="16"/>
<field name="SNR" units="" type="int8" elements="16"/> <field name="SNR" units="" type="int8" elements="16"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/> <telemetrygcs acked="false" updatemode="manual" period="0"/>