Skip to content

Commit c76e7a8

Browse files
nseidlenseidle
committed
Remove undulation during tilt compensation
Fix for issue #334
1 parent 45aeb81 commit c76e7a8

File tree

1 file changed

+63
-18
lines changed

1 file changed

+63
-18
lines changed

‎Firmware/RTK_Everywhere/Tilt.ino

Lines changed: 63 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -442,6 +442,9 @@ void tiltSensorFactoryReset()
442442
// Given a NMEA sentence, modify the sentence to use the latest tilt-compensated lat/lon/alt
443443
// Modifies the sentence directly. Updates sentence CRC.
444444
// Auto-detects sentence type and will only modify sentences that have lat/lon/alt (ie GGA yes, GSV no)
445+
// Which sentences have altitude? Yes: GGA, GNS No: RMC, GLL
446+
// Which sentences have undulation? Yes: GGA, GNS No: RMC, GLL
447+
// See issue: https://github.com/sparkfun/SparkFun_RTK_Everywhere_Firmware/issues/334
445448
void tiltApplyCompensation(char *nmeaSentence, int arraySize)
446449
{
447450
// Verify the sentence is null-terminated
@@ -481,22 +484,31 @@ void tiltApplyCompensation(char *nmeaSentence, int arraySize)
481484
}
482485

483486
// Modify a GNS sentence with tilt compensation
484-
//$GNGGA,213441.00,4005.4176871,N,10511.1034563,W,1,12,99.99,1581.450,M,-21.361,M,,*7D - Original
485-
//$GNGGA,213441.00,4005.41769994,N,10507.40740734,W,1,12,99.99,1580.987,M,-21.361,M,,*7E - Modified
487+
//$GNGNS,024034.00,4004.73854216,N,11614.19720023,E,ANAAA,28,0.8,1574.406,-8.4923,,,S*71 - Original
488+
//$GNGNS,024034.00,4004.73854216,N,11614.19720023,E,ANAAA,28,0.8,1589.4793,-8.4923,,,S*7E - Modified
489+
// 1580.987 is what is provided by the IMU and is the ellisoidal height
490+
// 1580.987 is called 'ellipsoidal height' in SW Maps and includes the MSL + undulation
491+
// To get mean sea level: 1580.987 - -8.4923 = 1589.4793
492+
// 1589.4793 is the orthometric height in meters (MSL reference) that we need to insert into the NMEA sentence
493+
// See issue: https://github.com/sparkfun/SparkFun_RTK_Everywhere_Firmware/issues/334
494+
// https://support.virtual-surveyor.com/support/solutions/articles/1000261349-the-difference-between-ellipsoidal-geoid-and-orthometric-elevations-
486495
void tiltApplyCompensationGNS(char *nmeaSentence, int arraySize)
487496
{
488497
char coordinateStringDDMM[strlen("10511.12345678") + 1] = {0}; // UM980 outputs 8 decimals in GGA sentence
489498

490499
const int latitudeComma = 2;
491500
const int longitudeComma = 4;
492501
const int altitudeComma = 9;
502+
const int undulationComma = 10;
493503

494504
uint8_t latitudeStart = 0;
495505
uint8_t latitudeStop = 0;
496506
uint8_t longitudeStart = 0;
497507
uint8_t longitudeStop = 0;
498508
uint8_t altitudeStart = 0;
499509
uint8_t altitudeStop = 0;
510+
uint8_t undulationStart = 0;
511+
uint8_t undulationStop = 0;
500512
uint8_t checksumStart = 0;
501513

502514
int commaCount = 0;
@@ -507,16 +519,20 @@ void tiltApplyCompensationGNS(char *nmeaSentence, int arraySize)
507519
commaCount++;
508520
if (commaCount == latitudeComma)
509521
latitudeStart = x + 1;
510-
else if (commaCount == latitudeComma + 1)
522+
if (commaCount == latitudeComma + 1)
511523
latitudeStop = x;
512-
else if (commaCount == longitudeComma)
524+
if (commaCount == longitudeComma)
513525
longitudeStart = x + 1;
514-
else if (commaCount == longitudeComma + 1)
526+
if (commaCount == longitudeComma + 1)
515527
longitudeStop = x;
516-
else if (commaCount == altitudeComma)
528+
if (commaCount == altitudeComma)
517529
altitudeStart = x + 1;
518-
else if (commaCount == altitudeComma + 1)
530+
if (commaCount == altitudeComma + 1)
519531
altitudeStop = x;
532+
if (commaCount == undulationComma)
533+
undulationStart = x + 1;
534+
if (commaCount == undulationComma + 1)
535+
undulationStop = x;
520536
}
521537
if (nmeaSentence[x] == '*')
522538
{
@@ -526,12 +542,20 @@ void tiltApplyCompensationGNS(char *nmeaSentence, int arraySize)
526542
}
527543

528544
if (latitudeStart == 0 || latitudeStop == 0 || longitudeStart == 0 || longitudeStop == 0 || altitudeStart == 0 ||
529-
altitudeStop == 0 || checksumStart == 0)
545+
altitudeStop == 0 || undulationStart == 0 || undulationStop == 0 || checksumStart == 0)
530546
{
531547
systemPrintln("Delineator not found");
532548
return;
533549
}
534550

551+
// Extract the undulation
552+
char undulationStr[strlen("-1602.3481") + 1]; // 4 decimals
553+
strncpy(undulationStr, &nmeaSentence[undulationStart], undulationStop - undulationStart);
554+
float undulation = (float)atof(undulationStr);
555+
556+
// Remove the undulation from the IMU's altitude
557+
float orthometricHeight = tiltSensor->getNaviAltitude() - undulation;
558+
535559
char newSentence[150] = {0};
536560

