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 1/2] 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; 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 2/2] 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();