diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index 6816b43053..63fa844dcd 100644 --- a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -36,6 +36,7 @@ */ #include "FlightTaskAutoLineSmoothVel.hpp" +#include using namespace matrix; @@ -313,3 +314,95 @@ void FlightTaskAutoLineSmoothVel::_updateTrajConstraints() _position_smoothing.setMaxVelocityZ(_param_mpc_z_vel_max_dn.get()); } } + +void FlightTaskAutoLineSmoothVel::_prepareIdleSetpoints() +{ + // Send zero thrust setpoint + _position_setpoint.setNaN(); // Don't require any position/velocity setpoints + _velocity_setpoint.setNaN(); + _acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust +} + +void FlightTaskAutoLineSmoothVel::_prepareLandSetpoints() +{ + _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; +} + +void FlightTaskAutoLineSmoothVel::_prepareTakeoffSetpoints() +{ + // Takeoff is completely defined by target position + _position_setpoint = _target; + _velocity_setpoint = Vector3f(NAN, NAN, NAN); + + _gear.landing_gear = landing_gear_s::GEAR_DOWN; +} + +void FlightTaskAutoLineSmoothVel::_prepareVelocitySetpoints() +{ + // XY Velocity waypoint + // TODO : Rewiew that. What is the expected behavior? + _position_setpoint = Vector3f(NAN, NAN, _position(2)); + Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed; + _velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN); +} + +void FlightTaskAutoLineSmoothVel::_preparePositionSetpoints() +{ + // Simple waypoint navigation: go to xyz target, with standard limitations + _position_setpoint = _target; + _velocity_setpoint.setNaN(); // No special velocity limitations +} + +void FlightTaskAutoLineSmoothVel::updateParams() +{ + FlightTaskAuto::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())); +} + +bool FlightTaskAutoLineSmoothVel::_highEnoughForLandingGear() +{ + // return true if altitude is above two meters + return _dist_to_ground > 2.0f; +} diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index a9de21d9dc..27305a6efd 100644 --- a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -83,6 +83,15 @@ protected: bool _checkTakeoff() override { return _want_takeoff; }; bool _want_takeoff{false}; + void _prepareIdleSetpoints(); + void _prepareLandSetpoints(); + void _prepareVelocitySetpoints(); + void _prepareTakeoffSetpoints(); + void _preparePositionSetpoints(); + bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */ + + void updateParams() override; /**< See ModuleParam class */ + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper, (ParamFloat) _param_mis_yaw_err, // yaw-error threshold (ParamFloat) _param_mpc_acc_hor, // acceleration in flight diff --git a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp index 4de1d1f69b..926380ecfd 100644 --- a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -36,103 +36,8 @@ */ #include "FlightTaskAutoMapper.hpp" -#include - -using namespace matrix; FlightTaskAutoMapper::FlightTaskAutoMapper() : _sticks(this), _stick_acceleration_xy(this) {} - -void FlightTaskAutoMapper::_prepareIdleSetpoints() -{ - // Send zero thrust setpoint - _position_setpoint.setNaN(); // Don't require any position/velocity setpoints - _velocity_setpoint.setNaN(); - _acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust -} - -void FlightTaskAutoMapper::_prepareLandSetpoints() -{ - _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; -} - -void FlightTaskAutoMapper::_prepareTakeoffSetpoints() -{ - // Takeoff is completely defined by target position - _position_setpoint = _target; - _velocity_setpoint = Vector3f(NAN, NAN, NAN); - - _gear.landing_gear = landing_gear_s::GEAR_DOWN; -} - -void FlightTaskAutoMapper::_prepareVelocitySetpoints() -{ - // XY Velocity waypoint - // TODO : Rewiew that. What is the expected behavior? - _position_setpoint = Vector3f(NAN, NAN, _position(2)); - Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed; - _velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN); -} - -void FlightTaskAutoMapper::_preparePositionSetpoints() -{ - // Simple waypoint navigation: go to xyz target, with standard limitations - _position_setpoint = _target; - _velocity_setpoint.setNaN(); // No special velocity limitations -} - -void FlightTaskAutoMapper::updateParams() -{ - FlightTaskAuto::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())); -} - -bool FlightTaskAutoMapper::_highEnoughForLandingGear() -{ - // return true if altitude is above two meters - return _dist_to_ground > 2.0f; -} diff --git a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp index e157feb19d..f0f599fdad 100644 --- a/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp @@ -52,13 +52,6 @@ public: virtual ~FlightTaskAutoMapper() = default; protected: - void _prepareIdleSetpoints(); - void _prepareLandSetpoints(); - void _prepareVelocitySetpoints(); - void _prepareTakeoffSetpoints(); - void _preparePositionSetpoints(); - - void updateParams() override; /**< See ModuleParam class */ DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto, (ParamFloat) _param_mpc_land_speed, @@ -80,5 +73,5 @@ protected: matrix::Vector3f _land_position; float _land_heading; WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */ - bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */ + };