Skip to content

Commit

Permalink
Remove undulation during tilt compensation
Browse files Browse the repository at this point in the history 
Fix for issue #334
  • Loading branch information
@nseidle
nseidle committed Jun 12, 2024
1 parent 45aeb81 commit c76e7a8
Showing 1 changed file with 63 additions and 18 deletions.
81 changes: 63 additions & 18 deletions Firmware/RTK_Everywhere/Tilt.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -442,6 +442,9 @@ void tiltSensorFactoryReset()
// Given a NMEA sentence, modify the sentence to use the latest tilt-compensated lat/lon/alt
// Modifies the sentence directly. Updates sentence CRC.
// Auto-detects sentence type and will only modify sentences that have lat/lon/alt (ie GGA yes, GSV no)
// Which sentences have altitude? Yes: GGA, GNS No: RMC, GLL
// Which sentences have undulation? Yes: GGA, GNS No: RMC, GLL
// See issue: https://github.com/sparkfun/SparkFun_RTK_Everywhere_Firmware/issues/334
void tiltApplyCompensation(char *nmeaSentence, int arraySize)
{
// Verify the sentence is null-terminated
Expand Down Expand Up @@ -481,22 +484,31 @@ void tiltApplyCompensation(char *nmeaSentence, int arraySize)
}

// Modify a GNS sentence with tilt compensation
//$GNGGA,213441.00,4005.4176871,N,10511.1034563,W,1,12,99.99,1581.450,M,-21.361,M,,*7D - Original
//$GNGGA,213441.00,4005.41769994,N,10507.40740734,W,1,12,99.99,1580.987,M,-21.361,M,,*7E - Modified
//$GNGNS,024034.00,4004.73854216,N,11614.19720023,E,ANAAA,28,0.8,1574.406,-8.4923,,,S*71 - Original
//$GNGNS,024034.00,4004.73854216,N,11614.19720023,E,ANAAA,28,0.8,1589.4793,-8.4923,,,S*7E - Modified
// 1580.987 is what is provided by the IMU and is the ellisoidal height
// 1580.987 is called 'ellipsoidal height' in SW Maps and includes the MSL + undulation
// To get mean sea level: 1580.987 - -8.4923 = 1589.4793
// 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 tiltApplyCompensationGNS(char *nmeaSentence, int arraySize)
{
char coordinateStringDDMM[strlen("10511.12345678") + 1] = {0}; // UM980 outputs 8 decimals in GGA sentence

const int latitudeComma = 2;
const int longitudeComma = 4;
const int altitudeComma = 9;
const int undulationComma = 10;

uint8_t latitudeStart = 0;
uint8_t latitudeStop = 0;
uint8_t longitudeStart = 0;
uint8_t longitudeStop = 0;
uint8_t altitudeStart = 0;
uint8_t altitudeStop = 0;
uint8_t undulationStart = 0;
uint8_t undulationStop = 0;
uint8_t checksumStart = 0;

int commaCount = 0;
Expand All @@ -507,16 +519,20 @@ void tiltApplyCompensationGNS(char *nmeaSentence, int arraySize)
commaCount++;
if (commaCount == latitudeComma)
latitudeStart = x + 1;
else if (commaCount == latitudeComma + 1)
if (commaCount == latitudeComma + 1)
latitudeStop = x;
else if (commaCount == longitudeComma)
if (commaCount == longitudeComma)
longitudeStart = x + 1;
else if (commaCount == longitudeComma + 1)
if (commaCount == longitudeComma + 1)
longitudeStop = x;
else if (commaCount == altitudeComma)
if (commaCount == altitudeComma)
altitudeStart = x + 1;
else if (commaCount == altitudeComma + 1)
if (commaCount == altitudeComma + 1)
altitudeStop = x;
if (commaCount == undulationComma)
undulationStart = x + 1;
if (commaCount == undulationComma + 1)
undulationStop = x;
}
if (nmeaSentence[x] == '*')
{
Expand All @@ -526,12 +542,20 @@ void tiltApplyCompensationGNS(char *nmeaSentence, int arraySize)
}

