Skip to content

Commit 9f9592c

Browse files
PaulZCPaulZC
committed
Fix nmeaApplyCompensation
Don't allow message length to change. Truncate / pad as necessary
1 parent 191c2b6 commit 9f9592c

File tree

1 file changed

+172
-28
lines changed

1 file changed

+172
-28
lines changed

‎Firmware/RTK_Everywhere/Tilt.ino‎

Lines changed: 172 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -554,19 +554,53 @@ void applyCompensationGNS(char *nmeaSentence, int sentenceLength)
554554
coordinateConvertInput(abs(tiltSensor->getNaviLatitude()), COORDINATE_INPUT_TYPE_DDMM, coordinateStringDDMM,
555555
sizeof(coordinateStringDDMM));
556556

557+
// Check if latitude length has changed
558+
if (strlen(coordinateStringDDMM) != (latitudeStop - latitudeStart))
559+
{
560+
if (settings.enableImuCompensationDebug == true && !inMainMenu)
561+
systemPrintf("Compensated latitude length has changed! Orig: %d New: %d\r\n",
562+
(latitudeStop - latitudeStart),
563+
strlen(coordinateStringDDMM));
564+
}
565+
557566
// Add tilt-compensated Latitude
558567
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
559568

569+
// We can't allow the message length to change. Truncate if needed
570+
while (strlen(newSentence) > latitudeStop)
571+
*(newSentence + strlen(newSentence) - 1) = 0; // Move the NULL terminator
572+
573+
// We can't allow the message length to change. Pad with zeros if needed
574+
while (strlen(newSentence) < latitudeStop)
575+
strncat(newSentence, "0", sizeof(newSentence) - 1);
576+
560577
// Add interstitial between end of lat and beginning of lon
561578
strncat(newSentence, nmeaSentence + latitudeStop, longitudeStart - latitudeStop);
562579

563580
// Convert tilt-compensated longitude to DDMM
564581
coordinateConvertInput(abs(tiltSensor->getNaviLongitude()), COORDINATE_INPUT_TYPE_DDMM, coordinateStringDDMM,
565582
sizeof(coordinateStringDDMM));
566583

584+
// Check if longitude length has changed
585+
if (strlen(coordinateStringDDMM) != (longitudeStop - longitudeStart))
586+
{
587+
if (settings.enableImuCompensationDebug == true && !inMainMenu)
588+
systemPrintf("Compensated longitude length has changed! Orig: %d New: %d\r\n",
589+
(longitudeStop - longitudeStart),
590+
strlen(coordinateStringDDMM));
591+
}
592+
567593
// Add tilt-compensated Longitude
568594
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
569595

596+
// We can't allow the message length to change. Truncate if needed
597+
while (strlen(newSentence) > longitudeStop)
598+
*(newSentence + strlen(newSentence) - 1) = 0; // Move the NULL terminator
599+
600+
// We can't allow the message length to change. Pad with zeros if needed
601+
while (strlen(newSentence) < longitudeStop)
602+
strncat(newSentence, "0", sizeof(newSentence) - 1);
603+
570604
// Add interstitial between end of lon and beginning of alt
571605
strncat(newSentence, nmeaSentence + longitudeStop, altitudeStart - longitudeStop);
572606
}
@@ -602,9 +636,27 @@ void applyCompensationGNS(char *nmeaSentence, int sentenceLength)
602636
// Convert altitude double to string
603637
snprintf(coordinateStringDDMM, sizeof(coordinateStringDDMM), "%0.3f", newAltitude);
604638

639+
// Check if altitude length has changed
640+
if (strlen(coordinateStringDDMM) != (altitudeStop - altitudeStart))
641+
{
642+
if (settings.enableImuCompensationDebug == true && !inMainMenu)
643+
systemPrintf("Compensated altitude length has changed! Orig: %d New: %d\r\n",
644+
(altitudeStop - altitudeStart),
645+
strlen(coordinateStringDDMM));
646+
}
647+
605648
// Add tilt-compensated Altitude
606649
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
607650

