diff --git a/src/modules/flight_mode_manager/tasks/Descend/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Descend/CMakeLists.txt index 5ef5777a71..416852123c 100644 --- a/src/modules/flight_mode_manager/tasks/Descend/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Descend/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-2023 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 @@ -35,5 +35,5 @@ px4_add_library(FlightTaskDescend FlightTaskDescend.cpp ) -target_link_libraries(FlightTaskDescend PUBLIC FlightTask) +target_link_libraries(FlightTaskDescend PUBLIC FlightTask FlightTaskUtility) target_include_directories(FlightTaskDescend PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp index e5c6b15a05..d07570e6ad 100644 --- a/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp +++ b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 @@ -36,16 +36,6 @@ #include "FlightTaskDescend.hpp" -bool FlightTaskDescend::activate(const trajectory_setpoint_s &last_setpoint) -{ - bool ret = FlightTask::activate(last_setpoint); - // stay level to minimize horizontal drift - _acceleration_setpoint = matrix::Vector3f(0.f, 0.f, NAN); - // keep heading - _yaw_setpoint = _yaw; - return ret; -} - bool FlightTaskDescend::update() { bool ret = FlightTask::update(); @@ -58,7 +48,28 @@ bool FlightTaskDescend::update() } else { // descend with constant acceleration (crash landing) _velocity_setpoint(2) = NAN; - _acceleration_setpoint(2) = .3f; + _acceleration_setpoint(2) = .15f; + } + + // Nudging + if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) { + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, + _is_yaw_good_for_control, _deltatime); + _acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw, + _yaw_setpoint); + + // Stick full up -1 -> stop, stick full down 1 -> double the value + _velocity_setpoint(2) *= (1 - _sticks.getThrottleZeroCenteredExpo()); + _acceleration_setpoint(2) -= _sticks.getThrottleZeroCentered() * 10.f; + + } else { + _acceleration_setpoint = matrix::Vector3f(0.f, 0.f, NAN); // stay level to minimize horizontal drift + _yawspeed_setpoint = NAN; + + // keep heading + if (!PX4_ISFINITE(_yaw_setpoint)) { + _yaw_setpoint = _yaw; + } } return ret; diff --git a/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp index 5b3433b330..f07fb9601d 100644 --- a/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp +++ b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2023 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 @@ -38,6 +38,9 @@ #pragma once #include "FlightTask.hpp" +#include "Sticks.hpp" +#include "StickTiltXY.hpp" +#include "StickYaw.hpp" class FlightTaskDescend : public FlightTask { @@ -46,11 +49,15 @@ public: virtual ~FlightTaskDescend() = default; bool update() override; - bool activate(const trajectory_setpoint_s &last_setpoint) override; private: + Sticks _sticks{this}; + StickTiltXY _stick_tilt_xy{this}; + StickYaw _stick_yaw{this}; + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, - (ParamFloat) _param_mpc_thr_hover, ///< thrust at hover equilibrium - (ParamFloat) _param_mpc_land_speed ///< velocity for controlled descend + (ParamInt) _param_mpc_land_rc_help, + (ParamFloat) _param_mpc_land_speed, ///< velocity for controlled descend + (ParamFloat) _param_mpc_thr_hover ///< thrust at hover equilibrium ) };