AP_GPS: u-blox verify ublox settings

The ublox driver will now continuosly poll for the settings from the GPS and correct any that are found to be in correct.
This status is then reported to the arming library as an additional arming check, allowing the user to be sure that the
gps is correctly configured before using it. If a user has a GPS2 configured that is not present they will fail the arming
checks until after they have disabled the second GPS.

2 new parameters were introduced as well:
  -GPS_AUTO_CONFIG: Will not request any configuration packets to attempt to change them. (If the packet is recieved then
    a update will be sent to it, but in testing this scenario never occured. This is set to 1 or 0 to change the setting.
    (Defaults to 1 enabling auto config)
  -GPS_GNSS_MODE2: Behaves the same way as GPS_GNSS_MODE but only applies to the second GPS.

GPS drivers are now allowed 2 seconds of non responsiveness before being unloaded

libraries/AP_GPS/AP_GPS.cpp

@@ -42,7 +42,6 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @DisplayName: Navigation filter setting
     // @Description: Navigation filter engine setting
     // @Values: 0:Portable,2:Stationary,3:Pedestrian,4:Automotive,5:Sea,6:Airborne1G,7:Airborne2G,8:Airborne4G
-    // @RebootRequired: True
     AP_GROUPINFO("NAVFILTER", 2, AP_GPS, _navfilter, GPS_ENGINE_AIRBORNE_4G),
 
     // @Param: AUTO_SWITCH
@@ -65,7 +64,6 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @Description: This sets the SBAS (satellite based augmentation system) mode if available on this GPS. If set to 2 then the SBAS mode is not changed in the GPS. Otherwise the GPS will be reconfigured to enable/disable SBAS. Disabling SBAS may be worthwhile in some parts of the world where an SBAS signal is available but the baseline is too long to be useful.
     // @Values: 0:Disabled,1:Enabled,2:NoChange
     // @User: Advanced
-    // @RebootRequired: True
     AP_GROUPINFO("SBAS_MODE", 5, AP_GPS, _sbas_mode, 2),
 
     // @Param: MIN_ELEV
@@ -74,7 +72,6 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @Range: -100 90
     // @Units: Degrees
     // @User: Advanced
-    // @RebootRequired: True
     AP_GROUPINFO("MIN_ELEV", 6, AP_GPS, _min_elevation, -100),
 
     // @Param: INJECT_TO
@@ -99,12 +96,11 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
 
     // @Param: GNSS_MODE
     // @DisplayName: GNSS system configuration
-    // @Description: Bitmask for what GNSS system to use (all unchecked or zero to leave GPS as configured)
+    // @Description: Bitmask for what GNSS system to use on the first GPS (all unchecked or zero to leave GPS as configured)
     // @Values: 0:Leave as currently configured, 1:GPS-NoSBAS, 3:GPS+SBAS, 4:Galileo-NoSBAS, 6:Galileo+SBAS, 8:Beidou, 51:GPS+IMES+QZSS+SBAS (Japan Only), 64:GLONASS, 66:GLONASS+SBAS, 67:GPS+GLONASS+SBAS
     // @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLOSNASS
     // @User: Advanced
-    // @RebootRequired: True
-    AP_GROUPINFO("GNSS_MODE", 10, AP_GPS, _gnss_mode, 0),
+    AP_GROUPINFO("GNSS_MODE", 10, AP_GPS, _gnss_mode[0], 0),
 
     // @Param: SAVE_CFG
     // @DisplayName: Save GPS configuration
@@ -113,6 +109,21 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("SAVE_CFG", 11, AP_GPS, _save_config, 0),
 
+    // @Param: GNSS_MODE2
+    // @DisplayName: GNSS system configuration
+    // @Description: Bitmask for what GNSS system to use on the second GPS (all unchecked or zero to leave GPS as configured)
+    // @Values: 0:Leave as currently configured, 1:GPS-NoSBAS, 3:GPS+SBAS, 4:Galileo-NoSBAS, 6:Galileo+SBAS, 8:Beidou, 51:GPS+IMES+QZSS+SBAS (Japan Only), 64:GLONASS, 66:GLONASS+SBAS, 67:GPS+GLONASS+SBAS
+    // @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLOSNASS
+    // @User: Advanced
+    AP_GROUPINFO("GNSS_MODE2", 12, AP_GPS, _gnss_mode[1], 0),
+
+    // @Param: AUTO_CONFIG
+    // @DisplayName: Automatic GPS configuration
+    // @Description: Controls if the autopilot should automatically configure the GPS based on the parameters and default settings
+    // @Values: 0:Disables automatic configuration,1:Enable automatic configuration
+    // @User: Advanced
+    AP_GROUPINFO("AUTO_CONFIG", 13, AP_GPS, _auto_config, 1),
+
     AP_GROUPEND
 };
 
@@ -355,11 +366,11 @@ AP_GPS::update_instance(uint8_t instance)
     bool result = drivers[instance]->read();
     uint32_t tnow = AP_HAL::millis();
 
-    // if we did not get a message, and the idle timer of 1.2 seconds
+    // if we did not get a message, and the idle timer of 2 seconds
     // has expired, re-initialise the GPS. This will cause GPS
     // detection to run again
     if (!result) {
-        if (tnow - timing[instance].last_message_time_ms > 1200) {
+        if (tnow - timing[instance].last_message_time_ms > 2000) {
             // free the driver before we run the next detection, so we
             // don't end up with two allocated at any time
             delete drivers[instance];
@@ -580,3 +591,13 @@ AP_GPS::send_mavlink_gps2_rtk(mavlink_channel_t chan)
         drivers[1]->send_mavlink_gps_rtk(chan);
     }
 }
+
+uint8_t
+AP_GPS::first_unconfigured_gps(void) const {
+    for(int i = 0; i < GPS_MAX_INSTANCES; i++) {
+        if(_type[i] != GPS_TYPE_NONE && (drivers[i] == NULL || !drivers[i]->is_configured())) {
+            return i;
+        }
+    }
+    return GPS_ALL_CONFIGURED;
+}

libraries/AP_GPS/AP_GPS.h

@@ -96,6 +96,10 @@ public:
         GPS_ENGINE_AIRBORNE_4G = 8
     };
 
