From 1f502539ca785ec98601f2a907ad57150e032d59 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Thu, 15 Aug 2024 19:48:03 +0200 Subject: [PATCH] implemented feedback --- .../flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp | 8 ++++---- src/modules/navigator/land.cpp | 9 ++++++--- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index f9165a863d54..b2bb5632b7bf 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(_triplet_target(0), _triplet_target(1))); - _initial_land_position = Vector3f(_triplet_target(0), _triplet_target(1), NAN); + _stick_acceleration_xy.resetPosition(Vector2f(_target(0), _target(1))); + _initial_land_position = Vector3f(_target(0), _target(1), NAN); } // Update xy-position in case of landing position changes (etc. precision landing) - _land_position = Vector3f(_triplet_target(0), _triplet_target(1), NAN); + _land_position = Vector3f(_target(0), _target(1), NAN); // User input assisted landing if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) { @@ -452,7 +452,7 @@ bool FlightTaskAuto::_evaluateTriplets() _triplet_prev_wp(2) = -(_sub_triplet_setpoint.get().previous.alt - _reference_altitude); } else { - _triplet_prev_wp = _position; + _triplet_prev_wp = _triplet_target; } _prev_was_valid = _sub_triplet_setpoint.get().previous.valid; diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 7672871adf5a..7ff80ff50fb5 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -57,10 +57,13 @@ 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); + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _navigator->calculate_breaking_stop(_mission_item.lat, _mission_item.lon); + } + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->previous.valid = false; pos_sp_triplet->next.valid = false; _navigator->set_position_setpoint_triplet_updated();