FollowMe: legacy implementation. NOTE: FOLLOW-ME is already broken on legacy code.

src/lib/FlightTasks/CMakeLists.txt

@@ -42,6 +42,7 @@ tasks/FlightTask.cpp
 		tasks/FlightTaskManualPositionSmooth.cpp
 		tasks/FlightTaskAuto.cpp
 		tasks/FlightTaskAutoLine.cpp
+		tasks/FlightTaskAutoFollowMe.cpp
 		tasks/Utility/ManualSmoothingZ.cpp
 		tasks/Utility/ManualSmoothingXY.cpp
 		SubscriptionArray.cpp

src/lib/FlightTasks/FlightTasks.cpp

@@ -86,6 +86,10 @@ int FlightTasks::switchTask(FlightTaskIndex new_task_index)
 		_current_task = new (&_task_union.autoLine) FlightTaskAutoLine();
 		break;
 
+	case FlightTaskIndex::AutoFollowMe:
+		_current_task = new (&_task_union.autoFollowMe) FlightTaskAutoFollowMe();
+		break;
+
 	default:
 		/* invalid task */
 		return -1;

src/lib/FlightTasks/FlightTasks.hpp

@@ -48,6 +48,7 @@
 #include "tasks/FlightTaskManualPositionSmooth.hpp"
 #include "tasks/FlightTaskManualStabilized.hpp"
 #include "tasks/FlightTaskAutoLine.hpp"
+#include "tasks/FlightTaskAutoFollowMe.hpp"
 #include "tasks/FlightTaskOrbit.hpp"
 #include "tasks/FlightTaskSport.hpp"
 
@@ -65,6 +66,7 @@ enum class FlightTaskIndex : int {
 	Orbit,
 	Sport,
 	AutoLine,
+	AutoFollowMe,
 
 	Count // number of tasks
 };
@@ -148,6 +150,7 @@ private:
 		FlightTaskOrbit orbit;
 		FlightTaskSport sport;
 		FlightTaskAutoLine autoLine;
+		FlightTaskAutoFollowMe autoFollowMe;
 	} _task_union; /**< storage for the currently active task */
 
 	FlightTask *_current_task = nullptr;

src/lib/FlightTasks/tasks/FlightTaskAuto.cpp

@@ -61,7 +61,6 @@ bool FlightTaskAuto::activate()
 {
 	bool ret = FlightTask::activate();
 	_prev_prev_wp = _prev_wp = _target = _next_wp = _position;
-	_yaw_wp = _yaw;
 	_setDefaultConstraints();
 
 	return ret;
@@ -91,7 +90,6 @@ bool FlightTaskAuto::_evaluateTriplets()
 	if (!_sub_triplet_setpoint->get().current.valid) {
 		// best we can do is to just set all waypoints to current state
 		_prev_prev_wp = _prev_wp = _target = _next_wp = _position;
-		_yaw_wp = _yaw;
 		_type = WaypointType::position;
 		return false;
 	}
@@ -112,12 +110,12 @@ bool FlightTaskAuto::_evaluateTriplets()
 			       _sub_triplet_setpoint->get().current.lat, _sub_triplet_setpoint->get().current.lon, &target(0), &target(1));
 	target(2) = -(_sub_triplet_setpoint->get().current.alt - _reference_altitude);
 
+	if (_sub_triplet_setpoint->get().current.yaw_valid) {
+		_yaw_setpoint = _sub_triplet_setpoint->get().current.yaw;
+	}
 
-	_yaw_wp = _sub_triplet_setpoint->get().current.yaw;
-
-	if (!PX4_ISFINITE(_yaw_wp)) {
-		_yaw_wp = _yaw;
-
+	if (_type == WaypointType::follow_target && _sub_triplet_setpoint->get().current.yawspeed_valid) {
+		_yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed;
 	}
 
 	// Check if anything has changed. We do that by comparing the target
@@ -201,3 +199,20 @@ void FlightTaskAuto::_setDefaultConstraints()
 		_constraints.speed_xy = MPC_XY_CRUISE.get();
 	}
 }
