AP_GPS: ublox support user controlled rate updates

Also corrects accepting bad nav rates as configured, and fixes a incorrect
index as to which GPS is being updated

libraries/AP_GPS/AP_GPS.cpp

@@ -140,6 +140,20 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("AUTO_CONFIG", 13, AP_GPS, _auto_config, 1),
 
+    // @Param: RATE_MS
+    // @DisplayName: GPS update rate in milliseconds
+    // @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz is not allowed
+    // @Values: 100:10Hz,125:8Hz,200:5Hz
+    // @User: Advanced
+    AP_GROUPINFO("RATE_MS", 14, AP_GPS, _rate_ms[0], 200),
+
+    // @Param: RATE_MS2
+    // @DisplayName: GPS 2 update rate in milliseconds
+    // @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz is not allowed
+    // @Values: 100:10Hz,125:8Hz,200:5Hz
+    // @User: Advanced
+    AP_GROUPINFO("RATE_MS2", 15, AP_GPS, _rate_ms[1], 200),
+
     AP_GROUPEND
 };
 

libraries/AP_GPS/AP_GPS.h

@@ -338,6 +338,7 @@ public:
     AP_Int8 _min_elevation;
     AP_Int8 _raw_data;
     AP_Int8 _gnss_mode[2];
+    AP_Int16 _rate_ms[2];
     AP_Int8 _save_config;
     AP_Int8 _auto_config;
     

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -608,7 +608,9 @@ AP_GPS_UBLOX::_parse_gps(void)
                     _unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
                     break;
                 case MSG_CFG_RATE:
-                    _unconfigured_messages &= ~CONFIG_RATE_NAV;
+                    // The GPS will ACK a update rate that is invalid. in order to detect this
+                    // only accept the rate as configured by reading the settings back and
+                   //  validating that they all match the target values
                     break;
                 case MSG_CFG_SBAS:
                     _unconfigured_messages &= ~CONFIG_SBAS;
@@ -760,7 +762,7 @@ AP_GPS_UBLOX::_parse_gps(void)
            _ublox_port = _buffer.prt.portID;
            return false;
         case MSG_CFG_RATE:
-            if(_buffer.nav_rate.measure_rate_ms != MEASURE_RATE ||
+            if(_buffer.nav_rate.measure_rate_ms != gps._rate_ms[state.instance] ||
                _buffer.nav_rate.nav_rate != 1 ||
                _buffer.nav_rate.timeref != 0) {
                _configure_rate();
@@ -1069,7 +1071,7 @@ AP_GPS_UBLOX::_save_cfg()
     _num_cfg_save_tries++;
     GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO,
                                      "GPS: u-blox %d saving config",
-                                     state.instance);
+                                     state.instance + 1);
 }
 
 /*
@@ -1144,7 +1146,8 @@ void
 AP_GPS_UBLOX::_configure_rate(void)
 {
     struct ubx_cfg_nav_rate msg;
-    msg.measure_rate_ms = MEASURE_RATE;
+    // require a minimum measurement rate of 5Hz
+    msg.measure_rate_ms = MIN(gps._rate_ms[state.instance], MINIMUM_MEASURE_RATE_MS);
     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

@@ -49,7 +49,7 @@
 #define UBX_MSG_TYPES 2
 
 #define UBLOX_MAX_PORTS 6
-#define MEASURE_RATE 200
+#define MINIMUM_MEASURE_RATE_MS 200
 
 #define RATE_POSLLH 1
 #define RATE_STATUS 1