diff --git a/src/modules/flight_mode_manager/tasks/Transition/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Transition/CMakeLists.txt index 372ebf566c..1e622230a4 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Transition/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(FlightTaskTransition FlightTaskTransition.cpp ) -target_link_libraries(FlightTaskTransition PUBLIC FlightTask) -target_include_directories(FlightTaskTransition PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) \ No newline at end of file +target_link_libraries(FlightTaskTransition PUBLIC FlightTask FlightTaskUtility) +target_include_directories(FlightTaskTransition PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp index e2737a8e09..784c3c5788 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp @@ -36,26 +36,77 @@ */ #include "FlightTaskTransition.hpp" +#include "Sticks.hpp" + +FlightTaskTransition::FlightTaskTransition() +{ + _param_handle_pitch_cruise_degrees = param_find("FW_PSP_OFF"); + + if (_param_handle_pitch_cruise_degrees != PARAM_INVALID) { + param_get(_param_handle_pitch_cruise_degrees, &_param_pitch_cruise_degrees); + } + +} bool FlightTaskTransition::updateInitialize() { + + updateParameters(); return FlightTask::updateInitialize(); } +void FlightTaskTransition::updateParameters() +{ +// check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + if (_param_handle_pitch_cruise_degrees != PARAM_INVALID) { + param_get(_param_handle_pitch_cruise_degrees, &_param_pitch_cruise_degrees); + } + } +} + bool FlightTaskTransition::activate(const vehicle_local_position_setpoint_s &last_setpoint) { - return FlightTask::activate(last_setpoint); + bool ret = FlightTask::activate(last_setpoint); + + _vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const); + + if (PX4_ISFINITE(last_setpoint.vz)) { + _vel_z_filter.reset(last_setpoint.vz); + + } else { + _vel_z_filter.reset(_velocity(2)); + } + + _velocity_setpoint(2) = _vel_z_filter.getState(); + + return ret; + + } bool FlightTaskTransition::update() { - bool ret = FlightTask::update(); - _acceleration_setpoint.xy() = matrix::Vector2f(0.f, 0.f); - // demand zero vertical velocity and level attitude // tailsitters will override attitude and thrust setpoint // tiltrotors and standard vtol will overrride roll and pitch setpoint but keep vertical thrust setpoint + bool ret = FlightTask::update(); + _position_setpoint.setAll(NAN); - _velocity_setpoint(2) = 0.0f; + + // calculate a horizontal acceleration vector which corresponds to an attitude composed of pitch up by _param_pitch_cruise_degrees + // and zero roll angle + matrix::Vector2f tmp(-1.0f, 0.0f); + Sticks::rotateIntoHeadingFrameXY(tmp, _yaw, NAN); + _acceleration_setpoint.xy() = tmp * tanf(math::radians(_param_pitch_cruise_degrees)) * CONSTANTS_ONE_G; + + // slowly move vertical velocity setpoint to zero + _vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const); + _velocity_setpoint(2) = _vel_z_filter.update(0.0f); _yaw_setpoint = NAN; return ret; diff --git a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp index ec71772a95..df9aafde0d 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp @@ -40,14 +40,34 @@ #pragma once #include "FlightTask.hpp" +#include +#include +#include +#include + +using namespace time_literals; + class FlightTaskTransition : public FlightTask { public: - FlightTaskTransition() = default; + FlightTaskTransition(); virtual ~FlightTaskTransition() = default; bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; bool updateInitialize() override; bool update() override; + +private: + + static constexpr float _vel_z_filter_time_const = 2.0f; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + param_t _param_handle_pitch_cruise_degrees{PARAM_INVALID}; + float _param_pitch_cruise_degrees{0.f}; + + AlphaFilter _vel_z_filter; + + void updateParameters(); + };