diff --git a/msg/px4_msgs_old/msg/HomePositionV0.msg b/msg/px4_msgs_old/msg/HomePositionV0.msg new file mode 100644 index 0000000000..a25bf24163 --- /dev/null +++ b/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 diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index 32828db007..e79765ac52 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/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" diff --git a/msg/translation_node/translations/translation_home_position_v1.h b/msg/translation_node/translations/translation_home_position_v1.h new file mode 100644 index 0000000000..c44b2c4541 --- /dev/null +++ b/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 +#include + +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); diff --git a/msg/versioned/HomePosition.msg b/msg/versioned/HomePosition.msg index a25bf24163..9dadd69016 100644 --- a/msg/versioned/HomePosition.msg +++ b/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 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 280d00d69c..424fe77f3a 100644 --- a/src/modules/commander/Commander.cpp +++ b/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; diff --git a/src/modules/commander/HomePosition.cpp b/src/modules/commander/HomePosition.cpp index 2f30a93be4..3825ffe9f5 100644 --- a/src/modules/commander/HomePosition.cpp +++ b/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; diff --git a/src/modules/commander/HomePosition.hpp b/src/modules/commander/HomePosition.hpp index 27a53c642c..dd218c902e 100644 --- a/src/modules/commander/HomePosition.hpp +++ b/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 #include #include +#include #include #include #include @@ -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 _global_position_sub{ORB_ID(vehicle_global_position)}; uORB::SubscriptionData _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::SubscriptionData _attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; uint64_t _last_gps_timestamp{0}; diff --git a/src/modules/mavlink/streams/HOME_POSITION.hpp b/src/modules/mavlink/streams/HOME_POSITION.hpp index bb67d14d2d..a18c6dff52 100644 --- a/src/modules/mavlink/streams/HOME_POSITION.hpp +++ b/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;