From 3850fd9862dfa5cc07ba733b26df1f919096f2a2 Mon Sep 17 00:00:00 2001 From: artzha Date: Tue, 3 Dec 2024 10:28:58 -0600 Subject: [PATCH] Added support for gps heading correction --- math/gps_util.h | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/math/gps_util.h b/math/gps_util.h index fe63681..f8d22fb 100644 --- a/math/gps_util.h +++ b/math/gps_util.h @@ -166,10 +166,10 @@ inline double gpsDistance(const GPSPoint& p0, const GPSPoint& p1) { const auto& [lat0, lon0] = std::make_tuple(p0.lat, p0.lon); const auto& [lat1, lon1] = std::make_tuple(p1.lat, p1.lon); // Convert latitude and longitude to radians - double lat0_rad = p0.lat * M_PI / 180.0; - double lon0_rad = p0.lon * M_PI / 180.0; - double lat1_rad = p1.lat * M_PI / 180.0; - double lon1_rad = p1.lon * M_PI / 180.0; + double lat0_rad = lat0 * M_PI / 180.0; + double lon0_rad = lon0 * M_PI / 180.0; + double lat1_rad = lat1 * M_PI / 180.0; + double lon1_rad = lon1 * M_PI / 180.0; // Haversine formula for distance between two GPS coordinates double dlat = lat1_rad - lat0_rad; @@ -196,6 +196,10 @@ inline Eigen::Vector2d gpsToGlobalCoord(const GPSPoint& p0, return {e1 - e0, n1 - n0}; // x y } +inline double gpsToGlobalHeading(const GPSPoint& p0) { + return DegToRad(90.0 - p0.heading); +} + inline Eigen::Affine2f gpsToLocal(const GPSPoint& current, const GPSPoint& goal) { // Step 1: Convert the GPS coordinates to global coordinates