fix(commander): capture home orientation on ground independent of local position (#27946)

ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim

@@ -12,6 +12,13 @@ fi
 if [ -n "${PX4_HOME_ALT}" ]; then
 	param set SIH_LOC_H0 ${PX4_HOME_ALT}
 fi
+if [ -n "${PX4_HOME_YAW}" ]; then
+	param set SIH_LOC_YAW0 ${PX4_HOME_YAW}
+fi
+if [ -n "${PX4_SIM_GPS_USED}" ]; then
+	# Override the simulated GPS satellite count (sensor_gps_sim). Used by tests to start with no fix
+	param set SIM_GPS_USED ${PX4_SIM_GPS_USED}
+fi
 
 if simulator_sih start; then
 

msg/px4_msgs_old/msg/HomePositionV1.msg

@@ -0,0 +1,25 @@
+# GPS home position in WGS84 coordinates.
+
+uint32 MESSAGE_VERSION = 1
+
+uint64 timestamp			# time since system start (microseconds)
+
+float64 lat				# Latitude in degrees
+float64 lon				# Longitude in degrees
+float32 alt				# Altitude in meters (AMSL)
+
+float32 x				# X coordinate in meters
+float32 y				# Y coordinate in meters
+float32 z				# Z coordinate in meters
+
+float32 roll				# Pitch angle in radians
+float32 pitch				# Roll angle in radians
+float32 yaw				# Yaw angle in radians
+
+bool valid_alt		# true when the altitude has been set
+bool valid_hpos		# true when the latitude and longitude have been set
+bool valid_lpos		# true when the local position (xyz) has been set
+
+bool manual_home	# true when home position was set manually
+
+uint32 update_count 	# update counter of the home position

msg/translation_node/translations/all_translations.h

@@ -15,6 +15,7 @@
 #include "translation_config_overrides_v1.h"
 #include "translation_event_v1.h"
 #include "translation_home_position_v1.h"
+#include "translation_home_position_v2.h"
 #include "translation_register_ext_component_reply_v1.h"
 #include "translation_register_ext_component_request_v1.h"
 #include "translation_register_ext_component_request_v2.h"

msg/translation_node/translations/translation_home_position_v1.h

@@ -6,14 +6,14 @@
 
 // Translate HomePosition v0 <--> v1
 #include <px4_msgs_old/msg/home_position_v0.hpp>
-#include <px4_msgs/msg/home_position.hpp>
+#include <px4_msgs_old/msg/home_position_v1.hpp>
 
 class HomePositionV1Translation {
 public:
 	using MessageOlder = px4_msgs_old::msg::HomePositionV0;
 	static_assert(MessageOlder::MESSAGE_VERSION == 0);
 
-	using MessageNewer = px4_msgs::msg::HomePosition;
+	using MessageNewer = px4_msgs_old::msg::HomePositionV1;
 	static_assert(MessageNewer::MESSAGE_VERSION == 1);
 
 	static constexpr const char* kTopic = "fmu/out/home_position";

msg/translation_node/translations/translation_home_position_v2.h

@@ -0,0 +1,66 @@
+/****************************************************************************
+ * Copyright (c) 2026 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+#include <cmath>
+
+// Translate HomePosition v1 <--> v2
+#include <px4_msgs_old/msg/home_position_v1.hpp>
+#include <px4_msgs/msg/home_position.hpp>
+
+class HomePositionV2Translation {
+public:
+	using MessageOlder = px4_msgs_old::msg::HomePositionV1;
+	static_assert(MessageOlder::MESSAGE_VERSION == 1);
+
+	using MessageNewer = px4_msgs::msg::HomePosition;
+	static_assert(MessageNewer::MESSAGE_VERSION == 2);
+
+	static constexpr const char* kTopic = "fmu/out/home_position";
+
+	static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) {
+		// Set msg_newer from msg_older
+		msg_newer.timestamp = msg_older.timestamp;
+		msg_newer.lat = msg_older.lat;
+		msg_newer.lon = msg_older.lon;
+		msg_newer.alt = msg_older.alt;
+		msg_newer.x = msg_older.x;
+		msg_newer.y = msg_older.y;
+		msg_newer.z = msg_older.z;
+		msg_newer.roll = msg_older.roll;
+		msg_newer.pitch = msg_older.pitch;
+		msg_newer.yaw = msg_older.yaw;
+		// New field in v2: infer validity from the finiteness of the orientation in v1
+		msg_newer.valid_attitude = std::isfinite(msg_older.roll) && std::isfinite(msg_older.pitch)
+					   && std::isfinite(msg_older.yaw);
+		msg_newer.valid_alt = msg_older.valid_alt;
+		msg_newer.valid_hpos = msg_older.valid_hpos;
+		msg_newer.valid_lpos = msg_older.valid_lpos;
+		msg_newer.manual_home = msg_older.manual_home;
+		msg_newer.update_count = msg_older.update_count;
+	}
+
+	static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) {
+		// Set msg_older from msg_newer
+		msg_older.timestamp = msg_newer.timestamp;
+		msg_older.lat = msg_newer.lat;
+		msg_older.lon = msg_newer.lon;
+		msg_older.alt = msg_newer.alt;
+		msg_older.x = msg_newer.x;
+		msg_older.y = msg_newer.y;
+		msg_older.z = msg_newer.z;
+		// Note: valid_attitude from v2 is dropped in v1; an unknown orientation is passed through as NAN
+		msg_older.roll = msg_newer.valid_attitude ? msg_newer.roll : NAN;
+		msg_older.pitch = msg_newer.valid_attitude ? msg_newer.pitch : NAN;
+		msg_older.yaw = msg_newer.valid_attitude ? msg_newer.yaw : NAN;
+		msg_older.valid_alt = msg_newer.valid_alt;
+		msg_older.valid_hpos = msg_newer.valid_hpos;
+		msg_older.valid_lpos = msg_newer.valid_lpos;
+		msg_older.manual_home = msg_newer.manual_home;
+		msg_older.update_count = msg_newer.update_count;
+	}
+};
+
+REGISTER_TOPIC_TRANSLATION_DIRECT(HomePositionV2Translation);

msg/versioned/HomePosition.msg

@@ -1,6 +1,6 @@
 # GPS home position in WGS84 coordinates.
 
-uint32 MESSAGE_VERSION = 1
+uint32 MESSAGE_VERSION = 2
 
 uint64 timestamp			# time since system start (microseconds)
 
@@ -12,13 +12,14 @@ float32 x				# X coordinate in meters
 float32 y				# Y coordinate in meters
 float32 z				# Z coordinate in meters
 
-float32 roll				# Pitch angle in radians
-float32 pitch				# Roll angle in radians
+float32 roll				# Roll angle in radians
+float32 pitch				# Pitch angle in radians
 float32 yaw				# Yaw angle in radians
 
 bool valid_alt		# true when the altitude has been set
 bool valid_hpos		# true when the latitude and longitude have been set
 bool valid_lpos		# true when the local position (xyz) has been set
+bool valid_attitude	# true when the orientation (roll, pitch, yaw) has been set
 
 bool manual_home	# true when home position was set manually
 

src/modules/commander/HomePosition.cpp

@@ -95,18 +95,34 @@ bool HomePosition::setHomePosition(bool force)
 
 	bool updated = false;
 	home_position_s home{};
+	home.x = NAN;
+	home.y = NAN;
+	home.z = NAN;
+	home.roll = NAN;
+	home.pitch = NAN;
+	home.yaw = NAN;
 
 	if (!_failsafe_flags.local_position_invalid) {
 		// Set home position in local coordinates
 		const vehicle_local_position_s &lpos = _local_position_sub.get();
 		_heading_reset_counter = lpos.heading_reset_counter; // TODO: should not be here
 
-		const vehicle_attitude_s &attitude = _attitude_sub.get();
-
-		fillLocalHomePos(home, lpos, attitude);
+		fillLocalHomePos(home, lpos);
 		updated = true;
 	}
 
+	if (!_failsafe_flags.attitude_invalid && _vehicle_land_detected_sub.get().landed) {
+		// Capture orientation only on the ground, where it is the orientation at the home point.
+		fillAttitude(home, _attitude_sub.get());
+		updated = true;
+
+	} else {
+		home.roll = _home_position_pub.get().roll;
+		home.pitch = _home_position_pub.get().pitch;
+		home.yaw = _home_position_pub.get().yaw;
+		home.valid_attitude = _home_position_pub.get().valid_attitude;
+	}
+
 	if (!_failsafe_flags.global_position_invalid) {
 		// Set home using the global position estimate (fused INS/GNSS)
 		const vehicle_global_position_s &gpos = _global_position_sub.get();
@@ -139,25 +155,26 @@ bool HomePosition::setHomePosition(bool force)
 	return updated;
 }
 
-void HomePosition::fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos,
-				    const vehicle_attitude_s &attitude)
+void HomePosition::fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos)
 {
-	matrix::Quatf q(attitude.q);
-	matrix::Eulerf euler(q);
-	fillLocalHomePos(home, lpos.x, lpos.y, lpos.z, euler(0), euler(1), euler(2));
+	fillLocalHomePos(home, lpos.x, lpos.y, lpos.z);
 }
 
-void HomePosition::fillLocalHomePos(home_position_s &home, float x, float y, float z, float roll, float pitch,
-				    float yaw)
+void HomePosition::fillLocalHomePos(home_position_s &home, float x, float y, float z)
 {
 	home.x = x;
 	home.y = y;
 	home.z = z;
 	home.valid_lpos = true;
+}
 
-	home.roll = roll;
-	home.pitch = pitch;
-	home.yaw = yaw;
+void HomePosition::fillAttitude(home_position_s &home, const vehicle_attitude_s &attitude)
+{
+	const matrix::Eulerf euler(matrix::Quatf(attitude.q));
+	home.roll = euler.phi();
+	home.pitch = euler.theta();
+	home.yaw = euler.psi();
+	home.valid_attitude = true;
 }
 
 void HomePosition::fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos)
@@ -239,7 +256,7 @@ void HomePosition::setInAirHomePosition()
 			ref_pos.project(home.lat, home.lon, home_x, home_y);
 
 			const float home_z = -(home.alt - lpos.ref_alt);
-			fillLocalHomePos(home, home_x, home_y, home_z, NAN, NAN, NAN);
+			fillLocalHomePos(home, home_x, home_y, home_z);
 
 			home.timestamp = hrt_absolute_time();
 			home.update_count++;
@@ -281,6 +298,7 @@ bool HomePosition::setManually(double lat, double lon, float alt, float roll, fl
 	home.roll = roll;
 	home.pitch = pitch;
 	home.yaw = yaw;
+	home.valid_attitude = PX4_ISFINITE(roll) && PX4_ISFINITE(pitch) && PX4_ISFINITE(yaw);
 
 	home.timestamp = hrt_absolute_time();
 	home.update_count++;
@@ -329,6 +347,7 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
 	_local_position_sub.update();
 	_global_position_sub.update();
 	_attitude_sub.update();
+	_vehicle_land_detected_sub.update();
 
 	if (_vehicle_air_data_sub.updated()) {
 		vehicle_air_data_s baro_data;
@@ -423,11 +442,13 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
 
 	if (check_if_changed && set_automatically) {
 		const bool can_set_home_lpos_first_time = !home.valid_lpos && !_failsafe_flags.local_position_invalid;
+		const bool can_set_home_attitude_first_time = !home.valid_attitude;
 		const bool can_set_home_gpos_first_time = ((!home.valid_hpos || !home.valid_alt)
 				&& (!_failsafe_flags.global_position_invalid || _gps_position_for_home_valid));
 		const bool can_set_home_alt_first_time = (!home.valid_alt && lpos.z_global);
 
 		if (can_set_home_lpos_first_time
+		    || can_set_home_attitude_first_time
 		    || can_set_home_gpos_first_time
 		    || can_set_home_alt_first_time
 		    || hasMovedFromCurrentHomeLocation()) {

src/modules/commander/HomePosition.hpp

@@ -40,6 +40,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/failsafe_flags.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
@@ -79,17 +80,18 @@ private:
 	void setHomePosValid();
 	void updateHomePositionYaw(float yaw);
 
-	static void fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos,
-				     const vehicle_attitude_s &attitude);
-	static void fillLocalHomePos(home_position_s &home, float x, float y, float z, float roll, float pitch, float yaw);
+	static void fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos);
+	static void fillLocalHomePos(home_position_s &home, float x, float y, float z);
 	static void fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos);
 	static void fillGlobalHomePos(home_position_s &home, double lat, double lon, double alt);
+	static void fillAttitude(home_position_s &home, const vehicle_attitude_s &attitude);
 
 	uORB::Subscription					_vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
 
 	uORB::SubscriptionData<vehicle_global_position_s>	_global_position_sub{ORB_ID(vehicle_global_position)};
 	uORB::SubscriptionData<vehicle_local_position_s>	_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::SubscriptionData<vehicle_attitude_s>	_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::SubscriptionData<vehicle_land_detected_s>	_vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
 
 	uint64_t _last_gps_timestamp{0};

src/modules/mavlink/streams/HOME_POSITION.hpp

@@ -71,12 +71,17 @@ private:
 				msg.longitude = home.lon * 1e7;
 				msg.altitude  = home.alt * 1e3f;
 
-				msg.x = home.x;
-				msg.y = home.y;
-				msg.z = home.z;
+				msg.x = home.valid_lpos ? home.x : 0.f;
+				msg.y = home.valid_lpos ? home.y : 0.f;
+				msg.z = home.valid_lpos ? home.z : 0.f;
 
-				matrix::Quatf q(matrix::Eulerf(home.roll, home.pitch, home.yaw));
-				q.copyTo(msg.q);
+				if (home.valid_attitude) {
+					const matrix::Quatf q(matrix::Eulerf(home.roll, home.pitch, home.yaw));
+					q.copyTo(msg.q);
+
+				} else {
+					msg.q[0] = msg.q[1] = msg.q[2] = msg.q[3] = NAN;
+				}
 
 				msg.approach_x = 0.f;
 				msg.approach_y = 0.f;

src/modules/simulation/simulator_sih/sih.cpp

@@ -332,7 +332,7 @@ void Sih::parameters_updated()
 		const Dcmf R_E2N = _lla.computeRotEcefToNed();
 		_R_N2E = R_E2N.transpose();
 		_v_E = _R_N2E * _v_N;
-
+		_q = Quatf(Eulerf(0.f, 0.f, _sih_yaw0.get()));
 		_q_E = Quatf(_R_N2E) * _q;
 		_q_E.normalize();
 	}

src/modules/simulation/simulator_sih/sih.hpp

@@ -334,6 +334,7 @@ private:
 		(ParamFloat<px4::params::SIH_LOC_LAT0>) _sih_lat0,
 		(ParamFloat<px4::params::SIH_LOC_LON0>) _sih_lon0,
 		(ParamFloat<px4::params::SIH_LOC_H0>) _sih_h0,
+		(ParamFloat<px4::params::SIH_LOC_YAW0>) _sih_yaw0,
 		(ParamFloat<px4::params::SIH_DISTSNSR_MIN>) _sih_distance_snsr_min,
 		(ParamFloat<px4::params::SIH_DISTSNSR_MAX>) _sih_distance_snsr_max,
 		(ParamFloat<px4::params::SIH_DISTSNSR_OVR>) _sih_distance_snsr_override,

src/modules/simulation/simulator_sih/sih_params.yaml

@@ -215,6 +215,18 @@ parameters:
       max: 8848.0
       decimal: 2
       increment: 0.01
+    SIH_LOC_YAW0:
+      description:
+        short: Initial heading (yaw) of the simulated vehicle
+        long: |-
+          This value represents the initial heading (yaw) of the simulated vehicle at the start of the simulation.
+      type: float
+      default: 0.0
+      unit: rad
+      min: -3.14159
+      max: 3.14159
+      decimal: 3
+      increment: 0.01
     SIH_DISTSNSR_MIN:
       description:
         short: distance sensor minimum range

test/mavsdk_tests/CMakeLists.txt

@@ -37,6 +37,7 @@ if(MAVSDK_FOUND)
         test_vtol_rtl.cpp
         test_vtol_mission_wind.cpp
         test_vtol_loiter_airspeed_failure_blockage.cpp
+        test_multicopter_home.cpp
         # test_multicopter_follow_me.cpp
     )
 

test/mavsdk_tests/autopilot_tester.cpp

@@ -776,6 +776,37 @@ void AutopilotTester::add_mavlink_message_callback(uint16_t message_id,
 	_mavlink_passthrough->subscribe_message(message_id, std::move(callback));
 }
 
+mavlink_home_position_t AutopilotTester::get_home_position(std::chrono::seconds timeout)
+{
+	auto home_position = std::make_shared<mavlink_home_position_t>();
+	auto received = std::make_shared<std::atomic<bool>>(false);
+
+	auto handle = _mavlink_passthrough->subscribe_message(
+			      MAVLINK_MSG_ID_HOME_POSITION,
+	[home_position, received](const mavlink_message_t &message) {
+		mavlink_msg_home_position_decode(&message, home_position.get());
+		received->store(true);
+	});
+
+	// Ask for a fresh HOME_POSITION so we don't rely on the periodic stream timing.
+	MavlinkPassthrough::CommandLong request{};
+	request.target_sysid = _mavlink_passthrough->get_target_sysid();
+	request.target_compid = _mavlink_passthrough->get_target_compid();
+	request.command = MAV_CMD_REQUEST_MESSAGE;
+	request.param1 = static_cast<float>(MAVLINK_MSG_ID_HOME_POSITION);
+	_mavlink_passthrough->send_command_long(request);
+
+	const bool got_it = poll_condition_with_timeout([received]() { return received->load(); }, timeout);
+	_mavlink_passthrough->unsubscribe_message(MAVLINK_MSG_ID_HOME_POSITION, handle);
+	REQUIRE(got_it);
+	return *home_position;
+}
+
+Telemetry::EulerAngle AutopilotTester::get_attitude_euler()
+{
+	return _telemetry->attitude_euler();
+}
+
 std::array<float, 3> AutopilotTester::get_current_position_ned()
 {
 	mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned();

test/mavsdk_tests/autopilot_tester.h

@@ -152,6 +152,10 @@ public:
 	void send_custom_mavlink_command(const MavlinkPassthrough::CommandInt &command);
 	void add_mavlink_message_callback(uint16_t message_id, std::function< void(const mavlink_message_t &)> callback);
 
+	mavlink_home_position_t get_home_position(std::chrono::seconds timeout = std::chrono::seconds(10));
+
+	Telemetry::EulerAngle get_attitude_euler();
+
 	void enable_fixedwing_mectrics();
 	void check_airspeed_is_valid();
 	void check_airspeed_is_invalid();

test/mavsdk_tests/configs/sih-sitl.json

@@ -18,6 +18,15 @@
                 "PX4_PARAM_ATT_EN": 1
             }
         },
+        {
+            "model": "quadx",
+            "test_filter": "[sih_home_yaw]",
+            "timeout_min": 10,
+            "env": {
+                "PX4_HOME_YAW": "2.3562",
+                "PX4_SIM_GPS_USED": "0"
+            }
+        },
         {
             "model": "standard_vtol",
             "test_filter": "[vtol]",

test/mavsdk_tests/test_multicopter_home.cpp

@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2026 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <chrono>
+#include <cmath>
+
+#include "autopilot_tester.h"
+
+// Reproduces the stale home-yaw bug: when home is first set from a raw-GPS fix while the local position
+// estimate is invalid, home.yaw must still end up at the true ground heading.
+TEST_CASE("SIH: home yaw captured when local position invalid", "[sih_home_yaw]")
+{
+	using namespace std::chrono_literals;
+
+	AutopilotTester tester;
+	tester.connect(connection_url);
+
+	// No GPS fix yet: wait for the magnetometer heading to align and settle, so that no later heading
+	// reset can refresh home.yaw and mask the bug.
+	tester.sleep_for(10s);
+	const float ground_yaw_deg = tester.get_attitude_euler().yaw_deg;
+
+	// Degraded GPS: good enough for the home position, too few sats for EKF fusion -> local position
+	// stays invalid. This is the exact condition under which the buggy code leaves home.yaw at 0.
+	tester.set_param_int("SIM_GPS_USED", 5);
+	tester.sleep_for(6s);
+
+	const mavlink_home_position_t home = tester.get_home_position();
+	// Copy out of the packed message struct before forming a float* for the conversion helper.
+	const float q[4] = {home.q[0], home.q[1], home.q[2], home.q[3]};
+	float roll_rad, pitch_rad, home_yaw_rad;
+	mavlink_quaternion_to_euler(q, &roll_rad, &pitch_rad, &home_yaw_rad);
+	const float home_yaw_deg = home_yaw_rad * 180.f / static_cast<float>(M_PI);
+
+	float diff_deg = std::fabs(ground_yaw_deg - home_yaw_deg);
+
+	if (diff_deg > 180.f) {
+		diff_deg = 360.f - diff_deg;
+	}
+
+	CAPTURE(ground_yaw_deg);
+	CAPTURE(home_yaw_deg);
+	// With the fix home.yaw tracks the true ground heading; with the bug it stays at 0 (North).
+	CHECK(diff_deg < 20.f);
+}