From 35a3f519f2f5f163083fbd321f5ab60fc8e53aca Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 14 Jul 2025 16:07:27 +0200 Subject: [PATCH] Revert "commander: publish full home attitude, not only yaw (#19717)" This reverts commit 6855aa57c4879bb5c67298f3ce1f738118fbafa0. --- msg/versioned/HomePosition.msg | 2 -- src/modules/commander/Commander.cpp | 6 +---- src/modules/commander/HomePosition.cpp | 27 ++++++------------- src/modules/commander/HomePosition.hpp | 11 +++----- src/modules/mavlink/streams/HOME_POSITION.hpp | 2 +- 5 files changed, 14 insertions(+), 34 deletions(-) diff --git a/msg/versioned/HomePosition.msg b/msg/versioned/HomePosition.msg index 7baafefb03..a25bf24163 100644 --- a/msg/versioned/HomePosition.msg +++ b/msg/versioned/HomePosition.msg @@ -12,8 +12,6 @@ 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 235bcb337a..b05d829945 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -999,10 +999,6 @@ 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; @@ -1011,7 +1007,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, roll, pitch, yaw)) { + if (_home_position.setManually(lat, lon, alt, 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 3825ffe9f5..2f30a93be4 100644 --- a/src/modules/commander/HomePosition.cpp +++ b/src/modules/commander/HomePosition.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022-2025 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 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,9 +99,7 @@ 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 - const vehicle_attitude_s &attitude = _attitude_sub.get(); - - fillLocalHomePos(home, lpos, attitude); + fillLocalHomePos(home, lpos); updated = true; } @@ -137,25 +135,19 @@ 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, lpos.heading); } -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, float heading) { home.x = x; home.y = y; home.z = z; home.valid_lpos = true; - home.roll = roll; - home.pitch = pitch; - home.yaw = yaw; + home.yaw = heading; } void HomePosition::fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos) @@ -237,7 +229,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, NAN); home.timestamp = hrt_absolute_time(); home.update_count++; @@ -253,7 +245,7 @@ void HomePosition::setInAirHomePosition() } } -bool HomePosition::setManually(double lat, double lon, float alt, float roll, float pitch, float yaw) +bool HomePosition::setManually(double lat, double lon, float alt, float yaw) { const vehicle_local_position_s &vehicle_local_position = _local_position_sub.get(); @@ -276,8 +268,6 @@ bool HomePosition::setManually(double lat, double lon, float alt, float roll, fl 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(); @@ -313,7 +303,6 @@ 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 dd218c902e..27a53c642c 100644 --- a/src/modules/commander/HomePosition.hpp +++ b/src/modules/commander/HomePosition.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022-2025 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 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,7 +39,6 @@ #include #include #include -#include #include #include #include @@ -65,7 +64,7 @@ public: bool setHomePosition(bool force = false); void setInAirHomePosition(); - bool setManually(double lat, double lon, float alt, float roll, float pitch, float yaw); + bool setManually(double lat, double lon, float alt, float yaw); void setTakeoffTime(uint64_t takeoff_time) { _takeoff_time = takeoff_time; } void update(bool set_automatically, bool check_if_changed); @@ -77,9 +76,8 @@ 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, float heading); 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); @@ -87,7 +85,6 @@ 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 a18c6dff52..bb67d14d2d 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(home.roll, home.pitch, home.yaw)); + matrix::Quatf q(matrix::Eulerf(0.f, 0.f, home.yaw)); q.copyTo(msg.q); msg.approach_x = 0.f;