Skip to content

Commit 114089f

Browse files
nseidlenseidle
authored
Merge pull request #533 from sparkfun/pcUpdates
mosaic-X5 SIV improvements : replace NrSV (from PVTGeodetic) with InTracking (from ChannelStatus)
2 parents 87e1188 + ec714f6 commit 114089f

File tree

3 files changed

+198
-41
lines changed

3 files changed

+198
-41
lines changed

‎Firmware/RTK_Everywhere/GNSS.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,7 @@ class GNSS
5454

5555
public:
5656
// Constructor
57-
GNSS() : _leapSeconds(18), _pvtArrivalMillis(0), _pvtUpdated(0)
57+
GNSS() : _leapSeconds(18), _pvtArrivalMillis(0), _pvtUpdated(0), _satellitesInView(0)
5858
{
5959
}
6060

‎Firmware/RTK_Everywhere/GNSS_Mosaic.h

Lines changed: 20 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,8 @@ typedef struct {
1818

1919
const mosaicExpectedID mosaicExpectedIDs[] = {
2020
{ 4007, true, 96, "PVTGeodetic" },
21+
{ 4013, false, 0, "ChannelStatus" },
22+
{ 4059, false, 0, "DiskStatus" },
2123
{ 4090, false, 0, "InputLink" },
2224
{ 4097, false, 0, "EncapsulatedOutput" },
2325
{ 5914, true, 24, "ReceiverTime" },
@@ -46,6 +48,11 @@ const mosaicExpectedID mosaicExpectedIDs[] = {
4648
// This indicates if RTCM corrections are being received - on COM2
4749
#define MOSAIC_SBF_INPUTLINK_STREAM (MOSAIC_SBF_EXTEVENT_STREAM + 1)
4850

51+
// Output SBF ChannelStatus and DiskStatus messages on this stream - on COM1 only
52+
// ChannelStatus provides the count of satellites being tracked
53+
// DiskStatus provides the disk usage
54+
#define MOSAIC_SBF_STATUS_STREAM (MOSAIC_SBF_INPUTLINK_STREAM + 1)
55+
4956
// TODO: allow the user to define their own SBF stream for logging to DSK1 - through the menu / web config
5057
// But, in the interim, the user can define their own SBF stream (>= Stream3) via the X5 web page over USB-C
5158
// The updated configuration can be saved by entering and exiting the main menu, or by:
@@ -575,13 +582,18 @@ class GNSS_MOSAIC : GNSS
575582
float _latStdDev;
576583
float _lonStdDev;
577584
bool _receiverSetupSeen;
585+
bool _diskStatusSeen;
586+
std::vector<uint8_t> svInTracking;
587+
//std::vector<uint8_t> svInPVT;
578588

579589
// Constructor
580590
GNSS_MOSAIC() : _determiningFixedPosition(true), _clkBias_ms(0),
581591
_latStdDev(999.9), _lonStdDev(999.9), _receiverSetupSeen(false),
582-
_radioExtBytesReceived_millis(0),
592+
_radioExtBytesReceived_millis(0), _diskStatusSeen(false),
583593
GNSS()
584594
{
595+
svInTracking.clear();
596+
//svInPVT.clear();
585597
}
586598

587599
// If we have decryption keys, configure module
@@ -1016,6 +1028,12 @@ class GNSS_MOSAIC : GNSS
10161028
// Save the data from the SBF Block 4007
10171029
void storeBlock4007(SEMP_PARSE_STATE *parse);
10181030

1031+
// Save the data from the SBF Block 4013
1032+
void storeBlock4013(SEMP_PARSE_STATE *parse);
1033+
1034+
// Save the data from the SBF Block 4059
1035+
void storeBlock4059(SEMP_PARSE_STATE *parse);
1036+
10191037
// Save the data from the SBF Block 4090
10201038
void storeBlock4090(SEMP_PARSE_STATE *parse);
10211039

@@ -1036,7 +1054,7 @@ class GNSS_MOSAIC : GNSS
10361054
// Poll routine to update the GNSS state
10371055
void update();
10381056

1039-
bool updateSD();
1057+
void updateSD();
10401058

10411059
void waitSBFReceiverSetup(unsigned long timeout);
10421060
};

‎Firmware/RTK_Everywhere/GNSS_Mosaic.ino

Lines changed: 177 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -579,6 +579,12 @@ bool GNSS_MOSAIC::configureOnce()
579579
setting = String("sso,Stream" + String(MOSAIC_SBF_INPUTLINK_STREAM) + ",COM1,InputLink,sec1\n\r");
580580
response &= sendWithResponse(setting, "SBFOutput");
581581

582+
// Output SBF ChannelStatus and DiskStatus on their own stream - at 0.5Hz - on COM1 only
583+
// For ChannelStatus: OnChange is too often. The message is typically 1000 bytes in size.
584+
// For DiskStatus: DiskUsage is slow to update. 0.5Hz is plenty fast enough.
585+
setting = String("sso,Stream" + String(MOSAIC_SBF_STATUS_STREAM) + ",COM1,ChannelStatus+DiskStatus,sec2\n\r");
586+
response &= sendWithResponse(setting, "SBFOutput");
587+
582588
response &= setElevation(settings.minElev);
583589

584590
response &= setMinCnoRadio(settings.minCNO);
@@ -2367,16 +2373,138 @@ void GNSS_MOSAIC::storeBlock4007(SEMP_PARSE_STATE *parse)
23672373
_longitude = sempSbfGetF8(parse, 24) * 180.0 / PI;
23682374
_altitude = (float)sempSbfGetF8(parse, 32);
23692375
_horizontalAccuracy = ((float)sempSbfGetU2(parse, 90)) / 100.0; // Convert from cm to m
2370-
_satellitesInView = sempSbfGetU1(parse, 74);
2371-
if (_satellitesInView == 255) // 255 indicates "Do-Not-Use"
2372-
_satellitesInView = 0;
2376+
2377+
// NrSV is the total number of satellites used in the PVT computation.
2378+
//_satellitesInView = sempSbfGetU1(parse, 74);
2379+
//if (_satellitesInView == 255) // 255 indicates "Do-Not-Use"
2380+
// _satellitesInView = 0;
2381+
23732382
_fixType = sempSbfGetU1(parse, 14) & 0x0F;
23742383
_determiningFixedPosition = (sempSbfGetU1(parse, 14) >> 6) & 0x01;
23752384
_clkBias_ms = sempSbfGetF8(parse, 60);
23762385
_pvtArrivalMillis = millis();
23772386
_pvtUpdated = true;
23782387
}
23792388

2389+
//----------------------------------------
2390+
// Save the data from the SBF Block 4013
2391+
//----------------------------------------
2392+
void GNSS_MOSAIC::storeBlock4013(SEMP_PARSE_STATE *parse)
2393+
{
2394+
uint16_t N = (uint16_t)sempSbfGetU1(parse, 14);
2395+
uint16_t SB1Length = (uint16_t)sempSbfGetU1(parse, 15);
2396+
uint16_t SB2Length = (uint16_t)sempSbfGetU1(parse, 16);
2397+
uint16_t ChannelInfoBytes = 0;
2398+
for (uint16_t i = 0; i < N; i++)
2399+
{
2400+
uint8_t SVID = sempSbfGetU1(parse, 20 + ChannelInfoBytes + 0);
2401+
2402+
uint16_t N2 = (uint16_t)sempSbfGetU1(parse, 20 + ChannelInfoBytes + 9);
2403+
2404+
for (uint16_t j = 0; j < N2; j++)
2405+
{
2406+
uint16_t TrackingStatus = sempSbfGetU2(parse, 20 + ChannelInfoBytes + SB1Length + (j * SB2Length) + 2);
2407+
2408+
bool Tracking = false;
2409+
for (uint16_t shift = 0; shift < 16; shift += 2) // Step through each 2-bit status field
2410+
{
2411+
if ((TrackingStatus & (0x0003 << shift)) == (0x0003 << shift)) // 3 : Tracking
2412+
{
2413+
Tracking = true;
2414+
}
2415+
}
2416+
2417+
if (Tracking)
2418+
{
2419+
// SV is being tracked. If it is not in svInTracking, add it
2420+
std::vector<uint8_t>::iterator pos =
2421+
std::find(svInTracking.begin(), svInTracking.end(), SVID);
2422+
if (pos == svInTracking.end())
2423+
svInTracking.push_back(SVID);
2424+
}
2425+
else
2426+
{
2427+
// SV is not being tracked. If it is in svInTracking, remove it
2428+
std::vector<uint8_t>::iterator pos =
2429+
std::find(svInTracking.begin(), svInTracking.end(), SVID);
2430+
if (pos != svInTracking.end())
2431+
svInTracking.erase(pos);
2432+
}
2433+
2434+
// uint16_t PVTStatus = sempSbfGetU2(parse, 20 + ChannelInfoBytes + SB1Length + (j * SB2Length) + 4);
2435+
2436+
// bool Used = false;
2437+
// for (uint16_t shift = 0; shift < 16; shift += 2) // Step through each 2-bit status field
2438+
// {
2439+
// if ((PVTStatus & (0x0003 << shift)) == (0x0002 << shift)) // 2 : Used
2440+
// {
2441+
// Used = true;
2442+
// }
2443+
// }
2444+
2445+
// if (Used)
2446+
// {
2447+
// // SV is being used for PVT. If it is not in svInPVT, add it
2448+
// std::vector<uint8_t>::iterator pos =
2449+
// std::find(svInPVT.begin(), svInPVT.end(), SVID);
2450+
// if (pos == svInPVT.end())
2451+
// svInPVT.push_back(SVID);
2452+
// }
2453+
// else
2454+
// {
2455+
// // SV is not being used for PVT. If it is in svInPVT, remove it
2456+
// std::vector<uint8_t>::iterator pos =
2457+
// std::find(svInPVT.begin(), svInPVT.end(), SVID);
2458+
// if (pos != svInPVT.end())
2459+
// svInPVT.erase(pos);
2460+
// }
2461+
2462+
}
2463+
2464+
ChannelInfoBytes += SB1Length + (N2 * SB2Length);
2465+
}
2466+
2467+
_satellitesInView = (uint8_t)std::distance(svInTracking.begin(), svInTracking.end());
2468+
2469+
// if (settings.debugGnss && !inMainMenu)
2470+
// {
2471+
// uint8_t _inPVT = (uint8_t)std::distance(svInPVT.begin(), svInPVT.end());
2472+
// systemPrintf("ChannelStatus: InTracking %d, InPVT %d\r\n", _satellitesInView, _inPVT);
2473+
// }
2474+
2475+
}
2476+
2477+
//----------------------------------------
2478+
// Save the data from the SBF Block 4059
2479+
//----------------------------------------
2480+
void GNSS_MOSAIC::storeBlock4059(SEMP_PARSE_STATE *parse)
2481+
{
2482+
if (sempSbfGetU1(parse, 14) < 1) // Check N is at least 1
2483+
return;
2484+
2485+
if (sempSbfGetU1(parse, 20 + 0) != 1) // Check DiskID is 1
2486+
return;
2487+
2488+
uint64_t diskUsageMSB = sempSbfGetU2(parse, 20 + 2); // DiskUsageMSB
2489+
uint64_t diskUsageLSB = sempSbfGetU4(parse, 20 + 4); // DiskUsageLSB
2490+
2491+
if ((diskUsageMSB == 65535) && (diskUsageLSB == 4294967295)) // Do-Not-Use
2492+
return;
2493+
2494+
uint64_t diskSizeMB = sempSbfGetU4(parse, 20 + 8); // DiskSize in megabytes
2495+
2496+
if (diskSizeMB == 0) // Do-Not-Use
2497+
return;
2498+
2499+
uint64_t diskUsage = (diskUsageMSB * 4294967296) + diskUsageLSB;
2500+
2501+
sdCardSize = diskSizeMB * 1048576; // Convert to bytes
2502+
2503+
sdFreeSpace = sdCardSize - diskUsage;
2504+
2505+
_diskStatusSeen = true;
2506+
}
2507+
23802508
//----------------------------------------
23812509
// Save the data from the SBF Block 4090
23822510
//----------------------------------------
@@ -2480,22 +2608,44 @@ void GNSS_MOSAIC::update()
24802608
static uint64_t previousFreeSpace = 0;
24812609
if (millis() > (sdCardSizeLastCheck + sdCardSizeCheckInterval))
24822610
{
2483-
updateSD();
2611+
updateSD(); // Check if the card has been removed / inserted
24842612

2485-
if (previousFreeSpace == 0)
2486-
previousFreeSpace = sdFreeSpace;
2487-
if (sdFreeSpace < previousFreeSpace)
2613+
if (_diskStatusSeen) // Check if the DiskStatus SBF message has been seen
24882614
{
2489-
previousFreeSpace = sdFreeSpace;
2490-
logIncreasing = true;
2491-
sdCardLastFreeChange = millis();
2615+
// If previousFreeSpace hasn't been initialized, initialize it
2616+
if (previousFreeSpace == 0)
2617+
previousFreeSpace = sdFreeSpace;
2618+
2619+
if (sdFreeSpace < previousFreeSpace)
2620+
{
2621+
// The free space is decreasing, so set logIncreasing to true
2622+
previousFreeSpace = sdFreeSpace;
2623+
logIncreasing = true;
2624+
sdCardLastFreeChange = millis();
2625+
}
2626+
else if (sdFreeSpace == previousFreeSpace)
2627+
{
2628+
// The free space has not changed
2629+
// X5 is slow to update free. Seems to be about every ~20s?
2630+
// So only set logIncreasing to false after 30s
2631+
if (millis() > (sdCardLastFreeChange + 30000))
2632+
logIncreasing = false;
2633+
}
2634+
else // if (sdFreeSpace > previousFreeSpace)
2635+
{
2636+
// User must have inserted a new SD card?
2637+
previousFreeSpace = sdFreeSpace;
2638+
}
24922639
}
24932640
else
24942641
{
2495-
if (millis() > (sdCardLastFreeChange + 30000)) // X5 is slow to update free. Seems to be about every ~20s?
2496-
logIncreasing = false;
2642+
// Disk status not seen
2643+
// (Unmounting the SD card will prevent _diskStatusSeen from going true)
2644+
logIncreasing = false;
24972645
}
2498-
sdCardSizeLastCheck = millis();
2646+
2647+
sdCardSizeLastCheck = millis(); // Update the timer
2648+
_diskStatusSeen = false; // Clear the flag
24992649
}
25002650

25012651
// Update spartnCorrectionsReceived
@@ -2510,7 +2660,7 @@ void GNSS_MOSAIC::update()
25102660
}
25112661