+   enum GPS_Config {
+       GPS_ALL_CONFIGURED = 255
+   };
+
     /*
       The GPS_State structure is filled in by the backend driver as it
       parses each message from the GPS.
@@ -317,8 +321,9 @@ public:
     AP_Int8 _sbas_mode;
     AP_Int8 _min_elevation;
     AP_Int8 _raw_data;
-    AP_Int8 _gnss_mode;
+    AP_Int8 _gnss_mode[2];
     AP_Int8 _save_config;
+    AP_Int8 _auto_config;
     
     // handle sending of initialisation strings to the GPS
     void send_blob_start(uint8_t instance, const char *_blob, uint16_t size);
@@ -338,6 +343,9 @@ public:
     void send_mavlink_gps_rtk(mavlink_channel_t chan);
     void send_mavlink_gps2_rtk(mavlink_channel_t chan);
 
+    // Returns the index of the first unconfigured GPS (returns GPS_ALL_CONFIGURED if all instances report as being configured)
+    uint8_t first_unconfigured_gps(void) const;
+
 private:
     struct GPS_timing {
         // the time we got our last fix in system milliseconds

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -21,13 +21,15 @@
 //
 #include "AP_GPS.h"
 #include "AP_GPS_UBLOX.h"
+#include <AP_HAL/Util.h>
 #include <DataFlash/DataFlash.h>
+#include <GCS_MAVLink/GCS.h>
 
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \
     CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
-    #define UBLOX_VERSION_AUTODETECTION 1
+    #define UBLOX_SPEED_CHANGE  1
 #else
-    #define UBLOX_VERSION_AUTODETECTION 0
+    #define UBLOX_SPEED_CHANGE 0
 #endif
 
 
@@ -48,100 +50,258 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
     _msg_id(0),
     _payload_length(0),
     _payload_counter(0),
-    _fix_count(0),
     _class(0),
     _cfg_saved(false),
     _last_cfg_sent_time(0),
     _num_cfg_save_tries(0),
+    _last_config_time(0),
+    _delay_time(0),
+    _next_message(STEP_RATE_NAV),
+    _ublox_port(255),
+    _have_version(false),
+    _unconfigured_messages(CONFIG_ALL),
+    _hardware_generation(0),
     _new_position(0),
     _new_speed(0),
-    need_rate_update(false),
     _disable_counter(0),
     next_fix(AP_GPS::NO_FIX),
-    rate_update_step(0),
     _last_5hz_time(0),
     noReceivedHdop(true)
 {
     // stop any config strings that are pending
     gps.send_blob_start(state.instance, NULL, 0);
 
-    // configure the GPS for the messages we want
-    _configure_gps();
+    _last_5hz_time = AP_HAL::millis();
+
+    // start the process of updating the GPS rates
+    _request_next_config();
 }
 
-/*
-  send the next step of rate updates to the GPS. This reconfigures the
-  GPS on the fly to have the right message rates. It needs to be
-  careful to only send a message if there is sufficient buffer space
-  available on the serial port to avoid it blocking the CPU
- */
 void
-AP_GPS_UBLOX::send_next_rate_update(void)
+AP_GPS_UBLOX::_request_next_config(void)
 {
+    // don't request config if we shouldn't configure the GPS
+    if (gps._auto_config == 0) {
+        return;
+    }
+
+    // Ensure there is enough space for the largest possible outgoing message
     if (port->txspace() < (int16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_nav_rate)+2)) {
         // not enough space - do it next time
         return;
     }
 
-    //hal.console->printf("next_rate: %u\n", (unsigned)rate_update_step);
+   Debug("Unconfigured messages: %d Current message: %d\n", _unconfigured_messages, _next_message);
 
-    switch (rate_update_step++) {
-    case 0:
-        _configure_navigation_rate(200);
+    switch (_next_message++) {
+    case STEP_RATE_NAV:
+        _configure_rate();
         break;
-    case 1:
-        _configure_message_rate(CLASS_NAV, MSG_POSLLH, 1); // 28+8 bytes
+    case STEP_RATE_POSLLH:
+        if(!_configure_message_rate(CLASS_NAV, MSG_POSLLH, RATE_POSLLH)) {
+            _next_message--;
+        }
         break;
-    case 2:
-        _configure_message_rate(CLASS_NAV, MSG_STATUS, 1); // 16+8 bytes
+    case STEP_RATE_VELNED:
+        if(!_configure_message_rate(CLASS_NAV, MSG_VELNED, RATE_VELNED)) {
+            _next_message--;
+        }
         break;
-    case 3:
-        _configure_message_rate(CLASS_NAV, MSG_SOL, 1);    // 52+8 bytes
+    case STEP_PORT:
+        _request_port();
         break;
-    case 4:
-        _configure_message_rate(CLASS_NAV, MSG_VELNED, 1); // 36+8 bytes
+    case STEP_POLL_SVINFO:
+        // not required once we know what generation we are on
+        if(_hardware_generation == 0) {
+            _send_message(CLASS_NAV, MSG_NAV_SVINFO, 0, 0);
+        }
         break;
-    case 5:
-        _configure_message_rate(CLASS_NAV, MSG_DOP, 1); // 18+8 bytes
+    case STEP_POLL_SBAS:
+        _send_message(CLASS_CFG, MSG_CFG_SBAS, NULL, 0);
+	break;
+    case STEP_POLL_NAV:
+        _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
         break;
-    case 6:
-#if UBLOX_HW_LOGGING
-        // gather MON_HW at 0.5Hz
-        _configure_message_rate(CLASS_MON, MSG_MON_HW, 2); // 64+8 bytes
-#endif
+    case STEP_POLL_GNSS:
+        _send_message(CLASS_CFG, MSG_CFG_GNSS, NULL, 0);
         break;
-    case 7:
-#if UBLOX_HW_LOGGING
-        // gather MON_HW2 at 0.5Hz
-        _configure_message_rate(CLASS_MON, MSG_MON_HW2, 2); // 24+8 bytes
-#endif
+    case STEP_NAV_RATE:
+        _request_navigation_rate();
         break;
-
-    case 8:
+    case STEP_POSLLH:
+        if(!_request_message_rate(CLASS_NAV, MSG_POSLLH)) {
+            _next_message--;
+        }
+        break;
+    case STEP_STATUS:
+        if(!_request_message_rate(CLASS_NAV, MSG_STATUS)) {
+            _next_message--;
+        }
+        break;
+    case STEP_SOL:
+        if(!_request_message_rate(CLASS_NAV, MSG_SOL)) {
+            _next_message--;
+        }
+        break;
+    case STEP_VELNED:
+        if(!_request_message_rate(CLASS_NAV, MSG_VELNED)) {
+            _next_message--;
+        }
+        break;
+    case STEP_DOP:
+       if(! _request_message_rate(CLASS_NAV, MSG_DOP)) {
+            _next_message--;
+        }
+        break;
+    case STEP_MON_HW:
+        if(!_request_message_rate(CLASS_MON, MSG_MON_HW)) {
+            _next_message--;
+        }
+        break;
+    case STEP_MON_HW2:
+        if(!_request_message_rate(CLASS_MON, MSG_MON_HW2)) {
+            _next_message--;
+        }
+        break;
+    case STEP_RAW:
 #if UBLOX_RXM_RAW_LOGGING
-        _configure_message_rate(CLASS_RXM, MSG_RXM_RAW, gps._raw_data);
+        if(gps._raw_data == 0) {
+            _unconfigured_messages &= ~CONFIG_RATE_RAW;
+        } else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAW)) {
+            _next_message--;
+        }
+#else
+        _unconfigured_messages & = ~COONFIG_RATE_RAW;
 #endif
         break;
