diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 7c3dc1dc5f..bda4b047b2 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index b2af1b65a0..a59d07cec3 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/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__ diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index 27e071e9e6..30f080ebc5 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/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), \ - "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,