diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index fb71637378..47ccead8ee 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -66,6 +66,10 @@ bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_arr return false; } + if (!subscription_array.get(ORB_ID(manual_control_setpoint), _sub_manual_control_setpoint)) { + return false; + } + if (!_obstacle_avoidance.initializeSubscriptions(subscription_array)) { return false; } diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index d9cc18ce79..4a42310978 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -99,6 +100,8 @@ protected: float _mc_cruise_speed{0.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */ WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */ uORB::SubscriptionPollable *_sub_home_position{nullptr}; + uORB::SubscriptionPollable *_sub_manual_control_setpoint{nullptr}; + uORB::SubscriptionPollable *_sub_vehicle_status{nullptr}; State _current_state{State::none}; float _target_acceptance_radius = 0.0f; /**< Acceptances radius of the target */ @@ -122,7 +125,6 @@ private: matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */ bool _yaw_lock = false; /**< if within acceptance radius, lock yaw to current yaw */ uORB::SubscriptionPollable *_sub_triplet_setpoint{nullptr}; - uORB::SubscriptionPollable *_sub_vehicle_status{nullptr}; matrix::Vector3f _triplet_target; /**< current triplet from navigator which may differ from the intenal one (_target) depending on the vehicle state. */ diff --git a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp index ef64afb22a..ab48ed5c06 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp +++ b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp @@ -129,11 +129,11 @@ void FlightTaskAutoMapper2::_prepareIdleSetpoints() void FlightTaskAutoMapper2::_prepareLandSetpoints() { + float land_speed = _getLandSpeed(); + // Keep xy-position and go down with landspeed _position_setpoint = Vector3f(_target(0), _target(1), NAN); - const float speed_lnd = (_alt_above_ground > _param_mpc_land_alt1.get()) ? _constraints.speed_down : - _param_mpc_land_speed.get(); - _velocity_setpoint = Vector3f(Vector3f(NAN, NAN, speed_lnd)); + _velocity_setpoint = Vector3f(Vector3f(NAN, NAN, land_speed)); // set constraints _constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get()); @@ -195,3 +195,34 @@ bool FlightTaskAutoMapper2::_highEnoughForLandingGear() // return true if altitude is above two meters return _alt_above_ground > 2.0f; } + +float FlightTaskAutoMapper2::_getLandSpeed() +{ + bool rc_assist_enabled = _param_mpc_land_rc_help.get(); + bool rc_is_valid = !_sub_vehicle_status->get().rc_signal_lost; + + float throttle = 0.5f; + + if (rc_is_valid && rc_assist_enabled) { + throttle = _sub_manual_control_setpoint->get().z; + } + + float speed = 0; + + if (_alt_above_ground > _param_mpc_land_alt1.get()) { + speed = _constraints.speed_down; + + } else { + float land_speed = _param_mpc_land_speed.get(); + float head_room = _constraints.speed_down - land_speed; + + speed = land_speed + 2 * (0.5f - throttle) * head_room; + + // Allow minimum assisted land speed to be half of parameter + if (speed < land_speed * 0.5f) { + speed = land_speed * 0.5f; + } + } + + return speed; +} diff --git a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp index 226a54427f..5b4e0603d9 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp +++ b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp @@ -57,6 +57,7 @@ protected: DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto, (ParamFloat) _param_mpc_land_speed, (ParamFloat) _param_mpc_tiltmax_lnd, + (ParamInt) _param_mpc_land_rc_help, (ParamFloat) _param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed (ParamFloat) @@ -80,4 +81,5 @@ private: 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. */ }; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 5769dfeee4..b3d3f3ff45 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -350,6 +350,18 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f); */ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f); +/** + * Enable user assisted descent speed for autonomous land routine. + * When enabled, descent speed will be equal to MPC_LAND_SPEED at half throttle, + * MPC_Z_VEL_MAX_DN at zero throttle, and 0.5 * MPC_LAND_SPEED at full throttle. + * + * @min 0 + * @max 1 + * @value 0 Fixed descent speed of MPC_LAND_SPEED + * @value 1 User assisted descent speed + */ +PARAM_DEFINE_INT32(MPC_LAND_RC_HELP, 0); + /** * Takeoff climb rate *