if (latitudeStart == 0 || latitudeStop == 0 || longitudeStart == 0 || longitudeStop == 0 || altitudeStart == 0 ||
altitudeStop == 0 || checksumStart == 0)
altitudeStop == 0 || undulationStart == 0 || undulationStop == 0 || checksumStart == 0)
{
systemPrintln("Delineator not found");
return;
}

// Extract the undulation
char undulationStr[strlen("-1602.3481") + 1]; // 4 decimals
strncpy(undulationStr, &nmeaSentence[undulationStart], undulationStop - undulationStart);
float undulation = (float)atof(undulationStr);

// Remove the undulation from the IMU's altitude
float orthometricHeight = tiltSensor->getNaviAltitude() - undulation;

char newSentence[150] = {0};

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

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

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

// Modify a GGA sentence with tilt compensation
//$GNGGA,213441.00,4005.4176871,N,10511.1034563,W,1,12,99.99,1581.450,M,-21.361,M,,*7D - Original
//$GNGGA,213441.00,4005.41769994,N,10507.40740734,W,1,12,99.99,1580.987,M,-21.361,M,,*7E - Modified
//$GNGGA,213441.00,4005.4176871,N,10511.1034563,W,1,12,99.99,1581.450,M,-21.3612,M,,*7D - Original
//$GNGGA,213441.00,4005.41769994,N,10507.40740734,W,1,12,99.99,1602.348,M,-21.3612,M,,*4C - Modified
// 1580.987 is what is provided by the IMU and is the ellisoidal height
//'Ellipsoidal height' includes the MSL + undulation
// To get mean sea level: 1580.987 - -21.3612 = 1602.3482
// 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 tiltApplyCompensationGGA(char *nmeaSentence, int arraySize)
{
const int latitudeComma = 2;
const int longitudeComma = 4;
const int altitudeComma = 9;
const int undulationComma = 11;

uint8_t latitudeStart = 0;
uint8_t latitudeStop = 0;
uint8_t longitudeStart = 0;
uint8_t longitudeStop = 0;
uint8_t altitudeStart = 0;
uint8_t altitudeStop = 0;
uint8_t undulationStart = 0;
uint8_t undulationStop = 0;
uint8_t checksumStart = 0;

if (settings.enableImuCompensationDebug == true)
Expand All @@ -796,16 +829,20 @@ void tiltApplyCompensationGGA(char *nmeaSentence, int arraySize)
commaCount++;
if (commaCount == latitudeComma)
latitudeStart = x + 1;
else if (commaCount == latitudeComma + 1)
if (commaCount == latitudeComma + 1)
latitudeStop = x;
else if (commaCount == longitudeComma)
if (commaCount == longitudeComma)
longitudeStart = x + 1;
else if (commaCount == longitudeComma + 1)
if (commaCount == longitudeComma + 1)
longitudeStop = x;
else if (commaCount == altitudeComma)
if (commaCount == altitudeComma)
altitudeStart = x + 1;
else if (commaCount == altitudeComma + 1)
if (commaCount == altitudeComma + 1)
altitudeStop = x;
if (commaCount == undulationComma)
undulationStart = x + 1;
if (commaCount == undulationComma + 1)
undulationStop = x;
}
if (nmeaSentence[x] == '*')
{
Expand All @@ -815,12 +852,20 @@ void tiltApplyCompensationGGA(char *nmeaSentence, int arraySize)
}

if (latitudeStart == 0 || latitudeStop == 0 || longitudeStart == 0 || longitudeStop == 0 || altitudeStart == 0 ||
altitudeStop == 0 || checksumStart == 0)
altitudeStop == 0 || undulationStart == 0 || undulationStop == 0 || checksumStart == 0)
{
systemPrintln("Delineator not found");
return;
}

// Extract the undulation
char undulationStr[strlen("-1602.3481") + 1]; // 4 decimals
strncpy(undulationStr, &nmeaSentence[undulationStart], undulationStop - undulationStart);
float undulation = (float)atof(undulationStr);

// Remove the undulation from the IMU's altitude
float orthometricHeight = tiltSensor->getNaviAltitude() - undulation;

char newSentence[150] = {0};

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

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

// Add tilt-compensated Altitude
strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
Expand Down

0 comments on commit c76e7a8

Please sign in to comment.