Move throttle check from land detector to posctrl

This commit is an attempt to fix a race condition happening on takeoff
between the land detector and the multicopter position controller.

Previously, an auto-takeoff leads to the following events:

1. A takeoff setpoint is given.
2. The thrust setpoint spikes because we don't enter smooth takeoff yet.
3. The land detector detects a takeoff because of the high thrust.
4. The position controller sees the landed state transition and
   initiates the smooth takeoff. Thrust goes back down.
5. Depending on control gains the takeoff is successful or fails
   if the smoothing takes too long which causes thrust to be too low, so
   the land detector detects land again.

The two obvious problems with this are:
- The intermittent spike.
- The failed takeoff because of the smoothing leads to a delay..

With this change, the logic for a takeoff detection is moved from the
land detector to the position controller.

The events are now:

1. A takeoff setpoint is given.
2. The position controller detects the takeoff setpoint and initiates
   the smooth takeoff.
3. As thrust ramps up, the land detector detects the take off.

In the same way, we now detect the intent to takeoff in manual,
altitude, control, position control in the position controller instead
of in the land detector.

ROMFS/px4fmu_common/init.d/4070_aerofc

@@ -25,8 +25,6 @@ then
 
 	param set COM_DISARM_LAND 3
 
-	param set LNDMC_MAN_DWNTHR 0.2500
-	param set LNDMC_POS_UPTHR 0.5500
 	param set LNDMC_Z_VEL_MAX 2.0000
 
 	param set MC_ROLL_P 8.0000

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -79,7 +79,6 @@ MulticopterLandDetector::MulticopterLandDetector() :
 	_actuatorsSub(-1),
 	_armingSub(-1),
 	_attitudeSub(-1),
-	_manualSub(-1),
 	_sensor_bias_sub(-1),
 	_vehicle_control_mode_sub(-1),
 	_battery_sub(-1),
@@ -88,7 +87,6 @@ MulticopterLandDetector::MulticopterLandDetector() :
 	_actuators{},
 	_arming{},
 	_vehicleAttitude{},
-	_manual{},
 	_sensors{},
 	_control_mode{},
 	_battery{},
@@ -104,9 +102,7 @@ MulticopterLandDetector::MulticopterLandDetector() :
 	_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
 	_paramHandle.freefall_acc_threshold = param_find("LNDMC_FFALL_THR");
 	_paramHandle.freefall_trigger_time = param_find("LNDMC_FFALL_TTRI");
-	_paramHandle.manual_stick_down_threshold = param_find("LNDMC_MAN_DWNTHR");
 	_paramHandle.altitude_max = param_find("LNDMC_ALT_MAX");
-	_paramHandle.manual_stick_up_position_takeoff_threshold = param_find("LNDMC_POS_UPTHR");
 	_paramHandle.landSpeed = param_find("MPC_LAND_SPEED");
 
 	// Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true).
@@ -124,7 +120,6 @@ void MulticopterLandDetector::_initialize_topics()
 	_actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0));
 	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
 	_parameterSub = orb_subscribe(ORB_ID(parameter_update));
-	_manualSub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
 	_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_battery_sub = orb_subscribe(ORB_ID(battery_status));
@@ -137,7 +132,6 @@ void MulticopterLandDetector::_update_topics()
 	_orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
 	_orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators);
 	_orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
-	_orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual);
 	_orb_update(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensors);
 	_orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode);
 	_orb_update(ORB_ID(battery_status), _battery_sub, &_battery);
@@ -156,9 +150,7 @@ void MulticopterLandDetector::_update_params()
 	param_get(_paramHandle.freefall_acc_threshold, &_params.freefall_acc_threshold);
 	param_get(_paramHandle.freefall_trigger_time, &_params.freefall_trigger_time);
 	_freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _params.freefall_trigger_time));
-	param_get(_paramHandle.manual_stick_down_threshold, &_params.manual_stick_down_threshold);
 	param_get(_paramHandle.altitude_max, &_params.altitude_max);
-	param_get(_paramHandle.manual_stick_up_position_takeoff_threshold, &_params.manual_stick_up_position_takeoff_threshold);
 	param_get(_paramHandle.landSpeed, &_params.landSpeed);
 }
 
@@ -190,13 +182,6 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 		return true;
 	}
 
-	// If in manual flight mode never report landed if the user has more than idle throttle
-	// Check if user commands throttle and if so, report no ground contact based on
-	// the user intent to take off (even if the system might physically still have
-	// ground contact at this point).
-	const bool manual_control_idle = (_has_manual_control_present() && _manual.z < _params.manual_stick_down_threshold);
-	const bool manual_control_idle_or_auto = manual_control_idle || !_control_mode.flag_control_manual_enabled;
-
 	// land speed threshold
 	float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f);
 
@@ -229,9 +214,8 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 			  && (_vehicleLocalPositionSetpoint.vz >= land_speed_threshold);
 	bool hit_ground = in_descend && !verticalMovement;
 
-	// If pilots commands down or in auto mode and we are already below minimal thrust and we do not move down we assume ground contact
 	// TODO: we need an accelerometer based check for vertical movement for flying without GPS
-	if (manual_control_idle_or_auto && (_has_low_thrust() || hit_ground) && (!horizontalMovement || !_has_position_lock())
+	if ((_has_low_thrust() || hit_ground) && (!horizontalMovement || !_has_position_lock())
 	    && (!verticalMovement || !_has_altitude_lock())) {
 		return true;
 	}
@@ -249,18 +233,6 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
 		return true;
 	}
 
-	// If we control manually and are still landed, we want to stay idle until the pilot rises the throttle for takeoff
-	if (_state == LandDetectionState::LANDED && _has_manual_control_present()) {
-		if (_manual.z < _get_takeoff_throttle()) {
-			return true;
-
-		} else {
-			// Pilot wants to take off, assume no groundcontact anymore and therefore allow thrust
-			_ground_contact_hysteresis.set_state_and_update(false);
-			return false;
-		}
-	}
-
 	if (_has_minimal_thrust()) {
 		if (_min_trust_start == 0) {
 			_min_trust_start = now;
@@ -321,26 +293,6 @@ bool MulticopterLandDetector::_get_landed_state()
 
 }
 
-float MulticopterLandDetector::_get_takeoff_throttle()
-{
-	/* Position mode */
-	if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_altitude_enabled) {
-		/* Should be above 0.5 because below that we do not gain altitude and won't take off.
-		 * Also it should be quite high such that we don't accidentally take off when using
-		 * a spring loaded throttle and have a useful vertical speed to start with. */
-		return _params.manual_stick_up_position_takeoff_threshold;
-	}
-
-	/* Manual/attitude mode */
-	if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) {
-		/* Should be quite low and certainly below hover throttle because pilot controls throttle manually. */
-		return 0.15f;
-	}
-
-	/* As default for example in acro mode we do not want to stay landed. */
-	return 0.0f;
-}
-
 float MulticopterLandDetector::_get_max_altitude()
 {
 	/* ToDo: add a meaningful altitude */
@@ -373,11 +325,6 @@ bool MulticopterLandDetector::_has_position_lock()
 	return _has_altitude_lock() && _vehicleLocalPosition.xy_valid;
 }
 
-bool MulticopterLandDetector::_has_manual_control_present()
-{
-	return _control_mode.flag_control_manual_enabled && _manual.timestamp > 0;
-}
-
 bool MulticopterLandDetector::_is_climb_rate_enabled()
 {
 	bool has_updated = (_vehicleLocalPositionSetpoint.timestamp != 0)

src/modules/land_detector/MulticopterLandDetector.h

@@ -52,7 +52,6 @@
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/battery_status.h>
-#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_bias.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -110,9 +109,7 @@ private:
 		param_t minManThrottle;
 		param_t freefall_acc_threshold;
 		param_t freefall_trigger_time;
-		param_t manual_stick_down_threshold;
 		param_t altitude_max;
-		param_t manual_stick_up_position_takeoff_threshold;
 		param_t landSpeed;
 	} _paramHandle;
 
