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

OP-1370 - Add support for mon-ver ack-ack and ack-nak messages for gps hw version detection

Browse Source
This commit is contained in:
Alessio Morale 2014-08-21 16:55:16 +02:00
parent b6c053565e
commit 002c35163a
4 changed files with 148 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -489,7 +489,7 @@ static bool nmeaProcessGPGGA(GPSPositionSensorData *GpsData, bool *gpsDataUpdate
// geoid separation
GpsData->GeoidSeparation = NMEA_real_to_float(param[11]);
GpsData->SensorType = GPSPOSITIONSENSOR_SENSORTYPE_NMEA;
return true;
}

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

@ -36,12 +36,17 @@
#include "inc/UBX.h"
#include "inc/GPS.h"
#include "auxmagsettings.h"
#include <string.h>
static float mag_bias[3] = { 0, 0, 0 };
static float mag_transform[3][3] = {
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
};
static bool useMag = false;
static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
static struct UBX_ACK_ACK last_ack;
static struct UBX_ACK_NAK last_nak;
static bool usePvt = false;
// If a PVT sentence is received in the last UBX_PVT_TIMEOUT (ms) timeframe it disables VELNED/POSLLH/SOL/TIMEUTC
#define UBX_PVT_TIMEOUT (1000)
@ -204,6 +209,9 @@ bool checksum_ubx_message(struct UBXPacket *ubx)
void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionSensorData *GpsPosition)
{
if (usePvt) {
return;
}
if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) {
if (GpsPosition->Status != GPSPOSITIONSENSOR_STATUS_NOFIX) {
GpsPosition->Altitude = (float)posllh->hMSL * 0.001f;
@ -216,6 +224,9 @@ void parse_ubx_nav_posllh(struct UBX_NAV_POSLLH *posllh, GPSPositionSensorData *
void parse_ubx_nav_sol(struct UBX_NAV_SOL *sol, GPSPositionSensorData *GpsPosition)
{
if (usePvt) {
return;
}
if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) {
GpsPosition->Satellites = sol->numSV;
@ -246,6 +257,9 @@ void parse_ubx_nav_dop(struct UBX_NAV_DOP *dop, GPSPositionSensorData *GpsPositi
void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionSensorData *GpsPosition)
{
if (usePvt) {
return;
}
GPSVelocitySensorData GpsVelocity;
if (check_msgtracker(velned->iTOW, VELNED_RECEIVED)) {
@ -300,7 +314,28 @@ void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPositi
GPSTimeSet(&GpsTime);
}
#endif
GpsPosition->SensorType = sensorType;
}
void parse_ubx_ack_ack(struct UBX_ACK_ACK *ack_ack)
{
last_ack = *ack_ack;
}
void parse_ubx_ack_nak(struct UBX_ACK_NAK *ack_nak)
{
last_nak = *ack_nak;
}
void parse_ubx_mon_ver(struct UBX_MON_VER *mon_ver)
{
uint32_t hwVersion = atoi(mon_ver->hwVersion);
sensorType = (hwVersion >= 8000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 :
((hwVersion >= 7000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX);
}
#if !defined(PIOS_GPS_MINIMAL)
void parse_ubx_op_sys(struct UBX_OP_SYSINFO *sysinfo)
{
@ -339,6 +374,9 @@ void parse_ubx_op_mag(struct UBX_OP_MAG *mag)
#if !defined(PIOS_GPS_MINIMAL)
void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc)
{
if (usePvt) {
return;
}
// Test if time is valid
if ((timeutc->valid & TIMEUTC_VALIDTOW) && (timeutc->valid & TIMEUTC_VALIDWKN)) {
// Time is valid, set GpsTime
@ -404,54 +442,70 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
ubxInitialized = true;
}
// is it using PVT?
bool usePvt = (lastPvtTime) && (PIOS_DELAY_GetuSSince(lastPvtTime) < UBX_PVT_TIMEOUT * 1000);
usePvt = (lastPvtTime) && (PIOS_DELAY_GetuSSince(lastPvtTime) < UBX_PVT_TIMEOUT * 1000);
switch (ubx->header.class) {
switch ((ubx_class)ubx->header.class) {
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;
// Set of messages to be decoded when not using pvt
switch ((ubx_class_nav_id)ubx->header.id) {
case UBX_ID_NAV_POSLLH:
parse_ubx_nav_posllh(&ubx->payload.nav_posllh, GpsPosition);
break;
case UBX_ID_NAV_SOL:
parse_ubx_nav_sol(&ubx->payload.nav_sol, GpsPosition);
break;
case UBX_ID_NAV_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;
case UBX_ID_NAV_TIMEUTC:
parse_ubx_nav_timeutc(&ubx->payload.nav_timeutc);
break;
#endif
}
}
// messages used always
switch (ubx->header.id) {
case UBX_ID_DOP:
case UBX_ID_NAV_DOP:
parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition);
break;
case UBX_ID_PVT:
case UBX_ID_NAV_PVT:
parse_ubx_nav_pvt(&ubx->payload.nav_pvt, GpsPosition);
lastPvtTime = PIOS_DELAY_GetuS();
break;
#if !defined(PIOS_GPS_MINIMAL)
case UBX_ID_SVINFO:
case UBX_ID_NAV_SVINFO:
parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo);
break;
#endif
default:
break;
}
break;
case UBX_CLASS_MON:
switch ((ubx_class_mon_id)ubx->header.id) {
case UBX_ID_MON_VER:
parse_ubx_mon_ver(&ubx->payload.mon_ver);
break;
default:
break;
}
case UBX_CLASS_ACK:
switch ((ubx_class_ack_id)ubx->header.id) {
case UBX_ID_ACK_ACK:
parse_ubx_ack_ack(&ubx->payload.ack_ack);
break;
case UBX_ID_ACK_NAK:
parse_ubx_ack_nak(&ubx->payload.ack_nak);
break;
default:
break;
}
break;
case UBX_CLASS_OP_CUST:
switch (ubx->header.id) {
switch ((ubx_class_op_id)ubx->header.id) {
#if !defined(PIOS_GPS_MINIMAL)
case UBX_ID_SYS:
case UBX_ID_OP_SYS:
parse_ubx_op_sys(&ubx->payload.op_sysinfo);
break;
#endif
case UBX_ID_MAG:
case UBX_ID_OP_MAG:
parse_ubx_op_mag(&ubx->payload.op_mag);
break;
}

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

@ -37,27 +37,45 @@
#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_OP_CUST 0x99
typedef enum {
UBX_CLASS_NAV = 0x01,
UBX_CLASS_ACK = 0x05,
UBX_CLASS_MON = 0x0A,
UBX_CLASS_OP_CUST = 0x99,
} ubx_class;
// 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_PVT 0x07
typedef enum {
UBX_ID_NAV_POSLLH = 0x02,
UBX_ID_NAV_STATUS = 0x03,
UBX_ID_NAV_DOP = 0x04,
UBX_ID_NAV_SOL = 0x06,
UBX_ID_NAV_VELNED = 0x12,
UBX_ID_NAV_TIMEUTC = 0x21,
UBX_ID_NAV_SVINFO = 0x30,
UBX_ID_NAV_PVT = 0x07,
} ubx_class_nav_id;
typedef enum {
UBX_ID_OP_SYS = 0x01,
UBX_ID_OP_MAG = 0x02,
} ubx_class_op_id;
typedef enum {
UBX_ID_MON_VER = 0x04,
} ubx_class_mon_id;
typedef enum {
UBX_ID_ACK_ACK = 0x01,
UBX_ID_ACK_NAK = 0x00,
} ubx_class_ack_id;
#define UBX_ID_SYS 0x01
#define UBX_ID_MAG 0x02
// private structures
// Geodetic Position Solution
@ -250,6 +268,29 @@ struct UBX_NAV_SVINFO {
struct UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times
};
// ACK message class
struct UBX_ACK_ACK {
uint8_t clsID; // ClassID
uint8_t msgID; // MessageID
};
struct UBX_ACK_NAK {
uint8_t clsID; // ClassID
uint8_t msgID; // MessageID
};
// MON message Class
#define UBX_MON_MAX_EXT 1
struct UBX_MON_VER {
char swVersion[30];
char hwVersion[10];
#if UBX_MON_MAX_EXT > 0
char extension[UBX_MON_MAX_EXT][30];
#endif
};
// OP custom messages
struct UBX_OP_SYSINFO {
uint32_t flightTime;
@ -261,14 +302,15 @@ struct UBX_OP_SYSINFO {
// OP custom messages
struct UBX_OP_MAG {
int16_t x;
int16_t y;
int16_t z;
int16_t x;
int16_t y;
int16_t z;
uint16_t Status;
};
typedef union {
uint8_t payload[0];
// Nav Class
struct UBX_NAV_POSLLH nav_posllh;
struct UBX_NAV_STATUS nav_status;
struct UBX_NAV_DOP nav_dop;
@ -279,6 +321,11 @@ typedef union {
struct UBX_NAV_TIMEUTC nav_timeutc;
struct UBX_NAV_SVINFO nav_svinfo;
#endif
// Ack Class
struct UBX_ACK_ACK ack_ack;
struct UBX_ACK_NAK ack_nak;
// Mon Class
struct UBX_MON_VER mon_ver;
struct UBX_OP_SYSINFO op_sysinfo;
struct UBX_OP_MAG op_mag;
} UBXPayload;

1
shared/uavobjectdefinition/gpspositionsensor.xml
View File

@ -12,6 +12,7 @@
<field name="PDOP" units="" type="float" elements="1"/>
<field name="HDOP" units="" type="float" elements="1"/>
<field name="VDOP" units="" type="float" elements="1"/>
<field name="SensorType" units="" type="enum" elements="1" options="Unknown,NMEA,UBX,UBX7,UBX8" defaultvalue="Unknown" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>