mc_pos_control/FlightTasks: trigger takeoff based on task

The initial idea of the flight task architecture was that
a task can freely set it's setpoints and doesn't have to
worry about takeoff and landing. It would just takeoff
when it's landed and there's a setpoint to go up and
land when it puts a setpoint that pushes into the ground.
With the takeoff logic there are some significant interface
problems depending on the way a task is implemented:
From the setpoint is not high enough to trigger to
an unexpected takeoff because of some estimator
fluctuation affecting the setpoint. It's easiest to solve this
by allowing the task to determine when a takeoff is triggered.
If no condition is implemented by default a task is not
allowing a takeoff  and cannot be used to get the vehicle
off the ground.

msg/vehicle_constraints.msg

@@ -1,12 +1,13 @@
 # Local setpoint constraints in NED frame
 # setting something to NaN means that no limit is provided
 
-uint64 timestamp	# time since system start (microseconds)
+uint64 timestamp # time since system start (microseconds)
 
-float32 yawspeed	# in radians/sec
+float32 yawspeed # in radians/sec
-float32 speed_xy	# in meters/sec
+float32 speed_xy # in meters/sec
-float32 speed_up	# in meters/sec
+float32 speed_up # in meters/sec
-float32 speed_down	# in meters/sec
+float32 speed_down # in meters/sec
-float32 tilt    # in radians [0, PI]
+float32 tilt # in radians [0, PI]
 float32 min_distance_to_ground # in meters
 float32 max_distance_to_ground # in meters
+bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight)

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp

@@ -106,6 +106,7 @@ bool FlightTaskAuto::updateFinalize()
 	// If the FlightTask generates a yaw or a yawrate setpoint that exceeds this value
 	// it will see its setpoint constrained here
 	_limitYawRate();
+	_constraints.want_takeoff = _checkTakeoff();
 	return true;
 }
 
@@ -128,6 +129,17 @@ void FlightTaskAuto::_limitYawRate()
 	}
 }
 
+bool FlightTaskAuto::_checkTakeoff() {
+	// position setpoint above the minimum altitude
+	float min_altitude = 0.2f;
+	const float min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min;
+	if (PX4_ISFINITE(min_distance_to_ground)) {
+		min_altitude = min_distance_to_ground + 0.05f;
+	}
+
+	return PX4_ISFINITE(_position_setpoint(2)) && _position_setpoint(2) < (_position(2) - min_altitude);
+}
+
 bool FlightTaskAuto::_evaluateTriplets()
 {
 	// TODO: fix the issues mentioned below

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp

@@ -91,6 +91,7 @@ protected:
 	matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/
 	void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */
 	bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */
+	virtual bool _checkTakeoff();  /**< determines when to trigger a takeoff (ignored in flight) */
 
 	matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */
 	matrix::Vector3f _prev_wp{}; /**< Previous waypoint  (local frame). If no previous triplet is available, the prev_wp is set to current position. */

src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp

@@ -143,4 +143,5 @@ void FlightTask::_setDefaultConstraints()
 	_constraints.tilt = math::radians(_param_mpc_tiltmax_air.get());
 	_constraints.min_distance_to_ground = NAN;
 	_constraints.max_distance_to_ground = NAN;
+	_constraints.want_takeoff = false;
 }

src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp

@@ -198,7 +198,7 @@ protected:
 	/**
 	 * Set constraints to default values
 	 */
-	virtual void  _setDefaultConstraints();
+	virtual void _setDefaultConstraints();
 
 	/* Time abstraction */
 	static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */

src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -345,10 +345,17 @@ void FlightTaskManualAltitude::_updateSetpoints()
 	_respectGroundSlowdown();
 }
 
+bool FlightTaskManualAltitude::_checkTakeoff()
+{
+	// stick is deflected above the middle 15% of the range
+	return _sticks(2) < -0.3f;
+}
+
 bool FlightTaskManualAltitude::update()
 {
 	_scaleSticks();
 	_updateSetpoints();
+	_constraints.want_takeoff = _checkTakeoff();
 
 	return true;
 }

src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -56,6 +56,7 @@ protected:
 	void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
 	virtual void _updateSetpoints(); /**< updates all setpoints */
 	virtual void _scaleSticks(); /**< scales sticks to velocity in z */
+	virtual bool _checkTakeoff(); /**< determines when to trigger a takeoff (ignored in flight) */
 
 	/**
 	 * rotates vector into local frame

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -232,12 +232,11 @@ private:
 
 	/**
 	 * Update smooth takeoff vertical velocity ramp to bring the vehicle off the ground without step
-	 * @param z_sp position setpoint in the z-Direction
 	 * @param vz_sp velocity setpoint in the z-Direction
-	 * @param jz_sp jerk setpoint in the z-Direction
+	 * @param z_sp position setpoint in the z-Direction
-	 * @param min_ground_clearance minimal distance to the ground in which e.g. optical flow works correctly
+	 * @param vz_constraint velocity to ramp to when there's only a position setpoint
 	 */