@@ -126,9 +123,7 @@ private:
 		float minManThrottle;
 		float freefall_acc_threshold;
 		float freefall_trigger_time;
-		float manual_stick_down_threshold;
 		float altitude_max;
-		float manual_stick_up_position_takeoff_threshold;
 		float landSpeed;
 	} _params;
 
@@ -137,7 +132,6 @@ private:
 	int _actuatorsSub;
 	int _armingSub;
 	int _attitudeSub;
-	int _manualSub;
 	int _sensor_bias_sub;
 	int _vehicle_control_mode_sub;
 	int _battery_sub;
@@ -147,7 +141,6 @@ private:
 	struct actuator_controls_s					_actuators;
 	struct actuator_armed_s						_arming;
 	struct vehicle_attitude_s					_vehicleAttitude;
-	struct manual_control_setpoint_s			_manual;
 	struct sensor_bias_s					_sensors;
 	struct vehicle_control_mode_s				_control_mode;
 	struct battery_status_s						_battery;
@@ -159,7 +152,6 @@ private:
 	float _get_takeoff_throttle();
 	bool _has_altitude_lock();
 	bool _has_position_lock();
-	bool _has_manual_control_present();
 	bool _has_minimal_thrust();
 	bool _has_low_thrust();
 	bool _is_climb_rate_enabled();

src/modules/land_detector/land_detector_params.c

@@ -119,37 +119,6 @@ PARAM_DEFINE_FLOAT(LNDMC_THR_RANGE, 0.1f);
  */
 PARAM_DEFINE_FLOAT(LNDMC_FFALL_TTRI, 0.3);
 
-/**
- * Manual flight stick down threshold for landing
- *
- * When controlling manually the throttle stick value (0 to 1)
- * has to be bellow this threshold in order to pass the check for landing.
- * So if set to 1 it's allowed to land with any stick position.
- *
- * @min 0
- * @max 1
- * @unit norm
- * @decimal 2
- *
- * @group Land Detector
- */
-PARAM_DEFINE_FLOAT(LNDMC_MAN_DWNTHR, 0.15f);
-
-/**
- * Manual position flight stick up threshold for taking off
- *
- * When controlling manually in position mode the throttle stick value (0 to 1)
- * has to get above this threshold after arming in order to take off.
- *
- * @min 0
- * @max 1
- * @unit norm
- * @decimal 2
- *
- * @group Land Detector
- */
-PARAM_DEFINE_FLOAT(LNDMC_POS_UPTHR, 0.65f);
-
 /**
  * Fixedwing max horizontal velocity
  *

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -128,7 +128,7 @@ private:
 	bool		 _reset_yaw_sp = true; 				/**<true if reset yaw setpoint */
 	bool 		_hold_offboard_xy = false; 			/**<TODO : check if we need this extra hold_offboard flag */
 	bool 		_hold_offboard_z = false;
-	bool 		_in_takeoff = false; 				/**<true if takeoff ramp is applied */
+	bool 		_in_smooth_takeoff = false; 				/**<true if takeoff ramp is applied */
 	bool 		_in_landing = false;				/**<true if landing descent (only used in auto) */
 	bool 		_lnd_reached_ground = false; 		/**<true if controller assumes the vehicle has reached the ground after landing */
 	bool 		_triplet_lat_lon_finite = true; 		/**<true if triplets current is non-finite */
@@ -385,6 +385,8 @@ private:
 
 	void warn_rate_limited(const char *str);
 
+	bool manual_wants_takeoff();
+
 	/**
 	 * Shim for calling task_main from task_create.
 	 */
@@ -1462,15 +1464,7 @@ MulticopterPositionControl::control_manual(float dt)
 		_vel_sp(1) = man_vel_sp(1);
 	}
 
