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 <[email protected]>
Ryanf55 · May 12, 2024 · 47a98ea · 47a98ea
1 parent fc105ae
commit 47a98ea
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 11 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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<float>(d->speedD))