AP_GPS: cope with different MON-HW message size in u-Blox Neo7

This commit is contained in:
Andrew Tridgell 2014-04-03 10:55:05 +11:00
parent cd3038fabd
commit 21c205a57e
2 changed files with 38 additions and 7 deletions

View File

@ -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) {
if (_payload_length == 60 || _payload_length == 68) {
log_mon_hw();
}
} else if (_msg_id == MSG_MON_HW2) {
if (_payload_length == 28) {
log_mon_hw2();
}
} else {
unexpected_message();
}

View File

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