From 327c88eaa2135d876fa5165f94528bd8f2d61d05 Mon Sep 17 00:00:00 2001 From: Jacopo Panerati Date: Wed, 2 Oct 2024 21:00:52 +0400 Subject: [PATCH] Use VehicleCommand heading for VTOL transition --- src/modules/navigator/navigator_main.cpp | 6 ++++++ src/modules/navigator/vtol_takeoff.cpp | 9 +++++++-- src/modules/navigator/vtol_takeoff.h | 2 ++ 3 files changed, 15 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cd73e7e7d37c..c104e99e13cf 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -651,6 +651,12 @@ void Navigator::run() _vtol_takeoff.setTransitionAltitudeAbsolute(cmd.param7); + float epsilon = 1e-6f; + if (std::fabs(cmd.param2 - 3.0f) < epsilon) { // Specified transition direction + PX4_WARN("[Navigator::run] setTransitionDirecton from cmd.param4"); + _vtol_takeoff.setTransitionDirection(cmd.param4); + } + // after the transition the vehicle will establish on a loiter at this position _vtol_takeoff.setLoiterLocation(matrix::Vector2d(cmd.param5, cmd.param6)); diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index f9a552ef58fc..e212951cede5 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -71,8 +71,13 @@ VtolTakeoff::on_active() position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.yaw = wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat, - _mission_item.lon, _loiter_location(0), _loiter_location(1))); + if (std::isnan(_transition_direction_deg)) { + _mission_item.yaw = wrap_pi(get_bearing_to_next_waypoint(_navigator->get_home_position()->lat, + _navigator->get_home_position()->lon, _loiter_location(0), _loiter_location(1))); + } else { + PX4_WARN("[VtolTakeoff] Transition direction from Command"); + _mission_item.yaw = wrap_pi(math::radians(_transition_direction_deg)); + } _mission_item.force_heading = true; mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->current.cruising_speed = -1.f; diff --git a/src/modules/navigator/vtol_takeoff.h b/src/modules/navigator/vtol_takeoff.h index a353d89ce83e..575331e11025 100644 --- a/src/modules/navigator/vtol_takeoff.h +++ b/src/modules/navigator/vtol_takeoff.h @@ -55,6 +55,7 @@ class VtolTakeoff : public MissionBlock, public ModuleParams void on_active() override; void setTransitionAltitudeAbsolute(const float alt_amsl) {_transition_alt_amsl = alt_amsl; } + void setTransitionDirection(const float tran_bear) {_transition_direction_deg = tran_bear; } void setLoiterLocation(matrix::Vector2d loiter_location) { _loiter_location = loiter_location; } void setLoiterHeight(const float height_m) { _loiter_height = height_m; } @@ -72,6 +73,7 @@ class VtolTakeoff : public MissionBlock, public ModuleParams float _transition_alt_amsl{0.f}; // absolute altitude at which vehicle will transition to forward flight matrix::Vector2d _loiter_location; float _loiter_height{0}; + float _transition_direction_deg{NAN}; DEFINE_PARAMETERS( (ParamFloat) _param_loiter_alt