FlightTaskOffboard: first commit similar in logic to legacy code

FlightTaskOffboard: ensure supported offboard setpoints

src/lib/FlightTasks/CMakeLists.txt

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

src/lib/FlightTasks/FlightTasks.cpp

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

src/lib/FlightTasks/FlightTasks.hpp

@@ -51,6 +51,7 @@
 #include "tasks/FlightTaskAutoFollowMe.hpp"
 #include "tasks/FlightTaskOrbit.hpp"
 #include "tasks/FlightTaskSport.hpp"
+#include "tasks/FlightTaskOffboard.hpp"
 
 #include "SubscriptionArray.hpp"
 
@@ -67,6 +68,7 @@ enum class FlightTaskIndex : int {
 	Sport,
 	AutoLine,
 	AutoFollowMe,
+	Offboard,
 
 	Count // number of tasks
 };
@@ -151,6 +153,7 @@ private:
 		FlightTaskSport sport;
 		FlightTaskAutoLine autoLine;
 		FlightTaskAutoFollowMe autoFollowMe;
+		FlightTaskOffboard offboard;
 	} _task_union; /**< storage for the currently active task */
 
 	FlightTask *_current_task = nullptr;

src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp

@@ -0,0 +1,169 @@
+/****************************************************************************
+ *
+ *   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 FlightTaskAuto.cpp
+ */
+#include "FlightTaskOffboard.hpp"
+#include <mathlib/mathlib.h>
+#include <float.h>
+
+using namespace matrix;
+
+bool FlightTaskOffboard::initializeSubscriptions(SubscriptionArray &subscription_array)
+{
+	if (!FlightTask::initializeSubscriptions(subscription_array)) {
+		return false;
+	}
+
+	if (!subscription_array.get(ORB_ID(position_setpoint_triplet), _sub_triplet_setpoint)) {
+		return false;
+	}
+
+	return true;
+}
+
+bool FlightTaskOffboard::update()
+{
+	if (!_sub_triplet_setpoint->get().current.valid) {
+		_resetSetpoints();
+		_position_setpoint = _position;
+		return false;
+	}
+
+	// reset setpoint for every loop
+	_resetSetpoints();
+
+	// Possible inputs:
+	// 1. position setpoint
+	// 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
+				     && _sub_vehicle_local_position->get().z_valid;
+	const bool velocity_ctrl_xy = _sub_triplet_setpoint->get().current.velocity_valid
+				      && _sub_vehicle_local_position->get().v_xy_valid;
+	const bool velocity_ctrl_z = _sub_triplet_setpoint->get().current.velocity_valid
+				     && _sub_vehicle_local_position->get().v_z_valid;
+	const bool feedforward_ctrl_xy = position_ctrl_xy && velocity_ctrl_xy;
+	const bool feedforward_ctrl_z = position_ctrl_z && velocity_ctrl_z;
+	const bool acceleration_ctrl = _sub_triplet_setpoint->get().current.acceleration_valid;
+
+	// if nothing is valid in xy, then exit offboard
+	if (!(position_ctrl_xy || velocity_ctrl_xy || acceleration_ctrl)) {
+		return false;
+	}
+
+	// if nothing is valid in z, then exit offboard
+	if (!(position_ctrl_z || velocity_ctrl_z || acceleration_ctrl)) {
+		return false;
+	}
+
+	// XY-direction
+	if (feedforward_ctrl_xy) {
+		_position_setpoint(0) = _sub_triplet_setpoint->get().current.x;
+		_position_setpoint(1) = _sub_triplet_setpoint->get().current.y;
+		_velocity_setpoint(0) = _sub_triplet_setpoint->get().current.vx;
+		_velocity_setpoint(1) = _sub_triplet_setpoint->get().current.vy;
+
+	} else if (position_ctrl_xy) {
+		_position_setpoint(0) = _sub_triplet_setpoint->get().current.x;
+		_position_setpoint(1) = _sub_triplet_setpoint->get().current.y;
+
+	} else if (velocity_ctrl_xy) {
+
+		if (_sub_triplet_setpoint->get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) {
+			// in local frame: don't require any transformation
+			_velocity_setpoint(0) = _sub_triplet_setpoint->get().current.vx;
+			_velocity_setpoint(1) = _sub_triplet_setpoint->get().current.vy;
+
+		} else if (_sub_triplet_setpoint->get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) {
+			// in body frame: need to transorm first
+			// Note, this transformation is wrong because body-xy is not neccessarily on the same plane as locale-xy
+			_velocity_setpoint(0) = cosf(_yaw) * _sub_triplet_setpoint->get().current.vx - sinf(
+							_yaw) * _sub_triplet_setpoint->get().current.vy;
+			_velocity_setpoint(1) = sinf(_yaw) * _sub_triplet_setpoint->get().current.vx + cosf(
+							_yaw) * _sub_triplet_setpoint->get().current.vy;
+
+		} else {
+			// no valid frame
+			return false;
+		}
+	}
+
+	// Z-direction
+
+	if (feedforward_ctrl_z) {
+		_position_setpoint(2) = _sub_triplet_setpoint->get().current.z;
+		_velocity_setpoint(2) = _sub_triplet_setpoint->get().current.vz;
+
+	} else if (position_ctrl_z) {
+		_position_setpoint(2) = _sub_triplet_setpoint->get().current.z;
+
+	} else if (velocity_ctrl_z) {
+		_velocity_setpoint(2) = _sub_triplet_setpoint->get().current.vz;
+	}
+
+	// Yaw / Yaw-speed
+
+	if (_sub_triplet_setpoint->get().current.yaw_valid) {
+		// yaw control required
+		_yaw_setpoint = _sub_triplet_setpoint->get().current.yaw;
+
+		if (_sub_triplet_setpoint->get().current.yawspeed_valid) {
+			// yawspeed is used as feedforward
+			_yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed;
+		}
+
+	} else if (_sub_triplet_setpoint->get().current.yawspeed_valid) {
+		// only yawspeed required
+		_yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed;
+		// set yaw setpoint to NAN since not used
+		_yaw_setpoint = NAN;
+
+	}
+
+	// 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;
+	}
+
+	return true;
+
+}

src/lib/FlightTasks/tasks/FlightTaskOffboard.hpp

@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   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 FlightTaskOffboard.hpp
+ */
+
+#pragma once
+
+#include "FlightTask.hpp"
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/position_setpoint.h>
+
+class FlightTaskOffboard : public FlightTask
+{
+public:
+	FlightTaskOffboard() = default;
+
+	virtual ~FlightTaskOffboard() = default;
+	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
+	bool update() override;
+
+protected:
+	uORB::Subscription<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
+};