Skip to content

Commit 1a8f351

Browse files
nseidlenseidle
committed
Calculate altitude based on tilt mode and outputTipAltitude setting
1 parent 43e741c commit 1a8f351

File tree

1 file changed

+143
-59
lines changed

1 file changed

+143
-59
lines changed

‎Firmware/RTK_Everywhere/Tilt.ino

Lines changed: 143 additions & 59 deletions
Original file line numberDiff line numberDiff line change
@@ -444,9 +444,19 @@ void tiltSensorFactoryReset()
444444
// Auto-detects sentence type and will only modify sentences that have lat/lon/alt (ie GGA yes, GSV no)
445445
// Which sentences have altitude? Yes: GGA, GNS No: RMC, GLL
446446
// Which sentences have undulation? Yes: GGA, GNS No: RMC, GLL
447-
// See issue: https://github.com/sparkfun/SparkFun_RTK_Everywhere_Firmware/issues/334
448-
void tiltApplyCompensation(char *nmeaSentence, int arraySize)
447+
// Four possible compensations:
448+
// If tilt is active, and outputTipAltitude is enabled, then subtract undulation from IMU altitude, and apply LLA
449+
// compensation. If tilt is active, and outputTipAltitude is disabled, then subtract undulation from IMU altitude, and
450+
// add pole+ARP. If tilt is off, and outputTipAltitude is enabled, then subtract pole+ARP from altitude. If tilt is off,
451+
// and outputTipAltitude is disabled, then pass GNSS data without modification. See issues:
452+
// https://github.com/sparkfun/SparkFun_RTK_Everywhere_Firmware/issues/334
453+
// https://github.com/sparkfun/SparkFun_RTK_Everywhere_Firmware/issues/343
454+
void nmeaApplyCompensation(char *nmeaSentence, int arraySize)
449455
{
456+
// If tilt is off, and outputTipAltitude is disabled, then pass GNSS data without modification
457+
if (tiltIsCorrecting() == false && settings.outputTipAltitude == false)
458+
return;
459+
450460
// Verify the sentence is null-terminated
451461
if (strnlen(nmeaSentence, arraySize) == arraySize)
452462
{
@@ -462,19 +472,19 @@ void tiltApplyCompensation(char *nmeaSentence, int arraySize)
462472
// GGA and GNS sentences get modified in the same way
463473
if (strncmp(sentenceType, "GGA", sizeof(sentenceType)) == 0)
464474
{
465-
tiltApplyCompensationGGA(nmeaSentence, arraySize);
475+
applyCompensationGGA(nmeaSentence, arraySize);
466476
}
467477
else if (strncmp(sentenceType, "GNS", sizeof(sentenceType)) == 0)
468478
{
469-
tiltApplyCompensationGNS(nmeaSentence, arraySize);
479+
applyCompensationGNS(nmeaSentence, arraySize);
470480
}
471481
else if (strncmp(sentenceType, "RMC", sizeof(sentenceType)) == 0)
472482
{
473-
tiltApplyCompensationRMC(nmeaSentence, arraySize);
483+
applyCompensationRMC(nmeaSentence, arraySize);
474484
}
475485
else if (strncmp(sentenceType, "GLL", sizeof(sentenceType)) == 0)
476486
{
477-
tiltApplyCompensationGLL(nmeaSentence, arraySize);
487+
applyCompensationGLL(nmeaSentence, arraySize);
478488
}
479489
else
480490
{
@@ -492,10 +502,8 @@ void tiltApplyCompensation(char *nmeaSentence, int arraySize)
492502
// 1589.4793 is the orthometric height in meters (MSL reference) that we need to insert into the NMEA sentence
493503
// See issue: https://github.com/sparkfun/SparkFun_RTK_Everywhere_Firmware/issues/334
494504
// https://support.virtual-surveyor.com/support/solutions/articles/1000261349-the-difference-between-ellipsoidal-geoid-and-orthometric-elevations-
495-
void tiltApplyCompensationGNS(char *nmeaSentence, int arraySize)
505+
void applyCompensationGNS(char *nmeaSentence, int arraySize)
496506
{
497-
char coordinateStringDDMM[strlen("10511.12345678") + 1] = {0}; // UM980 outputs 8 decimals in GGA sentence
498-
499507
const int latitudeComma = 2;
500508
const int longitudeComma = 4;
501509
const int altitudeComma = 9;
@@ -548,14 +556,16 @@ void tiltApplyCompensationGNS(char *nmeaSentence, int arraySize)
548556
return;
549557
}
550558

559+
// Extract the altitude
560+
char altitudeStr[strlen("-1602.3481") + 1]; // 4 decimals
561+
strncpy(altitudeStr, &nmeaSentence[altitudeStart], altitudeStop - altitudeStart);
562+
float altitude = (float)atof(altitudeStr);
563+
551564
// Extract the undulation
552565
char undulationStr[strlen("-1602.3481") + 1]; // 4 decimals
553566
strncpy(undulationStr, &nmeaSentence[undulationStart], undulationStop - undulationStart);
554567
float undulation = (float)atof(undulationStr);
555568

556-
// Remove the undulation from the IMU's altitude
557-
float orthometricHeight = tiltSensor->getNaviAltitude() - undulation;
558-
559569
char newSentence[150] = {0};
560570

561571
if (sizeof(newSentence) < arraySize)
@@ -564,37 +574,66 @@ void tiltApplyCompensationGNS(char *nmeaSentence, int arraySize)
564574
return;
565575
}
566576

577+
char coordinateStringDDMM[strlen("10511.12345678") + 1] = {0}; // UM980 outputs 8 decimals in GGA sentence
578+
567579
// strncat terminates
568-
// Add start of message up to latitude
569-
strncat(newSentence, nmeaSentence, latitudeStart);
570580

571-
// Convert tilt-compensated latitude to DDMM
572-
coordinateConvertInput(abs(tiltSensor->getNaviLatitude()), COORDINATE_INPUT_TYPE_DDMM, coordinateStringDDMM,
573-
sizeof(coordinateStringDDMM));
581+
if (tiltIsCorrecting() == true)
582+
{
583+
// Add start of message up to latitude
584+
strncat(newSentence, nmeaSentence, latitudeStart);
574585

575-
// Add tilt-compensated Latitude
576-
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
586+
// Convert tilt-compensated latitude to DDMM
587+
coordinateConvertInput(abs(tiltSensor->getNaviLatitude()), COORDINATE_INPUT_TYPE_DDMM, coordinateStringDDMM,
588+
sizeof(coordinateStringDDMM));
577589

578-
// Add interstitial between end of lat and beginning of lon
579-
strncat(newSentence, nmeaSentence + latitudeStop, longitudeStart - latitudeStop);
590+
// Add tilt-compensated Latitude
591+
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
580592

581-
// Convert tilt-compensated longitude to DDMM
582-
coordinateConvertInput(abs(tiltSensor->getNaviLongitude()), COORDINATE_INPUT_TYPE_DDMM, coordinateStringDDMM,
583-
sizeof(coordinateStringDDMM));
593+
// Add interstitial between end of lat and beginning of lon
594+
strncat(newSentence, nmeaSentence + latitudeStop, longitudeStart - latitudeStop);
584595

585-
// Add tilt-compensated Longitude
586-
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
596+
// Convert tilt-compensated longitude to DDMM
597+
coordinateConvertInput(abs(tiltSensor->getNaviLongitude()), COORDINATE_INPUT_TYPE_DDMM, coordinateStringDDMM,
598+
sizeof(coordinateStringDDMM));
587599

588-
// Add interstitial between end of lon and beginning of alt
589-
strncat(newSentence, nmeaSentence + longitudeStop, altitudeStart - longitudeStop);
600+
// Add tilt-compensated Longitude
601+
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
590602

591-
// If outputTipAltitude is enabled, then we don't have to make any adjustments: the tilt compensation module outputs tip altitude
592-
// If outputTipAltitude is disabled, then we need to add back the pole length and ARP
593-
if(settings.outputTipAltitude == false)
594-
orthometricHeight = orthometricHeight + settings.tiltPoleLength + (present.antennaReferencePoint_mm / 1000.0);
603+
// Add interstitial between end of lon and beginning of alt
604+
strncat(newSentence, nmeaSentence + longitudeStop, altitudeStart - longitudeStop);
605+
}
606+
else // No tilt compensation, no changes the lat/lon
607+
{
608+
// Add start of message up to altitude
609+
strncat(newSentence, nmeaSentence, altitudeStart);
610+
}
611+
612+
// Calculate newAltitude based on tilt mode and outputTipAltitude setting
613+
float newAltitude = 0;
614+
if (tiltIsCorrecting() == true)
615+
{
616+
// If tilt is active and outputTipAltitude is disabled, then subtract undulation from IMU altitude, and add
617+
// pole+ARP
618+
if (settings.outputTipAltitude == false)
619+
newAltitude = tiltSensor->getNaviAltitude() - undulation +
620+
(settings.tiltPoleLength + (present.antennaReferencePoint_mm / 1000.0));
621+
622+
// If tilt is active and outputTipAltitude is enabled, then subtract undulation from IMU altitude
623+
else if (settings.outputTipAltitude == true)
624+
newAltitude = tiltSensor->getNaviAltitude() - undulation;
625+
}
626+
else
627+
{
628+
// If tilt is off and outputTipAltitude is enabled, then subtract pole+ARP from altitude
629+
if (settings.outputTipAltitude == true)
630+
newAltitude = altitude - (settings.tiltPoleLength + (present.antennaReferencePoint_mm / 1000.0));
631+
632+
// If tilt is off and outputTipAltitude is disabled, then we should not be here
633+
}
595634

596635
// Convert altitude double to string
597-
snprintf(coordinateStringDDMM, sizeof(coordinateStringDDMM), "%0.3f", orthometricHeight);
636+
snprintf(coordinateStringDDMM, sizeof(coordinateStringDDMM), "%0.3f", newAltitude);
598637

599638
// Add tilt-compensated Altitude
600639
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
@@ -620,8 +659,12 @@ void tiltApplyCompensationGNS(char *nmeaSentence, int arraySize)
620659
// Modify a GLL sentence with tilt compensation
621660
//$GNGLL,4005.4176871,N,10511.1034563,W,214210.00,A,A*68 - Original
622661
//$GNGLL,4005.41769994,N,10507.40740734,W,214210.00,A,A*6D - Modified
623-
void tiltApplyCompensationGLL(char *nmeaSentence, int arraySize)
662+
void applyCompensationGLL(char *nmeaSentence, int arraySize)
624663
{
664+
// GLL only needs to be changed in tilt mode
665+
if (tiltIsCorrecting() == false)
666+
return;
667+
625668
char coordinateStringDDMM[strlen("10511.12345678") + 1] = {0}; // UM980 outputs 8 decimals in GGA sentence
626669

627670
const int latitudeComma = 1;
@@ -710,8 +753,12 @@ void tiltApplyCompensationGLL(char *nmeaSentence, int arraySize)
710753
// Modify a RMC sentence with tilt compensation
711754
//$GNRMC,214210.00,A,4005.4176871,N,10511.1034563,W,0.000,,070923,,,A,V*04 - Original
712755
//$GNRMC,214210.00,A,4005.41769994,N,10507.40740734,W,0.000,,070923,,,A,V*01 - Modified
713-
void tiltApplyCompensationRMC(char *nmeaSentence, int arraySize)
756+
void applyCompensationRMC(char *nmeaSentence, int arraySize)
714757
{
758+
// RMC only needs to be changed in tilt mode
759+
if (tiltIsCorrecting() == false)
760+
return;
761+
715762
char coordinateStringDDMM[strlen("10511.12345678") + 1] = {0}; // UM980 outputs 8 decimals in GGA sentence
716763

717764
const int latitudeComma = 3;
@@ -806,7 +853,7 @@ void tiltApplyCompensationRMC(char *nmeaSentence, int arraySize)
806853
// 1602.3482 is the orthometric height in meters (MSL reference) that we need to insert into the NMEA sentence
807854
// See issue: https://github.com/sparkfun/SparkFun_RTK_Everywhere_Firmware/issues/334
808855
// https://support.virtual-surveyor.com/support/solutions/articles/1000261349-the-difference-between-ellipsoidal-geoid-and-orthometric-elevations-
809-
void tiltApplyCompensationGGA(char *nmeaSentence, int arraySize)
856+
void applyCompensationGGA(char *nmeaSentence, int arraySize)
810857
{
811858
const int latitudeComma = 2;
812859
const int longitudeComma = 4;
@@ -863,13 +910,23 @@ void tiltApplyCompensationGGA(char *nmeaSentence, int arraySize)
863910
return;
864911
}
865912

913+
// Extract the altitude
914+
char altitudeStr[strlen("-1602.3481") + 1]; // 4 decimals
915+
strncpy(altitudeStr, &nmeaSentence[altitudeStart], altitudeStop - altitudeStart);
916+
float altitude = (float)atof(altitudeStr);
917+
866918
// Extract the undulation
867919
char undulationStr[strlen("-1602.3481") + 1]; // 4 decimals
868920
strncpy(undulationStr, &nmeaSentence[undulationStart], undulationStop - undulationStart);
869921
float undulation = (float)atof(undulationStr);
870922

871-
// Remove the undulation from the IMU's altitude
872-
float orthometricHeight = tiltSensor->getNaviAltitude() - undulation;
923+
float orthometricHeight = 0;
924+
925+
if (tiltIsCorrecting() == true)
926+
{
927+
// Remove the undulation from the IMU's altitude
928+
orthometricHeight = tiltSensor->getNaviAltitude() - undulation;
929+
}
873930

874931
char newSentence[150] = {0};
875932

@@ -882,36 +939,63 @@ void tiltApplyCompensationGGA(char *nmeaSentence, int arraySize)
882939
char coordinateStringDDMM[strlen("10511.12345678") + 1] = {0}; // UM980 outputs 8 decimals in GGA sentence
883940

884941
// strncat terminates
885-
// Add start of message up to latitude
886-
strncat(newSentence, nmeaSentence, latitudeStart);
887942

888-
// Convert tilt-compensated latitude to DDMM
889-
coordinateConvertInput(abs(tiltSensor->getNaviLatitude()), COORDINATE_INPUT_TYPE_DDMM, coordinateStringDDMM,
890-
sizeof(coordinateStringDDMM));
943+
if (tiltIsCorrecting() == true)
944+
{
945+
// Add start of message up to latitude
946+
strncat(newSentence, nmeaSentence, latitudeStart);
891947

892-
// Add tilt-compensated Latitude
893-
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
948+
// Convert tilt-compensated latitude to DDMM
949+
coordinateConvertInput(abs(tiltSensor->getNaviLatitude()), COORDINATE_INPUT_TYPE_DDMM, coordinateStringDDMM,
950+
sizeof(coordinateStringDDMM));
894951

895-
// Add interstitial between end of lat and beginning of lon
896-
strncat(newSentence, nmeaSentence + latitudeStop, longitudeStart - latitudeStop);
952+
// Add tilt-compensated Latitude
953+
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
897954

898-
// Convert tilt-compensated longitude to DDMM
899-
coordinateConvertInput(abs(tiltSensor->getNaviLongitude()), COORDINATE_INPUT_TYPE_DDMM, coordinateStringDDMM,
900-
sizeof(coordinateStringDDMM));
955+
// Add interstitial between end of lat and beginning of lon
956+
strncat(newSentence, nmeaSentence + latitudeStop, longitudeStart - latitudeStop);
901957

902-
// Add tilt-compensated Longitude
903-
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
958+
// Convert tilt-compensated longitude to DDMM
959+
coordinateConvertInput(abs(tiltSensor->getNaviLongitude()), COORDINATE_INPUT_TYPE_DDMM, coordinateStringDDMM,
960+
sizeof(coordinateStringDDMM));
961+
962+
// Add tilt-compensated Longitude
963+
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
964+
965+
// Add interstitial between end of lon and beginning of alt
966+
strncat(newSentence, nmeaSentence + longitudeStop, altitudeStart - longitudeStop);
967+
}
968+
else // No tilt compensation, no changes the lat/lon
969+
{
970+
// Add start of message up to altitude
971+
strncat(newSentence, nmeaSentence, altitudeStart);
972+
}
904973

905-
// Add interstitial between end of lon and beginning of alt
906-
strncat(newSentence, nmeaSentence + longitudeStop, altitudeStart - longitudeStop);
974+
// Calculate newAltitude based on tilt mode and outputTipAltitude setting
975+
float newAltitude = 0;
976+
if (tiltIsCorrecting() == true)
977+
{
978+
// If tilt is active and outputTipAltitude is disabled, then subtract undulation from IMU altitude, and add
979+
// pole+ARP
980+
if (settings.outputTipAltitude == false)
981+
newAltitude = tiltSensor->getNaviAltitude() - undulation +
982+
(settings.tiltPoleLength + (present.antennaReferencePoint_mm / 1000.0));
983+
984+
// If tilt is active and outputTipAltitude is enabled, then subtract undulation from IMU altitude
985+
else if (settings.outputTipAltitude == true)
986+
newAltitude = tiltSensor->getNaviAltitude() - undulation;
987+
}
988+
else
989+
{
990+
// If tilt is off and outputTipAltitude is enabled, then subtract pole+ARP from altitude
991+
if (settings.outputTipAltitude == true)
992+
newAltitude = altitude - (settings.tiltPoleLength + (present.antennaReferencePoint_mm / 1000.0));
907993

908-
// If outputTipAltitude is enabled, then we don't have to make any adjustments: the tilt compensation module outputs tip altitude
909-
// If outputTipAltitude is disabled, then we need to add back the pole length and ARP
910-
if(settings.outputTipAltitude == false)
911-
orthometricHeight = orthometricHeight + settings.tiltPoleLength + (present.antennaReferencePoint_mm / 1000.0);
994+
// If tilt is off and outputTipAltitude is disabled, then we should not be here
995+
}
912996

913997
// Convert altitude double to string
914-
snprintf(coordinateStringDDMM, sizeof(coordinateStringDDMM), "%0.3f", orthometricHeight);
998+
snprintf(coordinateStringDDMM, sizeof(coordinateStringDDMM), "%0.3f", newAltitude);
915999

9161000
// Add tilt-compensated Altitude
9171001
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);

0 commit comments

Comments
 (0)