Skip to content

Commit

Permalink
Add overlength debug messages
Browse files Browse the repository at this point in the history
  • Loading branch information
nseidle committed Jun 27, 2024
1 parent 420f8a9 commit 11079e3
Show file tree
Hide file tree
Showing 2 changed files with 45 additions and 5 deletions.
48 changes: 45 additions & 3 deletions Firmware/RTK_Everywhere/Tilt.ino
Original file line number Diff line number Diff line change
Expand Up @@ -473,6 +473,9 @@ void applyCompensationGNS(char *nmeaSentence, int arraySize)
uint8_t undulationStop = 0;
uint8_t checksumStart = 0;

if (settings.enableImuCompensationDebug == true && !inMainMenu)
systemPrintf("Original GNGNS:\r\n%s\r\n", nmeaSentence);

int commaCount = 0;
for (int x = 0; x < strnlen(nmeaSentence, arraySize); x++) // Assumes sentence is null terminated
{
Expand Down Expand Up @@ -606,8 +609,17 @@ void applyCompensationGNS(char *nmeaSentence, int arraySize)
// Add CRC
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);

if (strlen(newSentence) > arraySize)
{
if (settings.enableImuCompensationDebug == true && !inMainMenu)
systemPrintf("New compensated sentence too long! Orig: %d New: %d\r\n", arraySize, strlen(newSentence));
}

// Overwrite the original NMEA
strncpy(nmeaSentence, newSentence, arraySize);

if (settings.enableImuCompensationDebug == true && !inMainMenu)
systemPrintf("Compensated GNGNS:\r\n%s\r\n", nmeaSentence);
}

// Modify a GLL sentence with tilt compensation
Expand All @@ -619,6 +631,9 @@ void applyCompensationGLL(char *nmeaSentence, int arraySize)
if (tiltIsCorrecting() == false)
return;

if (settings.enableImuCompensationDebug == true && !inMainMenu)
systemPrintf("Original GNGLL:\r\n%s\r\n", nmeaSentence);

char coordinateStringDDMM[strlen("10511.12345678") + 1] = {0}; // UM980 outputs 8 decimals in GGA sentence

const int latitudeComma = 1;
Expand Down Expand Up @@ -700,8 +715,17 @@ void applyCompensationGLL(char *nmeaSentence, int arraySize)
// Add CRC
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);

if (strlen(newSentence) > arraySize)
{
if (settings.enableImuCompensationDebug == true && !inMainMenu)
systemPrintf("New compensated sentence too long! Orig: %d New: %d\r\n", arraySize, strlen(newSentence));
}

// Overwrite the original NMEA
strncpy(nmeaSentence, newSentence, arraySize);

if (settings.enableImuCompensationDebug == true && !inMainMenu)
systemPrintf("Compensated GNGLL:\r\n%s\r\n", nmeaSentence);
}

// Modify a RMC sentence with tilt compensation
Expand All @@ -713,6 +737,9 @@ void applyCompensationRMC(char *nmeaSentence, int arraySize)
if (tiltIsCorrecting() == false)
return;

if (settings.enableImuCompensationDebug == true && !inMainMenu)
systemPrintf("Original GNRMC:\r\n%s\r\n", nmeaSentence);

char coordinateStringDDMM[strlen("10511.12345678") + 1] = {0}; // UM980 outputs 8 decimals in GGA sentence

const int latitudeComma = 3;
Expand Down Expand Up @@ -794,8 +821,17 @@ void applyCompensationRMC(char *nmeaSentence, int arraySize)
// Add CRC
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);

if (strlen(newSentence) > arraySize)
{
if (settings.enableImuCompensationDebug == true && !inMainMenu)
systemPrintf("New compensated sentence too long! Orig: %d New: %d\r\n", arraySize, strlen(newSentence));
}

// Overwrite the original NMEA
strncpy(nmeaSentence, newSentence, arraySize);

if (settings.enableImuCompensationDebug == true && !inMainMenu)
systemPrintf("Compensated GNRMC:\r\n%s\r\n", nmeaSentence);
}

// Modify a GGA sentence with tilt compensation
Expand Down Expand Up @@ -825,7 +861,7 @@ void applyCompensationGGA(char *nmeaSentence, int arraySize)
uint8_t checksumStart = 0;

if (settings.enableImuCompensationDebug == true && !inMainMenu)
systemPrintf("Original GNGGA: %s\r\n", nmeaSentence);
systemPrintf("Original GNGGA:\r\n%s\r\n", nmeaSentence);

int commaCount = 0;
for (int x = 0; x < strnlen(nmeaSentence, arraySize); x++) // Assumes sentence is null terminated
Expand Down Expand Up @@ -941,7 +977,7 @@ void applyCompensationGGA(char *nmeaSentence, int arraySize)
}

// Convert altitude double to string
snprintf(coordinateStringDDMM, sizeof(coordinateStringDDMM), "%0.3f", newAltitude);
snprintf(coordinateStringDDMM, sizeof(coordinateStringDDMM), "%0.4f", newAltitude);

// Add tilt-compensated Altitude
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
Expand All @@ -960,11 +996,17 @@ void applyCompensationGGA(char *nmeaSentence, int arraySize)
// Add CRC
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);

if (strlen(newSentence) > arraySize)
{
if (settings.enableImuCompensationDebug == true && !inMainMenu)
systemPrintf("New compensated sentence too long! Orig: %d New: %d\r\n", arraySize, strlen(newSentence));
}

// Overwrite the original NMEA
strncpy(nmeaSentence, newSentence, arraySize);

if (settings.enableImuCompensationDebug == true && !inMainMenu)
systemPrintf("Compensated GNGGA: %s\r\n", nmeaSentence);
systemPrintf("Compensated GNGGA:\r\n%s\r\n", nmeaSentence);
}

#endif // COMPILE_IM19_IMU
2 changes: 0 additions & 2 deletions Firmware/RTK_Everywhere/menuSystem.ino
Original file line number Diff line number Diff line change
Expand Up @@ -1385,9 +1385,7 @@ void menuInstrument()

if (getNewSetting("Enter the antenna height (a.k.a. pole length) in meters", -15.0, 15.0, &antennaHeight) ==
INPUT_RESPONSE_VALID)
{
settings.antennaHeight_mm = truncf(antennaHeight * 1000.0);
}
}
else if (incoming == 2)
{
Expand Down

0 comments on commit 11079e3

Please sign in to comment.