From ebd7ddf256cbf56b96240e41ddc8ce0b61730162 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 11 Jun 2019 22:59:43 -0600 Subject: [PATCH] Fixed calculation of descent speed. Replaced time check with vehicle_status.rc_signal_lost. Added a parameter for this feature that defaults to disabled. --- src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp | 2 +- .../tasks/AutoMapper2/FlightTaskAutoMapper2.cpp | 8 +++++--- .../tasks/AutoMapper2/FlightTaskAutoMapper2.hpp | 1 + src/modules/mc_pos_control/mc_pos_control_params.c | 12 ++++++++++++ 4 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index 5a41918ef70c..4a4231097852 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -101,6 +101,7 @@ class FlightTaskAuto : public FlightTask 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 */ @@ -124,7 +125,6 @@ class FlightTaskAuto : public FlightTask 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 969255fda978..68cf1dea7e26 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp +++ b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp @@ -198,10 +198,12 @@ bool FlightTaskAutoMapper2::_highEnoughForLandingGear() 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; - uint64_t timestamp_us = _sub_manual_control_setpoint->get().timestamp; - if (hrt_absolute_time() - timestamp_us < 500000) { // 500ms + if (rc_is_valid && rc_assist_enabled) { throttle = _sub_manual_control_setpoint->get().z; } @@ -214,7 +216,7 @@ float FlightTaskAutoMapper2::_getLandSpeed() float land_speed = _param_mpc_land_speed.get(); float head_room = _constraints.speed_down - land_speed; - speed = land_speed + (0.5f - throttle) * head_room; + 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) { diff --git a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp index 03f17bafbe44..5b4e0603d9e1 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp +++ b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp @@ -57,6 +57,7 @@ class FlightTaskAutoMapper2 : public FlightTaskAuto 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) 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 5769dfeee4c3..d6149487e1ad 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 m/s 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 *