-	if (_vehicle_land_detected.landed) {
-		/* don't run controller when landed
-		 * NOTE:
-		 * This only works in manual since once we give throttle input, the
-		 * landdetector will exit the landing state */
-
-	} else {
-		control_position(dt);
-	}
+	control_position(dt);
 }
 
 void
@@ -2487,8 +2481,8 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
 	_vel_sp_prev = _vel_sp;
 
 	/* special velocity setpoint limitation for smooth takeoff (after slewrate!) */
-	if (_in_takeoff) {
-		_in_takeoff = _takeoff_vel_limit < -_vel_sp(2);
+	if (_in_smooth_takeoff) {
+		_in_smooth_takeoff = _takeoff_vel_limit < -_vel_sp(2);
 		/* ramp vertical velocity limit up to takeoff speed */
 		_takeoff_vel_limit += -_vel_sp(2) * dt / _takeoff_ramp_time.get();
 		/* limit vertical velocity to the current ramp value */
@@ -2557,7 +2551,7 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt)
 		thrust_sp(1) = 0.0f;
 	}
 
-	if (!in_auto_takeoff()) {
+	if (!in_auto_takeoff() && !manual_wants_takeoff()) {
 		if (_vehicle_land_detected.ground_contact) {
 			/* if still or already on ground command zero xy thrust_sp in body
 			 * frame to consider uneven ground */
@@ -2606,7 +2600,7 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt)
 	// We can only run the control if we're already in-air, have a takeoff setpoint,
 	// or if we're in offboard control.
 	// Otherwise, we should just bail out
-	if (_vehicle_land_detected.landed && !in_auto_takeoff()) {
+	if (_vehicle_land_detected.landed && !in_auto_takeoff() && !manual_wants_takeoff()) {
 		// Keep throttle low while still on ground.
 		thr_max = 0.0f;
 
@@ -2966,6 +2960,16 @@ MulticopterPositionControl::generate_attitude_setpoint(float dt)
 	_att_sp.timestamp = hrt_absolute_time();
 }
 
+bool MulticopterPositionControl::manual_wants_takeoff()
+{
+	const bool has_manual_control_present = _control_mode.flag_control_manual_enabled && _manual.timestamp > 0;
+
+	// If the throttle stick is well above 50%, so above 62.5% (50% + 0.15 * 50%) we trigger manual takeoff.
+	const bool manual_wants_takeoff = (has_manual_control_present && _manual.z > 0.15f);
+
+	return manual_wants_takeoff;
+}
+
 void
 MulticopterPositionControl::task_main()
 {
@@ -3048,7 +3052,6 @@ MulticopterPositionControl::task_main()
 			_reset_yaw_sp = true;
 			_hold_offboard_xy = false;
 			_hold_offboard_z = false;
-			_in_takeoff  = false;
 			_in_landing = false;
 			_lnd_reached_ground = false;
 
@@ -3073,12 +3076,19 @@ MulticopterPositionControl::task_main()
 			_vel_sp_prev = _vel;
 		}
 
-		/* switch to smooth takeoff if we got out of landed state */
-		if (!_vehicle_land_detected.landed && was_landed) {
-			_in_takeoff = true;
+		if (!_in_smooth_takeoff && _vehicle_land_detected.landed && _control_mode.flag_armed &&
+		    (in_auto_takeoff() || manual_wants_takeoff())) {
+			_in_smooth_takeoff = true;
+			// This ramp starts negative and goes to positive later because we want to
+			// be as smooth as possible. If we start at 0, we alrady jump to hover throttle.
 			_takeoff_vel_limit = -0.5f;
 		}
 
+		else if (!_control_mode.flag_armed) {
+			// If we're disarmed and for some reason were in a smooth takeoff, we reset that.
+			_in_smooth_takeoff = false;
+		}
+
 		/* set triplets to invalid if we just landed */
 		if (_vehicle_land_detected.landed && !was_landed) {
 			_pos_sp_triplet.current.valid = false;