Skip to content

Commit a85daf0

Browse files
nseidlenseidle
committed
Get setConstellations working
1 parent b26872d commit a85daf0

File tree

1 file changed

+284
-0
lines changed

1 file changed

+284
-0
lines changed

‎Firmware/Test Sketches/SetConstellations/SetConstellations.ino

Lines changed: 284 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,284 @@
1+
/*
2+
Set correct bits to enable/disable constallations on the ZED-F9P
3+
4+
See -CFG-GNSS in ZED's protocol doc
5+
6+
21:20:31 0000 B5 62 06 3E (len)34 00(len)
7+
00(ver) 3C(tracking chan) 3C(chan) 06(config blocks)
8+
00(gnssid) 08 10 00 01(enable) 00 11(sigCfgMask) 11 GPS
9+
01 03 03 00 01 00 01 01 SBAS
10+
02 0A 12 00 01 00 21 21 Gal
11+
03 02 05 00 01 00 11 11 Bei
12+
05 00 04 00 01 00 11 15 QZSS <-- Note 04 IMES is missing
13+
06 08 0C 00 01 00 11 11 GLO
14+
Above is poll of default config from u-center
15+
16+
21:20:31 0000 B5 62 06 3E 34 00
17+
00 00 3C 06
18+
00 08 10 00 00 00 11 01 GPS
19+
01 03 03 00 01 00 01 01 SBAS
20+
02 0A 12 00 01 00 21 01 Gal
21+
03 02 05 00 01 00 11 01 Bei
22+
05 00 04 00 00 00 11 01 QZSS
23+
06 08 0C 00 01 00 11 01 GLO
24+
Above is command for GPS and QZSS turned off
25+
26+
Ugh. The issue is that the doc says IMES is gnssid 4 but really QZSS is in 4th position but with ID 5.
27+
28+
Works:
29+
GPS
30+
GLONASS
31+
SBAS
32+
Galileo
33+
BeiDou
34+
QZSS
35+
36+
Not working:
37+
IMES - Does not seem to be supported
38+
39+
*/
40+
41+
#include <Wire.h> //Needed for I2C to GPS
42+
43+
#include <SparkFun_u-blox_GNSS_Arduino_Library.h> //http://librarymanager/All#SparkFun_u-blox_GNSS
44+
SFE_UBLOX_GNSS i2cGNSS;
45+
46+
#define MAX_PAYLOAD_SIZE 384 // Override MAX_PAYLOAD_SIZE for getModuleInfo which can return up to 348 bytes
47+
48+
void setup()
49+
{
50+
Serial.begin(115200);
51+
delay(200); //Wait for ESP32
52+
Serial.println("SparkFun u-blox Example");
53+
54+
Wire.begin();
55+
56+
//i2cGNSS.enableDebugging(); // Uncomment this line to enable debug messages
57+
58+
if (i2cGNSS.begin() == false) //Connect to the Ublox module using Wire port
59+
{
60+
Serial.println(F("u-blox GPS not detected at default I2C address. Please check wiring. Freezing."));
61+
while (1);
62+
}
63+
64+
}
65+
66+
void loop()
67+
{
68+
if (Serial.available())
69+
{
70+
byte incoming = Serial.read();
71+
72+
if (incoming == 'G')
73+
{
74+
if (setConstellation(SFE_UBLOX_GNSS_ID_GPS, true) == false)
75+
Serial.println("Enable GPS Fail");
76+
else
77+
Serial.println("Enable GPS Success");
78+
}
79+
else if (incoming == 'g')
80+
{
81+
if (setConstellation(SFE_UBLOX_GNSS_ID_GPS, false) == false)
82+
Serial.println("Disable GPS Fail");
83+
else
84+
Serial.println("Disable GPS Success");
85+
}
86+
else if (incoming == 'R')
87+
{
88+
if (setConstellation(SFE_UBLOX_GNSS_ID_GLONASS, true) == false)
89+
Serial.println("Enable GLONASS Fail");
90+
else
91+
Serial.println("Enable GLONASS Success");
92+
}
93+
else if (incoming == 'r')
94+
{
95+
if (setConstellation(SFE_UBLOX_GNSS_ID_GLONASS, false) == false)
96+
Serial.println("Disable GLONASS Fail");
97+
else
98+
Serial.println("Disable GLONASS Success");
99+
}
100+
else if (incoming == 'S')
101+
{
102+
if (setConstellation(SFE_UBLOX_GNSS_ID_SBAS, true) == false)
103+
Serial.println("Enable SBAS Fail");
104+
else
105+
Serial.println("Enable SBAS Success");
106+
}
107+
else if (incoming == 's')
108+
{
109+
if (setConstellation(SFE_UBLOX_GNSS_ID_SBAS, false) == false)
110+
Serial.println("Disable SBAS Fail");
111+
else
112+
Serial.println("Disable SBAS Success");
113+
}
114+
else if (incoming == 'A')
115+
{
116+
if (setConstellation(SFE_UBLOX_GNSS_ID_GALILEO, true) == false)
117+
Serial.println("Enable Galileo Fail");
118+
else
119+
Serial.println("Enable Galileo Success");
120+
}
121+
else if (incoming == 'a')
122+
{
123+
if (setConstellation(SFE_UBLOX_GNSS_ID_GALILEO, false) == false)
124+
Serial.println("Disable Galileo Fail");
125+
else
126+
Serial.println("Disable Galileo Success");
127+
}
128+
else if (incoming == 'B')
129+
{
130+
if (setConstellation(SFE_UBLOX_GNSS_ID_BEIDOU, true) == false)
131+
Serial.println("Enable BeiDou Fail");
132+
else
133+
Serial.println("Enable BeiDou Success");
134+
}
135+
else if (incoming == 'b')
136+
{
137+
if (setConstellation(SFE_UBLOX_GNSS_ID_BEIDOU, false) == false)
138+
Serial.println("Disable BeiDou Fail");
139+
else
140+
Serial.println("Disable BeiDou Success");
141+
}
142+
else if (incoming == 'Q')
143+
{
144+
if (setConstellation(SFE_UBLOX_GNSS_ID_QZSS, true) == false)
145+
Serial.println("Enable QZSS Fail");
146+
else
147+
Serial.println("Enable QZSS Success");
148+
}
149+
else if (incoming == 'q')
150+
{
151+
if (setConstellation(SFE_UBLOX_GNSS_ID_QZSS, false) == false)
152+
Serial.println("Disable QZSS Fail");
153+
else
154+
Serial.println("Disable QZSS Success");
155+
}
156+
else
157+
{
158+
//Serial.println("Unknown");
159+
}
160+
}
161+
162+
}
163+
164+
//The u-blox library doesn't directly support constellation control so let's do it manually
165+
//Also allows the enable/disable of any constellation (BeiDou, Galileo, etc)
166+
bool setConstellation(uint8_t constellation, bool enable)
167+
{
168+
uint8_t customPayload[MAX_PAYLOAD_SIZE]; // This array holds the payload data bytes
169+
ubxPacket customCfg = {0, 0, 0, 0, 0, customPayload, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
170+
171+
customCfg.cls = UBX_CLASS_CFG; // This is the message Class
172+
customCfg.id = UBX_CFG_GNSS; // This is the message ID
173+
customCfg.len = 0; // Setting the len (length) to zero lets us poll the current settings
174+
customCfg.startingSpot = 0; // Always set the startingSpot to zero (unless you really know what you are doing)
175+
176+
uint16_t maxWait = 1250; // Wait for up to 250ms (Serial may need a lot longer e.g. 1100)
177+
178+
// Read the current setting. The results will be loaded into customCfg.
179+
if (i2cGNSS.sendCommand(&customCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are expecting data and an ACK
180+
{
181+
Serial.println(F("Set Constellation failed"));
182+
return (false);
183+
}
184+
185+
if (enable)
186+
{
187+
if (constellation == SFE_UBLOX_GNSS_ID_GPS || constellation == SFE_UBLOX_GNSS_ID_QZSS)
188+
{
189+
//QZSS must follow GPS
190+
customPayload[locateGNSSID(customPayload, SFE_UBLOX_GNSS_ID_GPS) + 4] |= (1 << 0); //Set the enable bit
191+
customPayload[locateGNSSID(customPayload, SFE_UBLOX_GNSS_ID_QZSS) + 4] |= (1 << 0); //Set the enable bit
192+
}
193+
else
194+
{
195+
customPayload[locateGNSSID(customPayload, constellation) + 4] |= (1 << 0); //Set the enable bit
196+
}
197+
198+
//Set sigCfgMask as well
199+
if (constellation == SFE_UBLOX_GNSS_ID_GPS || constellation == SFE_UBLOX_GNSS_ID_QZSS)
200+
{
201+
customPayload[locateGNSSID(customPayload, SFE_UBLOX_GNSS_ID_GPS) + 6] |= 0x11; //Enable GPS L1C/A, and L2C
202+
203+
//QZSS must follow GPS
204+
customPayload[locateGNSSID(customPayload, SFE_UBLOX_GNSS_ID_QZSS) + 6] = 0x11; //Enable QZSS L1C/A, and L2C - Follow u-center
205+
//customPayload[locateGNSSID(customPayload, SFE_UBLOX_GNSS_ID_QZSS) + 6] = 0x15; //Enable QZSS L1C/A, L1S, and L2C
206+
}
207+
else if (constellation == SFE_UBLOX_GNSS_ID_SBAS)
208+
{
209+
customPayload[locateGNSSID(customPayload, constellation) + 6] |= 0x01; //Enable SBAS L1C/A
210+
}
211+
else if (constellation == SFE_UBLOX_GNSS_ID_GALILEO)
212+
{
213+
customPayload[locateGNSSID(customPayload, constellation) + 6] |= 0x21; //Enable Galileo E1/E5b
214+
}
215+
else if (constellation == SFE_UBLOX_GNSS_ID_BEIDOU)
216+
{
217+
customPayload[locateGNSSID(customPayload, constellation) + 6] |= 0x11; //Enable BeiDou B1I/B2I
218+
}
219+
// else if (constellation == SFE_UBLOX_GNSS_ID_IMES) //Does not exist in u-center v21.02
220+
// {
221+
// customPayload[locateGNSSID(customPayload, constellation) + 6] |= 0x01; //Enable IMES L1
222+
// }
223+
else if (constellation == SFE_UBLOX_GNSS_ID_GLONASS)
224+
{
225+
customPayload[locateGNSSID(customPayload, constellation) + 6] |= 0x11; //Enable GLONASS L1 and L2
226+
}
227+
}
228+
else //Disable
229+
{
230+
//QZSS must follow GPS
231+
if (constellation == SFE_UBLOX_GNSS_ID_GPS || constellation == SFE_UBLOX_GNSS_ID_QZSS)
232+
{
233+
customPayload[locateGNSSID(customPayload, SFE_UBLOX_GNSS_ID_GPS) + 4] &= ~(1 << 0); //Clear the enable bit
234+
235+
customPayload[locateGNSSID(customPayload, SFE_UBLOX_GNSS_ID_QZSS) + 4] &= ~(1 << 0); //Clear the enable bit
236+
}
237+
else
238+
{
239+
customPayload[locateGNSSID(customPayload, constellation) + 4] &= ~(1 << 0); //Clear the enable bit
240+
}
241+
242+
}
243+
244+
Serial.println("Custom payload:");
245+
for (int x = 0 ; x < 4 + 8 * 6 ; x++)
246+
{
247+
if (x == 4 + 8 * 0) Serial.println(); //Hdr
248+
if (x == 4 + 8 * 1) Serial.println(); //GPS
249+
if (x == 4 + 8 * 2) Serial.println();
250+
if (x == 4 + 8 * 3) Serial.println();
251+
if (x == 4 + 8 * 4) Serial.println();
252+
if (x == 4 + 8 * 5) Serial.println();
253+
if (x == 4 + 8 * 6) Serial.println();
254+
if (x == 4 + 8 * 7) Serial.println();
255+
Serial.print(" ");
256+
if (customPayload[x] < 0x10) Serial.print("0");
257+
Serial.print(customPayload[x], HEX);
258+
}
259+
Serial.println();
260+
261+
// Now we write the custom packet back again to change the setting
262+
if (i2cGNSS.sendCommand(&customCfg, maxWait) != SFE_UBLOX_STATUS_DATA_SENT) // This time we are only expecting an ACK
263+
{
264+
Serial.println(F("Constellation setting failed"));
265+
return (false);
266+
}
267+
268+
return (true);
269+
}
270+
271+
//Given a payload, return the location of a given constellation
272+
//This is needed because IMES is not currently returned in the query packet
273+
//so QZSS and GLONAS are offset by -8 bytes.
274+
uint8_t locateGNSSID(uint8_t *customPayload, uint8_t constellation)
275+
{
276+
for (int x = 0 ; x < 7 ; x++) //Assume max of 7 constellations
277+
{
278+
if (customPayload[4 + 8 * x] == constellation) //Test gnssid
279+
return (4 + x * 8);
280+
}
281+
282+
Serial.println(F("locateGNSSID failed"));
283+
return (0);
284+
}

0 commit comments

Comments
 (0)