Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-02 19:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Support UBX messages for dual UBLOX GPS setups.

Browse Source 
Add extra DOP information to UBX precision messages.
This commit is contained in:
Andy Piper 2015-07-21 20:06:05 +01:00 committed by Michael du Breuil
parent eee9522ca5
commit 5adb6d2b89
3 changed files with 52 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -33,8 +33,12 @@
#define UBLOX_DEBUGGING 0
#define UBLOX_FAKE_3DLOCK 0
#define UBX_MAX_GPS_INSTANCES 2
extern const AP_HAL::HAL& hal;
uint8_t AP_GPS_UBLOX::ublox_instances = 0;
#if UBLOX_DEBUGGING
# define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
#else
@ -49,6 +53,9 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
_payload_counter(0),
_fix_count(0),
_class(0),
_gdop(0),
_pdop(0),
_vdop(0),
_new_position(0),
_new_speed(0),
need_rate_update(false),
@ -58,6 +65,8 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
_last_5hz_time(0),
noReceivedHdop(true)
{
ublox_instance = ublox_instances++;
// stop any config strings that are pending
gps.send_blob_start(state.instance, NULL, 0);
@ -270,11 +279,12 @@ AP_GPS_UBLOX::read(void)
void AP_GPS_UBLOX::log_mon_hw(void)
{
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()
|| ublox_instance >= UBX_MAX_GPS_INSTANCES) {
return;
}
struct log_Ubx1 pkt = {
LOG_PACKET_HEADER_INIT(LOG_UBX1_MSG),
LOG_PACKET_HEADER_INIT(ubx_msg_index(LOG_GPS_UBX1_MSG)),
time_us : hal.scheduler->micros64(),
instance : state.instance,
noisePerMS : _buffer.mon_hw_60.noisePerMS,
@ -293,12 +303,13 @@ void AP_GPS_UBLOX::log_mon_hw(void)
void AP_GPS_UBLOX::log_mon_hw2(void)
{
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()
|| ublox_instance >= UBX_MAX_GPS_INSTANCES) {
return;
}
struct log_Ubx2 pkt = {
LOG_PACKET_HEADER_INIT(LOG_UBX2_MSG),
LOG_PACKET_HEADER_INIT(ubx_msg_index(LOG_GPS_UBX2_MSG)),
time_us : hal.scheduler->micros64(),
instance : state.instance,
ofsI : _buffer.mon_hw2.ofsI,
@ -571,6 +582,9 @@ AP_GPS_UBLOX::_parse_gps(void)
noReceivedHdop = false;
state.hdop = _buffer.dop.hDOP;
state.vdop = _buffer.dop.vDOP;
_vdop = _buffer.dop.vDOP;
_pdop = _buffer.dop.pDOP;
_gdop = _buffer.dop.gDOP;
#if UBLOX_FAKE_3DLOCK
state.hdop = 130;
state.hdop = 170;

14
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -54,6 +54,7 @@
#endif
#define UBLOX_MAX_GNSS_CONFIG_BLOCKS 7
#define UBX_MSG_TYPES 3
class AP_GPS_UBLOX : public AP_GPS_Backend
{
@ -385,6 +386,11 @@ private:
uint32_t _last_vel_time;
uint32_t _last_pos_time;
// UBX specific dilution of precision
uint16_t _gdop;
uint16_t _pdop;
uint16_t _vdop;
// do we have new position information?
bool _new_position:1;
// do we have new speed information?
@ -403,6 +409,9 @@ private:
uint32_t _last_5hz_time;
bool noReceivedHdop = true;
// number of active ublox instances used to determine UBX processing
uint8_t ublox_instance;
static uint8_t ublox_instances;
void _configure_navigation_rate(uint16_t rate_ms);
void _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
@ -419,6 +428,11 @@ private:
void log_mon_hw2(void);
void log_rxm_raw(const struct ubx_rxm_raw &raw);
void log_rxm_rawx(const struct ubx_rxm_rawx &raw);
// Actual LogMessage index we are going to use
uint8_t ubx_msg_index(uint8_t ubx_msg) {
return (uint8_t)(ubx_msg + (ublox_instance * UBX_MSG_TYPES));
}
};
#endif // __AP_GPS_UBLOX_H__

27
libraries/DataFlash/DataFlash.h
View File

@ -615,6 +615,10 @@ struct PACKED log_Ubx3 {
float hAcc;
float vAcc;
float sAcc;
uint16_t hDOP;
uint16_t vDOP;
uint16_t pDOP;
uint16_t gDOP;
};
struct PACKED log_GPS_RAW {
@ -829,12 +833,18 @@ Format characters in the format string for binary log messages
"NKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \
{ LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \
"TERR","QBLLHffHH","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded" }, \
{ LOG_UBX1_MSG, sizeof(log_Ubx1), \
{ LOG_GPS_UBX1_MSG, sizeof(log_Ubx1), \
"UBX1", "QBHBBH", "TimeUS,Instance,noisePerMS,jamInd,aPower,agcCnt" }, \
{ LOG_UBX2_MSG, sizeof(log_Ubx2), \
{ LOG_GPS_UBX2_MSG, sizeof(log_Ubx2), \
"UBX2", "QBbBbB", "TimeUS,Instance,ofsI,magI,ofsQ,magQ" }, \
{ LOG_UBX3_MSG, sizeof(log_Ubx3), \
"UBX3", "QBfff", "TimeUS,Instance,hAcc,vAcc,sAcc" }, \
{ LOG_GPS_UBX3_MSG, sizeof(log_Ubx3), \
"UBX3", "QBfffcccc", "TimeUS,Instance,hAcc,vAcc,sAcc,hDOP,vDOP,pDOP,gDOP" }, \
{ LOG_GPS2_UBX1_MSG, sizeof(log_Ubx1), \
"UBY1", "QBHBBH", "TimeUS,Instance,noisePerMS,jamInd,aPower,agcCnt" }, \
{ LOG_GPS2_UBX2_MSG, sizeof(log_Ubx2), \
"UBY2", "QBbBbB", "TimeUS,Instance,ofsI,magI,ofsQ,magQ" }, \
{ LOG_GPS2_UBX3_MSG, sizeof(log_Ubx3), \
"UBY3", "QBfffcccc", "TimeUS,Instance,hAcc,vAcc,sAcc,hDOP,vDOP,pDOP,gDOP" }, \
{ LOG_GPS_RAW_MSG, sizeof(log_GPS_RAW), \
"GRAW", "QIHBBddfBbB", "TimeUS,WkMS,Week,numSV,sv,cpMes,prMes,doMes,mesQI,cno,lli" }, \
{ LOG_GPS_RAWH_MSG, sizeof(log_GPS_RAWH), \
@ -936,9 +946,12 @@ enum LogMessages {
LOG_CAMERA_MSG,
LOG_IMU3_MSG,
LOG_TERRAIN_MSG,
LOG_UBX1_MSG,
LOG_UBX2_MSG,
LOG_UBX3_MSG,
LOG_GPS_UBX1_MSG,
LOG_GPS_UBX2_MSG,
LOG_GPS_UBX3_MSG,
LOG_GPS2_UBX1_MSG,
LOG_GPS2_UBX2_MSG,
LOG_GPS2_UBX3_MSG,
LOG_ESC1_MSG,
LOG_ESC2_MSG,
LOG_ESC3_MSG,