diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 3e1d963160..0cd4ff20f4 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -23,6 +23,8 @@ #include "AP_GPS_UBLOX.h" #include +#define UBLOX_VERSION_AUTODETECTION 1 + #define UBLOX_DEBUGGING 0 #define UBLOX_FAKE_3DLOCK 0 @@ -109,6 +111,11 @@ AP_GPS_UBLOX::send_next_rate_update(void) // gather MON_HW2 at 0.5Hz _configure_message_rate(CLASS_MON, MSG_MON_HW2, 2); // 24+8 bytes break; +#endif +#if UBLOX_VERSION_AUTODETECTION + case 7: + _request_version(); + break; #endif default: need_rate_update = false; @@ -485,6 +492,31 @@ AP_GPS_UBLOX::_parse_gps(void) state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f; _new_speed = true; break; +#if UBLOX_VERSION_AUTODETECTION + case MSG_NAV_SVINFO: + { + Debug("MSG_NAV_SVINFO\n"); + static const uint8_t HardwareGenerationMask = 0x07; + uint8_t hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask; + switch (hardware_generation) { + case UBLOX_5: + case UBLOX_6: + /*speed already configured */; + break; + case UBLOX_7: + case UBLOX_M8: + port->begin(4000000U); + Debug("Changed speed to 5Mhzfor SPI-driven UBlox\n"); + break; + default: + hal.console->printf("Wrong Ublox' Hardware Version%u\n", hardware_generation); + break; + }; + /* We don't need that anymore */ + _configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 0); + break; + } +#endif default: Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id); if (++_disable_counter == 0) { @@ -676,3 +708,9 @@ reset: } return false; } + +void +AP_GPS_UBLOX::_request_version(void) +{ + _configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 2); +} diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 5fa6ecc9bf..ecb493ebbf 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -194,6 +194,12 @@ private: uint32_t postStatus; uint32_t reserved2; }; + struct PACKED ubx_nav_svinfo_header { + uint32_t itow; + uint8_t numCh; + uint8_t globalFlags; + uint16_t reserved; + }; // Receive buffer union PACKED { ubx_nav_posllh posllh; @@ -205,6 +211,7 @@ private: ubx_mon_hw_68 mon_hw_68; ubx_mon_hw2 mon_hw2; ubx_cfg_sbas sbas; + ubx_nav_svinfo_header svinfo_header; uint8_t bytes[]; } _buffer; @@ -227,7 +234,8 @@ private: MSG_CFG_NAV_SETTINGS = 0x24, MSG_CFG_SBAS = 0x16, MSG_MON_HW = 0x09, - MSG_MON_HW2 = 0x0B + MSG_MON_HW2 = 0x0B, + MSG_NAV_SVINFO = 0x30 }; enum ubs_nav_fix_type { FIX_NONE = 0, @@ -240,6 +248,13 @@ private: enum ubx_nav_status_bits { NAV_STATUS_FIX_VALID = 1 }; + enum ubx_hardware_version { + ANTARIS = 0, + UBLOX_5, + UBLOX_6, + UBLOX_7, + UBLOX_M8 + }; // Packet checksum accumulators uint8_t _ck_a; @@ -284,6 +299,7 @@ private: void _update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_t &ck_b); void _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size); void send_next_rate_update(void); + void _request_version(void); void unexpected_message(void); void write_logging_headers(void);