Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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 <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman 2024-05-11 21:02:44 -06:00 committed by Andrew Tridgell
parent 3b1ba19640
commit fbb785859a
4 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/SITL/SIM_GPS.cpp
View File

@ -462,10 +462,9 @@ GPS_Data GPS::interpolate_data(const GPS_Data &d, uint32_t delay_ms)
return _gps_history[N-1]; return _gps_history[N-1];
} }
float GPS_Data::heading() const float GPS_Data::ground_track_rad() const
{ {
const auto velocity = Vector2d{speedE, speedN}; return atan2f(speedE, speedN);
return velocity.angle();
} }
float GPS_Data::speed_2d() const float GPS_Data::speed_2d() const

5
libraries/SITL/SIM_GPS.h
View File

@ -49,8 +49,9 @@ struct GPS_Data {
float speed_acc; float speed_acc;
uint8_t num_sats; uint8_t num_sats;
// Get heading [rad], where 0 = North in WGS-84 coordinate system // Get course over ground [rad], where 0 = North in WGS-84 coordinate system.
float heading() const WARN_IF_UNUSED; // Calculated from 2D velocity.
float ground_track_rad() const WARN_IF_UNUSED;
// Get 2D speed [m/s] in WGS-84 coordinate system // Get 2D speed [m/s] in WGS-84 coordinate system
float speed_2d() const WARN_IF_UNUSED; float speed_2d() const WARN_IF_UNUSED;

10
libraries/SITL/SIM_GPS_NMEA.cpp
View File

@ -79,13 +79,13 @@ void GPS_NMEA::publish(const GPS_Data *d)
const float speed_mps = d->speed_2d(); const float speed_mps = d->speed_2d();
const float speed_knots = speed_mps * M_PER_SEC_TO_KNOTS; 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 //$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", nmea_printf("$GPVTG,%.2f,T,%.2f,M,%.2f,N,%.2f,K,A",
tstring, tstring,
heading_rad, ground_track_deg,
heading_rad, ground_track_deg,
speed_knots, speed_knots,
speed_knots * KNOTS_TO_METERS_PER_SECOND * 3.6); speed_knots * KNOTS_TO_METERS_PER_SECOND * 3.6);
@ -95,7 +95,7 @@ void GPS_NMEA::publish(const GPS_Data *d)
lat_string, lat_string,
lng_string, lng_string,
speed_knots, speed_knots,
heading_rad, ground_track_deg,
dstring); dstring);
if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_HDT) { 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, d->altitude,
wrap_360(d->yaw_deg), wrap_360(d->yaw_deg),
d->pitch_deg, d->pitch_deg,
heading_rad, ground_track_deg,
speed_mps, speed_mps,
d->roll_deg, d->roll_deg,
d->have_lock?1:0, // 2=rtkfloat 3=rtkfixed, d->have_lock?1:0, // 2=rtkfloat 3=rtkfixed,

2
libraries/SITL/SIM_GPS_Trimble.cpp
View File

@ -175,7 +175,7 @@ void GPS_Trimble::publish(const GPS_Data *d)
GSOF_VEL_LEN, GSOF_VEL_LEN,
vel_flags, vel_flags,
gsof_pack_float(d->speed_2d()), gsof_pack_float(d->speed_2d()),
gsof_pack_float(d->heading()), gsof_pack_float(d->ground_track_rad()),
// Trimble API has ambiguous direction here. // Trimble API has ambiguous direction here.
// Intentionally narrow from double. // Intentionally narrow from double.
gsof_pack_float(static_cast<float>(d->speedD)) gsof_pack_float(static_cast<float>(d->speedD))