commander/mavlink: use home attitude, not only yaw

According to the mavlink spec we should be publishing the home attitude
as a quaternion rather than just the yaw/heading.

Additionally, this allows setting the landing roll and pitch angle using
DO_SET_HOME (this yet needs to go into the MAVLink spec though).

This time including the message versioning and translation.

msg/px4_msgs_old/msg/HomePositionV0.msg

@@ -0,0 +1,23 @@
+# GPS home position in WGS84 coordinates.
+
+uint32 MESSAGE_VERSION = 0
+
+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 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

@@ -10,5 +10,6 @@
 #include "translation_arming_check_reply_v1.h"
 #include "translation_battery_status_v1.h"
 #include "translation_event_v1.h"
+#include "translation_home_position_v1.h"
 #include "translation_vehicle_attitude_setpoint_v1.h"
 #include "translation_vehicle_status_v1.h"

msg/translation_node/translations/translation_home_position_v1.h

@@ -0,0 +1,65 @@
+/****************************************************************************
+ * Copyright (c) 2025 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+// Translate HomePosition v0 <--> v1
+#include <px4_msgs_old/msg/home_position_v0.hpp>
+#include <px4_msgs/msg/home_position.hpp>
+
+class HomePositionV1Translation {
+public:
+	using MessageOlder = px4_msgs_old::msg::HomePositionV0;
+	static_assert(MessageOlder::MESSAGE_VERSION == 0);
+
+	using MessageNewer = px4_msgs::msg::HomePosition;
+	static_assert(MessageNewer::MESSAGE_VERSION == 1);
+
+	static constexpr const char* kTopic = "fmu/out/home_position";
+
+	static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) {
+
+		// Update for HomePosition
+	
+		// 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 = 0.0f;  // New field in v1, set to 0
+		msg_newer.pitch = 0.0f; // New field in v1, set to 0
+		msg_newer.yaw = 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) {
+
+		// Update for HomePosition
+	
+		// 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: roll and pitch from v1 are ignored in v0
+		msg_older.yaw = msg_newer.yaw;
+		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(HomePositionV1Translation);

msg/versioned/HomePosition.msg

@@ -1,6 +1,6 @@
 # GPS home position in WGS84 coordinates.
 
-uint32 MESSAGE_VERSION = 0
+uint32 MESSAGE_VERSION = 1
 
 uint64 timestamp			# time since system start (microseconds)
 
@@ -12,6 +12,8 @@ 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

src/modules/commander/Commander.cpp

@@ -999,6 +999,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 					}
 
 				} else {
+					float roll = matrix::wrap_2pi(math::radians(cmd.param2));
+					roll = PX4_ISFINITE(roll) ? roll : 0.0f;
+					float pitch = matrix::wrap_2pi(math::radians(cmd.param3));
+					pitch = PX4_ISFINITE(pitch) ? pitch : 0.0f;
 					float yaw = matrix::wrap_2pi(math::radians(cmd.param4));
 					yaw = PX4_ISFINITE(yaw) ? yaw : (float)NAN;
 					const double lat = cmd.param5;
@@ -1007,7 +1011,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 					if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt)) {
 
-						if (_home_position.setManually(lat, lon, alt, yaw)) {
+						if (_home_position.setManually(lat, lon, alt, roll, pitch, yaw)) {
 
 							cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 

src/modules/commander/HomePosition.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-2025 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
@@ -99,7 +99,9 @@ bool HomePosition::setHomePosition(bool force)
 		const vehicle_local_position_s &lpos = _local_position_sub.get();
 		_heading_reset_counter = lpos.heading_reset_counter; // TODO: should not be here
 
-		fillLocalHomePos(home, lpos);
+		const vehicle_attitude_s &attitude = _attitude_sub.get();
+
+		fillLocalHomePos(home, lpos, attitude);
 		updated = true;
 	}
 
@@ -135,19 +137,25 @@ bool HomePosition::setHomePosition(bool force)
 	return updated;
 }
 
-void HomePosition::fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos)
+void HomePosition::fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos,
+				    const vehicle_attitude_s &attitude)
 {
-	fillLocalHomePos(home, lpos.x, lpos.y, lpos.z, lpos.heading);
+	matrix::Quatf q(attitude.q);
+	matrix::Eulerf euler(q);
+	fillLocalHomePos(home, lpos.x, lpos.y, lpos.z, euler(0), euler(1), euler(2));
 }
 
-void HomePosition::fillLocalHomePos(home_position_s &home, float x, float y, float z, float heading)
+void HomePosition::fillLocalHomePos(home_position_s &home, float x, float y, float z, float roll, float pitch,
+				    float yaw)
 {
 	home.x = x;
 	home.y = y;
 	home.z = z;
 	home.valid_lpos = true;
 
-	home.yaw = heading;
+	home.roll = roll;
+	home.pitch = pitch;
+	home.yaw = yaw;
 }
 
 void HomePosition::fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos)
@@ -229,7 +237,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);
+			fillLocalHomePos(home, home_x, home_y, home_z, NAN, NAN, NAN);
 
 			home.timestamp = hrt_absolute_time();
 			home.update_count++;
@@ -245,7 +253,7 @@ void HomePosition::setInAirHomePosition()
 	}
 }
 
-bool HomePosition::setManually(double lat, double lon, float alt, float yaw)
+bool HomePosition::setManually(double lat, double lon, float alt, float roll, float pitch, float yaw)
 {
 	const vehicle_local_position_s &vehicle_local_position = _local_position_sub.get();
 
@@ -268,6 +276,8 @@ bool HomePosition::setManually(double lat, double lon, float alt, float yaw)
 	home.z = -(alt - vehicle_local_position.ref_alt);
 	home.valid_lpos = vehicle_local_position.xy_valid && vehicle_local_position.z_valid;
 
+	home.roll = roll;
+	home.pitch = pitch;
 	home.yaw = yaw;
 
 	home.timestamp = hrt_absolute_time();
@@ -303,6 +313,7 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
 {
 	_local_position_sub.update();
 	_global_position_sub.update();
+	_attitude_sub.update();
 
 	if (_vehicle_air_data_sub.updated()) {
 		vehicle_air_data_s baro_data;

src/modules/commander/HomePosition.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-2025 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
@@ -39,6 +39,7 @@
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/failsafe_flags.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
@@ -64,7 +65,7 @@ public:
 
 	bool setHomePosition(bool force = false);
 	void setInAirHomePosition();
-	bool setManually(double lat, double lon, float alt, float yaw);
+	bool setManually(double lat, double lon, float alt, float roll, float pitch, float yaw);
 	void setTakeoffTime(uint64_t takeoff_time) { _takeoff_time = takeoff_time; }
 
 	void update(bool set_automatically, bool check_if_changed);
@@ -76,8 +77,9 @@ private:
 	void setHomePosValid();
 	void updateHomePositionYaw(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, float heading);
+	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 fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos);
 	static void fillGlobalHomePos(home_position_s &home, double lat, double lon, double alt);
 
@@ -85,6 +87,7 @@ private:
 
 	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::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
 
 	uint64_t _last_gps_timestamp{0};

src/modules/mavlink/streams/HOME_POSITION.hpp

@@ -75,7 +75,7 @@ private:
 				msg.y = home.y;
 				msg.z = home.z;
 
-				matrix::Quatf q(matrix::Eulerf(0.f, 0.f, home.yaw));
+				matrix::Quatf q(matrix::Eulerf(home.roll, home.pitch, home.yaw));
 				q.copyTo(msg.q);
 
 				msg.approach_x = 0.f;