537561
if (sizeof(newSentence) < arraySize)
@@ -565,7 +589,7 @@ void tiltApplyCompensationGNS(char *nmeaSentence, int arraySize)
565589
strncat(newSentence, nmeaSentence + longitudeStop, altitudeStart - longitudeStop);
566590

567591
// Convert altitude double to string
568-
snprintf(coordinateStringDDMM, sizeof(coordinateStringDDMM), "%0.3f", tiltSensor->getNaviAltitude());
592+
snprintf(coordinateStringDDMM, sizeof(coordinateStringDDMM), "%0.3f", orthometricHeight);
569593

570594
// Add tilt-compensated Altitude
571595
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
@@ -769,20 +793,29 @@ void tiltApplyCompensationRMC(char *nmeaSentence, int arraySize)
769793
}
770794

771795
// Modify a GGA sentence with tilt compensation
772-
//$GNGGA,213441.00,4005.4176871,N,10511.1034563,W,1,12,99.99,1581.450,M,-21.361,M,,*7D - Original
773-
//$GNGGA,213441.00,4005.41769994,N,10507.40740734,W,1,12,99.99,1580.987,M,-21.361,M,,*7E - Modified
796+
//$GNGGA,213441.00,4005.4176871,N,10511.1034563,W,1,12,99.99,1581.450,M,-21.3612,M,,*7D - Original
797+
//$GNGGA,213441.00,4005.41769994,N,10507.40740734,W,1,12,99.99,1602.348,M,-21.3612,M,,*4C - Modified
798+
// 1580.987 is what is provided by the IMU and is the ellisoidal height
799+
//'Ellipsoidal height' includes the MSL + undulation
800+
// To get mean sea level: 1580.987 - -21.3612 = 1602.3482
801+
// 1602.3482 is the orthometric height in meters (MSL reference) that we need to insert into the NMEA sentence
802+
// See issue: https://github.com/sparkfun/SparkFun_RTK_Everywhere_Firmware/issues/334
803+
// https://support.virtual-surveyor.com/support/solutions/articles/1000261349-the-difference-between-ellipsoidal-geoid-and-orthometric-elevations-
774804
void tiltApplyCompensationGGA(char *nmeaSentence, int arraySize)
775805
{
776806
const int latitudeComma = 2;
777807
const int longitudeComma = 4;
778808
const int altitudeComma = 9;
809+
const int undulationComma = 11;
779810

780811
uint8_t latitudeStart = 0;
781812
uint8_t latitudeStop = 0;
782813
uint8_t longitudeStart = 0;
783814
uint8_t longitudeStop = 0;
784815
uint8_t altitudeStart = 0;
785816
uint8_t altitudeStop = 0;
817+
uint8_t undulationStart = 0;
818+
uint8_t undulationStop = 0;
786819
uint8_t checksumStart = 0;
787820

788821
if (settings.enableImuCompensationDebug == true)
@@ -796,16 +829,20 @@ void tiltApplyCompensationGGA(char *nmeaSentence, int arraySize)
796829
commaCount++;
797830
if (commaCount == latitudeComma)
798831
latitudeStart = x + 1;
799-
else if (commaCount == latitudeComma + 1)
832+
if (commaCount == latitudeComma + 1)
800833
latitudeStop = x;
801-
else if (commaCount == longitudeComma)
834+
if (commaCount == longitudeComma)
802835
longitudeStart = x + 1;
803-
else if (commaCount == longitudeComma + 1)
836+
if (commaCount == longitudeComma + 1)
804837
longitudeStop = x;
805-
else if (commaCount == altitudeComma)
838+
if (commaCount == altitudeComma)
806839
altitudeStart = x + 1;
807-
else if (commaCount == altitudeComma + 1)
840+
if (commaCount == altitudeComma + 1)
808841
altitudeStop = x;
842+
if (commaCount == undulationComma)
843+
undulationStart = x + 1;
844+
if (commaCount == undulationComma + 1)
845+
undulationStop = x;
809846
}
810847
if (nmeaSentence[x] == '*')
811848
{
@@ -815,12 +852,20 @@ void tiltApplyCompensationGGA(char *nmeaSentence, int arraySize)
815852
}
816853

817854
if (latitudeStart == 0 || latitudeStop == 0 || longitudeStart == 0 || longitudeStop == 0 || altitudeStart == 0 ||
818-
altitudeStop == 0 || checksumStart == 0)
855+
altitudeStop == 0 || undulationStart == 0 || undulationStop == 0 || checksumStart == 0)
819856
{
820857
systemPrintln("Delineator not found");
821858
return;
822859
}
823860

861+
// Extract the undulation
862+
char undulationStr[strlen("-1602.3481") + 1]; // 4 decimals
863+
strncpy(undulationStr, &nmeaSentence[undulationStart], undulationStop - undulationStart);
864+
float undulation = (float)atof(undulationStr);
865+
866+
// Remove the undulation from the IMU's altitude
867+
float orthometricHeight = tiltSensor->getNaviAltitude() - undulation;
868+
824869
char newSentence[150] = {0};
825870

826871
if (sizeof(newSentence) < arraySize)
@@ -856,7 +901,7 @@ void tiltApplyCompensationGGA(char *nmeaSentence, int arraySize)
856901
strncat(newSentence, nmeaSentence + longitudeStop, altitudeStart - longitudeStop);
857902

858903
// Convert altitude double to string
859-
snprintf(coordinateStringDDMM, sizeof(coordinateStringDDMM), "%0.3f", tiltSensor->getNaviAltitude());
904+
snprintf(coordinateStringDDMM, sizeof(coordinateStringDDMM), "%0.3f", orthometricHeight);
860905

861906
// Add tilt-compensated Altitude
862907
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);

0 commit comments

Comments
 (0)