From 17f93b8a944ec314b1db70b4842505591d788918 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 20 Jul 2021 17:37:03 +1000 Subject: [PATCH] AP_DAL: update GPS yaw API to add timestamp --- libraries/AP_DAL/AP_DAL_GPS.cpp | 2 +- libraries/AP_DAL/AP_DAL_GPS.h | 7 ++----- libraries/AP_DAL/LogStructure.h | 3 ++- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/libraries/AP_DAL/AP_DAL_GPS.cpp b/libraries/AP_DAL/AP_DAL_GPS.cpp index efd4b76c1c..8246157691 100644 --- a/libraries/AP_DAL/AP_DAL_GPS.cpp +++ b/libraries/AP_DAL/AP_DAL_GPS.cpp @@ -57,7 +57,7 @@ void AP_DAL_GPS::start_frame() RGPJ.velocity = gps.velocity(i); RGPI.speed_accuracy_returncode = gps.speed_accuracy(i, RGPJ.sacc); - RGPI.gps_yaw_deg_returncode = gps.gps_yaw_deg(i, RGPJ.yaw_deg, RGPJ.yaw_accuracy_deg); + RGPI.gps_yaw_deg_returncode = gps.gps_yaw_deg(i, RGPJ.yaw_deg, RGPJ.yaw_accuracy_deg, RGPJ.yaw_deg_time_ms); WRITE_REPLAY_BLOCK_IFCHANGED(RGPI, RGPI, old_RGPI); WRITE_REPLAY_BLOCK_IFCHANGED(RGPJ, RGPJ, old_RGPJ); diff --git a/libraries/AP_DAL/AP_DAL_GPS.h b/libraries/AP_DAL/AP_DAL_GPS.h index d91a77405f..034a8a5508 100644 --- a/libraries/AP_DAL/AP_DAL_GPS.h +++ b/libraries/AP_DAL/AP_DAL_GPS.h @@ -92,13 +92,10 @@ public: return speed_accuracy(primary_sensor(), sacc); } - bool gps_yaw_deg(float &yaw_deg, float &accuracy_deg) const { - return gps_yaw_deg(_RGPH.primary_sensor, yaw_deg, accuracy_deg); - } - - bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg) const { + bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const { yaw_deg = _RGPJ[instance].yaw_deg; accuracy_deg = _RGPJ[instance].yaw_accuracy_deg; + time_ms = _RGPJ[instance].yaw_deg_time_ms; return _RGPI[instance].gps_yaw_deg_returncode; } diff --git a/libraries/AP_DAL/LogStructure.h b/libraries/AP_DAL/LogStructure.h index 360881c845..488e058b26 100644 --- a/libraries/AP_DAL/LogStructure.h +++ b/libraries/AP_DAL/LogStructure.h @@ -208,6 +208,7 @@ struct log_RGPJ { float sacc; float yaw_deg; float yaw_accuracy_deg; + uint32_t yaw_deg_time_ms; int32_t lat; int32_t lng; int32_t alt; @@ -399,7 +400,7 @@ struct log_RBOH { { LOG_RGPI_MSG, RLOG_SIZE(RGPI), \ "RGPI", "ffffBBBB", "OX,OY,OZ,Lg,Flags,Stat,NSats,I", "-------#", "--------" }, \ { LOG_RGPJ_MSG, RLOG_SIZE(RGPJ), \ - "RGPJ", "IffffffiiiffHB", "TS,VX,VY,VZ,SA,Y,YA,Lat,Lon,Alt,HA,VA,HD,I", "-------------#", "--------------" }, \ + "RGPJ", "IffffffIiiiffHB", "TS,VX,VY,VZ,SA,Y,YA,YT,Lat,Lon,Alt,HA,VA,HD,I", "--------------#", "---------------" }, \ { LOG_RMGH_MSG, RLOG_SIZE(RMGH), \ "RMGH", "BBfBBBB", "Dec,NumInst,AutoDec,NumEna,LOE,C,FUsable", "-------", "-------" }, \ { LOG_RMGI_MSG, RLOG_SIZE(RMGI), \