Skip to content

Commit 8d1e600

Browse files
PaulZCPaulZC
committed
Retry startFixedBase up to 3 times
1 parent de3af28 commit 8d1e600

File tree

1 file changed

+84
-68
lines changed

1 file changed

+84
-68
lines changed

‎Firmware/RTK_Surveyor/Base.ino‎

Lines changed: 84 additions & 68 deletions
Original file line numberDiff line numberDiff line change
@@ -239,76 +239,92 @@ bool surveyInReset()
239239
bool startFixedBase()
240240
{
241241
bool response = true;
242+
int retries = 0;
243+
uint16_t maxWait = 1100;
242244

243-
if (settings.fixedBaseCoordinateType == COORD_TYPE_ECEF)
244-
{
245-
// Break ECEF into main and high precision parts
246-
// The type casting should not effect rounding of original double cast coordinate
247-
long majorEcefX = floor((settings.fixedEcefX * 100.0) + 0.5);
248-
long minorEcefX = floor((((settings.fixedEcefX * 100.0) - majorEcefX) * 100.0) + 0.5);
249-
long majorEcefY = floor((settings.fixedEcefY * 100) + 0.5);
250-
long minorEcefY = floor((((settings.fixedEcefY * 100.0) - majorEcefY) * 100.0) + 0.5);
251-
long majorEcefZ = floor((settings.fixedEcefZ * 100) + 0.5);
252-
long minorEcefZ = floor((((settings.fixedEcefZ * 100.0) - majorEcefZ) * 100.0) + 0.5);
253-
254-
// systemPrintf("fixedEcefY (should be -4716808.5807): %0.04f\r\n", settings.fixedEcefY);
255-
// systemPrintf("major (should be -471680858): %ld\r\n", majorEcefY);
256-
// systemPrintf("minor (should be -7): %ld\r\n", minorEcefY);
257-
258-
// Units are cm with a high precision extension so -1234.5678 should be called: (-123456, -78)
259-
//-1280208.308,-4716803.847,4086665.811 is SparkFun HQ so...
260-
261-
response &= theGNSS.newCfgValset();
262-
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_MODE, 2); // Fixed
263-
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_POS_TYPE, 0); // Position in ECEF
264-
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_ECEF_X, majorEcefX);
265-
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_ECEF_X_HP, minorEcefX);
266-
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_ECEF_Y, majorEcefY);
267-
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_ECEF_Y_HP, minorEcefY);
268-
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_ECEF_Z, majorEcefZ);
269-
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_ECEF_Z_HP, minorEcefZ);
270-
response &= theGNSS.sendCfgValset();
271-
}
272-
else if (settings.fixedBaseCoordinateType == COORD_TYPE_GEODETIC)
273-
{
274-
// Add height of instrument (HI) to fixed altitude
275-
// https://www.e-education.psu.edu/geog862/node/1853
276-
// For example, if HAE is at 100.0m, + 2m stick + 73mm ARP = 102.073
277-
float totalFixedAltitude =
278-
settings.fixedAltitude + (settings.antennaHeight / 1000.0) + (settings.antennaReferencePoint / 1000.0);
279-
280-
// Break coordinates into main and high precision parts
281-
// The type casting should not effect rounding of original double cast coordinate
282-
int64_t majorLat = settings.fixedLat * 10000000;
283-
int64_t minorLat = ((settings.fixedLat * 10000000) - majorLat) * 100;
284-
int64_t majorLong = settings.fixedLong * 10000000;
285-
int64_t minorLong = ((settings.fixedLong * 10000000) - majorLong) * 100;
286-
int32_t majorAlt = totalFixedAltitude * 100;
287-
int32_t minorAlt = ((totalFixedAltitude * 100) - majorAlt) * 100;
288-
289-
// systemPrintf("fixedLong (should be -105.184774720): %0.09f\r\n", settings.fixedLong);
290-
// systemPrintf("major (should be -1051847747): %lld\r\n", majorLat);
291-
// systemPrintf("minor (should be -20): %lld\r\n", minorLat);
292-
//
293-
// systemPrintf("fixedLat (should be 40.090335429): %0.09f\r\n", settings.fixedLat);
294-
// systemPrintf("major (should be 400903354): %lld\r\n", majorLong);
295-
// systemPrintf("minor (should be 29): %lld\r\n", minorLong);
296-
//
297-
// systemPrintf("fixedAlt (should be 1560.2284): %0.04f\r\n", settings.fixedAltitude);
298-
// systemPrintf("major (should be 156022): %ld\r\n", majorAlt);
299-
// systemPrintf("minor (should be 84): %ld\r\n", minorAlt);
245+
do {
246+
if (settings.fixedBaseCoordinateType == COORD_TYPE_ECEF)
247+
{
248+
// Break ECEF into main and high precision parts
249+
// The type casting should not effect rounding of original double cast coordinate
250+
long majorEcefX = floor((settings.fixedEcefX * 100.0) + 0.5);
251+
long minorEcefX = floor((((settings.fixedEcefX * 100.0) - majorEcefX) * 100.0) + 0.5);
252+
long majorEcefY = floor((settings.fixedEcefY * 100) + 0.5);
253+
long minorEcefY = floor((((settings.fixedEcefY * 100.0) - majorEcefY) * 100.0) + 0.5);
254+
long majorEcefZ = floor((settings.fixedEcefZ * 100) + 0.5);
255+
long minorEcefZ = floor((((settings.fixedEcefZ * 100.0) - majorEcefZ) * 100.0) + 0.5);
256+
257+
// systemPrintf("fixedEcefY (should be -4716808.5807): %0.04f\r\n", settings.fixedEcefY);
258+
// systemPrintf("major (should be -471680858): %ld\r\n", majorEcefY);
259+
// systemPrintf("minor (should be -7): %ld\r\n", minorEcefY);
260+
261+
// Units are cm with a high precision extension so -1234.5678 should be called: (-123456, -78)
262+
//-1280208.308,-4716803.847,4086665.811 is SparkFun HQ so...
263+
264+
response &= theGNSS.newCfgValset();
265+
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_MODE, 2); // Fixed
266+
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_POS_TYPE, 0); // Position in ECEF
267+
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_ECEF_X, majorEcefX);
268+
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_ECEF_X_HP, minorEcefX);
269+
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_ECEF_Y, majorEcefY);
270+
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_ECEF_Y_HP, minorEcefY);
271+
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_ECEF_Z, majorEcefZ);
272+
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_ECEF_Z_HP, minorEcefZ);
273+
response &= theGNSS.sendCfgValset();
274+
}
275+
else if (settings.fixedBaseCoordinateType == COORD_TYPE_GEODETIC)
276+
{
277+
// Add height of instrument (HI) to fixed altitude
278+
// https://www.e-education.psu.edu/geog862/node/1853
279+
// For example, if HAE is at 100.0m, + 2m stick + 73mm ARP = 102.073
280+
float totalFixedAltitude =
281+
settings.fixedAltitude + (settings.antennaHeight / 1000.0) + (settings.antennaReferencePoint / 1000.0);
282+
283+
// Break coordinates into main and high precision parts
284+
// The type casting should not effect rounding of original double cast coordinate
285+
int64_t majorLat = settings.fixedLat * 10000000;
286+
int64_t minorLat = ((settings.fixedLat * 10000000) - majorLat) * 100;
287+
int64_t majorLong = settings.fixedLong * 10000000;
288+
int64_t minorLong = ((settings.fixedLong * 10000000) - majorLong) * 100;
289+
int32_t majorAlt = totalFixedAltitude * 100;
290+
int32_t minorAlt = ((totalFixedAltitude * 100) - majorAlt) * 100;
291+
292+
// systemPrintf("fixedLong (should be -105.184774720): %0.09f\r\n", settings.fixedLong);
293+
// systemPrintf("major (should be -1051847747): %lld\r\n", majorLat);
294+
// systemPrintf("minor (should be -20): %lld\r\n", minorLat);
295+
//
296+
// systemPrintf("fixedLat (should be 40.090335429): %0.09f\r\n", settings.fixedLat);
297+
// systemPrintf("major (should be 400903354): %lld\r\n", majorLong);
298+
// systemPrintf("minor (should be 29): %lld\r\n", minorLong);
299+
//
300+
// systemPrintf("fixedAlt (should be 1560.2284): %0.04f\r\n", settings.fixedAltitude);
301+
// systemPrintf("major (should be 156022): %ld\r\n", majorAlt);
302+
// systemPrintf("minor (should be 84): %ld\r\n", minorAlt);
303+
304+
response &= theGNSS.newCfgValset();
305+
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_MODE, 2); // Fixed
306+
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_POS_TYPE, 1); // Position in LLH
307+
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_LAT, majorLat);
308+
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_LAT_HP, minorLat);
309+
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_LON, majorLong);
310+
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_LON_HP, minorLong);
311+
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_HEIGHT, majorAlt);
312+
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_HEIGHT_HP, minorAlt);
313+
response &= theGNSS.sendCfgValset(maxWait);
314+
}
300315

301-
response &= theGNSS.newCfgValset();
302-
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_MODE, 2); // Fixed
303-
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_POS_TYPE, 1); // Position in LLH
304-
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_LAT, majorLat);
305-
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_LAT_HP, minorLat);
306-
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_LON, majorLong);
307-
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_LON_HP, minorLong);
308-
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_HEIGHT, majorAlt);
309-
response &= theGNSS.addCfgValset(UBLOX_CFG_TMODE_HEIGHT_HP, minorAlt);
310-
response &= theGNSS.sendCfgValset();
311-
}
316+
if (!response) {
317+
systemPrint("startFixedBase failed! ");
318+
if (retries <= 2) {
319+
systemPrintln("Retrying...");
320+
delay(1000);
321+
maxWait = 2200;
322+
}
323+
else {
324+
systemPrintln("Giving up and going into Rover mode!");
325+
}
326+
}
327+
} while ((!response) && (++retries <= 3));
312328

313329
return (response);
314330
}

0 commit comments

Comments
 (0)