122 lines
4.9 KiB
Arduino
122 lines
4.9 KiB
Arduino
/*
|
|
Get the high precision ECEF coordinates using double
|
|
By: Paul Clark
|
|
SparkFun Electronics
|
|
Date: September 8th, 2022
|
|
License: MIT. See license file for more information but you can
|
|
basically do whatever you want with this code.
|
|
|
|
This example shows how to read the high-precision ECEF
|
|
positional solution. Please see below for information about the units.
|
|
|
|
** This example will only work correctly on platforms which support 64-bit double **
|
|
|
|
Feel like supporting open source hardware?
|
|
Buy a board from SparkFun!
|
|
ZED-F9P RTK2: https://www.sparkfun.com/products/15136
|
|
NEO-M8P RTK: https://www.sparkfun.com/products/15005
|
|
|
|
Hardware Connections:
|
|
Plug a Qwiic cable into the GNSS and (e.g.) a Redboard Artemis https://www.sparkfun.com/products/15444
|
|
or an Artemis Thing Plus https://www.sparkfun.com/products/15574
|
|
If you don't have a platform with a Qwiic connection use the SparkFun Qwiic Breadboard Jumper (https://www.sparkfun.com/products/14425)
|
|
Open the serial monitor at 115200 baud to see the output
|
|
*/
|
|
|
|
#include <Wire.h> // Needed for I2C to GNSS
|
|
|
|
#define myWire Wire // This will work on the Redboard Artemis and the Artemis Thing Plus using Qwiic
|
|
//#define myWire Wire1 // Uncomment this line if you are using the extra SCL1/SDA1 pins (D17 and D16) on the Thing Plus
|
|
|
|
#include <SparkFun_u-blox_GNSS_Arduino_Library.h> //http://librarymanager/All#SparkFun_u-blox_GNSS
|
|
SFE_UBLOX_GNSS myGNSS;
|
|
|
|
long lastTime = 0; //Simple local timer. Limits amount if I2C traffic to u-blox module.
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin(115200);
|
|
while (!Serial); //Wait for user to open terminal
|
|
|
|
myWire.begin();
|
|
|
|
//myGNSS.enableDebugging(Serial); // Uncomment this line to enable debug messages
|
|
|
|
if (myGNSS.begin(myWire) == false) //Connect to the u-blox module using Wire port
|
|
{
|
|
Serial.println(F("u-blox GNSS not detected at default I2C address. Please check wiring. Freezing."));
|
|
while (1)
|
|
;
|
|
}
|
|
|
|
// Check that this platform supports 64-bit (8 byte) double
|
|
if (sizeof(double) < 8)
|
|
{
|
|
Serial.println(F("Warning! Your platform does not support 64-bit double."));
|
|
Serial.println(F("The ECEF coordinates will be inaccurate."));
|
|
}
|
|
|
|
myGNSS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise)
|
|
//myGNSS.saveConfiguration(); //Save the current settings to flash and BBR
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
//Query module only every second.
|
|
//The module only responds when a new position is available.
|
|
if (millis() - lastTime > 1000)
|
|
{
|
|
lastTime = millis(); //Update the timer
|
|
|
|
// getHighResECEFX: returns the X coordinate from HPPOSECEF as an int32_t in cm
|
|
// getHighResECEFXHp: returns the high resolution component of the X coordinate from HPPOSECEF as an int8_t in mm*10^-1 (0.1mm)
|
|
// getHighResECEFY: returns the Y coordinate from HPPOSECEF as an int32_t in cm
|
|
// getHighResECEFYHp: returns the high resolution component of the Y coordinate from HPPOSECEF as an int8_t in mm*10^-1 (0.1mm)
|
|
// getHighResECEFZ: returns the Z coordinate from HPPOSECEF as an int32_t in cm
|
|
// getHighResECEFZHp: returns the high resolution component of the Z coordinate from HPPOSECEF as an int8_t in mm*10^-1 (0.1mm)
|
|
// getPositionAccuracy: returns the position accuracy estimate from HPPOSLLH as an uint32_t in mm (note: not 0.1mm)
|
|
|
|
// First, let's collect the position data
|
|
int32_t ECEFX = myGNSS.getHighResECEFX();
|
|
int8_t ECEFXHp = myGNSS.getHighResECEFXHp();
|
|
int32_t ECEFY = myGNSS.getHighResECEFY();
|
|
int8_t ECEFYHp = myGNSS.getHighResECEFYHp();
|
|
int32_t ECEFZ = myGNSS.getHighResECEFZ();
|
|
int8_t ECEFZHp = myGNSS.getHighResECEFZHp();
|
|
uint32_t accuracy = myGNSS.getPositionAccuracy();
|
|
|
|
// Defines storage for the ECEF coordinates as double
|
|
double d_ECEFX;
|
|
double d_ECEFY;
|
|
double d_ECEFZ;
|
|
|
|
// Assemble the high precision coordinates
|
|
d_ECEFX = ((double)ECEFX) / 100.0; // Convert from cm to m
|
|
d_ECEFX += ((double)ECEFXHp) / 10000.0; // Now add the high resolution component ( mm * 10^-1 = m * 10^-4 )
|
|
d_ECEFY = ((double)ECEFY) / 100.0; // Convert from cm to m
|
|
d_ECEFY += ((double)ECEFYHp) / 10000.0; // Now add the high resolution component ( mm * 10^-1 = m * 10^-4 )
|
|
d_ECEFZ = ((double)ECEFZ) / 100.0; // Convert from cm to m
|
|
d_ECEFZ += ((double)ECEFZHp) / 10000.0; // Now add the high resolution component ( mm * 10^-1 = m * 10^-4 )
|
|
|
|
// Print the coordinates with 4 decimal places (0.1mm)
|
|
Serial.print("X (m): ");
|
|
Serial.print(d_ECEFX, 4);
|
|
Serial.print(", Y (m): ");
|
|
Serial.print(d_ECEFY, 4);
|
|
Serial.print(", Z (m): ");
|
|
Serial.print(d_ECEFZ, 4);
|
|
|
|
// Now define float storage for the accuracy
|
|
float f_accuracy;
|
|
|
|
// Convert the horizontal accuracy (mm) to a float
|
|
f_accuracy = accuracy;
|
|
// Now convert to m
|
|
f_accuracy = f_accuracy / 1000.0; // Convert from mm to m
|
|
|
|
// Finally, do the printing
|
|
Serial.print(", Accuracy (m): ");
|
|
Serial.println(f_accuracy, 3); // Print the accuracy with 3 decimal places
|
|
}
|
|
}
|