diff --git a/examples/Example31_Great_Circle_Distance/Example31_Great_Circle_Distance.ino b/examples/Example31_Great_Circle_Distance/Example31_Great_Circle_Distance.ino new file mode 100644 index 0000000..f444282 --- /dev/null +++ b/examples/Example31_Great_Circle_Distance/Example31_Great_Circle_Distance.ino @@ -0,0 +1,152 @@ +/* + Calculating the Great Circle Distance and Course to a target position + By: Paul Clark + SparkFun Electronics + Date: September 21st, 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 query a u-blox module for its latitude/longitude and then + calculate the Great Circle Distance and Course to a target location. + + Thanks! distanceBetween and courseTo were taken from Mikal Hart's TinyGPSPlus: + https://github.com/mikalhart/TinyGPSPlus/blob/ca29434514a5c5172bd807af0608df7f296582a2/src/TinyGPS%2B%2B.cpp#L285-L328 + + Note: Lat/Long are large numbers because they are degrees * 10^-7. To convert lat/long + to something google maps understands simply divide the numbers by 10,000,000. We + do this so that we don't have to use floating point numbers. + + 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 + SAM-M8Q: https://www.sparkfun.com/products/15106 + + Hardware Connections: + Plug a Qwiic cable into the GNSS and a BlackBoard + 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 //Needed for I2C to GNSS + +#include //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. + +//#include //Uncomment if required. May be needed for sqrt, atan2, etc.. + +double distanceBetween(long lat1_l, long long1_l, long lat2_l, long long2_l) +{ + // returns distance in meters between two positions, both specified + // as signed decimal-degrees latitude and longitude. Uses great-circle + // distance computation for hypothetical sphere of radius 6372795 meters. + // Because Earth is no exact sphere, rounding errors may be up to 0.5%. + // Courtesy of Maarten Lamers + double lat1 = (double)lat1_l / 10000000.0; // Convert lat and long to degrees + double long1 = (double)long1_l / 10000000.0; + double lat2 = (double)lat2_l / 10000000.0; + double long2 = (double)long2_l / 10000000.0; + double delta = radians(long1-long2); + double sdlong = sin(delta); + double cdlong = cos(delta); + lat1 = radians(lat1); + lat2 = radians(lat2); + double slat1 = sin(lat1); + double clat1 = cos(lat1); + double slat2 = sin(lat2); + double clat2 = cos(lat2); + delta = (clat1 * slat2) - (slat1 * clat2 * cdlong); + delta = sq(delta); + delta += sq(clat2 * sdlong); + delta = sqrt(delta); + double denom = (slat1 * slat2) + (clat1 * clat2 * cdlong); + delta = atan2(delta, denom); + return delta * 6372795; +} + +double courseTo(long lat1_l, long long1_l, long lat2_l, long long2_l) +{ + // returns course in degrees (North=0, West=270) from position 1 to position 2, + // both specified as signed decimal-degrees latitude and longitude. + // Because Earth is no exact sphere, calculated course may be off by a tiny fraction. + // Courtesy of Maarten Lamers + double lat1 = (double)lat1_l / 10000000.0; // Convert lat and long to degrees + double long1 = (double)long1_l / 10000000.0; + double lat2 = (double)lat2_l / 10000000.0; + double long2 = (double)long2_l / 10000000.0; + double dlon = radians(long2-long1); + lat1 = radians(lat1); + lat2 = radians(lat2); + double a1 = sin(dlon) * cos(lat2); + double a2 = sin(lat1) * cos(lat2) * cos(dlon); + a2 = cos(lat1) * sin(lat2) - a2; + a2 = atan2(a1, a2); + if (a2 < 0.0) + { + a2 += TWO_PI; + } + return degrees(a2); +} + +void setup() +{ + Serial.begin(115200); + while (!Serial); //Wait for user to open terminal + Serial.println("SparkFun u-blox Example"); + + Wire.begin(); + + //myGNSS.enableDebugging(); // Uncomment this line to enable helpful debug messages on Serial + + if (myGNSS.begin() == 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); + } + + myGNSS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise) + myGNSS.saveConfigSelective(VAL_CFG_SUBSEC_IOPORT); //Save (only) the communications port settings to flash and BBR +} + +void loop() +{ + //Query module only every second. Doing it more often will just cause I2C traffic. + //The module only responds when a new position is available + if (millis() - lastTime > 1000) + { + lastTime = millis(); //Update the timer + + long latitude = myGNSS.getLatitude(); + Serial.print(F("Lat: ")); + Serial.print(latitude); + + long longitude = myGNSS.getLongitude(); + Serial.print(F(" Long: ")); + Serial.print(longitude); + Serial.println(F(" (degrees * 10^-7)")); + + static const long TARGET_LAT = 400909142, TARGET_LON = -1051849833; // SparkFun's location: degrees * 10^-7 (40.091 N, 105.185 W) + + double distanceToTarget = distanceBetween( + latitude, + longitude, + TARGET_LAT, + TARGET_LON); + + Serial.print(F("Distance to target: ")); + Serial.print(distanceToTarget, 2); + Serial.print(F(" (m) ")); + + double courseToTarget = courseTo( + latitude, + longitude, + TARGET_LAT, + TARGET_LON); + + Serial.print(F("Course to target: ")); + Serial.print(courseToTarget, 1); + Serial.println(F(" (degrees)")); + } +}