-    case 9:
+    case STEP_RAWX:
 #if UBLOX_RXM_RAW_LOGGING
-        _configure_message_rate(CLASS_RXM, MSG_RXM_RAWX, gps._raw_data);
+        if(gps._raw_data == 0) {
+            _unconfigured_messages &= ~CONFIG_RATE_RAW;
+        } else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAWX)) {
+            _next_message--;
+        }
+#else
+        _unconfigured_messages & = ~COONFIG_RATE_RAW;
 #endif
         break;
-
-    case 10:
-#if UBLOX_VERSION_AUTODETECTION
-        _request_version();
-#endif
+    case STEP_VERSION:
+         if(!_have_version && !hal.util->get_soft_armed()) {
+            _request_version();
+        }
+        // no need to send the initial rates, move to checking only
+        _next_message = STEP_PORT;
         break;
-
     default:
-        need_rate_update = false;
-        rate_update_step = 0;
+        // this case should never be reached, do a full reset if it is hit
+        _next_message = STEP_RATE_NAV;
         break;
     }
 }
 
+void
+AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
+    switch(msg_class) {
+    case CLASS_NAV:
+        switch(msg_id) {
+        case MSG_POSLLH:
+            if(rate == RATE_POSLLH) {
+                _unconfigured_messages &= ~CONFIG_RATE_POSLLH;
+            } else {
+                _configure_message_rate(msg_class, msg_id, RATE_POSLLH);
+                _unconfigured_messages |= CONFIG_RATE_POSLLH;
+            }
+            break;
+        case MSG_STATUS:
+            if(rate == RATE_STATUS) {
+                _unconfigured_messages &= ~CONFIG_RATE_STATUS;
+            } else {
+                _configure_message_rate(msg_class, msg_id, RATE_STATUS);
+                _unconfigured_messages |= CONFIG_RATE_STATUS;
+            }
+            break;
+        case MSG_SOL:
+            if(rate == RATE_SOL) {
+                _unconfigured_messages &= ~CONFIG_RATE_SOL;
+            } else {
+                _configure_message_rate(msg_class, msg_id, RATE_SOL);
+                _unconfigured_messages |= CONFIG_RATE_SOL;
+            }
+            break;
+        case MSG_VELNED:
+            if(rate == RATE_VELNED) {
+                _unconfigured_messages &= ~CONFIG_RATE_VELNED;
+            } else {
+                _configure_message_rate(msg_class, msg_id, RATE_VELNED);
+                _unconfigured_messages |= CONFIG_RATE_VELNED;
+            }
+            break;
+        case MSG_DOP:
+            if(rate == RATE_DOP) {
+                _unconfigured_messages &= ~CONFIG_RATE_DOP;
+            } else {
+                _configure_message_rate(msg_class, msg_id, RATE_DOP);
+                _unconfigured_messages |= CONFIG_RATE_DOP;
+            }
+            break;
+        }
+        break;
+    case CLASS_MON:
+        switch(msg_id) {
+        case MSG_MON_HW:
+            if(rate == RATE_HW) {
+                _unconfigured_messages &= ~CONFIG_RATE_MON_HW;
+            } else {
+                _configure_message_rate(msg_class, msg_id, RATE_HW);
+                _unconfigured_messages |= CONFIG_RATE_MON_HW;
+            }
+            break;
+        case MSG_MON_HW2:
+            if(rate == RATE_HW2) {
+                _unconfigured_messages &= ~CONFIG_RATE_MON_HW2;
+            } else {
+                _configure_message_rate(msg_class, msg_id, RATE_HW2);
+                _unconfigured_messages |= CONFIG_RATE_MON_HW2;
+            }
+            break;
+        }
+        break;
+#if UBLOX_RXM_RAW_LOGGING
+    case CLASS_RXM:
+        switch(msg_id) {
+        case MSG_RXM_RAW:
+            if(rate == gps._raw_data) {
+                _unconfigured_messages &= ~CONFIG_RATE_RAW;
+            } else {
+                _configure_message_rate(msg_class, msg_id, gps._raw_data);
+                _unconfigured_messages |= CONFIG_RATE_RAW;
+            }
+            break;
+        case MSG_RXM_RAWX:
+            if(rate == gps._raw_data) {
+                _unconfigured_messages &= ~CONFIG_RATE_RAW;
+            } else {
+                _configure_message_rate(msg_class, msg_id, gps._raw_data);
+                _unconfigured_messages |= CONFIG_RATE_RAW;
+            }
+            break;
+        }
+        break;
+#endif // UBLOX_RXM_RAW_LOGGING
+    }
+}
 
+// Requests the ublox driver to identify what port we are using to communicate
+void
+AP_GPS_UBLOX::_request_port(void)
+{
+    if (port->txspace() < (int16_t)(sizeof(struct ubx_header)+2)) {
+        // not enough space - do it next time
+        return;
+    }
+    _send_message(CLASS_CFG, MSG_CFG_PRT, NULL, 0);
+}
+    // Ensure there is enough space for the largest possible outgoing message
 // Process bytes available from the stream
 //
 // The stream is assumed to contain only messages we recognise.  If it
