Fix constellation enable/disable.

Author: nseidle

Firmware/RTK_Surveyor/Rover.ino

@@ -100,7 +100,7 @@ bool setNMEASettings()
 }
 
 //Returns true if constellation is enabled
-bool getConstellation(uint8_t constellation)
+bool getConstellation(uint8_t constellationID)
 {
   uint8_t customPayload[MAX_PAYLOAD_SIZE]; // This array holds the payload data bytes
   ubxPacket customCfg = {0, 0, 0, 0, 0, customPayload, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
@@ -110,7 +110,7 @@ bool getConstellation(uint8_t constellation)
   customCfg.len = 0; // Setting the len (length) to zero lets us poll the current settings
   customCfg.startingSpot = 0; // Always set the startingSpot to zero (unless you really know what you are doing)
 
-  uint16_t maxWait = 1250; // Wait for up to 250ms (Serial may need a lot longer e.g. 1100)
+  uint16_t maxWait = 1250; // Wait for up to 1250ms (Serial may need a lot longer e.g. 1100)
 
   // Read the current setting. The results will be loaded into customCfg.
   if (i2cGNSS.sendCommand(&customCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are expecting data and an ACK
@@ -119,7 +119,7 @@ bool getConstellation(uint8_t constellation)
     return (false);
   }
 
-  if (customPayload[8 + 8 * constellation] & (1 << 0)) return true; //Check if bit 0 is set
+  if (customPayload[locateGNSSID(customPayload, constellationID) + 4] & (1 << 0)) return true; //Check if enable bit is set
   return false;
 }
 
@@ -214,7 +214,7 @@ bool setConstellation(uint8_t constellation, bool enable)
 //so QZSS and GLONAS are offset by -8 bytes.
 uint8_t locateGNSSID(uint8_t *customPayload, uint8_t constellation)
 {
-  for (int x = 0 ; x < 7 ; x++) //Assume max of 7 constellations
+  for (int x = 0 ; x < MAX_CONSTELLATIONS ; x++)
   {
     if (customPayload[4 + 8 * x] == constellation) //Test gnssid
       return (4 + x * 8);

Firmware/RTK_Surveyor/menuGNSS.ino

@@ -162,13 +162,15 @@ void menuConstellations()
 
     if (incoming >= 1 && incoming <= MAX_CONSTELLATIONS)
     {
-      ubxConstellations[incoming - 1].enabled ^= 1;
+      incoming--; //Align choice to constallation array of 0 to 5
+      
+      ubxConstellations[incoming].enabled ^= 1;
 
       //3.10.6: To avoid cross-correlation issues, it is recommended that GPS and QZSS are always both enabled or both disabled.
-      if ((incoming - 1) == SFE_UBLOX_GNSS_ID_GPS || (incoming - 1) == SFE_UBLOX_GNSS_ID_QZSS)
+      if (incoming == SFE_UBLOX_GNSS_ID_GPS || incoming == 4) //QZSS ID is 5 but array location is 4
       {
-        ubxConstellations[SFE_UBLOX_GNSS_ID_GPS].enabled = ubxConstellations[incoming - 1].enabled;
-        ubxConstellations[SFE_UBLOX_GNSS_ID_QZSS].enabled = ubxConstellations[incoming - 1].enabled;
+        ubxConstellations[SFE_UBLOX_GNSS_ID_GPS].enabled = ubxConstellations[incoming].enabled; //GPS ID is 0 and array location is 0
+        ubxConstellations[4].enabled = ubxConstellations[incoming].enabled; //QZSS ID is 5 but array location is 4
       }
     }
     else if (incoming == STATUS_PRESSED_X)

Firmware/Test Sketches/SetConstellations/SetConstellations.ino

@@ -30,8 +30,8 @@
   05 00 04 00 01 00 11 11
   06 08 0C 00 00 00 11 11
   00 00 00 00 00 00 00 00
-Above, on ZED-F9P v1.12, BeiDou is disabled. Why is SBAS not being reported?
-Ah, it's a v1.12 bug. Works fine in v1.13 and on ZED-F9R v1.0
+  Above, on ZED-F9P v1.12, BeiDou is disabled. Why is SBAS not being reported?
+  Ah, it's a v1.12 bug. Works fine in v1.13 and on ZED-F9R v1.0
 
   Ugh. The issue is that the doc says IMES is gnssid 4 but really QZSS is in 4th position but with ID 5.
 
@@ -63,7 +63,7 @@ void setup()
 
   Wire.begin();
   Wire.setClock(400000);
-  
+
   //i2cGNSS.enableDebugging(); // Uncomment this line to enable debug messages
 
   if (i2cGNSS.begin() == false) //Connect to the Ublox module using Wire port
@@ -164,10 +164,41 @@ void loop()
       else
         Serial.println("Disable QZSS Success");
     }
+    else if(incoming == '!')
+    {
+    for(int x = 0 ; x < 6 ; x++)
+    {
+      bool isEnabled = false;
+      if(x == 0) isEnabled = getConstellation(SFE_UBLOX_GNSS_ID_GPS);
+      else if(x == 1) isEnabled = getConstellation(SFE_UBLOX_GNSS_ID_SBAS);
+      else if(x == 2) isEnabled = getConstellation(SFE_UBLOX_GNSS_ID_GALILEO);
+      else if(x == 3) isEnabled = getConstellation(SFE_UBLOX_GNSS_ID_BEIDOU);
+      else if(x == 4) isEnabled = getConstellation(SFE_UBLOX_GNSS_ID_QZSS);
+      else if(x == 5) isEnabled = getConstellation(SFE_UBLOX_GNSS_ID_GLONASS);
+
+      Serial.print("Module reports ");
+      if(x == 0) Serial.print("GPS");
+      else if(x == 1) Serial.print("SBAS");
+      else if(x == 2) Serial.print("GALILEO");
+      else if(x == 3) Serial.print("BeiDou");
+      else if(x == 4) Serial.print("QZSS");
+      else if(x == 5) Serial.print("GLONASS");
+      Serial.print(": ");
+      if (isEnabled == true)
+        Serial.println("Enabled");
+      else
+        Serial.println("Disabled");
+    }      
+    }
+    else if(incoming == '\n' || incoming == '\r')
+    {
+      //Do nothing
+    }
     else
     {
       //Serial.println("Unknown");
     }
+   
   }
 
 }
@@ -293,3 +324,27 @@ uint8_t locateGNSSID(uint8_t *customPayload, uint8_t constellation)
   Serial.println(F("locateGNSSID failed"));
   return (0);
 }
+
+//Returns true if constellation is enabled
+bool getConstellation(uint8_t constellationID)
+{
+  uint8_t customPayload[MAX_PAYLOAD_SIZE]; // This array holds the payload data bytes
+  ubxPacket customCfg = {0, 0, 0, 0, 0, customPayload, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
+
+  customCfg.cls = UBX_CLASS_CFG; // This is the message Class
+  customCfg.id = UBX_CFG_GNSS; // This is the message ID
+  customCfg.len = 0; // Setting the len (length) to zero lets us poll the current settings
+  customCfg.startingSpot = 0; // Always set the startingSpot to zero (unless you really know what you are doing)
+
+  uint16_t maxWait = 1250; // Wait for up to 250ms (Serial may need a lot longer e.g. 1100)
+
+  // Read the current setting. The results will be loaded into customCfg.
+  if (i2cGNSS.sendCommand(&customCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are expecting data and an ACK
+  {
+    Serial.println(F("Get Constellation failed"));
+    return (false);
+  }
+
+  if (customPayload[locateGNSSID(customPayload, constellationID) + 4] & (1 << 0)) return true; //Check if bit 0 is set
+  return false;
+}