+
+matrix::Vector2f FlightTaskAuto::_getTargetVelocityXY()
+{
+	// guard against any bad velocity values
+	const float vx = _sub_triplet_setpoint->get().current.vx;
+	const float vy = _sub_triplet_setpoint->get().current.vy;
+	bool velocity_valid = PX4_ISFINITE(vx) && PX4_ISFINITE(vy) &&
+			      _sub_triplet_setpoint->get().current.velocity_valid;
+
+	if (velocity_valid) {
+		return matrix::Vector2f(vx, vy);
+
+	} else {
+		// just return zero speed
+		return matrix::Vector2f{};
+	}
+}

src/lib/FlightTasks/tasks/FlightTaskAuto.hpp

@@ -56,7 +56,9 @@ enum class WaypointType : int {
 	loiter,
 	takeoff,
 	land,
-	idle
+	idle,
+	offboard, // only part of this structure due to legacy reason. It is not used
+	follow_target
 };
 
 class FlightTaskAuto : public FlightTask
@@ -72,15 +74,14 @@ public:
 protected:
 	void _setDefaultConstraints() override;
 	float _getMaxCruiseSpeed() {return MPC_XY_CRUISE.get();} /**< getter for default cruise speed */
+	matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/
 
 	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. */
 	matrix::Vector3f _target{}; /**< Target waypoint  (local frame).*/
 	matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */
-	float _yaw_wp{0.0f}; /**< Triplet yaw waypoint. Currently it is not a yaw-waypoint, but rather a yaw setpoint at each time stamp. */
 	float _mc_cruise_speed{0.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */
 	WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
-
 	uORB::Subscription<home_position_s> *_sub_home_position{nullptr};
 
 private:

src/lib/FlightTasks/tasks/FlightTaskAutoFollowMe.cpp

@@ -0,0 +1,49 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskAutoFollowMe.cpp
+ */
+
+#include "FlightTaskAutoFollowMe.hpp"
+#include <mathlib/mathlib.h>
+
+using namespace matrix;
+
+bool FlightTaskAutoFollowMe::update()
+{
+	_position_setpoint = _target;
+	matrix::Vector2f vel_sp = _getTargetVelocityXY();
+	_velocity_setpoint = matrix::Vector3f(vel_sp(0), vel_sp(1), 0.0f);
+	return true;
+}

src/lib/FlightTasks/tasks/FlightTaskAutoFollowMe.hpp

@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskAutoFollowMe.hpp
+ *
+ * Flight task for autonomous, gps driven follow-me mode.
+ */
+
+#pragma once
+
+#include "FlightTaskAuto.hpp"
+
+class FlightTaskAutoFollowMe : public FlightTaskAuto
+{
+public:
+	FlightTaskAutoFollowMe() = default;
+	virtual ~FlightTaskAutoFollowMe() = default;
+	bool update() override;
+};

src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp

@@ -89,12 +89,6 @@ bool FlightTaskAutoLine::update()
 		_generateVelocitySetpoints();
 	}
 
-	// For now yaw-setpoint comes directly form triplets.
-	// TODO: In the future, however, yaw should be set in this
-	// task based on flag: yaw along path, yaw based on gimbal, yaw
-	// same as home yaw ...
-	_yaw_setpoint = _yaw_wp;
-
 	// update previous type
 	_type_previous = _type;
 
@@ -362,8 +356,8 @@ void FlightTaskAutoLine::_generateXYsetpoints()
 
 			float yaw_diff = 0.0f;
 
-			if (PX4_ISFINITE(_yaw_wp)) {
-				yaw_diff = _wrap_pi(_yaw_wp - _yaw);
+			if (PX4_ISFINITE(_yaw_setpoint)) {
+				yaw_diff = _wrap_pi(_yaw_setpoint - _yaw);
 			}
 
 			// If yaw offset is large, only accelerate with 0.5 m/s^2.