@@ -159,12 +319,26 @@ AP_GPS_UBLOX::read(void)
     bool parsed = false;
     uint32_t millis_now = AP_HAL::millis();
 
-    if (need_rate_update) {
-        send_next_rate_update();
-    }else if(!_cfg_saved && gps._save_config && _num_cfg_save_tries < 5 && (millis_now - _last_cfg_sent_time) > 5000)  {         //save the configuration sent until now
-        _last_cfg_sent_time = millis_now;
+    // walk through the gps configuration at 1 message per second
+    if (millis_now - _last_config_time >= _delay_time) {
+        _request_next_config();
+        _last_config_time = millis_now;
+        if (_unconfigured_messages) { // send the updates faster until fully configured
+            if (_next_message < STEP_PORT) { // blast the initial settings out
+                _delay_time = 0;
+            } else {
+                _delay_time = 750;
+            }
+        } else {
+            _delay_time = 2000;
+        }
+    }
+
+    if(!_unconfigured_messages && gps._save_config &&
+       _num_cfg_save_tries < 5 && (millis_now - _last_cfg_sent_time) > 5000 &&
+       !hal.util->get_soft_armed()) {
+        //save the configuration sent until now
         _save_cfg();
-        _num_cfg_save_tries++;
     }
 
     numc = port->available();
@@ -275,8 +449,6 @@ AP_GPS_UBLOX::read(void)
 }
 
 // Private Methods /////////////////////////////////////////////////////////////
-#if UBLOX_HW_LOGGING
-
 void AP_GPS_UBLOX::log_mon_hw(void)
 {
     if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
@@ -318,8 +490,6 @@ void AP_GPS_UBLOX::log_mon_hw2(void)
     gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
 }
 
-#endif // UBLOX_HW_LOGGING
-
 #if UBLOX_RXM_RAW_LOGGING
 void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw)
 {
@@ -405,120 +575,218 @@ AP_GPS_UBLOX::_parse_gps(void)
     if (_class == CLASS_ACK) {
         Debug("ACK %u", (unsigned)_msg_id);
 
-        if(_msg_id == MSG_ACK_ACK && _buffer.ack.clsID == CLASS_CFG && _buffer.ack.msgID == MSG_CFG_CFG) {
-            _cfg_saved = true;
+        if(_msg_id == MSG_ACK_ACK) {
+            switch(_buffer.ack.clsID) {
+            case CLASS_CFG:
+                switch(_buffer.ack.msgID) {
+                case MSG_CFG_CFG:
+                    _cfg_saved = true;
+                    break;
+                case MSG_CFG_GNSS:
+                    _unconfigured_messages &= ~CONFIG_GNSS;
+                    break;
+                case MSG_CFG_MSG:
+                    // There is no way to know what MSG config was ack'ed, assume it was the last
+                    // one requested. To verify it rerequest the last config we sent. If we miss
+                    // the actual ack we will catch it next time through the poll loop, but that
+                    // will be a good chunk of time later.
+                    break;
+                case MSG_CFG_NAV_SETTINGS:
+                    _unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
+                    break;
+                case MSG_CFG_RATE:
+                    _unconfigured_messages &= ~CONFIG_RATE_NAV;
+                    break;
+                case MSG_CFG_SBAS:
+                    _unconfigured_messages &= ~CONFIG_SBAS;
+                    break;
+                }
+                break;
+            case CLASS_MON:
+                switch(_buffer.ack.msgID) {
+                case MSG_MON_HW:
+                    _unconfigured_messages &= ~CONFIG_RATE_MON_HW;
+                    break;
+                case MSG_MON_HW2:
+                    _unconfigured_messages &= ~CONFIG_RATE_MON_HW2;
+                    break;
+                }
+            }
         }
         return false;
     }
 
-    if (_class == CLASS_CFG && _msg_id == MSG_CFG_NAV_SETTINGS) {
-		Debug("Got settings %u min_elev %d drLimit %u\n", 
-              (unsigned)_buffer.nav_settings.dynModel,
-              (int)_buffer.nav_settings.minElev,
-              (unsigned)_buffer.nav_settings.drLimit);
-        _buffer.nav_settings.mask = 0;
-        if (gps._navfilter != AP_GPS::GPS_ENGINE_NONE &&
-            _buffer.nav_settings.dynModel != gps._navfilter) {
-            // we've received the current nav settings, change the engine
-            // settings and send them back
-            Debug("Changing engine setting from %u to %u\n",
-                  (unsigned)_buffer.nav_settings.dynModel, (unsigned)gps._navfilter);
-            _buffer.nav_settings.dynModel = gps._navfilter;
-            _buffer.nav_settings.mask |= 1;
-        }
-        if (gps._min_elevation != -100 &&
-            _buffer.nav_settings.minElev != gps._min_elevation) {
-            Debug("Changing min elevation to %d\n", (int)gps._min_elevation);
-            _buffer.nav_settings.minElev = gps._min_elevation;
-            _buffer.nav_settings.mask |= 2;
-        }
-        if (_buffer.nav_settings.mask != 0) {
-            _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
-                          &_buffer.nav_settings,
-                          sizeof(_buffer.nav_settings));
-            _cfg_saved = false;     //save configuration
-        }
-        return false;
-    }
+    if (_class == CLASS_CFG) {
+        switch(_msg_id) {
+        case  MSG_CFG_NAV_SETTINGS:
+	    Debug("Got settings %u min_elev %d drLimit %u\n", 
+                  (unsigned)_buffer.nav_settings.dynModel,
+                  (int)_buffer.nav_settings.minElev,
+                  (unsigned)_buffer.nav_settings.drLimit);
+            _buffer.nav_settings.mask = 0;
+            if (gps._navfilter != AP_GPS::GPS_ENGINE_NONE &&
+                _buffer.nav_settings.dynModel != gps._navfilter) {
+                // we've received the current nav settings, change the engine
+                // settings and send them back
+                Debug("Changing engine setting from %u to %u\n",
+                      (unsigned)_buffer.nav_settings.dynModel, (unsigned)gps._navfilter);
+                _buffer.nav_settings.dynModel = gps._navfilter;
+                _buffer.nav_settings.mask |= 1;
+            }
+            if (gps._min_elevation != -100 &&
+                _buffer.nav_settings.minElev != gps._min_elevation) {
+                Debug("Changing min elevation to %d\n", (int)gps._min_elevation);
+                _buffer.nav_settings.minElev = gps._min_elevation;
+                _buffer.nav_settings.mask |= 2;
+            }
+            if (_buffer.nav_settings.mask != 0) {
+                _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
+                              &_buffer.nav_settings,
+                              sizeof(_buffer.nav_settings));
+                _unconfigured_messages |= CONFIG_NAV_SETTINGS;
+            } else {
+                _unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
+            }
+            return false;
 
 #if UBLOX_GNSS_SETTINGS
-    if (_class == CLASS_CFG && _msg_id == MSG_CFG_GNSS && gps._gnss_mode != 0) {
-        uint8_t gnssCount = 0;
-        Debug("Got GNSS Settings %u %u %u %u:\n",
-            (unsigned)_buffer.gnss.msgVer,
-            (unsigned)_buffer.gnss.numTrkChHw,
-            (unsigned)_buffer.gnss.numTrkChUse,
-            (unsigned)_buffer.gnss.numConfigBlocks);
+        case MSG_CFG_GNSS:
+            if (gps._gnss_mode[state.instance] != 0) {
+                struct ubx_cfg_gnss start_gnss = _buffer.gnss;
+                uint8_t gnssCount = 0;
+                Debug("Got GNSS Settings %u %u %u %u:\n",
+                    (unsigned)_buffer.gnss.msgVer,
+                    (unsigned)_buffer.gnss.numTrkChHw,
+                    (unsigned)_buffer.gnss.numTrkChUse,
+                    (unsigned)_buffer.gnss.numConfigBlocks);
 #if UBLOX_DEBUGGING
-        for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
-            Debug("  %u %u %u 0x%08x\n",
-            (unsigned)_buffer.gnss.configBlock[i].gnssId,
-            (unsigned)_buffer.gnss.configBlock[i].resTrkCh,
-            (unsigned)_buffer.gnss.configBlock[i].maxTrkCh,
-            (unsigned)_buffer.gnss.configBlock[i].flags);
-        }
+                for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
+                    Debug("  %u %u %u 0x%08x\n",
+                    (unsigned)_buffer.gnss.configBlock[i].gnssId,
+                    (unsigned)_buffer.gnss.configBlock[i].resTrkCh,
+                    (unsigned)_buffer.gnss.configBlock[i].maxTrkCh,
+                    (unsigned)_buffer.gnss.configBlock[i].flags);
+                }
 #endif
 
-        for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
-            if((gps._gnss_mode & (1 << i)) && i != GNSS_SBAS) {
-                gnssCount++;
-            }
-        }
+                for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
+                    if((gps._gnss_mode[state.instance] & (1 << i)) && i != GNSS_SBAS) {
+                        gnssCount++;
+                    }
+                }
 
