Skip to content

Commit

Permalink
Use given sentence length during compensation rather than string term…
Browse files Browse the repository at this point in the history 
…inator
  • Loading branch information
@nseidle
nseidle committed Jul 18, 2024
1 parent 703a66d commit 3265f59
Show file tree
Hide file tree
Showing 2 changed files with 29 additions and 37 deletions.
1 change: 0 additions & 1 deletion Firmware/RTK_Everywhere/Tasks.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -479,7 +479,6 @@ void processUart1Message(SEMP_PARSE_STATE *parse, uint16_t type)
// Handle LLA compensation due to tilt or outputTipAltitude setting
if (type == RTK_NMEA_PARSER_INDEX)
{
parse->buffer[parse->length++] = '\0'; // Null terminate string
nmeaApplyCompensation((char *)parse->buffer, parse->length);
}

Expand Down
65 changes: 29 additions & 36 deletions Firmware/RTK_Everywhere/Tilt.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -405,19 +405,12 @@ void tiltSensorFactoryReset()
// and outputTipAltitude is disabled, then pass GNSS data without modification. See issues:
// https://github.com/sparkfun/SparkFun_RTK_Everywhere_Firmware/issues/334
// https://github.com/sparkfun/SparkFun_RTK_Everywhere_Firmware/issues/343
void nmeaApplyCompensation(char *nmeaSentence, int arraySize)
void nmeaApplyCompensation(char *nmeaSentence, int sentenceLength)
{
// If tilt is off, and outputTipAltitude is disabled, then pass GNSS data without modification
if (tiltIsCorrecting() == false && settings.outputTipAltitude == false)
return;

// Verify the sentence is null-terminated
if (strnlen(nmeaSentence, arraySize) == arraySize)
{
systemPrintln("Sentence is not null terminated!");
return;
}

// Identify sentence type
char sentenceType[strlen("GGA") + 1] = {0};
strncpy(sentenceType, &nmeaSentence[3],
Expand All @@ -426,19 +419,19 @@ void nmeaApplyCompensation(char *nmeaSentence, int arraySize)
// GGA and GNS sentences get modified in the same way
if (strncmp(sentenceType, "GGA", sizeof(sentenceType)) == 0)
{
applyCompensationGGA(nmeaSentence, arraySize);
applyCompensationGGA(nmeaSentence, sentenceLength);
}
else if (strncmp(sentenceType, "GNS", sizeof(sentenceType)) == 0)
{
applyCompensationGNS(nmeaSentence, arraySize);
applyCompensationGNS(nmeaSentence, sentenceLength);
}
else if (strncmp(sentenceType, "RMC", sizeof(sentenceType)) == 0)
{
applyCompensationRMC(nmeaSentence, arraySize);
applyCompensationRMC(nmeaSentence, sentenceLength);
}
else if (strncmp(sentenceType, "GLL", sizeof(sentenceType)) == 0)
{
applyCompensationGLL(nmeaSentence, arraySize);
applyCompensationGLL(nmeaSentence, sentenceLength);
}
else
{
Expand All @@ -456,7 +449,7 @@ void nmeaApplyCompensation(char *nmeaSentence, int arraySize)
// 1589.4793 is the orthometric height in meters (MSL reference) that we need to insert into the NMEA sentence
// See issue: https://github.com/sparkfun/SparkFun_RTK_Everywhere_Firmware/issues/334
// https://support.virtual-surveyor.com/support/solutions/articles/1000261349-the-difference-between-ellipsoidal-geoid-and-orthometric-elevations-
void applyCompensationGNS(char *nmeaSentence, int arraySize)
void applyCompensationGNS(char *nmeaSentence, int sentenceLength)
{
const int latitudeComma = 2;
const int longitudeComma = 4;
Expand All @@ -477,7 +470,7 @@ void applyCompensationGNS(char *nmeaSentence, int arraySize)
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
for (int x = 0; x < strnlen(nmeaSentence, sentenceLength); x++) // Assumes sentence is null terminated
{
if (nmeaSentence[x] == ',')
{
Expand Down Expand Up @@ -525,7 +518,7 @@ void applyCompensationGNS(char *nmeaSentence, int arraySize)

char newSentence[150] = {0};

if (sizeof(newSentence) < arraySize)
if (sizeof(newSentence) < sentenceLength)
{
systemPrintln("newSentence not big enough!");
return;
Expand Down Expand Up @@ -609,14 +602,14 @@ void applyCompensationGNS(char *nmeaSentence, int arraySize)
// Add CRC
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);

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

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

if (settings.enableImuCompensationDebug == true && !inMainMenu)
systemPrintf("Compensated GNGNS:\r\n%s\r\n", nmeaSentence);
Expand All @@ -625,7 +618,7 @@ void applyCompensationGNS(char *nmeaSentence, int arraySize)
// Modify a GLL sentence with tilt compensation
//$GNGLL,4005.4176871,N,10511.1034563,W,214210.00,A,A*68 - Original
//$GNGLL,4005.41769994,N,10507.40740734,W,214210.00,A,A*6D - Modified
void applyCompensationGLL(char *nmeaSentence, int arraySize)
void applyCompensationGLL(char *nmeaSentence, int sentenceLength)
{
// GLL only needs to be changed in tilt mode
if (tiltIsCorrecting() == false)
Expand All @@ -646,7 +639,7 @@ void applyCompensationGLL(char *nmeaSentence, int arraySize)
uint8_t checksumStart = 0;

int commaCount = 0;
for (int x = 0; x < strnlen(nmeaSentence, arraySize); x++) // Assumes sentence is null terminated
for (int x = 0; x < strnlen(nmeaSentence, sentenceLength); x++) // Assumes sentence is null terminated
{
if (nmeaSentence[x] == ',')
{
Expand Down Expand Up @@ -674,7 +667,7 @@ void applyCompensationGLL(char *nmeaSentence, int arraySize)

char newSentence[150] = {0};

if (sizeof(newSentence) < arraySize)
if (sizeof(newSentence) < sentenceLength)
{
systemPrintln("newSentence not big enough!");
return;
Expand Down Expand Up @@ -715,14 +708,14 @@ void applyCompensationGLL(char *nmeaSentence, int arraySize)
// Add CRC
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);

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

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

if (settings.enableImuCompensationDebug == true && !inMainMenu)
systemPrintf("Compensated GNGLL:\r\n%s\r\n", nmeaSentence);
Expand All @@ -731,7 +724,7 @@ void applyCompensationGLL(char *nmeaSentence, int arraySize)
// Modify a RMC sentence with tilt compensation
//$GNRMC,214210.00,A,4005.4176871,N,10511.1034563,W,0.000,,070923,,,A,V*04 - Original
//$GNRMC,214210.00,A,4005.41769994,N,10507.40740734,W,0.000,,070923,,,A,V*01 - Modified
void applyCompensationRMC(char *nmeaSentence, int arraySize)
void applyCompensationRMC(char *nmeaSentence, int sentenceLength)
{
// RMC only needs to be changed in tilt mode
if (tiltIsCorrecting() == false)
Expand All @@ -752,7 +745,7 @@ void applyCompensationRMC(char *nmeaSentence, int arraySize)
uint8_t checksumStart = 0;

int commaCount = 0;
for (int x = 0; x < strnlen(nmeaSentence, arraySize); x++) // Assumes sentence is null terminated
for (int x = 0; x < strnlen(nmeaSentence, sentenceLength); x++) // Assumes sentence is null terminated
{
if (nmeaSentence[x] == ',')
{
Expand Down Expand Up @@ -780,7 +773,7 @@ void applyCompensationRMC(char *nmeaSentence, int arraySize)

char newSentence[150] = {0};

if (sizeof(newSentence) < arraySize)
if (sizeof(newSentence) < sentenceLength)
{
systemPrintln("newSentence not big enough!");
return;
Expand Down Expand Up @@ -821,14 +814,14 @@ void applyCompensationRMC(char *nmeaSentence, int arraySize)
// Add CRC
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);

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

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

if (settings.enableImuCompensationDebug == true && !inMainMenu)
systemPrintf("Compensated GNRMC:\r\n%s\r\n", nmeaSentence);
Expand All @@ -843,7 +836,7 @@ void applyCompensationRMC(char *nmeaSentence, int arraySize)
// 1602.3482 is the orthometric height in meters (MSL reference) that we need to insert into the NMEA sentence
// See issue: https://github.com/sparkfun/SparkFun_RTK_Everywhere_Firmware/issues/334
// https://support.virtual-surveyor.com/support/solutions/articles/1000261349-the-difference-between-ellipsoidal-geoid-and-orthometric-elevations-
void applyCompensationGGA(char *nmeaSentence, int arraySize)
void applyCompensationGGA(char *nmeaSentence, int sentenceLength)
{
const int latitudeComma = 2;
const int longitudeComma = 4;
Expand All @@ -864,7 +857,7 @@ void applyCompensationGGA(char *nmeaSentence, int arraySize)
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
for (int x = 0; x < strnlen(nmeaSentence, sentenceLength); x++) // Assumes sentence is null terminated
{
if (nmeaSentence[x] == ',')
{
Expand Down Expand Up @@ -912,7 +905,7 @@ void applyCompensationGGA(char *nmeaSentence, int arraySize)

char newSentence[150] = {0};

if (sizeof(newSentence) < arraySize)
if (sizeof(newSentence) < sentenceLength)
{
systemPrintln("newSentence not big enough!");
return;
Expand Down Expand Up @@ -996,14 +989,14 @@ void applyCompensationGGA(char *nmeaSentence, int arraySize)
// Add CRC
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);

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

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

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

0 comments on commit 3265f59

Please sign in to comment.