From 160cd7f8801c20c6545c14ab1858c1c0e651ff28 Mon Sep 17 00:00:00 2001 From: oystub Date: Fri, 12 Apr 2024 17:16:02 +0200 Subject: [PATCH] populate Fix2 with PX4 heading fields --- AP_Periph/sensor_gps.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/AP_Periph/sensor_gps.cpp b/AP_Periph/sensor_gps.cpp index 520f384..93ffbd8 100644 --- a/AP_Periph/sensor_gps.cpp +++ b/AP_Periph/sensor_gps.cpp @@ -147,6 +147,23 @@ void AP_Periph_DroneCAN::can_gps_update(void) float vc3 = sq(sacc); pkt.covariance.data[3] = pkt.covariance.data[4] = pkt.covariance.data[5] = vc3; } + +#if defined(HAL_PERIPH_ENABLE_GPS) && GPS_MOVING_BASELINE + // PX4 relies on the ecef_position_velocity field for heading, heading offset and heading accuracy, + // since there is no support for RelPosHeading as of 1.14 stable + float yaw; + float yaw_accuracy; + uint32_t curr_timestamp; + static float last_timestamp = 0; + + if (gps.gps_yaw_deg(yaw, yaw_accuracy, curr_timestamp) && curr_timestamp != last_timestamp){ + pkt.ecef_position_velocity.len = 1; + pkt.ecef_position_velocity.data[0].velocity_xyz[0] = wrap_PI(yaw * DEG_TO_RAD); + pkt.ecef_position_velocity.data[0].velocity_xyz[1] = 0; // Offset already compensated for. + pkt.ecef_position_velocity.data[0].velocity_xyz[2] = yaw_accuracy * DEG_TO_RAD; + } + last_timestamp = curr_timestamp; +#endif fix2_pub.broadcast(pkt); }