-        for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
-            // Reserve an equal portion of channels for all enabled systems
-            if(gps._gnss_mode & (1 << _buffer.gnss.configBlock[i].gnssId)) {
-                if(GNSS_SBAS !=_buffer.gnss.configBlock[i].gnssId) {
-                    _buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
-                    _buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw;
+                for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
+                    // Reserve an equal portion of channels for all enabled systems
+                    if(gps._gnss_mode[state.instance] & (1 << _buffer.gnss.configBlock[i].gnssId)) {
+                        if(GNSS_SBAS !=_buffer.gnss.configBlock[i].gnssId) {
+                            _buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
+                            _buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw;
+                        } else {
+                            _buffer.gnss.configBlock[i].resTrkCh = 1;
+                            _buffer.gnss.configBlock[i].maxTrkCh = 3;
+                        }
+                        _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags | 0x00000001;
+                    } else {
+                        _buffer.gnss.configBlock[i].resTrkCh = 0;
+                        _buffer.gnss.configBlock[i].maxTrkCh = 0;
+                        _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags & 0xFFFFFFFE;
+                    }
+                }
+                if (!memcmp(&start_gnss, &_buffer.gnss, sizeof(start_gnss))) {
+                    _send_message(CLASS_CFG, MSG_CFG_GNSS, &_buffer.gnss, 4 + (8 * _buffer.gnss.numConfigBlocks));
+                    _unconfigured_messages |= CONFIG_GNSS;
                 } else {
-                    _buffer.gnss.configBlock[i].resTrkCh = 1;
-                    _buffer.gnss.configBlock[i].maxTrkCh = 3;
+                    _unconfigured_messages &= ~CONFIG_GNSS;
                 }
-                _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags | 0x00000001;
             } else {
-                _buffer.gnss.configBlock[i].resTrkCh = 0;
-                _buffer.gnss.configBlock[i].maxTrkCh = 0;
-                _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags & 0xFFFFFFFE;
+                _unconfigured_messages &= ~CONFIG_GNSS;
             }
-        }
-        _send_message(CLASS_CFG, MSG_CFG_GNSS, &_buffer.gnss, 4 + (8 * _buffer.gnss.numConfigBlocks));
-        return false;
-    }
+            return false;
 #endif
 