-	void update_takeoff_ramp(const float z_sp, const float vz_sp, const float jz_sp, const float min_distance_to_ground);
+	void update_takeoff_ramp(const bool want_takeoff, const float vz_sp, const float z_sp, const float vz_constraint);
 
 	/**
 	 * Adjust the setpoint during landing.
@@ -647,7 +646,7 @@ MulticopterPositionControl::run()
 
 			// do smooth takeoff after delay if there's a valid vertical velocity or position
 			if (_spoolup_time_hysteresis.get_state() && PX4_ISFINITE(_states.position(2)) && PX4_ISFINITE(_states.velocity(2))) {
-				update_takeoff_ramp(setpoint.z, setpoint.vz, setpoint.jerk_z, constraints.min_distance_to_ground);
+				update_takeoff_ramp(constraints.want_takeoff, setpoint.vz, setpoint.z, constraints.speed_up);
 			}
 
 			// disable horizontal / yaw control during smooth takeoff and limit maximum speed upwards
@@ -978,62 +977,40 @@ MulticopterPositionControl::start_flight_task()
 }
 
 void
-MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz_sp, const float jz_sp, const float min_distance_to_ground)
+MulticopterPositionControl::update_takeoff_ramp(const bool want_takeoff, const float vz_sp, const float z_sp, const float vz_constraint)
 {
 	const float takeoff_ramp_initialization = -0.7f;
 
 	// check if takeoff is triggered
-	if (_vehicle_land_detected.landed && !_in_takeoff_ramp) {
+	if (_vehicle_land_detected.landed && !_in_takeoff_ramp && want_takeoff) {
-		// vehicle is still landed and no takeoff was initiated yet, check if takeoff is triggered
+		// takeoff was triggered, start velocity ramp
-		// minimum takeoff altitude either 20cm or if valid above minimum altitude specified by flight task/estimator
+		_takeoff_ramp_velocity = takeoff_ramp_initialization;
-		float min_altitude = 0.2f;
+		_in_takeoff_ramp = true;
-		if (PX4_ISFINITE(min_distance_to_ground)) {
+		_takeoff_reference_z = _states.position(2);
-			min_altitude = min_distance_to_ground + 0.05f;
-		}
-
-		// upwards velocity setpoint larger than a minimal speed
-		const bool velocity_triggered_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.3f;
-		// upwards jerk setpoint
-		const bool jerk_triggered_takeoff = PX4_ISFINITE(jz_sp) && jz_sp < -FLT_EPSILON;
-		// position setpoint above the minimum altitude
-		_position_triggered_takeoff = PX4_ISFINITE(z_sp) && (z_sp < (_states.position(2) - min_altitude));
-
-		if (velocity_triggered_takeoff || jerk_triggered_takeoff || _position_triggered_takeoff) {
-			// takeoff was triggered, start velocity ramp
-			_takeoff_ramp_velocity = takeoff_ramp_initialization;
-			_in_takeoff_ramp = true;
-			_takeoff_reference_z = _states.position(2);
-		}
 	}
 
 	// if in smooth takeoff limit upwards velocity setpoint to a smooth ramp
 	if (_in_takeoff_ramp) {
 		float takeoff_desired_velocity = -vz_sp;
 
-		// if takeoff was triggered by a position setpoint, ramp to the takeoff speed parameter
+		// if there's only a position setpoint we go up with the configured takeoff speed
-		if (_position_triggered_takeoff) {
+		if (!PX4_ISFINITE(vz_sp) && PX4_ISFINITE(z_sp)) {
-			takeoff_desired_velocity = _param_mpc_tko_speed.get();
+			takeoff_desired_velocity = vz_constraint;
 		}
 
-		// ramp up vrtical velocity in TKO_RAMP_T seconds
+
 		if (_param_mpc_tko_ramp_t.get() > _dt) {
 			_takeoff_ramp_velocity += (takeoff_desired_velocity - takeoff_ramp_initialization) * _dt / _param_mpc_tko_ramp_t.get();
-
 		} else {
-			// no ramp, directly go to desired takeoff speed
+			// no ramp time, directly jump to desired velocity
 			_takeoff_ramp_velocity = takeoff_desired_velocity;
 		}
 
 		// make sure we cannot overshoot the desired setpoint with the ramp
 		_takeoff_ramp_velocity = math::min(_takeoff_ramp_velocity, takeoff_desired_velocity);
 
-		// smooth takeoff is achieved once desired altitude/velocity setpoint is reached
+		// smooth takeoff is finished once desired velocity setpoint is reached
-		if (_position_triggered_takeoff) {
+		_in_takeoff_ramp = (_takeoff_ramp_velocity < takeoff_desired_velocity);
-			_in_takeoff_ramp = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - _param_mpc_land_alt2.get());
-
-		} else {
-			_in_takeoff_ramp = (_takeoff_ramp_velocity < -vz_sp);
-		}
 	}
 }