Takeoff Revision

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 speed_xy	# in meters/sec
-float32 speed_up	# in meters/sec
-float32 speed_down	# in meters/sec
-float32 tilt    # in radians [0, PI]
+float32 yawspeed # in radians/sec
+float32 speed_xy # in meters/sec
+float32 speed_up # in meters/sec
+float32 speed_down # in meters/sec
+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;
 }
 

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

@@ -57,10 +57,12 @@ bool FlightTaskAutoLineSmoothVel::activate()
 
 void FlightTaskAutoLineSmoothVel::reActivate()
 {
-	// Don't reset during takeoff TODO: Find a proper solution
-	// The issue here is that with a small increment of velocity setpoint (generated by this flight task), the
-	// land detector doesn't detect takeoff and without takeoff detection, the
-	// flight task is always reset.
+	// On ground, reset acceleration and velocity to zero
+	for (int i = 0; i < 2; ++i) {
+		_trajectory[i].reset(0.f, 0.f, _position(i));
+	}
+
+	_trajectory[2].reset(0.f, 0.7f, _position(2));
 }
 
 void FlightTaskAutoLineSmoothVel::_generateSetpoints()
@@ -143,6 +145,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 	// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.
 
 	_checkEkfResetCounters();
+	_want_takeoff = false;
 
 	if (PX4_ISFINITE(_position_setpoint(0)) &&
 	    PX4_ISFINITE(_position_setpoint(1))) {
@@ -191,6 +194,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 			_velocity_setpoint(2) = vel_sp_z;
 		}
 
+		_want_takeoff = _velocity_setpoint(2) < -0.3f;
 	}
 }
 

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp

@@ -67,6 +67,9 @@ class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper2
 
 	void _generateSetpoints() override; /**< Generate setpoints along line. */
 
+	/** determines when to trigger a takeoff (ignored in flight) */
+	bool _checkTakeoff() override { return _want_takeoff; };
+
 	inline float _constrainOneSide(float val, float constrain);
 	void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
 	void _generateHeading();
@@ -75,7 +78,9 @@ class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper2
 	void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
 	void _generateTrajectory();
 	VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions
-	float _yaw_sp_prev;
+
+	float _yaw_sp_prev{NAN};
+	bool _want_takeoff{false};
 
 	/* counters for estimator local position resets */
 	struct {

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

@@ -6,7 +6,7 @@ constexpr uint64_t FlightTask::_timeout;
 // First index of empty_setpoint corresponds to time-stamp and requires a finite number.
 const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}};
 
-const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {}};
+const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
 const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
 
 bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
@@ -143,4 +143,32 @@ 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;
+}
+
+bool FlightTask::_checkTakeoff()
+{
+	// position setpoint above the minimum altitude
+	bool position_triggered_takeoff = false;
+
+	if (PX4_ISFINITE(_position_setpoint(2))) {
+		//minimal altitude either 20cm or what is necessary for correct estimation e.g. optical flow
+		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;
+		}
+
+		position_triggered_takeoff = _position_setpoint(2) < (_position(2) - min_altitude);
+	}
+
+	// upwards velocity setpoint
+	bool velocity_triggered_takeoff = false;
+
+	if (PX4_ISFINITE(_velocity_setpoint(2))) {
+		velocity_triggered_takeoff = _velocity_setpoint(2) < -0.3f;
+	}
+
+	return position_triggered_takeoff || velocity_triggered_takeoff;
 }

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

@@ -185,20 +185,17 @@ class FlightTask : public ModuleParams
 	uORB::Subscription<vehicle_attitude_s> *_sub_attitude{nullptr};
 	uint8_t _heading_reset_counter{0}; /**< estimator heading reset */
 
-	/**
-	 * Reset all setpoints to NAN
-	 */
+	/** Reset all setpoints to NAN */
 	void _resetSetpoints();
 
-	/**
-	 * Check and update local position
-	 */
+	/** Check and update local position */
 	void _evaluateVehicleLocalPosition();
 
