AP_GPS: Support UBX messages for dual UBLOX GPS setups.

Add extra DOP information to UBX precision messages.

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -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;

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -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__

libraries/DataFlash/DataFlash.h

@@ -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), \
+    { LOG_GPS_UBX3_MSG, sizeof(log_Ubx3), \
-      "UBX3", "QBfff", "TimeUS,Instance,hAcc,vAcc,sAcc" }, \
+      "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_GPS_UBX1_MSG,
-    LOG_UBX2_MSG,
+    LOG_GPS_UBX2_MSG,
-    LOG_UBX3_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,