651+
// We can't allow the message length to change. Truncate if needed
652+
// altitudeStop is the position of the comma.
653+
while (strlen(newSentence) > altitudeStop)
654+
*(newSentence + strlen(newSentence) - 1) = 0; // Move the NULL terminator
655+
656+
// We can't allow the message length to change. Pad with zeros if needed
657+
while (strlen(newSentence) < altitudeStop)
658+
strncat(newSentence, "0", sizeof(newSentence) - 1);
659+
608660
// Add remainder of the sentence up to checksum
609661
strncat(newSentence, nmeaSentence + altitudeStop, checksumStart - altitudeStop);
610662

@@ -619,13 +671,6 @@ void applyCompensationGNS(char *nmeaSentence, int sentenceLength)
619671
// Add CRC
620672
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
621673

622-
if (strlen(newSentence) > sentenceLength)
623-
{
624-
if (settings.enableImuCompensationDebug == true && !inMainMenu)
625-
systemPrintf("New compensated sentence too long! Orig: %d New: %d\r\n", sentenceLength,
626-
strlen(newSentence));
627-
}
628-
629674
// Overwrite the original NMEA
630675
strncpy(nmeaSentence, newSentence, sentenceLength);
631676

@@ -699,19 +744,53 @@ void applyCompensationGLL(char *nmeaSentence, int sentenceLength)
699744
coordinateConvertInput(abs(tiltSensor->getNaviLatitude()), COORDINATE_INPUT_TYPE_DDMM, coordinateStringDDMM,
700745
sizeof(coordinateStringDDMM));
701746

747+
// Check if latitude length has changed
748+
if (strlen(coordinateStringDDMM) != (latitudeStop - latitudeStart))
749+
{
750+
if (settings.enableImuCompensationDebug == true && !inMainMenu)
751+
systemPrintf("Compensated latitude length has changed! Orig: %d New: %d\r\n",
752+
(latitudeStop - latitudeStart),
753+
strlen(coordinateStringDDMM));
754+
}
755+
702756
// Add tilt-compensated Latitude
703757
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
704758

759+
// We can't allow the message length to change. Truncate if needed
760+
while (strlen(newSentence) > latitudeStop)
761+
*(newSentence + strlen(newSentence) - 1) = 0; // Move the NULL terminator
762+
763+
// We can't allow the message length to change. Pad with zeros if needed
764+
while (strlen(newSentence) < latitudeStop)
765+
strncat(newSentence, "0", sizeof(newSentence) - 1);
766+
705767
// Add interstitial between end of lat and beginning of lon
706768
strncat(newSentence, nmeaSentence + latitudeStop, longitudeStart - latitudeStop);
707769

708770
// Convert tilt-compensated longitude to DDMM
709771
coordinateConvertInput(abs(tiltSensor->getNaviLongitude()), COORDINATE_INPUT_TYPE_DDMM, coordinateStringDDMM,
710772
sizeof(coordinateStringDDMM));
711773

774+
// Check if longitude length has changed
775+
if (strlen(coordinateStringDDMM) != (longitudeStop - longitudeStart))
776+
{
777+
if (settings.enableImuCompensationDebug == true && !inMainMenu)
778+
systemPrintf("Compensated longitude length has changed! Orig: %d New: %d\r\n",
779+
(longitudeStop - longitudeStart),
780+
strlen(coordinateStringDDMM));
781+
}
782+
712783
// Add tilt-compensated Longitude
713784
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
714785

786+
// We can't allow the message length to change. Truncate if needed
787+
while (strlen(newSentence) > longitudeStop)
788+
*(newSentence + strlen(newSentence) - 1) = 0; // Move the NULL terminator
789+
790+
// We can't allow the message length to change. Pad with zeros if needed
791+
while (strlen(newSentence) < longitudeStop)
792+
strncat(newSentence, "0", sizeof(newSentence) - 1);
793+
715794
// Add remainder of the sentence up to checksum
716795
strncat(newSentence, nmeaSentence + longitudeStop, checksumStart - longitudeStop);
717796

@@ -726,13 +805,6 @@ void applyCompensationGLL(char *nmeaSentence, int sentenceLength)
726805
// Add CRC
727806
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
728807

729-
if (strlen(newSentence) > sentenceLength)
730-
{
731-
if (settings.enableImuCompensationDebug == true && !inMainMenu)
732-
systemPrintf("New compensated sentence too long! Orig: %d New: %d\r\n", sentenceLength,
733-
strlen(newSentence));
734-
}
735-
736808
// Overwrite the original NMEA
737809
strncpy(nmeaSentence, newSentence, sentenceLength);
738810

