diff --git a/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp b/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp index 2421bbc331..457469ad66 100644 --- a/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp +++ b/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp @@ -67,11 +67,9 @@ FlightTaskLand::activate(const trajectory_setpoint_s &last_setpoint) _position_setpoint = pos_prev; + // Initialize the Landing locations and parameters - // calculate where to land based on the current velocity and acceleration constraints - // set this as the target location for position smoothing - _CalculateBrakingLocation(); _initial_land_position = _land_position; @@ -82,7 +80,6 @@ void FlightTaskLand::reActivate() { FlightTask::reActivate(); - PX4_ERR("FlightTaskLand reActivate was called!"); // On ground, reset acceleration and velocity to zero _position_smoothing.reset({0.f, 0.f, 0.f}, {0.f, 0.f, 0.7f}, _position); } @@ -93,27 +90,18 @@ FlightTaskLand::update() bool ret = FlightTask::update(); if (!_is_initialized) { - PX4_ERR("initializing _position_smoothing"); - PX4_ERR("_velocity_setpoint: %f, %f, %f", (double)_velocity_setpoint(0), (double)_velocity_setpoint(1), - (double)_velocity_setpoint(2)); - PX4_ERR("_position_setpoint: %f, %f, %f", (double)_position_setpoint(0), (double)_position_setpoint(1), - (double)_position_setpoint(2)); - PX4_ERR("_acceleration_setpoint: %f, %f, %f", (double)_acceleration_setpoint(0), (double)_acceleration_setpoint(1), - (double)_acceleration_setpoint(2)); _position_smoothing.reset(_acceleration_setpoint, _velocity, _position); _is_initialized = true; } - + if (_velocity.norm() < 1.f && !_landing) { + _landing = true; + } if (_landing) { _PerformLanding(); - PX4_WARN("_position: %f, %f, %f", (double)_position(0), (double)_position(1), (double)_position(2)); - PX4_WARN("_land_position: %f, %f, %f", (double)_land_position(0), (double)_land_position(1), - (double)_land_position(2)); } else { - //_CalculateBrakingLocation(); _SmoothBrakingPath(); } @@ -138,6 +126,54 @@ FlightTaskLand::_PerformLanding() 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; @@ -154,37 +190,24 @@ FlightTaskLand::_SmoothBrakingPath() _position_smoothing.generateSetpoints( _position, _land_position, - {0.f, 0.f, 0.f}, - _deltatime, - false, - out_setpoints + 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; - //_velocity_setpoint(2) = 0; _position_setpoint = out_setpoints.position; _yaw_setpoint = _land_heading; - PX4_WARN("_position_setpoint: %f, %f, %f", (double)_position_setpoint(0), (double)_position_setpoint(1), - (double)_position_setpoint(2)); - PX4_WARN("_land_position: %f, %f, %f", (double)_land_position(0), (double)_land_position(1), - (double)_land_position(2)); - - // should be 3d vel? - if (_velocity.xy().norm() < - _param_nav_mc_alt_rad.get()) { //} || _velocity(2) < _param_mpc_z_v_auto_dn.get()){ // not sure about the last parts, check!) - _landing = true; - PX4_WARN("Landing: Vehicle is not moving, perform landing"); - } - } 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(); + _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, @@ -192,12 +215,10 @@ FlightTaskLand::_CalculateBrakingLocation() float braking_dist_z = 0.0f; if (_velocity(2) < -0.1f) { - PX4_WARN("Moving upwards: %f", (double)_velocity(2)); braking_dist_z = math::trajectory::computeBrakingDistanceFromVelocity(_velocity(2), _param_mpc_jerk_auto.get(), 9.81f + _param_mpc_acc_down_max.get(), delay_scale * _param_mpc_jerk_auto.get()); } else if (_velocity(2) > 0.1f) { - PX4_WARN("Moving downwards"); braking_dist_z = math::trajectory::computeBrakingDistanceFromVelocity(_velocity(2), _param_mpc_jerk_auto.get(), _param_mpc_acc_up_max.get(), delay_scale * _param_mpc_jerk_auto.get()); } @@ -226,11 +247,12 @@ FlightTaskLand::_HandleHighVelocities() _exceeded_max_vel = false; _position_smoothing.reset(_acceleration_setpoint, _velocity, _position); _CalculateBrakingLocation(); + _initial_land_position = _land_position; } } void -FlightTaskLand::_updateTrajConstraints() +FlightTaskLand::_UpdateTrajConstraints() { // update params of the position smoothing _position_smoothing.setCruiseSpeed(_param_mpc_xy_vel_max.get()); @@ -247,17 +269,14 @@ FlightTaskLand::_updateTrajConstraints() // 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) { - PX4_ERR("Moving upwards: %f", (double)_velocity(2)); _position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get()); _position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_up.get()); } else if (!_landing) { - PX4_ERR("Moving downwards %f", (double)_velocity(2)); _position_smoothing.setMaxAccelerationZ(_param_mpc_acc_up_max.get()); _position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get()); } else { - PX4_ERR("Not moving Vertially"); _position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get()); _position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get()); } diff --git a/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp b/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp index f983b6940a..f174378278 100644 --- a/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp +++ b/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp @@ -47,6 +47,8 @@ #include #include #include "Sticks.hpp" +#include "StickAccelerationXY.hpp" +#include "StickYaw.hpp" using matrix::Vector3f; using matrix::Vector2f; @@ -59,12 +61,13 @@ public: bool update() override; bool activate(const trajectory_setpoint_s &last_setpoint) override; - void reActivate() override; protected: 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)}; @@ -79,6 +82,8 @@ protected: (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, @@ -92,11 +97,12 @@ private: void _HandleHighVelocities(); void _PerformLanding(); void _SmoothBrakingPath(); - void _updateTrajConstraints(); + 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};