From 20f6666ee10f0cadd1b22ef30af1b03adb5ccb5f Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Wed, 14 Aug 2024 14:42:15 +0200 Subject: [PATCH] initial working --- .../flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp | 6 +++--- src/modules/navigator/land.cpp | 2 ++ 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index dad05e3c31db..f9165a863d54 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -247,12 +247,12 @@ void FlightTaskAuto::_prepareLandSetpoints() if (_type_previous != WaypointType::land) { // initialize yaw and xy-position _land_heading = _yaw_setpoint; - _stick_acceleration_xy.resetPosition(Vector2f(_target(0), _target(1))); - _initial_land_position = Vector3f(_target(0), _target(1), NAN); + _stick_acceleration_xy.resetPosition(Vector2f(_triplet_target(0), _triplet_target(1))); + _initial_land_position = Vector3f(_triplet_target(0), _triplet_target(1), NAN); } // Update xy-position in case of landing position changes (etc. precision landing) - _land_position = Vector3f(_target(0), _target(1), NAN); + _land_position = Vector3f(_triplet_target(0), _triplet_target(1), NAN); // User input assisted landing if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) { diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index b6a951d1b766..7672871adf5a 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -58,6 +58,8 @@ Land::on_activation() /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); pos_sp_triplet->previous.valid = false; + // set current mission_item so that we can breake before reaching the landing waypoint + _navigator->calculate_breaking_stop(_mission_item.lat, _mission_item.lon); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false;