-    if (_class == CLASS_CFG && _msg_id == MSG_CFG_SBAS && gps._sbas_mode != 2) {
-		Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n", 
-              (unsigned)_buffer.sbas.mode,
-              (unsigned)_buffer.sbas.usage,
-              (unsigned)_buffer.sbas.maxSBAS,
-              (unsigned)_buffer.sbas.scanmode2,
-              (unsigned)_buffer.sbas.scanmode1);
-        if (_buffer.sbas.mode != gps._sbas_mode) {
-            _buffer.sbas.mode = gps._sbas_mode;
-            _send_message(CLASS_CFG, MSG_CFG_SBAS,
-                          &_buffer.sbas,
-                          sizeof(_buffer.sbas));
-            _cfg_saved = false;
+        case MSG_CFG_SBAS:
+            if (gps._sbas_mode != 2) {
+	        Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n", 
+                      (unsigned)_buffer.sbas.mode,
+                      (unsigned)_buffer.sbas.usage,
+                      (unsigned)_buffer.sbas.maxSBAS,
+                      (unsigned)_buffer.sbas.scanmode2,
+                      (unsigned)_buffer.sbas.scanmode1);
+                if (_buffer.sbas.mode != gps._sbas_mode) {
+                    _buffer.sbas.mode = gps._sbas_mode;
+                    _send_message(CLASS_CFG, MSG_CFG_SBAS,
+                                  &_buffer.sbas,
+                                  sizeof(_buffer.sbas));
+                    _unconfigured_messages |= CONFIG_SBAS;
+                } else {
+                    _unconfigured_messages &= ~CONFIG_SBAS;
+                }
+            } else {
+                    _unconfigured_messages &= ~CONFIG_SBAS;
+            }
+            return false;
+        case MSG_CFG_MSG:
+            if(_payload_length == sizeof(ubx_cfg_msg_rate_6)) {
+                // can't verify the setting without knowing the port
+                // request the port again
+                if(_ublox_port >= UBLOX_MAX_PORTS) {
+                    _request_port();
+                    return false;
+                }
+                _verify_rate(_buffer.msg_rate_6.msg_class, _buffer.msg_rate_6.msg_id,
+                             _buffer.msg_rate_6.rates[_ublox_port]);
+            } else {
+                _verify_rate(_buffer.msg_rate.msg_class, _buffer.msg_rate.msg_id,
+                             _buffer.msg_rate.rate);
+            }
+            return false;
+        case MSG_CFG_PRT:
+           _ublox_port = _buffer.prt.portID;
+           return false;
+        case MSG_CFG_RATE:
+            if(_buffer.nav_rate.measure_rate_ms != MEASURE_RATE ||
+               _buffer.nav_rate.nav_rate != 1 ||
+               _buffer.nav_rate.timeref != 0) {
+               _configure_rate();
+                _unconfigured_messages |= CONFIG_RATE_NAV;
+            } else {
+                _unconfigured_messages &= ~CONFIG_RATE_NAV;
+            }
+            return false;
         }
+           
     }
 
-#if UBLOX_HW_LOGGING
     if (_class == CLASS_MON) {
-        if (_msg_id == MSG_MON_HW) {
+        switch(_msg_id) {
+        case MSG_MON_HW:
             if (_payload_length == 60 || _payload_length == 68) {
                 log_mon_hw();
             }
-        } else if (_msg_id == MSG_MON_HW2) {
+            break;
+        case MSG_MON_HW2:
             if (_payload_length == 28) {
                 log_mon_hw2();  
             }
-        } else {
+            break;
+        case MSG_MON_VER:
+            char version_buffer[46]; //17 string swVersion 30 hwVersion 10
+            _have_version = true;
+            hal.util->snprintf(version_buffer, sizeof(version_buffer),
+                               "u-blox %d HW: %s SW: %s",
+                                state.instance,
+                                _buffer.mon_ver.hwVersion,
+                                _buffer.mon_ver.swVersion);
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, version_buffer);
+            if (gps._DataFlash != NULL && gps._DataFlash->logging_started()) {
+                gps._DataFlash->Log_Write_Message(version_buffer);
+            }
+            break;
+        default:
             unexpected_message();
         }
         return false;
     }
-#endif // UBLOX_HW_LOGGING
 
 #if UBLOX_RXM_RAW_LOGGING
     if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAW && gps._raw_data != 0) {
@@ -652,31 +920,33 @@ AP_GPS_UBLOX::_parse_gps(void)
 #endif
         _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) {
+        _hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask;
+        switch (_hardware_generation) {
             case UBLOX_5:
             case UBLOX_6:
-                /*speed already configured */;
+                // only 7 and newer support CONFIG_GNSS
+                _unconfigured_messages &= ~CONFIG_GNSS;
                 break;
             case UBLOX_7:
             case UBLOX_M8:
+#if UBLOX_SPEED_CHANGE
                 port->begin(4000000U);
-                Debug("Changed speed to 5Mhzfor SPI-driven UBlox\n");
+                Debug("Changed speed to 4Mhz for SPI-driven UBlox\n");
+#endif
                 break;
             default:
-                hal.console->printf("Wrong Ublox' Hardware Version%u\n", hardware_generation);
+                hal.console->printf("Wrong Ublox Hardware Version%u\n", _hardware_generation);
                 break;
         };
+        _unconfigured_messages &= ~CONFIG_VERSION;
         /* 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) {
@@ -690,27 +960,16 @@ AP_GPS_UBLOX::_parse_gps(void)
     // this ensures we don't use stale data
     if (_new_position && _new_speed && _last_vel_time == _last_pos_time) {
         _new_speed = _new_position = false;
-		_fix_count++;
-        if ((AP_HAL::millis() - _last_5hz_time) > 15000U && !need_rate_update) {
-            // the GPS is running slow. It possibly browned out and
-            // restarted with incorrect parameters. We will slowly
-            // send out new parameters to fix it
-            need_rate_update = true;
-            rate_update_step = 0;
+        if (AP_HAL::millis() - _last_5hz_time > 10000U) {
+            // the gps seems to be slow, possibly due to a brown out
+            // invalidate the config so it can be reset
             _last_5hz_time = AP_HAL::millis();
+            _unconfigured_messages = CONFIG_ALL;
+            _request_next_config();
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL,
+                                             "GPS: u-blox %d is not maintaining a 5Hz update",
+                                             state.instance);
         }
-
-		if (_fix_count == 50 && gps._sbas_mode != 2) {
-			// ask for SBAS settings every 20 seconds
-			Debug("Asking for SBAS setting\n");
-			_send_message(CLASS_CFG, MSG_CFG_SBAS, NULL, 0);
-		}
-		if (_fix_count == 100) {
-			// ask for nav settings every 20 seconds
-			Debug("Asking for engine setting\n");
-			_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
-            _fix_count = 0;
-		}
         return true;
     }
     return false;
@@ -756,49 +1015,54 @@ AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16
     port->write((const uint8_t *)&ck_b, 1);
 }
 
+/*
+ *  requests the given message rate for a specific message class
+ *  and msg_id
+ *  returns true if it sent the request, false if waiting on knowing the port
+ */
+bool
+AP_GPS_UBLOX::_request_message_rate(uint8_t msg_class, uint8_t msg_id)
+{
+    // Without knowing what communication port is being used it isn't possible to verify
+    // always ensure we have a port before sending the request
+    if(_ublox_port >= UBLOX_MAX_PORTS) {
+        _request_port();
+        return false;
+    } else {
+        struct ubx_cfg_msg msg;
+        msg.msg_class = msg_class;
+        msg.msg_id    = msg_id;
+        _send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg));
+        return true;
+    }
+}
 
 /*
  *  configure a UBlox GPS for the given message rate for a specific
  *  message class and msg_id
  */
