diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 499e82fab7..f22d1e3ad3 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -295,10 +295,15 @@ void AP_GPS_UBLOX::log_mon_hw(void) LOG_PACKET_HEADER_INIT(LOG_MSG_UBX1), timestamp : hal.scheduler->millis(), instance : state.instance, - noisePerMS : _buffer.mon_hw.noisePerMS, - jamInd : _buffer.mon_hw.jamInd, - aPower : _buffer.mon_hw.aPower + noisePerMS : _buffer.mon_hw_60.noisePerMS, + jamInd : _buffer.mon_hw_60.jamInd, + aPower : _buffer.mon_hw_60.aPower }; + if (_payload_length == 68) { + pkt.noisePerMS = _buffer.mon_hw_68.noisePerMS; + pkt.jamInd = _buffer.mon_hw_68.jamInd; + pkt.aPower = _buffer.mon_hw_68.aPower; + } gps._DataFlash->WriteBlock(&pkt, sizeof(pkt)); } @@ -364,9 +369,13 @@ AP_GPS_UBLOX::_parse_gps(void) #if UBLOX_HW_LOGGING if (_class == CLASS_MON) { if (_msg_id == MSG_MON_HW) { - log_mon_hw(); + if (_payload_length == 60 || _payload_length == 68) { + log_mon_hw(); + } } else if (_msg_id == MSG_MON_HW2) { - log_mon_hw2(); + if (_payload_length == 28) { + log_mon_hw2(); + } } else { unexpected_message(); } diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 3e4ec4cb4f..69af2ef86f 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -135,7 +135,28 @@ private: uint32_t speed_accuracy; uint32_t heading_accuracy; }; - struct PACKED ubx_mon_hw { + // Lea6 uses a 60 byte message + struct PACKED ubx_mon_hw_60 { + uint32_t pinSel; + uint32_t pinBank; + uint32_t pinDir; + uint32_t pinVal; + uint16_t noisePerMS; + uint16_t agcCnt; + uint8_t aStatus; + uint8_t aPower; + uint8_t flags; + uint8_t reserved1; + uint32_t usedMask; + uint8_t VP[17]; + uint8_t jamInd; + uint16_t reserved3; + uint32_t pinIrq; + uint32_t pullH; + uint32_t pullL; + }; + // Neo7 uses a 68 byte message + struct PACKED ubx_mon_hw_68 { uint32_t pinSel; uint32_t pinBank; uint32_t pinDir; @@ -173,7 +194,8 @@ private: ubx_nav_solution solution; ubx_nav_velned velned; ubx_cfg_nav_settings nav_settings; - ubx_mon_hw mon_hw; + ubx_mon_hw_60 mon_hw_60; + ubx_mon_hw_68 mon_hw_68; ubx_mon_hw2 mon_hw2; uint8_t bytes[]; } _buffer;