-	/**
-	 * Set constraints to default values
-	 */
-	virtual void  _setDefaultConstraints();
+	/** Set constraints to default values */
+	virtual void _setDefaultConstraints();
+
+	/** determines when to trigger a takeoff (ignored in flight) */
+	virtual bool _checkTakeoff();
 
 	/* 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 @@ class FlightTaskManualAltitude : public FlightTaskManual
 	void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
 	virtual void _updateSetpoints(); /**< updates all setpoints */
 	virtual void _scaleSticks(); /**< scales sticks to velocity in z */
+	bool _checkTakeoff() override;
 
 	/**
 	 * rotates vector into local frame

src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp

@@ -75,17 +75,16 @@ bool FlightTaskOffboard::activate()
 
 bool FlightTaskOffboard::update()
 {
+	// reset setpoint for every loop
+	_resetSetpoints();
+
 	if (!_sub_triplet_setpoint->get().current.valid) {
-		_resetSetpoints();
+		_setDefaultConstraints();
 		_position_setpoint = _position;
 		return false;
 	}
 
-	// reset setpoint for every loop
-	_resetSetpoints();
-
 	// Yaw / Yaw-speed
-
 	if (_sub_triplet_setpoint->get().current.yaw_valid) {
 		// yaw control required
 		_yaw_setpoint = _sub_triplet_setpoint->get().current.yaw;
@@ -169,7 +168,6 @@ bool FlightTaskOffboard::update()
 	// 2. position setpoint + velocity setpoint (velocity used as feedforward)
 	// 3. velocity setpoint
 	// 4. acceleration setpoint -> this will be mapped to normalized thrust setpoint because acceleration is not supported
-
 	const bool position_ctrl_xy = _sub_triplet_setpoint->get().current.position_valid
 				      && _sub_vehicle_local_position->get().xy_valid;
 	const bool position_ctrl_z = _sub_triplet_setpoint->get().current.alt_valid
@@ -225,7 +223,6 @@ bool FlightTaskOffboard::update()
 	}
 
 	// Z-direction
-
 	if (feedforward_ctrl_z) {
 		_position_setpoint(2) = _sub_triplet_setpoint->get().current.z;
 		_velocity_setpoint(2) = _sub_triplet_setpoint->get().current.vz;
@@ -239,12 +236,14 @@ bool FlightTaskOffboard::update()
 
 	// Acceleration
 	// Note: this is not supported yet and will be mapped to normalized thrust directly.
-
 	if (_sub_triplet_setpoint->get().current.acceleration_valid) {
 		_thrust_setpoint(0) = _sub_triplet_setpoint->get().current.a_x;
 		_thrust_setpoint(1) = _sub_triplet_setpoint->get().current.a_y;
 		_thrust_setpoint(2) = _sub_triplet_setpoint->get().current.a_z;
 	}
 
+	// use default conditions of upwards position or velocity to take off
+	_constraints.want_takeoff = _checkTakeoff();
+
 	return true;
 }

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -500,7 +500,10 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
 	q_sp.copyTo(attitude_setpoint.q_d);
 	attitude_setpoint.q_d_valid = true;
 
-	attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z);
+	// hotfix: make sure to leave a zero thrust setpoint for position controller to take over after initialization in manual mode
+	// without this the actuator_controls thrust stays like it was set here when switching to a position controlled mode and the
+	// land detector will immediately detect takeoff when arming
+	attitude_setpoint.thrust_body[2] = _v_control_mode.flag_armed ? -throttle_curve(_manual_control_sp.z) : 0.0f;
 	attitude_setpoint.timestamp = hrt_absolute_time();
 	if (_attitude_sp_id != nullptr) {
 		orb_publish_auto(_attitude_sp_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT);

src/modules/mc_pos_control/CMakeLists.txt

@@ -30,6 +30,9 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+
+add_subdirectory(Takeoff)
+
 px4_add_module(
 	MODULE modules__mc_pos_control
 	MAIN mc_pos_control
@@ -46,4 +49,5 @@ px4_add_module(
 		ecl_geo
 		WeatherVane
 		CollisionPrevention
+		Takeoff
 	)

src/modules/mc_pos_control/Takeoff/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(Takeoff
+	Takeoff.cpp
+)
+target_include_directories(Takeoff
+	PUBLIC
+	${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+px4_add_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff)