From ca141642aca9be590155144f456e09beaedb7f8d Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Fri, 6 Sep 2024 14:19:07 +0200 Subject: [PATCH] MPC: LAND: Split Landing out of the FlightTask Auto, any by that is able to compensate initial velocities better. --- src/lib/motion_planning/VelocitySmoothing.hpp | 1 - .../flight_mode_manager/CMakeLists.txt | 1 + .../flight_mode_manager/FlightModeManager.cpp | 16 +- .../tasks/Auto/FlightTaskAuto.cpp | 2 + .../tasks/Land/CMakeLists.txt | 39 +++ .../tasks/Land/FlightTaskLand.cpp | 293 ++++++++++++++++++ .../tasks/Land/FlightTaskLand.hpp | 114 +++++++ src/modules/navigator/land.cpp | 8 +- 8 files changed, 464 insertions(+), 10 deletions(-) create mode 100644 src/modules/flight_mode_manager/tasks/Land/CMakeLists.txt create mode 100644 src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp create mode 100644 src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp diff --git a/src/lib/motion_planning/VelocitySmoothing.hpp b/src/lib/motion_planning/VelocitySmoothing.hpp index 2ea584d156..96f26098f6 100644 --- a/src/lib/motion_planning/VelocitySmoothing.hpp +++ b/src/lib/motion_planning/VelocitySmoothing.hpp @@ -43,7 +43,6 @@ struct Trajectory { /** * @class VelocitySmoothing * - * TODO: document the algorithm * |T1| T2 |T3| * ___ * __| |____ __ Jerk diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt index 84ccba6ac0..95b50dc4b7 100644 --- a/src/modules/flight_mode_manager/CMakeLists.txt +++ b/src/modules/flight_mode_manager/CMakeLists.txt @@ -42,6 +42,7 @@ list(APPEND flight_tasks_all Auto Descend Failsafe + Land ManualAcceleration ManualAccelerationSlow ManualAltitude diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index c58a7fa157..4e652efedf 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -153,6 +153,7 @@ void FlightModeManager::start_flight_task() bool matching_task_running = true; bool task_failure = false; const bool nav_state_descend = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND); + const bool nav_state_land = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_LAND); // Follow me if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) { @@ -185,9 +186,20 @@ void FlightModeManager::start_flight_task() } } + // Landing + if (nav_state_land) { + found_some_task = true; + FlightTaskError error = switchTask(FlightTaskIndex::Land); + + if (error != FlightTaskError::NoError) { + matching_task_running = false; + task_failure = true; + } + } + // Navigator interface for autonomous modes if (_vehicle_control_mode_sub.get().flag_control_auto_enabled - && !nav_state_descend) { + && !nav_state_descend && !nav_state_land) { found_some_task = true; if (switchTask(FlightTaskIndex::Auto) != FlightTaskError::NoError) { @@ -388,7 +400,7 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index) if (!isAnyTaskActive()) { // no task running - return FlightTaskError::NoError; + return FlightTaskError::NoE rror; } // activation failed diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 834f79465f..c08ce4b773 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -231,6 +231,8 @@ void FlightTaskAuto::rcHelpModifyYaw(float &yaw_sp) void FlightTaskAuto::_prepareLandSetpoints() { + // IMPORTANT: This Landing logic is only used for Precision Landing, for the other Landing see FlightTaskLand + _velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint // Slow down automatic descend close to ground diff --git a/src/modules/flight_mode_manager/tasks/Land/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Land/CMakeLists.txt new file mode 100644 index 0000000000..157a74b9ff --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/Land/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(FlightTaskLand + FlightTaskLand.cpp +) + +target_link_libraries(FlightTaskLand PUBLIC FlightTask FlightTaskUtility) +target_include_directories(FlightTaskLand PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp b/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp new file mode 100644 index 0000000000..6b4fd557d5 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp @@ -0,0 +1,293 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file FlightTaskLand.cpp + */ + +#include "FlightTaskLand.hpp" + +bool +FlightTaskLand::activate(const trajectory_setpoint_s &last_setpoint) +{ + bool ret = FlightTask::activate(last_setpoint); + Vector3f vel_prev{last_setpoint.velocity}; + Vector3f pos_prev{last_setpoint.position}; + Vector3f accel_prev{last_setpoint.acceleration}; + + for (int i = 0; i < 3; i++) { + // If the position setpoint is unknown, set to the current position + if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); } + + // If the velocity setpoint is unknown, set to the current velocity + if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); } + + // If no acceleration estimate available, set to zero if the setpoint is NAN + if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; } + + + } + + _yaw_setpoint = _land_heading = _yaw; // set the yaw setpoint to the current yaw + _position_smoothing.reset(pos_prev, vel_prev, accel_prev); + + + _acceleration_setpoint = accel_prev; + _velocity_setpoint = vel_prev; + _position_setpoint = pos_prev; + + + + // Initialize the Landing locations and parameters + // calculate where to land based on the current velocity and acceleration constraints + _CalculateBrakingLocation(); + _initial_land_position = _land_position; + + return ret; +} + +void +FlightTaskLand::reActivate() +{ + FlightTask::reActivate(); + // On ground, reset acceleration and velocity to zero + _position_smoothing.reset({0.f, 0.f, 0.f}, {0.f, 0.f, 0.7f}, _position); +} + +bool +FlightTaskLand::update() +{ + bool ret = FlightTask::update(); + + if (!_is_initialized) { + _position_smoothing.reset(_acceleration_setpoint, _velocity, _position); + _is_initialized = true; + } + + if (_velocity.norm() < 0.1f * _param_mpc_xy_vel_max.get() && !_landing) { + _landing = true; + } + + if (_landing) { + _PerformLanding(); + + } else { + _SmoothBrakingPath(); + } + + return ret; +} + +void +FlightTaskLand::_PerformLanding() +{ + // Perform 3 phase landing + _velocity_setpoint.setNaN(); + + // Calculate the vertical speed based on the distance to the ground + float vertical_speed = math::interpolate(_dist_to_ground, + _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), + _param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get()); + + bool range_dist_available = PX4_ISFINITE(_dist_to_bottom); + + // If we are below the altitude of the third landing phase , use the crawl speed + if (range_dist_available && _dist_to_bottom <= _param_mpc_land_alt3.get()) { + vertical_speed = _param_mpc_land_crawl_speed.get(); + } + + // User input assisted landing + if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) { + // Stick full up -1 -> stop, stick full down 1 -> double the speed + vertical_speed *= (1 - _sticks.getThrottleZeroCenteredExpo()); + + if (fabsf(_sticks.getYawExpo()) > FLT_EPSILON) { + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, _sticks.getYawExpo(), _yaw, _deltatime); + } + + Vector2f sticks_xy = _sticks.getPitchRollExpo(); + Vector2f sticks_ne = sticks_xy; + Sticks::rotateIntoHeadingFrameXY(sticks_ne, _yaw, _land_heading); + + const float distance_to_circle = math::trajectory::getMaxDistanceToCircle(_position.xy(), _initial_land_position.xy(), + _param_mpc_land_radius.get(), sticks_ne); + float max_speed; + + if (PX4_ISFINITE(distance_to_circle)) { + max_speed = math::trajectory::computeMaxSpeedFromDistance(_stick_acceleration_xy.getMaxJerk(), + _stick_acceleration_xy.getMaxAcceleration(), distance_to_circle, 0.f); + + if (max_speed < 0.5f) { + sticks_xy.setZero(); + } + + } else { + max_speed = 0.f; + sticks_xy.setZero(); + } + + // If ground distance estimate valid (distance sensor) during nudging then limit horizontal speed + if (PX4_ISFINITE(_dist_to_bottom)) { + // Below 50cm no horizontal speed, above allow per meter altitude 0.5m/s speed + max_speed = math::max(0.f, math::min(max_speed, (_dist_to_bottom - .5f) * .5f)); + } + + _stick_acceleration_xy.setVelocityConstraint(max_speed); + _stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, _position, + _velocity_setpoint_feedback.xy(), _deltatime); + _stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint); + + } else { + // Make sure we have a valid land position even in the case we loose RC while amending it + if (!PX4_ISFINITE(_land_position(0))) { + _land_position.xy() = Vector2f(_position); + } + } + + _position_setpoint = {_land_position(0), _land_position(1), NAN}; // The last element of the land position has to stay NAN + _yaw_setpoint = _land_heading; + _velocity_setpoint(2) = vertical_speed; + _gear.landing_gear = landing_gear_s::GEAR_DOWN; + +} +void +FlightTaskLand::_SmoothBrakingPath() +{ + PositionSmoothing::PositionSmoothingSetpoints out_setpoints; + + _HandleHighVelocities(); + + _position_smoothing.generateSetpoints( + _position, + _land_position, + Vector3f{0.f, 0.f, 0.f}, + _deltatime, + false, + out_setpoints + ); + + _jerk_setpoint = out_setpoints.jerk; + _acceleration_setpoint = out_setpoints.acceleration; + _velocity_setpoint = out_setpoints.velocity; + _position_setpoint = out_setpoints.position; + _yaw_setpoint = _land_heading; +} + +void +FlightTaskLand::_CalculateBrakingLocation() +{ + // Calculate the 3D point where we until where we can slow down smoothly and then land based on the current velocities and system constraints on jerk and acceleration. + _UpdateTrajConstraints(); + float delay_scale = 0.4f; // delay scale factor + const float velocity_hor_abs = sqrtf(_velocity(0) * _velocity(0) + _velocity(1) * _velocity(1)); + const float braking_dist_xy = math::trajectory::computeBrakingDistanceFromVelocity(velocity_hor_abs, + _param_mpc_jerk_auto.get(), _param_mpc_acc_hor.get(), delay_scale * _param_mpc_jerk_auto.get()); + float braking_dist_z = 0.0f; + + if (_velocity(2) < -0.1f) { + braking_dist_z = math::trajectory::computeBrakingDistanceFromVelocity(_velocity(2), + _param_mpc_jerk_max.get(), _param_mpc_acc_down_max.get(), 0.f); + + } else if (_velocity(2) > 0.1f) { + braking_dist_z = math::trajectory::computeBrakingDistanceFromVelocity(_velocity(2), + _param_mpc_jerk_max.get(), _param_mpc_acc_up_max.get(), 0.f); + } + + const Vector3f braking_dir = _velocity.unit_or_zero(); + const Vector3f braking_dist = {braking_dist_xy, braking_dist_xy, braking_dist_z}; + _land_position = _position + braking_dir.emult(braking_dist); +} + + +void +FlightTaskLand::_HandleHighVelocities() +{ + // This logic here is to fix the problem that the trajectory generator will generate a smoot trajectory from the current velocity to zero velocity, + // but if the velocity is too high, the Position Controller will be able to comand higher accelerations than set by the parameter which means the vehicle will break faster than expected predicted by the trajectory generator. + // But if we then do a reset the deceleration will be smooth again. + const bool _exceeded_vel_z = fabsf(_velocity(2)) > math::max(_param_mpc_z_v_auto_dn.get(), + _param_mpc_z_vel_max_up.get()); + const bool _exceeded_vel_xy = _velocity.xy().norm() > _param_mpc_xy_vel_max.get(); + + if ((_exceeded_vel_xy || _exceeded_vel_z) && !_exceeded_max_vel) { + _exceeded_max_vel = true; + + } else if ((!_exceeded_vel_xy && !_exceeded_vel_z) && _exceeded_max_vel) { + // This Reset will be called when the velocity is again in the normal range and will be called only once. + _exceeded_max_vel = false; + _position_smoothing.reset(_acceleration_setpoint, _velocity, _position); + _CalculateBrakingLocation(); + _initial_land_position = _land_position; + } +} + +void +FlightTaskLand::_UpdateTrajConstraints() +{ + // update params of the position smoothing + _position_smoothing.setCruiseSpeed(_param_mpc_xy_vel_max.get()); + _position_smoothing.setHorizontalTrajectoryGain(_param_mpc_xy_traj_p.get()); + _position_smoothing.setMaxAllowedHorizontalError(_param_mpc_xy_err_max.get()); + _position_smoothing.setTargetAcceptanceRadius(_param_nav_mc_alt_rad.get()); + _position_smoothing.setVerticalAcceptanceRadius(_param_nav_mc_alt_rad.get()); + + // Update the constraints of the trajectories + _position_smoothing.setMaxAccelerationXY(_param_mpc_acc_hor.get()); + _position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get()); + _position_smoothing.setMaxJerkXY(_param_mpc_jerk_auto.get()); + _position_smoothing.setMaxJerkZ(_param_mpc_jerk_max.get()); + + // set the constraints for the vertical direction + // if moving up, acceleration constraint is always in deceleration direction, eg opposite to the velocity + if (_velocity(2) < 0.0f && !_landing) { + _position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get()); + _position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_up.get()); + + } else if (!_landing) { + _position_smoothing.setMaxAccelerationZ(_param_mpc_acc_up_max.get()); + _position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get()); + + } else { + _position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get()); + _position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get()); + } +} + +void +FlightTaskLand::updateParams() +{ + FlightTask::updateParams(); + + // make sure that alt1 is above alt2 + _param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get())); +} diff --git a/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp b/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp new file mode 100644 index 0000000000..0e930f9a07 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file FlightTaskLand.hpp + * + * Flight task for landing + * + * @author Claudio Chies + */ +#pragma once + +#include "FlightTask.hpp" +#include +#include +#include +#include +#include +#include "Sticks.hpp" +#include "StickAccelerationXY.hpp" +#include "StickYaw.hpp" + +using matrix::Vector3f; +using matrix::Vector2f; + +class FlightTaskLand : public FlightTask +{ +public: + FlightTaskLand() = default; + virtual ~FlightTaskLand() = default; + + bool update() override; + bool activate(const trajectory_setpoint_s &last_setpoint) override; + void reActivate() override; + +protected: + void updateParams() override; + + PositionSmoothing _position_smoothing; + Sticks _sticks{this}; + StickAccelerationXY _stick_acceleration_xy{this}; + StickYaw _stick_yaw{this}; + + uORB::SubscriptionData _sub_home_position{ORB_ID(home_position)}; + uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; + uORB::SubscriptionData _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, + (ParamFloat) _param_mpc_acc_down_max, + (ParamFloat) _param_mpc_acc_hor, + (ParamFloat) _param_mpc_acc_up_max, + (ParamFloat) _param_mpc_jerk_auto, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_land_alt1, + (ParamFloat) _param_mpc_land_alt2, + (ParamFloat) _param_mpc_land_alt3, + (ParamFloat) _param_mpc_land_crawl_speed, + (ParamFloat) _param_mpc_land_radius, + (ParamInt) _param_mpc_land_rc_help, + (ParamFloat) _param_mpc_land_speed, + (ParamFloat) _param_mpc_xy_err_max, + (ParamFloat) _param_mpc_xy_traj_p, + (ParamFloat) _param_mpc_xy_vel_max, + (ParamFloat) _param_mpc_z_v_auto_dn, + (ParamFloat) _param_mpc_z_v_auto_up, + (ParamFloat) _param_nav_mc_alt_rad + ); +private: + void _CalculateBrakingLocation(); + void _HandleHighVelocities(); + void _PerformLanding(); + void _SmoothBrakingPath(); + void _UpdateTrajConstraints(); + + bool _landing{false}; + bool _is_initialized{false}; + bool _exceeded_max_vel{false}; + + Vector3f _initial_land_position; + Vector3f _land_position; + float _land_heading{0.0f}; + + +}; diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 2c351ffa6e..4198e916ae 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -54,16 +54,10 @@ Land::on_activation() _navigator->get_mission_result()->finished = false; _navigator->set_mission_result_updated(); reset_mission_item_reached(); - /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && _navigator->get_local_position()->xy_global) { // only execute if global position is valid - _navigator->preproject_stop_point(_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;