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<FixType>
 	}
 
 	// 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<float>(vehicle_gps_position.altitude_msl_m);
 		const float altitude_ellipsoid = static_cast<float>(vehicle_gps_position.altitude_ellipsoid_m);