@@ -806,19 +878,53 @@ void applyCompensationRMC(char *nmeaSentence, int sentenceLength)
806878
coordinateConvertInput(abs(tiltSensor->getNaviLatitude()), COORDINATE_INPUT_TYPE_DDMM, coordinateStringDDMM,
807879
sizeof(coordinateStringDDMM));
808880

881+
// Check if latitude length has changed
882+
if (strlen(coordinateStringDDMM) != (latitudeStop - latitudeStart))
883+
{
884+
if (settings.enableImuCompensationDebug == true && !inMainMenu)
885+
systemPrintf("Compensated latitude length has changed! Orig: %d New: %d\r\n",
886+
(latitudeStop - latitudeStart),
887+
strlen(coordinateStringDDMM));
888+
}
889+
809890
// Add tilt-compensated Latitude
810891
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
811892

893+
// We can't allow the message length to change. Truncate if needed
894+
while (strlen(newSentence) > latitudeStop)
895+
*(newSentence + strlen(newSentence) - 1) = 0; // Move the NULL terminator
896+
897+
// We can't allow the message length to change. Pad with zeros if needed
898+
while (strlen(newSentence) < latitudeStop)
899+
strncat(newSentence, "0", sizeof(newSentence) - 1);
900+
812901
// Add interstitial between end of lat and beginning of lon
813902
strncat(newSentence, nmeaSentence + latitudeStop, longitudeStart - latitudeStop);
814903

815904
// Convert tilt-compensated longitude to DDMM
816905
coordinateConvertInput(abs(tiltSensor->getNaviLongitude()), COORDINATE_INPUT_TYPE_DDMM, coordinateStringDDMM,
817906
sizeof(coordinateStringDDMM));
818907

908+
// Check if longitude length has changed
909+
if (strlen(coordinateStringDDMM) != (longitudeStop - longitudeStart))
910+
{
911+
if (settings.enableImuCompensationDebug == true && !inMainMenu)
912+
systemPrintf("Compensated longitude length has changed! Orig: %d New: %d\r\n",
913+
(longitudeStop - longitudeStart),
914+
strlen(coordinateStringDDMM));
915+
}
916+
819917
// Add tilt-compensated Longitude
820918
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
821919

920+
// We can't allow the message length to change. Truncate if needed
921+
while (strlen(newSentence) > longitudeStop)
922+
*(newSentence + strlen(newSentence) - 1) = 0; // Move the NULL terminator
923+
924+
// We can't allow the message length to change. Pad with zeros if needed
925+
while (strlen(newSentence) < longitudeStop)
926+
strncat(newSentence, "0", sizeof(newSentence) - 1);
927+
822928
// Add remainder of the sentence up to checksum
823929
strncat(newSentence, nmeaSentence + longitudeStop, checksumStart - longitudeStop);
824930

@@ -833,13 +939,6 @@ void applyCompensationRMC(char *nmeaSentence, int sentenceLength)
833939
// Add CRC
834940
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
835941

836-
if (strlen(newSentence) > sentenceLength)
837-
{
838-
if (settings.enableImuCompensationDebug == true && !inMainMenu)
839-
systemPrintf("New compensated sentence too long! Orig: %d New: %d\r\n", sentenceLength,
840-
strlen(newSentence));
841-
}
842-
843942
// Overwrite the original NMEA
844943
strncpy(nmeaSentence, newSentence, sentenceLength);
845944

@@ -944,19 +1043,53 @@ void applyCompensationGGA(char *nmeaSentence, int sentenceLength)
9441043
coordinateConvertInput(abs(tiltSensor->getNaviLatitude()), COORDINATE_INPUT_TYPE_DDMM, coordinateStringDDMM,
9451044
sizeof(coordinateStringDDMM));
9461045

1046+
// Check if latitude length has changed
1047+
if (strlen(coordinateStringDDMM) != (latitudeStop - latitudeStart))
1048+
{
1049+
if (settings.enableImuCompensationDebug == true && !inMainMenu)
1050+
systemPrintf("Compensated latitude length has changed! Orig: %d New: %d\r\n",
1051+
(latitudeStop - latitudeStart),
1052+
strlen(coordinateStringDDMM));
1053+
}
1054+
9471055
// Add tilt-compensated Latitude
9481056
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
9491057

