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.
This commit is contained in:
Julian Oes
2025-07-20 07:51:14 +12:00
parent 2065f577d6
commit 2c97a875bf
8 changed files with 124 additions and 15 deletions
+23
View File
@@ -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
@@ -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"
@@ -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);
+3 -1
View File
@@ -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