From 47a98ea8ca322cfe7b4fb0d00a5434a1ae53cc9b Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sat, 11 May 2024 21:02:44 -0600 Subject: [PATCH] SITL: fix GPS headings * Rename NMEA heading to ground_course_deg * Rename heading() utility to ground course (it was wrong) * Add _rad prefix to be pedantic about units * Add missing degrees conversion in NMEA because NMEA is not SI Signed-off-by: Ryan Friedman --- libraries/SITL/SIM_GPS.cpp | 5 ++--- libraries/SITL/SIM_GPS.h | 5 +++-- libraries/SITL/SIM_GPS_NMEA.cpp | 10 +++++----- libraries/SITL/SIM_GPS_Trimble.cpp | 2 +- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index 38735248d97e2..15eafb90b01c0 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/libraries/SITL/SIM_GPS.cpp @@ -462,10 +462,9 @@ GPS_Data GPS::interpolate_data(const GPS_Data &d, uint32_t delay_ms) return _gps_history[N-1]; } -float GPS_Data::heading() const +float GPS_Data::ground_track_rad() const { - const auto velocity = Vector2d{speedE, speedN}; - return velocity.angle(); + return atan2f(speedE, speedN); } float GPS_Data::speed_2d() const diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h index f92375f55844a..954bbd45acef3 100644 --- a/libraries/SITL/SIM_GPS.h +++ b/libraries/SITL/SIM_GPS.h @@ -49,8 +49,9 @@ struct GPS_Data { float speed_acc; uint8_t num_sats; - // Get heading [rad], where 0 = North in WGS-84 coordinate system - float heading() const WARN_IF_UNUSED; + // Get course over ground [rad], where 0 = North in WGS-84 coordinate system. + // Calculated from 2D velocity. + float ground_track_rad() const WARN_IF_UNUSED; // Get 2D speed [m/s] in WGS-84 coordinate system float speed_2d() const WARN_IF_UNUSED; diff --git a/libraries/SITL/SIM_GPS_NMEA.cpp b/libraries/SITL/SIM_GPS_NMEA.cpp index c4b16b89da44f..bda820cabfbdd 100644 --- a/libraries/SITL/SIM_GPS_NMEA.cpp +++ b/libraries/SITL/SIM_GPS_NMEA.cpp @@ -79,13 +79,13 @@ void GPS_NMEA::publish(const GPS_Data *d) const float speed_mps = d->speed_2d(); const float speed_knots = speed_mps * M_PER_SEC_TO_KNOTS; - const auto heading_rad = d->heading(); + const auto ground_track_deg = degrees(d->ground_track_rad()); //$GPVTG,133.18,T,120.79,M,0.11,N,0.20,K,A*24 nmea_printf("$GPVTG,%.2f,T,%.2f,M,%.2f,N,%.2f,K,A", tstring, - heading_rad, - heading_rad, + ground_track_deg, + ground_track_deg, speed_knots, speed_knots * KNOTS_TO_METERS_PER_SECOND * 3.6); @@ -95,7 +95,7 @@ void GPS_NMEA::publish(const GPS_Data *d) lat_string, lng_string, speed_knots, - heading_rad, + ground_track_deg, dstring); if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_HDT) { @@ -112,7 +112,7 @@ void GPS_NMEA::publish(const GPS_Data *d) d->altitude, wrap_360(d->yaw_deg), d->pitch_deg, - heading_rad, + ground_track_deg, speed_mps, d->roll_deg, d->have_lock?1:0, // 2=rtkfloat 3=rtkfixed, diff --git a/libraries/SITL/SIM_GPS_Trimble.cpp b/libraries/SITL/SIM_GPS_Trimble.cpp index 1e1726b33376b..9bb3edd541eac 100644 --- a/libraries/SITL/SIM_GPS_Trimble.cpp +++ b/libraries/SITL/SIM_GPS_Trimble.cpp @@ -175,7 +175,7 @@ void GPS_Trimble::publish(const GPS_Data *d) GSOF_VEL_LEN, vel_flags, gsof_pack_float(d->speed_2d()), - gsof_pack_float(d->heading()), + gsof_pack_float(d->ground_track_rad()), // Trimble API has ambiguous direction here. // Intentionally narrow from double. gsof_pack_float(static_cast(d->speedD))