-void
+bool
 AP_GPS_UBLOX::_configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
 {
+    if (port->txspace() < (int16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_msg_rate)+2)) {
+        return false;
+    }
+
     struct ubx_cfg_msg_rate msg;
     msg.msg_class = msg_class;
     msg.msg_id    = msg_id;
     msg.rate          = rate;
-    _send_message(CLASS_CFG, MSG_CFG_SET_RATE, &msg, sizeof(msg));
-}
-
-/*
- *  configure a UBlox GPS navigation solution rate of 200ms
- */
-void
-AP_GPS_UBLOX::_configure_navigation_rate(uint16_t rate_ms)
-{
-    struct ubx_cfg_nav_rate msg;
-    msg.measure_rate_ms = rate_ms;
-    msg.nav_rate        = 1;
-    msg.timeref         = 0;     // UTC time
-    _send_message(CLASS_CFG, MSG_CFG_RATE, &msg, sizeof(msg));
+    _send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg));
+    return true;
 }
 
 /*
- *  configure a UBlox GPS for the given message rate
+ *  request the current naviation solution rate
  */
 void
-AP_GPS_UBLOX::_configure_gps(void)
+AP_GPS_UBLOX::_request_navigation_rate(void)
 {
-    // start the process of updating the GPS rates
-    need_rate_update = true;
-    _last_5hz_time = AP_HAL::millis();
-    rate_update_step = 0;
-
-    // ask for the current navigation settings
-	Debug("Asking for engine setting\n");
-    _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
-    _send_message(CLASS_CFG, MSG_CFG_GNSS, NULL, 0);
+    _send_message(CLASS_CFG, MSG_CFG_RATE, 0, 0);
 }
 
 /*
@@ -813,6 +1077,8 @@ AP_GPS_UBLOX::_save_cfg()
     save_cfg.saveMask = SAVE_CFG_ALL;
     save_cfg.loadMask = 0;
     _send_message(CLASS_CFG, MSG_CFG_CFG, &save_cfg, sizeof(save_cfg));
+    _last_cfg_sent_time = AP_HAL::millis();
+    _num_cfg_save_tries++;
 }
 
 /*
@@ -880,5 +1146,15 @@ reset:
 void
 AP_GPS_UBLOX::_request_version(void)
 {
-    _configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 2);
+    _send_message(CLASS_MON, MSG_MON_VER, NULL, 0);
+}
+
+void
+AP_GPS_UBLOX::_configure_rate(void)
+{
+    struct ubx_cfg_nav_rate msg;
+    msg.measure_rate_ms = MEASURE_RATE;
+    msg.nav_rate        = 1;
+    msg.timeref         = 0;     // UTC time
+    _send_message(CLASS_CFG, MSG_CFG_RATE, &msg, sizeof(msg));
 }

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -45,11 +45,39 @@
 #define UBLOX_MAX_RXM_RAW_SATS 22
 #define UBLOX_MAX_RXM_RAWX_SATS 32
 #define UBLOX_GNSS_SETTINGS 1
-#define UBLOX_HW_LOGGING 1
 
 #define UBLOX_MAX_GNSS_CONFIG_BLOCKS 7
 #define UBX_MSG_TYPES 2
 
+#define UBLOX_MAX_PORTS 6
+#define MEASURE_RATE 200
+
+#define RATE_POSLLH 1
+#define RATE_STATUS 1
+#define RATE_SOL 1
+#define RATE_VELNED 1
+#define RATE_DOP 1
+#define RATE_HW 5
+#define RATE_HW2 5
+
+#define CONFIG_RATE_NAV      (1<<0)
+#define CONFIG_RATE_POSLLH   (1<<1)
+#define CONFIG_RATE_STATUS   (1<<2)
+#define CONFIG_RATE_SOL      (1<<3)
+#define CONFIG_RATE_VELNED   (1<<4)
+#define CONFIG_RATE_DOP      (1<<5)
+#define CONFIG_RATE_MON_HW   (1<<6)
+#define CONFIG_RATE_MON_HW2  (1<<7)
+#define CONFIG_RATE_RAW      (1<<8)
+#define CONFIG_VERSION       (1<<9)
+#define CONFIG_NAV_SETTINGS  (1<<10)
+#define CONFIG_GNSS          (1<<11)
+#define CONFIG_SBAS          (1<<12)
+
+#define CONFIG_ALL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_SOL | CONFIG_RATE_VELNED \
+                    | CONFIG_RATE_DOP | CONFIG_RATE_MON_HW | CONFIG_RATE_MON_HW2 | CONFIG_RATE_RAW | CONFIG_VERSION \
+                    | CONFIG_NAV_SETTINGS | CONFIG_GNSS | CONFIG_SBAS)
+
 //Configuration Sub-Sections
 #define SAVE_CFG_IO     (1<<0)
 #define SAVE_CFG_MSG    (1<<1)
@@ -72,6 +100,14 @@ public:
 
     static bool _detect(struct UBLOX_detect_state &state, uint8_t data);
 
+    bool is_configured(void) {
+        if (!gps._auto_config) {
+            return true;
+        } else {
+            return !_unconfigured_messages;
+        }
+    }
+
 private:
     // u-blox UBX protocol essentials
     struct PACKED ubx_header {
@@ -101,11 +137,20 @@ private:
         uint16_t nav_rate;
         uint16_t timeref;
     };
+    struct PACKED ubx_cfg_msg {
+        uint8_t msg_class;
+        uint8_t msg_id;
+    };
     struct PACKED ubx_cfg_msg_rate {
         uint8_t msg_class;
         uint8_t msg_id;
         uint8_t rate;
     };
+    struct PACKED ubx_cfg_msg_rate_6 {
+        uint8_t msg_class;
+        uint8_t msg_id;
+        uint8_t rates[6];
+    };
     struct PACKED ubx_cfg_nav_settings {
         uint16_t mask;
         uint8_t dynModel;
@@ -124,6 +169,9 @@ private:
         uint32_t res3;
         uint32_t res4;
     };
+    struct PACKED ubx_cfg_prt {
+        uint8_t portID;
+    };
     struct PACKED ubx_cfg_sbas {
         uint8_t mode;
         uint8_t usage;
@@ -190,7 +238,6 @@ private:
         uint32_t heading_accuracy;
     };
 
-#if UBLOX_HW_LOGGING
     // Lea6 uses a 60 byte message
     struct PACKED ubx_mon_hw_60 {
         uint32_t pinSel;
@@ -243,7 +290,11 @@ private:
         uint32_t postStatus;
         uint32_t reserved2;
     };
-#endif
+    struct PACKED ubx_mon_ver {
+        char swVersion[30];
+        char hwVersion[10];
+        char extension; // extensions are not enabled
+    };
     struct PACKED ubx_nav_svinfo_header {
         uint32_t itow;
         uint8_t numCh;
@@ -311,12 +362,15 @@ private:
         ubx_nav_dop dop;
         ubx_nav_solution solution;
         ubx_nav_velned velned;
+        ubx_cfg_msg_rate msg_rate;
+        ubx_cfg_msg_rate_6 msg_rate_6;
         ubx_cfg_nav_settings nav_settings;
-#if UBLOX_HW_LOGGING
+        ubx_cfg_nav_rate nav_rate;
+        ubx_cfg_prt prt;
         ubx_mon_hw_60 mon_hw_60;
         ubx_mon_hw_68 mon_hw_68;
         ubx_mon_hw2 mon_hw2;
-#endif
+        ubx_mon_ver mon_ver;
 #if UBLOX_GNSS_SETTINGS
         ubx_cfg_gnss gnss;
 #endif
@@ -346,14 +400,15 @@ private:
         MSG_SOL = 0x6,
         MSG_VELNED = 0x12,
         MSG_CFG_CFG = 0x09,
-        MSG_CFG_PRT = 0x00,
         MSG_CFG_RATE = 0x08,
-        MSG_CFG_SET_RATE = 0x01,
+        MSG_CFG_MSG = 0x01,
         MSG_CFG_NAV_SETTINGS = 0x24,
+        MSG_CFG_PRT = 0x00,
         MSG_CFG_SBAS = 0x16,
         MSG_CFG_GNSS = 0x3E,
         MSG_MON_HW = 0x09,
         MSG_MON_HW2 = 0x0B,
+        MSG_MON_VER = 0x04,
         MSG_NAV_SVINFO = 0x30,
         MSG_RXM_RAW = 0x10,
         MSG_RXM_RAWX = 0x15
@@ -387,6 +442,29 @@ private:
         UBLOX_M8
     };
 
+    enum config_step {
+        STEP_RATE_NAV = 0,
+        STEP_RATE_POSLLH,
+        STEP_RATE_VELNED,
+        STEP_PORT,
+        STEP_POLL_SVINFO,
+        STEP_POLL_SBAS,
+        STEP_POLL_NAV,
+        STEP_POLL_GNSS,
+        STEP_NAV_RATE,
+        STEP_POSLLH,
+        STEP_STATUS,
+        STEP_SOL,
+        STEP_VELNED,
+        STEP_DOP,
+        STEP_MON_HW,
+        STEP_MON_HW2,
+        STEP_RAW,
+        STEP_RAWX,
+        STEP_VERSION,
+        STEP_LAST
+    };
+
     // Packet checksum accumulators
     uint8_t         _ck_a;
     uint8_t         _ck_b;
@@ -397,9 +475,6 @@ private:
     uint16_t        _payload_length;
     uint16_t        _payload_counter;
 
-    // 8 bit count of fix messages processed, used for periodic
-    // processing
-    uint8_t         _fix_count;
     uint8_t         _class;
     bool            _cfg_saved;
 
@@ -407,12 +482,19 @@ private:
     uint32_t        _last_pos_time;
     uint32_t        _last_cfg_sent_time;
     uint8_t         _num_cfg_save_tries;
+    uint32_t        _last_config_time;
+    uint16_t        _delay_time;
+    uint8_t         _next_message;
+    uint8_t         _ublox_port;
+    bool            _have_version;
+    uint32_t        _unconfigured_messages;
+    uint8_t         _hardware_generation;
+
 
     // do we have new position information?
     bool            _new_position:1;
     // do we have new speed information?
     bool            _new_speed:1;
-    bool            need_rate_update:1;
 
     uint8_t         _disable_counter;
 
@@ -422,25 +504,29 @@ private:
     // used to update fix between status and position packets
     AP_GPS::GPS_Status next_fix;
 
-    uint8_t rate_update_step;
     uint32_t _last_5hz_time;
 
     bool noReceivedHdop;
 
-    void 	    _configure_navigation_rate(uint16_t rate_ms);
-    void        _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
-    void        _configure_gps(void);
+    bool        _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
+    void        _configure_rate(void);
     void        _configure_sbas(bool enable);
     void        _update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b);
     void        _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size);
-    void		send_next_rate_update(void);
+    void	send_next_rate_update(void);
+    bool        _request_message_rate(uint8_t msg_class, uint8_t msg_id);
+    void        _request_navigation_rate(void);
+    void        _request_next_config(void);
+    void        _request_port(void);
     void        _request_version(void);
     void        _save_cfg(void);
+    void        _verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
 
     void unexpected_message(void);
     void write_logging_headers(void);
     void log_mon_hw(void);
     void log_mon_hw2(void);
+    void log_mon_ver(void);
     void log_rxm_raw(const struct ubx_rxm_raw &raw);
     void log_rxm_rawx(const struct ubx_rxm_rawx &raw);
 

libraries/AP_GPS/GPS_Backend.h

@@ -37,12 +37,14 @@ public:
     // valid packet from the GPS.
     virtual bool read() = 0;
 
-    virtual void inject_data(uint8_t *data, uint8_t len) { return; }
-
     // Highest status supported by this GPS. 
     // Allows external system to identify type of receiver connected.
     virtual AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D; }
 
+    virtual bool is_configured(void) { return true; }
+
+    virtual void inject_data(uint8_t *data, uint8_t len) { return; }
+
     //MAVLink methods
     virtual void send_mavlink_gps_rtk(mavlink_channel_t chan) { return ; }