25122662
//----------------------------------------
2513-
bool GNSS_MOSAIC::updateSD()
2663+
void GNSS_MOSAIC::updateSD()
25142664
{
25152665
// See comments in update() above
25162666
// updateSD() is probably the best place to check if an SD card has been inserted / removed
@@ -2531,7 +2681,11 @@ bool GNSS_MOSAIC::updateSD()
25312681

25322682
// Allow many retries
25332683
int retries = 0;
2534-
int retryLimit = 20;
2684+
int retryLimit = 30;
2685+
2686+
// If the card has been removed, the soft reset takes extra time. Allow more retries
2687+
if (!previousCardPresent)
2688+
retryLimit = 40;
25352689

25362690
// Set COM4 to: CMD input (only), SBF output (only)
25372691
while (!sendWithResponse("sdio,COM4,CMD,SBF\n\r", "DataInOut"))
@@ -2545,31 +2699,8 @@ bool GNSS_MOSAIC::updateSD()
25452699
if (retries == retryLimit)
25462700
{
25472701
systemPrintln("Soft reset failed!");
2548-
return false;
25492702
}
25502703
}
2551-
2552-
if (!previousCardPresent) // If the card is not present, skip the list disk info
2553-
return false;
2554-
2555-
char diskInfo[200];
2556-
bool response =
2557-
sendAndWaitForIdle("ldi,DSK1\n\r", "DiskInfo", 1000, 25, &diskInfo[0], sizeof(diskInfo), false); // No debug
2558-
if (response)
2559-
{
2560-
char *ptr = strstr(diskInfo, " total=\"");
2561-
if (ptr == nullptr)
2562-
return false;
2563-
ptr += strlen(" total=\"");
2564-
sscanf(ptr, "%llu\"", &sdCardSize);
2565-
ptr = strstr(ptr, " free=\"");
2566-
if (ptr == nullptr)
2567-
return false;
2568-
ptr += strlen(" free=\"");
2569-
sscanf(ptr, "%llu\"", &sdFreeSpace);
2570-
}
2571-
2572-
return response;
25732704
}
25742705

25752706
//----------------------------------------
@@ -2726,6 +2857,14 @@ void processUart1SBF(SEMP_PARSE_STATE *parse, uint16_t type)
27262857
if (sempSbfGetBlockNumber(parse) == 4007)
27272858
mosaic->storeBlock4007(parse);
27282859

2860+
// If this is ChannelStatus, extract some data
2861+
if (sempSbfGetBlockNumber(parse) == 4013)
2862+
mosaic->storeBlock4013(parse);
2863+
2864+
// If this is DiskStatus, extract some data
2865+
if (sempSbfGetBlockNumber(parse) == 4059)
2866+
mosaic->storeBlock4059(parse);
2867+
27292868
// If this is InputLink, extract some data
27302869
if (sempSbfGetBlockNumber(parse) == 4090)
27312870
mosaic->storeBlock4090(parse);

0 commit comments

Comments
 (0)