diff --git a/msg/versioned/HomePosition.msg b/msg/versioned/HomePosition.msg index a25bf24163..7baafefb03 100644 --- a/msg/versioned/HomePosition.msg +++ b/msg/versioned/HomePosition.msg @@ -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 643d45617f..5e03832ec1 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;