From 57fdda597be4e14e3f493f6b55ef550bbb9bdccd Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 21 Jan 2025 16:52:25 +0300 Subject: [PATCH] vtol_takeoff: store altitude on takeoff and don't use home position altitude as vehicle does not need to be close to home position Signed-off-by: RomanBapst --- src/modules/navigator/vtol_takeoff.cpp | 4 +++- src/modules/navigator/vtol_takeoff.h | 1 + 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index f9a552ef58..503a0279f9 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -118,7 +118,7 @@ VtolTakeoff::on_active() _mission_item.time_inside = 1.f; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.altitude = _navigator->get_home_position()->alt + _param_loiter_alt.get(); + _mission_item.altitude = _takeoff_alt_msl + _param_loiter_alt.get(); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->current.lat = _loiter_location(0); @@ -170,6 +170,8 @@ VtolTakeoff::set_takeoff_position() // set current mission item to takeoff set_takeoff_item(&_mission_item, _transition_alt_amsl); + _takeoff_alt_msl = _navigator->get_global_position()->alt; + _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; diff --git a/src/modules/navigator/vtol_takeoff.h b/src/modules/navigator/vtol_takeoff.h index a353d89ce8..3ba32ce7df 100644 --- a/src/modules/navigator/vtol_takeoff.h +++ b/src/modules/navigator/vtol_takeoff.h @@ -70,6 +70,7 @@ private: } _takeoff_state; float _transition_alt_amsl{0.f}; // absolute altitude at which vehicle will transition to forward flight + float _takeoff_alt_msl{0.f}; matrix::Vector2d _loiter_location; float _loiter_height{0};