1058+
// We can't allow the message length to change. Truncate if needed
1059+
while (strlen(newSentence) > latitudeStop)
1060+
*(newSentence + strlen(newSentence) - 1) = 0; // Move the NULL terminator
1061+
1062+
// We can't allow the message length to change. Pad with zeros if needed
1063+
while (strlen(newSentence) < latitudeStop)
1064+
strncat(newSentence, "0", sizeof(newSentence) - 1);
1065+
9501066
// Add interstitial between end of lat and beginning of lon
9511067
strncat(newSentence, nmeaSentence + latitudeStop, longitudeStart - latitudeStop);
9521068

9531069
// Convert tilt-compensated longitude to DDMM
9541070
coordinateConvertInput(abs(tiltSensor->getNaviLongitude()), COORDINATE_INPUT_TYPE_DDMM, coordinateStringDDMM,
9551071
sizeof(coordinateStringDDMM));
9561072

1073+
// Check if longitude length has changed
1074+
if (strlen(coordinateStringDDMM) != (longitudeStop - longitudeStart))
1075+
{
1076+
if (settings.enableImuCompensationDebug == true && !inMainMenu)
1077+
systemPrintf("Compensated longitude length has changed! Orig: %d New: %d\r\n",
1078+
(longitudeStop - longitudeStart),
1079+
strlen(coordinateStringDDMM));
1080+
}
1081+
9571082
// Add tilt-compensated Longitude
9581083
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
9591084

1085+
// We can't allow the message length to change. Truncate if needed
1086+
while (strlen(newSentence) > longitudeStop)
1087+
*(newSentence + strlen(newSentence) - 1) = 0; // Move the NULL terminator
1088+
1089+
// We can't allow the message length to change. Pad with zeros if needed
1090+
while (strlen(newSentence) < longitudeStop)
1091+
strncat(newSentence, "0", sizeof(newSentence) - 1);
1092+
9601093
// Add interstitial between end of lon and beginning of alt
9611094
strncat(newSentence, nmeaSentence + longitudeStop, altitudeStart - longitudeStop);
9621095
}
@@ -992,9 +1125,27 @@ void applyCompensationGGA(char *nmeaSentence, int sentenceLength)
9921125
// Convert altitude double to string
9931126
snprintf(coordinateStringDDMM, sizeof(coordinateStringDDMM), "%0.4f", newAltitude);
9941127

1128+
// Check if altitude length has changed
1129+
if (strlen(coordinateStringDDMM) != (altitudeStop - altitudeStart))
1130+
{
1131+
if (settings.enableImuCompensationDebug == true && !inMainMenu)
1132+
systemPrintf("Compensated altitude length has changed! Orig: %d New: %d\r\n",
1133+
(altitudeStop - altitudeStart),
1134+
strlen(coordinateStringDDMM));
1135+
}
1136+
9951137
// Add tilt-compensated Altitude
9961138
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
9971139

1140+
// We can't allow the message length to change. Truncate if needed
1141+
// altitudeStop is the position of the comma.
1142+
while (strlen(newSentence) > altitudeStop)
1143+
*(newSentence + strlen(newSentence) - 1) = 0; // Move the NULL terminator
1144+
1145+
// We can't allow the message length to change. Pad with zeros if needed
1146+
while (strlen(newSentence) < altitudeStop)
1147+
strncat(newSentence, "0", sizeof(newSentence) - 1);
1148+
9981149
// Add remainder of the sentence up to checksum
9991150
strncat(newSentence, nmeaSentence + altitudeStop, checksumStart - altitudeStop);
10001151

@@ -1009,13 +1160,6 @@ void applyCompensationGGA(char *nmeaSentence, int sentenceLength)
10091160
// Add CRC
10101161
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
10111162

1012-
if (strlen(newSentence) > sentenceLength)
1013-
{
1014-
if (settings.enableImuCompensationDebug == true && !inMainMenu)
1015-
systemPrintf("New compensated sentence too long! Orig: %d New: %d\r\n", sentenceLength,
1016-
strlen(newSentence));
1017-
}
1018-
10191163
// Overwrite the original NMEA
10201164
strncpy(nmeaSentence, newSentence, sentenceLength);
10211165

0 commit comments

Comments
 (0)