FlightTasks: fix takeoff trigger for offboard

MaEtUgR · LorenzMeier · commit 7c7d980cf0f5 · 2019-05-22T22:05:38.000+02:00
diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
@@ -129,17 +129,6 @@ 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
diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
@@ -91,7 +91,6 @@ class FlightTaskAuto : public FlightTask
 	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. */
diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp b/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)
@@ -145,3 +145,30 @@ void FlightTask::_setDefaultConstraints()
 	_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;
+}
diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
@@ -185,21 +185,18 @@ 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
-	 */
+	/** 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 */
 	float _time = 0; /**< passed time in seconds since the task was activated */
diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
@@ -56,7 +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 */
-	virtual bool _checkTakeoff(); /**< determines when to trigger a takeoff (ignored in flight) */
+	bool _checkTakeoff() override;
 
 	/**
 	 * rotates vector into local frame
diff --git a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp b/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;
 }

Original file line number	Diff line number	Diff line change
`@@ -129,17 +129,6 @@ void FlightTaskAuto::_limitYawRate()`
`129`	`129`	`}`
`130`	`130`	`}`
`131`	`131`
`132`		`-bool FlightTaskAuto::_checkTakeoff() {`
`133`		`- // position setpoint above the minimum altitude`
`134`		`- float min_altitude = 0.2f;`
`135`		`- const float min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min;`
`136`		`- if (PX4_ISFINITE(min_distance_to_ground)) {`
`137`		`- min_altitude = min_distance_to_ground + 0.05f;`
`138`		`- }`
`139`		`-`
`140`		`- return PX4_ISFINITE(_position_setpoint(2)) && _position_setpoint(2) < (_position(2) - min_altitude);`
`141`		`-}`
`142`		`-`
`143`	`132`	`bool FlightTaskAuto::_evaluateTriplets()`
`144`	`133`	`{`
`145`	`134`	`// TODO: fix the issues mentioned below`