AP_GPS: Enable selecting what GNSS system to use if the reciever supports it

Ublox 7 and 8 seires use a UBX-CFG-GNSS message to enable satellite constellations. The default value does not enable any additional ones, and any constellations the reciever doesn't report knowing about are not configured.

libraries/AP_GPS/AP_GPS.cpp

@@ -106,6 +106,13 @@ const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = {
     AP_GROUPINFO("RAW_DATA", 9, AP_GPS, _raw_data, 0),
 #endif
 
+    // @Param: GNSS_MODE
+    // @DisplayName: GNSS system configuration
+    // @Description: Bitmask for what GNSS system to use
+    // @Values: 0: Leave as currently configured 1: GPS 2: SBAS 4: Galileo 8: Beidou 16: IMES 32: QZSS 64: GLONASS
+    // @User: Advanced
+    AP_GROUPINFO("GNSS_MODE", 10, AP_GPS, _gnss_mode, 0),
+
     AP_GROUPEND
 };
 

libraries/AP_GPS/AP_GPS.h

@@ -338,6 +338,7 @@ public:
     AP_Int8 _sbas_mode;
     AP_Int8 _min_elevation;
     AP_Int8 _raw_data;
+    AP_Int8 _gnss_mode;
     
     // handle sending of initialisation strings to the GPS
     void send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size);

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -452,6 +452,50 @@ AP_GPS_UBLOX::_parse_gps(void)
         return false;
     }
 
+    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);
+#if UBLOX_DEBUG
+        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 = 1; i <= UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
+            if((gps._gnss_mode & (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;
+                } else {
+                    _buffer.gnss.configBlock[i].resTrkCh = 1;
+                    _buffer.gnss.configBlock[i].resTrkCh = 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;
+            }
+        }
+        _send_message(CLASS_CFG, MSG_CFG_GNSS, &_buffer.gnss, 4 + (8 * _buffer.gnss.numConfigBlocks));
+        return false;
+    }
+
     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,
@@ -755,6 +799,7 @@ AP_GPS_UBLOX::_configure_gps(void)
     // 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);
 }
 
 

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -49,6 +49,8 @@
 #define UBLOX_RXM_RAW_LOGGING 0
 #endif
 
+#define UBLOX_MAX_GNSS_CONFIG_BLOCKS 7
+
 class AP_GPS_UBLOX : public AP_GPS_Backend
 {
 public:
@@ -70,6 +72,19 @@ private:
         uint8_t msg_id;
         uint16_t length;
     };
+    struct PACKED ubx_cfg_gnss {
+        uint8_t msgVer;
+        uint8_t numTrkChHw;
+        uint8_t numTrkChUse;
+        uint8_t numConfigBlocks;
+        struct configBlock {
+            uint8_t gnssId;
+            uint8_t resTrkCh;
+            uint8_t maxTrkCh;
+            uint8_t reserved1;
+            uint32_t flags;
+        } configBlock[UBLOX_MAX_GNSS_CONFIG_BLOCKS];
+    };
     struct PACKED ubx_cfg_nav_rate {
         uint16_t measure_rate_ms;
         uint16_t nav_rate;
@@ -272,6 +287,7 @@ private:
         ubx_mon_hw_60 mon_hw_60;
         ubx_mon_hw_68 mon_hw_68;
         ubx_mon_hw2 mon_hw2;
+        ubx_cfg_gnss gnss;
         ubx_cfg_sbas sbas;
         ubx_nav_svinfo_header svinfo_header;
 #if UBLOX_RXM_RAW_LOGGING
@@ -301,12 +317,22 @@ private:
         MSG_CFG_SET_RATE = 0x01,
         MSG_CFG_NAV_SETTINGS = 0x24,
         MSG_CFG_SBAS = 0x16,
+        MSG_CFG_GNSS = 0x3E,
         MSG_MON_HW = 0x09,
         MSG_MON_HW2 = 0x0B,
         MSG_NAV_SVINFO = 0x30,
         MSG_RXM_RAW = 0x10,
         MSG_RXM_RAWX = 0x15
     };
+    enum ubx_gnss_identifier {
+        GNSS_GPS     = 0x00,
+        GNSS_SBAS    = 0x01,
+        GNSS_GALILEO = 0x02,
+        GNSS_BEIDOU  = 0x03,
+        GNSS_IMES    = 0x04,
+        GNSS_QZSS    = 0x05,
+        GNSS_GLONASS = 0x06
+    };
     enum ubs_nav_fix_type {
         FIX_NONE = 0,
         FIX_DEAD_RECKONING = 1,