From 9d25e2c087256c1f327fc2bd1a0f49a8a7663721 Mon Sep 17 00:00:00 2001 From: dirksavage88 Date: Wed, 30 Oct 2024 16:47:08 -0400 Subject: [PATCH] fixes to conditional Signed-off-by: dirksavage88 --- src/drivers/uavcan/sensors/gnss.cpp | 5 +---- src/modules/ekf2/EKF2.cpp | 3 ++- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index dde8f46b6832..5d6fe896cc85 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -489,11 +489,8 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure } // Only use dual antenna gps yaw if fix type is (6) - if ((hrt_elapsed_time(&_last_gnss_relative_timestamp) < 2_s) && _rel_heading_valid && _carrier_solution_fixed) { + if (_rel_heading_valid && _carrier_solution_fixed) { - // Apply offset and report corrected heading - // float corrected_heading = _rel_heading - _yaw_offset_rads; - // report.heading = corrected_heading; report.heading = _rel_heading; report.heading_offset = NAN; report.heading_accuracy = _rel_heading_accuracy; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index dfd645ff205f..5c881e095a7d 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2406,13 +2406,14 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) } if (fabsf(_param_ekf2_gps_yaw_off.get()) > 0.f) { - if (PX4_ISFINITE(vehicle_gps_position.heading_offset) && PX4_ISFINITE(vehicle_gps_position.heading)) { + if (!PX4_ISFINITE(vehicle_gps_position.heading_offset) && PX4_ISFINITE(vehicle_gps_position.heading)) { // Convert the offset to radians & appy offset float raw_yaw_offset = matrix::wrap_pi(_param_ekf2_gps_yaw_off.get()); vehicle_gps_position.heading_offset = raw_yaw_offset; vehicle_gps_position.heading -= raw_yaw_offset; } } + const float altitude_amsl = static_cast(vehicle_gps_position.altitude_msl_m); const float altitude_ellipsoid = static_cast(vehicle_gps_position.altitude_ellipsoid_m);