Fixed calculation of descent speed. Replaced time check with vehicle_…

…status.rc_signal_lost. Added a parameter for this feature that defaults to disabled.
PX4 · Jun 16, 2019 · ebd7ddf · ebd7ddf
1 parent fdf53b6
commit ebd7ddf
Show file tree

Hide file tree

Showing 4 changed files with 19 additions and 4 deletions.
diff --git 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<home_position_s> *_sub_home_position{nullptr};
 	uORB::SubscriptionPollable<manual_control_setpoint_s> *_sub_manual_control_setpoint{nullptr};
+	uORB::SubscriptionPollable<vehicle_status_s> *_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<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
-	uORB::SubscriptionPollable<vehicle_status_s> *_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
@@ -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
@@ -57,6 +57,7 @@ class FlightTaskAutoMapper2 : public FlightTaskAuto
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
 					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
 					(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,
+					(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
 					(ParamFloat<px4::params::MPC_LAND_ALT1>)
 					_param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
 					(ParamFloat<px4::params::MPC_LAND_ALT2>)

diff --git 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
  *