diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index d2a3d53609..edec9722ac 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -53,7 +53,7 @@ bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setp bool ret = FlightTask::activate(last_setpoint); _position_setpoint = _position; _velocity_setpoint = _velocity; - _yaw_setpoint = _yaw_sp_prev = _yaw; + _yaw_setpoint = _yaw; _yawspeed_setpoint = 0.0f; _setDefaultConstraints(); return ret; @@ -107,7 +107,6 @@ void FlightTaskAuto::_limitYawRate() _yaw_sp_aligned = fabsf(matrix::wrap_pi(_yaw_setpoint - yaw_setpoint_sat)) < math::radians(_param_mis_yaw_err.get()); _yaw_setpoint = yaw_setpoint_sat; - _yaw_sp_prev = _yaw_setpoint; if (!PX4_ISFINITE(_yawspeed_setpoint) && (_deltatime > FLT_EPSILON)) { // Create a feedforward @@ -115,6 +114,8 @@ void FlightTaskAuto::_limitYawRate() } } + _yaw_sp_prev = _yaw_setpoint; + if (PX4_ISFINITE(_yawspeed_setpoint)) { // The yaw setpoint is aligned when its rate is not saturated _yaw_sp_aligned = _yaw_sp_aligned && (fabsf(_yawspeed_setpoint) < yawrate_max); diff --git a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp index 0699af4d37..e67472314b 100644 --- a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -41,7 +41,8 @@ using namespace matrix; FlightTaskAutoMapper::FlightTaskAutoMapper() : - _sticks(this) + _sticks(this), + _stick_acceleration_xy(this) {} bool FlightTaskAutoMapper::activate(const vehicle_local_position_setpoint_s &last_setpoint) @@ -132,10 +133,46 @@ void FlightTaskAutoMapper::_prepareIdleSetpoints() void FlightTaskAutoMapper::_prepareLandSetpoints() { - // Keep xy-position and go down with landspeed - float land_speed = _getLandSpeed(); - _position_setpoint = Vector3f(_target(0), _target(1), NAN); - _velocity_setpoint = Vector3f(Vector3f(NAN, NAN, land_speed)); + _velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint + + // Slow down automatic descend close to ground + float land_speed = math::gradual(_dist_to_ground, + _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), + _param_mpc_land_speed.get(), _constraints.speed_down); + + if (_type_previous != WaypointType::land) { + // initialize xy-position and yaw to waypoint such that home is reached exactly without user input + _land_position = Vector3f(_target(0), _target(1), NAN); + _land_heading = _yaw_setpoint; + _stick_acceleration_xy.resetPosition(Vector2f(_target(0), _target(1))); + } + + // User input assisted landing + if (_param_mpc_land_rc_help.get() && _sticks.checkAndSetStickInputs()) { + // Stick full up -1 -> stop, stick full down 1 -> double the speed + land_speed *= (1 + _sticks.getPositionExpo()(2)); + + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, + _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime); + _stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position, + _velocity_setpoint_feedback.xy(), _deltatime); + _stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint); + + // Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input + if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) { + _yaw_sp_aligned = true; + } + + } 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; // The last element of the land position has to stay NAN + _yaw_setpoint = _land_heading; + _velocity_setpoint(2) = land_speed; _gear.landing_gear = landing_gear_s::GEAR_DOWN; } @@ -177,20 +214,3 @@ bool FlightTaskAutoMapper::_highEnoughForLandingGear() // return true if altitude is above two meters return _dist_to_ground > 2.0f; } - -float FlightTaskAutoMapper::_getLandSpeed() -{ - float land_speed = math::gradual(_dist_to_ground, - _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), - _param_mpc_land_speed.get(), _constraints.speed_down); - - // user input assisted land speed - if (_param_mpc_land_rc_help.get() - && (_dist_to_ground < _param_mpc_land_alt1.get()) - && _sticks.checkAndSetStickInputs()) { - // stick full up -1 -> stop, stick full down 1 -> double the speed - land_speed *= (1 + _sticks.getPositionExpo()(2)); - } - - return land_speed; -} diff --git a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp index 678d45f6b1..e206274221 100644 --- a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp @@ -42,6 +42,8 @@ #include "FlightTaskAuto.hpp" #include "Sticks.hpp" +#include "StickAccelerationXY.hpp" +#include "StickYaw.hpp" class FlightTaskAutoMapper : public FlightTaskAuto { @@ -72,13 +74,17 @@ protected: _param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed (ParamFloat) _param_mpc_tko_speed, (ParamFloat) - _param_mpc_tko_ramp_t // time constant for smooth takeoff ramp + _param_mpc_tko_ramp_t, // time constant for smooth takeoff ramp + (ParamFloat) _param_mpc_man_y_max ); private: Sticks _sticks; + StickAccelerationXY _stick_acceleration_xy; + StickYaw _stick_yaw; + matrix::Vector3f _land_position; + float _land_heading; void _reset(); /**< Resets member variables to current vehicle state */ WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */ bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */ - float _getLandSpeed(); /**< Returns landing descent speed. */ };