feat: spacecraft support (#24734)

* rft: initial merging of controllers for spacecraft vehicles * feat: rate controller nominal * feat: spacecraft tooling for commander and VehicleStatus * feat: spacecraft tooling for commander and VehicleStatus * fix: format * fix: format * fix: remove iostream * fix: remove iostream * feat: spacecraft attitude control and minor refactoring of params * feat: add position controller * fix: format * fix: moved trajectories to new message, removed derivative filters * fix: format * fix: removed extra newline * fix: spacecraft allocation builds * feat: add thrusters to effectivenes, add spacecraft build to cmake, clean comments * feat: required changes for allocation * feat: thruster simulation interface * fix: update maximum and minimums * fix: format * fix: added newline at the end of spacecraft actuator effectiveness * feat: configurable board pwm freq from Kconfig * feat: mavlink compliant spacecraft definition * feat: add orbiter to define * boards: Increase TELEM2 rx buffer size for DDS over serial use-case (ARK Jetson) feat: spacecraft tooling for commander and VehicleStatus fix: format fix: remove iostream feat: mavlink compliant spacecraft definition * feat: add orbiter to define * feat: add orbiter to define * fix: change mav_type to new spacecraft orbiter enum value * fix: build issue * feat: update mavlink * feat: update mavlink to latest master with spacecraft * feat: update mavlink * feat: update mavlink to latest * feat: cleanup and synchronization with new mavlink vehicle definition * fix: get away without specifying spacecraft vehicle * fix: removed unnecessary definition * fix: format * feat: cmake variant for spacecraft * feat: proper mav_type and rc init * fix: removed dart from build system * add: thrusters to actuator type * rft: reordering actuator type * rft: initial merging of controllers for spacecraft vehicles * feat: rate controller nominal * fix: format * feat: spacecraft attitude control and minor refactoring of params * feat: add position controller * fix: format * fix: moved trajectories to new message, removed derivative filters * fix: format * fix: removed extra newline * fix: spacecraft allocation builds * feat: add thrusters to effectivenes, add spacecraft build to cmake, clean comments * feat: required changes for allocation * feat: thruster simulation interface * fix: update maximum and minimums * fix: format * fix: added newline at the end of spacecraft actuator effectiveness * feat: configurable board pwm freq from Kconfig * feat: add orbiter to define * feat: cleanup and synchronization with new mavlink vehicle definition * fix: get away without specifying spacecraft vehicle * fix: conflicts * fix: format * fix: remove duplicate entry * rft: remove Kconfig changes * rft: revert main Kconfig * rft: revert main kcoonfig on platforms * rft: remove changes to board PWm (go on another PR) * rft: revert changes to commander (main is correct) * fix: extra char on commander_helper * rft: removed extra spaces * rft: moved effectiveness to spacecraft * fix: spacecraft effectiveness * fix: extra space * feat: preliminary version, still using thrusters * rft: initial pipeline on PX4 side with rotors instead of thrusters * feat: add atmos model * feat: spacecraft with rotor pipeline tested, working * feat: update gz * rft: removed thruster interfaces * fix: format * fix: remove control allocation * fix: thruster normalization * fix: format * fix: nuttx version * fix: clang tidy error * feat: updated gz to add atmos model * fix: update gz * fix: update mavlink * fix: remove friend class from allocation lib * fix: remove actuator_outputs/motors --------- Co-authored-by: Alexander Lerach <alexander@auterion.com>
2026-05-22 14:27:34 +08:00 · 2025-07-23 17:26:27 +02:00
parent 6474e5d7c1
commit 2f55dff8b9
45 changed files with 5573 additions and 361 deletions
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2015-2020 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.
+#
+############################################################################
+
+add_subdirectory(PositionControl)
+
+px4_add_library(SpacecraftPositionControl
+	SpacecraftPositionControl.cpp
+)
+
+target_link_libraries(SpacecraftPositionControl PUBLIC mathlib)
+target_link_libraries(SpacecraftPositionControl PUBLIC PositionControlLibrary)
+target_include_directories(SpacecraftPositionControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   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(PositionControlLibrary
+	ControlMath.cpp
+	ControlMath.hpp
+	PositionControl.cpp
+	PositionControl.hpp
+)
+target_include_directories(PositionControlLibrary PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+# TODO: add unit tests
+# px4_add_unit_gtest(SRC ScControlMathTest.cpp LINKLIBS SpacecraftPositionControl)
+# px4_add_unit_gtest(SRC ScPositionControlTest.cpp LINKLIBS SpacecraftPositionControl)
@@ -0,0 +1,254 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2018 - 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ControlMath.cpp
+ */
+
+#include "ControlMath.hpp"
+#include <px4_platform_common/defines.h>
+#include <float.h>
+#include <mathlib/mathlib.h>
+
+using namespace matrix;
+
+namespace ControlMath
+{
+void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
+{
+	bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
+	att_sp.thrust_body[2] = -thr_sp.length();
+}
+
+void limitTilt(Vector3f &body_unit, const Vector3f &world_unit, const float max_angle)
+{
+	// determine tilt
+	const float dot_product_unit = body_unit.dot(world_unit);
+	float angle = acosf(dot_product_unit);
+	// limit tilt
+	angle = math::min(angle, max_angle);
+	Vector3f rejection = body_unit - (dot_product_unit * world_unit);
+
+	// corner case exactly parallel vectors
+	if (rejection.norm_squared() < FLT_EPSILON) {
+		rejection(0) = 1.f;
+	}
+
+	body_unit = cosf(angle) * world_unit + sinf(angle) * rejection.unit();
+}
+
+void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
+{
+	// zero vector, no direction, set safe level value
+	if (body_z.norm_squared() < FLT_EPSILON) {
+		body_z(2) = 1.f;
+	}
+
+	body_z.normalize();
+
+	// vector of desired yaw direction in XY plane, rotated by PI/2
+	const Vector3f y_C{-sinf(yaw_sp), cosf(yaw_sp), 0.f};
+
+	// desired body_x axis, orthogonal to body_z
+	Vector3f body_x = y_C % body_z;
+
+	// keep nose to front while inverted upside down
+	if (body_z(2) < 0.f) {
+		body_x = -body_x;
+	}
+
+	if (fabsf(body_z(2)) < 0.000001f) {
+		// desired thrust is in XY plane, set X downside to construct correct matrix,
+		// but yaw component will not be used actually
+		body_x.zero();
+		body_x(2) = 1.f;
+	}
+
+	body_x.normalize();
+
+	// desired body_y axis
+	const Vector3f body_y = body_z % body_x;
+
+	Dcmf R_sp;
+
+	// fill rotation matrix
+	for (int i = 0; i < 3; i++) {
+		R_sp(i, 0) = body_x(i);
+		R_sp(i, 1) = body_y(i);
+		R_sp(i, 2) = body_z(i);
+	}
+
+	// copy quaternion setpoint to attitude setpoint topic
+	const Quatf q_sp{R_sp};
+	q_sp.copyTo(att_sp.q_d);
+}
+
+Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
+{
+	if (Vector2f(v0 + v1).norm() <= max) {
+		// vector does not exceed maximum magnitude
+		return v0 + v1;
+
+	} else if (v0.length() >= max) {
+		// the magnitude along v0, which has priority, already exceeds maximum.
+		return v0.normalized() * max;
+
+	} else if (fabsf(Vector2f(v1 - v0).norm()) < 0.001f) {
+		// the two vectors are equal
+		return v0.normalized() * max;
+
+	} else if (v0.length() < 0.001f) {
+		// the first vector is 0.
+		return v1.normalized() * max;
+
+	} else {
+		// vf = final vector with ||vf|| <= max
+		// s = scaling factor
+		// u1 = unit of v1
+		// vf = v0 + v1 = v0 + s * u1
+		// constraint: ||vf|| <= max
+		//
+		// solve for s: ||vf|| = ||v0 + s * u1|| <= max
+		//
+		// Derivation:
+		// For simplicity, replace v0 -> v, u1 -> u
+		// 				   		   v0(0/1/2) -> v0/1/2
+		// 				   		   u1(0/1/2) -> u0/1/2
+		//
+		// ||v + s * u||^2 = (v0+s*u0)^2+(v1+s*u1)^2+(v2+s*u2)^2 = max^2
+		// v0^2+2*s*u0*v0+s^2*u0^2 + v1^2+2*s*u1*v1+s^2*u1^2 + v2^2+2*s*u2*v2+s^2*u2^2 = max^2
+		// s^2*(u0^2+u1^2+u2^2) + s*2*(u0*v0+u1*v1+u2*v2) + (v0^2+v1^2+v2^2-max^2) = 0
+		//
+		// quadratic equation:
+		// -> s^2*a + s*b + c = 0 with solution: s1/2 = (-b +- sqrt(b^2 - 4*a*c))/(2*a)
+		//
+		// b = 2 * u.dot(v)
+		// a = 1 (because u is normalized)
+		// c = (v0^2+v1^2+v2^2-max^2) = -max^2 + ||v||^2
+		//
+		// sqrt(b^2 - 4*a*c) =
+		// 		sqrt(4*u.dot(v)^2 - 4*(||v||^2 - max^2)) = 2*sqrt(u.dot(v)^2 +- (||v||^2 -max^2))
+		//
+		// s1/2 = ( -2*u.dot(v) +- 2*sqrt(u.dot(v)^2 - (||v||^2 -max^2)) / 2
+		//      =  -u.dot(v) +- sqrt(u.dot(v)^2 - (||v||^2 -max^2))
+		// m = u.dot(v)
+		// s = -m + sqrt(m^2 - c)
+		//
+		//
+		//
+		// notes:
+		// 	- s (=scaling factor) needs to be positive
+		// 	- (max - ||v||) always larger than zero, otherwise it never entered this if-statement
+		Vector2f u1 = v1.normalized();
+		float m = u1.dot(v0);
+		float c = v0.dot(v0) - max * max;
+		float s = -m + sqrtf(m * m - c);
+		return v0 + u1 * s;
+	}
+}
+
+bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r,
+		       const Vector3f &line_a, const Vector3f &line_b, Vector3f &res)
+{
+	// project center of sphere on line  normalized AB
+	Vector3f ab_norm = line_b - line_a;
+
+	if (ab_norm.length() < 0.01f) {
+		return true;
+	}
+
+	ab_norm.normalize();
+	Vector3f d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
+	float cd_len = (sphere_c - d).length();
+
+	if (sphere_r > cd_len) {
+		// we have triangle CDX with known CD and CX = R, find DX
+		float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);
+
+		if ((sphere_c - line_b) * ab_norm > 0.f) {
+			// target waypoint is already behind us
+			res = line_b;
+
+		} else {
+			// target is in front of us
+			res = d + ab_norm * dx_len; // vector A->B on line
+		}
+
+		return true;
+
+	} else {
+
+		// have no roots, return D
+		res = d; // go directly to line
+
+		// previous waypoint is still in front of us
+		if ((sphere_c - line_a) * ab_norm < 0.f) {
+			res = line_a;
+		}
+
+		// target waypoint is already behind us
+		if ((sphere_c - line_b) * ab_norm > 0.f) {
+			res = line_b;
+		}
+
+		return false;
+	}
+}
+
+void addIfNotNan(float &setpoint, const float addition)
+{
+	if (PX4_ISFINITE(setpoint) && PX4_ISFINITE(addition)) {
+		// No NAN, add to the setpoint
+		setpoint += addition;
+
+	} else if (!PX4_ISFINITE(setpoint)) {
+		// Setpoint NAN, take addition
+		setpoint = addition;
+	}
+
+	// Addition is NAN or both are NAN, nothing to do
+}
+
+void addIfNotNanVector3f(Vector3f &setpoint, const Vector3f &addition)
+{
+	for (int i = 0; i < 3; i++) {
+		addIfNotNan(setpoint(i), addition(i));
+	}
+}
+
+void setZeroIfNanVector3f(Vector3f &vector)
+{
+	// Adding zero vector overwrites elements that are NaN with zero
+	addIfNotNanVector3f(vector, Vector3f());
+}
+}
@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2018 - 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ControlMath.hpp
+ *
+ * Simple functions for vector manipulation that do not fit into matrix lib.
+ * These functions are specific for controls.
+ */
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+
+namespace ControlMath
+{
+/**
+ * Converts thrust vector and yaw set-point to a desired attitude.
+ * @param thr_sp desired 3D thrust vector
+ * @param yaw_sp the desired yaw
+ * @param att_sp attitude setpoint to fill
+ */
+void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
+
+/**
+ * Limits the tilt angle between two unit vectors
+ * @param body_unit unit vector that will get adjusted if angle is too big
+ * @param world_unit fixed vector to measure the angle against
+ * @param max_angle maximum tilt angle between vectors in radians
+ */
+void limitTilt(matrix::Vector3f &body_unit, const matrix::Vector3f &world_unit, const float max_angle);
+
+/**
+ * Converts a body z vector and yaw set-point to a desired attitude.
+ * @param body_z a world frame 3D vector in direction of the desired body z axis
+ * @param yaw_sp the desired yaw setpoint
+ * @param att_sp attitude setpoint to fill
+ */
+void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
+
+/**
+ * Outputs the sum of two vectors but respecting the limits and priority.
+ * The sum of two vectors are constraint such that v0 has priority over v1.
+ * This means that if the length of (v0+v1) exceeds max, then it is constraint such
+ * that v0 has priority.
+ *
+ * @param v0 a 2D vector that has priority given the maximum available magnitude.
+ * @param v1 a 2D vector that less priority given the maximum available magnitude.
+ * @return 2D vector
+ */
+matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f &v1, const float &max);
+
+/**
+ * This method was used for smoothing the corners along two lines.
+ *
+ * @param sphere_c
+ * @param sphere_r
+ * @param line_a
+ * @param line_b
+ * @param res
+ * return boolean
+ *
+ * Note: this method is not used anywhere and first requires review before usage.
+ */
+bool cross_sphere_line(const matrix::Vector3f &sphere_c, const float sphere_r, const matrix::Vector3f &line_a,
+		       const matrix::Vector3f &line_b, matrix::Vector3f &res);
+
+/**
+ * Adds e.g. feed-forward to the setpoint making sure existing or added NANs have no influence on control.
+ * This function is udeful to support all the different setpoint combinations of position, velocity, acceleration with NAN representing an uncommitted value.
+ * @param setpoint existing possibly NAN setpoint to add to
+ * @param addition value/NAN to add to the setpoint
+ */
+void addIfNotNan(float &setpoint, const float addition);
+
+/**
+ * _addIfNotNan for Vector3f treating each element individually
+ * @see _addIfNotNan
+ */
+void addIfNotNanVector3f(matrix::Vector3f &setpoint, const matrix::Vector3f &addition);
+
+/**
+ * Overwrites elements of a Vector3f which are NaN with zero
+ * @param vector possibly containing NAN elements
+ */
+void setZeroIfNanVector3f(matrix::Vector3f &vector);
+}
@@ -0,0 +1,220 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 - 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file PositionControl.cpp
+ */
+
+#include "PositionControl.hpp"
+#include "ControlMath.hpp"
+#include <float.h>
+#include <mathlib/mathlib.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/log.h>
+#include <geo/geo.h>
+
+using namespace matrix;
+
+const trajectory_setpoint6dof_s ScPositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN, NAN}, {NAN, NAN, NAN}};
+
+void ScPositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
+{
+	_gain_vel_p = P;
+	_gain_vel_i = I;
+	_gain_vel_d = D;
+}
+
+void ScPositionControl::setPositionGains(const Vector3f &P, const Vector3f &I)
+{
+	_gain_pos_p = P;
+	_gain_pos_i = I;
+}
+
+void ScPositionControl::setPositionIntegralLimits(const float lim)
+{
+	_pos_int_lim = lim;
+}
+
+void ScPositionControl::setVelocityIntegralLimits(const float lim)
+{
+	_vel_int_lim = lim;
+}
+
+void ScPositionControl::setVelocityLimits(const float vel_limit)
+{
+	_lim_vel = vel_limit;
+}
+
+void ScPositionControl::setThrustLimit(const float max)
+{
+	_lim_thr_max = max;
+}
+
+void ScPositionControl::setState(const PositionControlStates &states)
+{
+	_pos = states.position;
+	_vel = states.velocity;
+	_vel_dot = states.acceleration;
+	_att_q = states.quaternion;
+}
+
+void ScPositionControl::setInputSetpoint(const trajectory_setpoint6dof_s &setpoint)
+{
+	_pos_sp = Vector3f(setpoint.position);
+	_vel_sp = Vector3f(setpoint.velocity);
+	_acc_sp = Vector3f(setpoint.acceleration);
+	_quat_sp = Quatf(setpoint.quaternion);
+}
+
+bool ScPositionControl::update(const float dt)
+{
+	bool valid = _inputValid();
+
+	if (valid) {
+		_positionControl(dt);
+		_velocityControl(dt);
+	}
+
+	// There has to be a valid output acceleration and thrust setpoint otherwise something went wrong
+	return valid && _acc_sp.isAllFinite() && _thr_sp.isAllFinite();
+}
+
+void ScPositionControl::_positionControl(const float dt)
+{
+	// Constrain vertical velocity integral
+	_pos_int(0) = math::constrain(_vel_int(0), -_pos_int_lim, _pos_int_lim);
+	_pos_int(1) = math::constrain(_vel_int(1), -_pos_int_lim, _pos_int_lim);
+	_pos_int(2) = math::constrain(_vel_int(2), -_pos_int_lim, _pos_int_lim);
+
+	// P-position controller
+	ControlMath::setZeroIfNanVector3f(_pos_sp);
+	Vector3f pos_error = _pos_sp - _pos;
+	Vector3f vel_sp_position = pos_error.emult(_gain_pos_p) + _pos_int;
+
+	// Update integral part of position control
+	_vel_int += pos_error.emult(_gain_pos_i) * dt;
+
+	// Position and feed-forward velocity setpoints or position states being NAN results in them not having an influence
+	ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position);
+	// make sure there are no NAN elements for further reference while constraining
+	ControlMath::setZeroIfNanVector3f(vel_sp_position);
+
+	// Constrain velocity setpoints
+	_vel_sp(0) = math::constrain(_vel_sp(0), -_lim_vel, _lim_vel);
+	_vel_sp(1) = math::constrain(_vel_sp(1), -_lim_vel, _lim_vel);
+	_vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel, _lim_vel);
+
+}
+
+void ScPositionControl::_velocityControl(const float dt)
+{
+	// Constrain vertical velocity integral
+	// _vel_int(2) = math::constrain(_vel_int(2), -CONSTANTS_ONE_G, CONSTANTS_ONE_G);
+	// Constrain vertical velocity integral
+	_vel_int(0) = math::constrain(_vel_int(0), -_vel_int_lim, _vel_int_lim);
+	_vel_int(1) = math::constrain(_vel_int(1), -_vel_int_lim, _vel_int_lim);
+	_vel_int(2) = math::constrain(_vel_int(2), -_vel_int_lim, _vel_int_lim);
+
+
+	// PID velocity control
+	Vector3f vel_error = _vel_sp - _vel;
+	Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d);
+
+	// No control input from setpoints or corresponding states which are NAN
+	ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);
+
+	// Accelaration to Thrust
+	_thr_sp = _acc_sp;
+	_thr_sp(0) = math::constrain(_thr_sp(0), -_lim_thr_max, _lim_thr_max);
+	_thr_sp(1) = math::constrain(_thr_sp(1), -_lim_thr_max, _lim_thr_max);
+	_thr_sp(2) = math::constrain(_thr_sp(2), -_lim_thr_max, _lim_thr_max);
+
+	// Make sure integral doesn't get NAN
+	ControlMath::setZeroIfNanVector3f(vel_error);
+
+	// Update integral part of velocity control
+	_vel_int += vel_error.emult(_gain_vel_i) * dt;
+}
+
+bool ScPositionControl::_inputValid()
+{
+	bool valid = true;
+
+	// Every axis x, y, z needs to have some setpoint
+	for (int i = 0; i <= 2; i++) {
+		valid = valid && (PX4_ISFINITE(_pos_sp(i)) || PX4_ISFINITE(_vel_sp(i)) || PX4_ISFINITE(_acc_sp(i)));
+	}
+
+	// x and y input setpoints always have to come in pairs
+	valid = valid && (PX4_ISFINITE(_pos_sp(0)) == PX4_ISFINITE(_pos_sp(1)));
+	valid = valid && (PX4_ISFINITE(_vel_sp(0)) == PX4_ISFINITE(_vel_sp(1)));
+	valid = valid && (PX4_ISFINITE(_acc_sp(0)) == PX4_ISFINITE(_acc_sp(1)));
+
+	// For each controlled state the estimate has to be valid
+	for (int i = 0; i <= 2; i++) {
+		if (PX4_ISFINITE(_pos_sp(i))) {
+			valid = valid && PX4_ISFINITE(_pos(i));
+		}
+
+		if (PX4_ISFINITE(_vel_sp(i))) {
+			valid = valid && PX4_ISFINITE(_vel(i)) && PX4_ISFINITE(_vel_dot(i));
+		}
+	}
+
+	return valid;
+}
+
+void ScPositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint,
+		vehicle_attitude_s &v_att) const
+{
+	// Set thrust setpoint
+	const Dcmf R_to_body(Quatf(v_att.q).inversed());
+	matrix::Vector3f b_thr_sp = R_to_body * _thr_sp;
+	attitude_setpoint.thrust_body[0] = b_thr_sp(0);
+	attitude_setpoint.thrust_body[1] = b_thr_sp(1);
+	attitude_setpoint.thrust_body[2] = b_thr_sp(2);
+
+	// Bypass attitude control by giving same attitude setpoint to att control
+	if (PX4_ISFINITE(_quat_sp(0)) && PX4_ISFINITE(_quat_sp(1)) && PX4_ISFINITE(_quat_sp(2)) && PX4_ISFINITE(_quat_sp(3))) {
+		attitude_setpoint.q_d[0] = _quat_sp(0);
+		attitude_setpoint.q_d[1] = _quat_sp(1);
+		attitude_setpoint.q_d[2] = _quat_sp(2);
+		attitude_setpoint.q_d[3] = _quat_sp(3);
+
+	} else {
+		attitude_setpoint.q_d[0] = v_att.q[0];
+		attitude_setpoint.q_d[1] = v_att.q[1];
+		attitude_setpoint.q_d[2] = v_att.q[2];
+		attitude_setpoint.q_d[3] = v_att.q[3];
+	}
+}
@@ -0,0 +1,206 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 - 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file PositionControl.hpp
+ *
+ * A cascaded position controller for position/velocity control only.
+ */
+
+#pragma once
+
+#include <lib/mathlib/mathlib.h>
+#include <matrix/matrix/math.hpp>
+#include <uORB/topics/trajectory_setpoint6dof.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+
+struct PositionControlStates {
+	matrix::Vector3f position;
+	matrix::Vector3f velocity;
+	matrix::Vector3f acceleration;
+	matrix::Quatf quaternion;  // bypassed to attitude controller
+};
+
+/**
+ * 	Core Position-Control for spacecrafts.
+ * 	This class contains P-controller for position and
+ * 	PID-controller for velocity.
+ *
+ * 	Inputs:
+ * 		vehicle position/velocity/attitude
+ * 		desired set-point position/velocity/thrust/attitude
+ * 	Output
+ * 		thrust vector and quaternion for attitude control
+ *
+ * 	A setpoint that is NAN is considered as not set.
+ * 	If there is a position/velocity- and thrust-setpoint present, then
+ *  the thrust-setpoint is ommitted and recomputed from position-velocity-PID-loop.
+ */
+class ScPositionControl
+{
+public:
+
+	ScPositionControl() = default;
+	~ScPositionControl() = default;
+
+	/**
+	 * Set the position control gains
+	 * @param P 3D vector of proportional gains for x,y,z axis
+	 * @param I 3D vector of integral gains for x,y,z axis
+	 */
+	void setPositionGains(const matrix::Vector3f &P, const matrix::Vector3f &I);
+
+	/**
+	 * @brief Set the Position Integral Limits object
+	 *
+	 * @param lim float limit to be set (on all axis)
+	 */
+	void setPositionIntegralLimits(const float lim);
+
+	/**
+	 * Set the velocity control gains
+	 * @param P 3D vector of proportional gains for x,y,z axis
+	 * @param I 3D vector of integral gains
+	 * @param D 3D vector of derivative gains
+	 */
+	void setVelocityGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
+
+	/**
+	 * Set the maximum velocity to execute with feed forward and position control
+	 * @param vel_limit velocity limit
+	 */
+	void setVelocityLimits(const float vel_limit);
+
+	/**
+	 * @brief Set the Velocity Integral Limits object
+	 *
+	 * @param lim float limit to be set (on all axis)
+	 */
+	void setVelocityIntegralLimits(const float lim);
+
+	/**
+	 * Set the minimum and maximum collective normalized thrust [0,1] that can be output by the controller
+	 * @param min minimum thrust e.g. 0.1 or 0
+	 * @param max maximum thrust e.g. 0.9 or 1
+	 */
+	void setThrustLimit(const float max);
+
+	/**
+	 * Pass the current vehicle state to the controller
+	 * @param PositionControlStates structure
+	 */
+	void setState(const PositionControlStates &states);
+
+	/**
+	 * Pass the desired setpoints
+	 * Note: NAN value means no feed forward/leave state uncontrolled if there's no higher order setpoint.
+	 * @param setpoint setpoints including feed-forwards to execute in update()
+	 */
+	void setInputSetpoint(const trajectory_setpoint6dof_s &setpoint);
+
+	/**
+	 * Apply P-position and PID-velocity controller that updates the member
+	 * thrust, yaw- and yawspeed-setpoints.
+	 * @see _thr_sp
+	 * @see _yaw_sp
+	 * @see _yawspeed_sp
+	 * @param dt time in seconds since last iteration
+	 * @return true if update succeeded and output setpoint is executable, false if not
+	 */
+	bool update(const float dt);
+
+	/**
+	 * Set the integral term in xy to 0.
+	 * @see _vel_int
+	 */
+	void resetIntegral()
+	{
+		_pos_int.setZero();
+		_vel_int.setZero();
+	}
+
+	/**
+	 * Get the controllers output attitude setpoint
+	 * This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control.
+	 * It needs to be executed by the attitude controller to achieve velocity and position tracking.
+	 * @param attitude_setpoint reference to struct to fill up
+	 */
+	void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, vehicle_attitude_s &v_att) const;
+
+	/**
+	 * All setpoints are set to NAN (uncontrolled). Timestampt zero.
+	 */
+	static const trajectory_setpoint6dof_s empty_trajectory_setpoint;
+
+private:
+	// The range limits of the hover thrust configuration/estimate
+	static constexpr float HOVER_THRUST_MIN = 0.05f;
+	static constexpr float HOVER_THRUST_MAX = 0.9f;
+
+	bool _inputValid();
+
+	void _positionControl(const float dt); ///< Position PI control
+	void _velocityControl(const float dt); ///< Velocity PID control
+
+	// Gains
+	matrix::Vector3f _gain_pos_p; ///< Position control proportional gain
+	matrix::Vector3f _gain_pos_i; ///< Position control integral gain
+	matrix::Vector3f _gain_vel_p; ///< Velocity control proportional gain
+	matrix::Vector3f _gain_vel_i; ///< Velocity control integral gain
+	matrix::Vector3f _gain_vel_d; ///< Velocity control derivative gain
+
+	// Limits
+	float _lim_vel{}; ///< Horizontal velocity limit with feed forward and position control
+	float _lim_thr_min{}; ///< Minimum collective thrust allowed as output [-1,0] e.g. -0.9
+	float _lim_thr_max{}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1
+	float _pos_int_lim{}; ///< Anti-windup for position control
+	float _vel_int_lim{}; ///< Anti-windup for velocity control
+
+	// States
+	matrix::Vector3f _pos; /**< current position */
+	matrix::Vector3f _pos_int; /**< integral term of the position controller */
+	matrix::Vector3f _vel; /**< current velocity */
+	matrix::Vector3f _vel_dot; /**< velocity derivative (replacement for acceleration estimate) */
+	matrix::Vector3f _vel_int; /**< integral term of the velocity controller */
+	matrix::Quatf _att_q; /**< current attitude */
+	float _yaw{}; /**< current heading */
+
+	// Setpoints
+	matrix::Vector3f _pos_sp; /**< desired position */
+	matrix::Vector3f _vel_sp; /**< desired velocity */
+	matrix::Vector3f _acc_sp; /**< desired acceleration */
+	matrix::Vector3f _thr_sp; /**< desired thrust */
+	matrix::Quatf _quat_sp;   /**< desired attitude */
+};
@@ -0,0 +1,258 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include <cmath>
+
+#include <gtest/gtest.h>
+#include <ControlMath.hpp>
+#include <px4_platform_common/defines.h>
+
+using namespace matrix;
+using namespace ControlMath;
+
+TEST(ControlMathTest, LimitTiltUnchanged)
+{
+	Vector3f body = Vector3f(0.f, 0.f, 1.f).normalized();
+	Vector3f body_before = body;
+	limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f);
+	EXPECT_EQ(body, body_before);
+
+	body = Vector3f(0.f, .1f, 1.f).normalized();
+	body_before = body;
+	limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f);
+	EXPECT_EQ(body, body_before);
+}
+
+TEST(ControlMathTest, LimitTiltOpposite)
+{
+	Vector3f body = Vector3f(0.f, 0.f, -1.f).normalized();
+	limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f);
+	float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f)));
+	EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 45.f, 1e-4f);
+	EXPECT_FLOAT_EQ(body.length(), 1.f);
+}
+
+TEST(ControlMathTest, LimitTiltAlmostOpposite)
+{
+	// This case doesn't trigger corner case handling but is very close to it
+	Vector3f body = Vector3f(0.001f, 0.f, -1.f).normalized();
+	limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f);
+	float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f)));
+	EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 45.f, 1e-4f);
+	EXPECT_FLOAT_EQ(body.length(), 1.f);
+}
+
+TEST(ControlMathTest, LimitTilt45degree)
+{
+	Vector3f body = Vector3f(1.f, 0.f, 0.f);
+	limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f);
+	EXPECT_EQ(body, Vector3f(M_SQRT1_2_F, 0, M_SQRT1_2_F));
+
+	body = Vector3f(0.f, 1.f, 0.f);
+	limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f);
+	EXPECT_EQ(body, Vector3f(0.f, M_SQRT1_2_F, M_SQRT1_2_F));
+}
+
+TEST(ControlMathTest, LimitTilt10degree)
+{
+	Vector3f body = Vector3f(1.f, 1.f, .1f).normalized();
+	limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 10.f);
+	float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f)));
+	EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 10.f, 1e-4f);
+	EXPECT_FLOAT_EQ(body.length(), 1.f);
+	EXPECT_FLOAT_EQ(body(0), body(1));
+
+	body = Vector3f(1, 2, .2f);
+	limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 10.f);
+	angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f)));
+	EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 10.f, 1e-4f);
+	EXPECT_FLOAT_EQ(body.length(), 1.f);
+	EXPECT_FLOAT_EQ(2.f * body(0), body(1));
+}
+
+TEST(ControlMathTest, ThrottleAttitudeMapping)
+{
+	/* expected: zero roll, zero pitch, zero yaw, full thr mag
+	 * reason: thrust pointing full upward */
+	Vector3f thr{0.f, 0.f, -1.f};
+	float yaw = 0.f;
+	vehicle_attitude_setpoint_s att{};
+	thrustToAttitude(thr, yaw, att);
+	EXPECT_FLOAT_EQ(att.roll_body, 0.f);
+	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
+	EXPECT_FLOAT_EQ(att.yaw_body, 0.f);
+	EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
+
+	/* expected: same as before but with 90 yaw
+	 * reason: only yaw changed */
+	yaw = M_PI_2_F;
+	thrustToAttitude(thr, yaw, att);
+	EXPECT_FLOAT_EQ(att.roll_body, 0.f);
+	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
+	EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
+	EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
+
+	/* expected: same as before but roll 180
+	 * reason: thrust points straight down and order Euler
+	 * order is: 1. roll, 2. pitch, 3. yaw */
+	thr = Vector3f(0.f, 0.f, 1.f);
+	thrustToAttitude(thr, yaw, att);
+	EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F);
+	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
+	EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
+	EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
+}
+
+TEST(ControlMathTest, ConstrainXYPriorities)
+{
+	const float max = 5.f;
+	// v0 already at max
+	Vector2f v0(max, 0.f);
+	Vector2f v1(v0(1), -v0(0));
+
+	Vector2f v_r = constrainXY(v0, v1, max);
+	EXPECT_FLOAT_EQ(v_r(0), max);
+	EXPECT_FLOAT_EQ(v_r(1), 0.f);
+
+	// norm of v1 exceeds max but v0 is zero
+	v0.zero();
+	v_r = constrainXY(v0, v1, max);
+	EXPECT_FLOAT_EQ(v_r(1), -max);
+	EXPECT_FLOAT_EQ(v_r(0), 0.f);
+
+	v0 = Vector2f(.5f, .5f);
+	v1 = Vector2f(.5f, -.5f);
+	v_r = constrainXY(v0, v1, max);
+	const float diff = Vector2f(v_r - (v0 + v1)).length();
+	EXPECT_FLOAT_EQ(diff, 0.f);
+
+	// v0 and v1 exceed max and are perpendicular
+	v0 = Vector2f(4.f, 0.f);
+	v1 = Vector2f(0.f, -4.f);
+	v_r = constrainXY(v0, v1, max);
+	EXPECT_FLOAT_EQ(v_r(0), v0(0));
+	EXPECT_GT(v_r(0), 0.f);
+	const float remaining = sqrtf(max * max - (v0(0) * v0(0)));
+	EXPECT_FLOAT_EQ(v_r(1), -remaining);
+}
+
+TEST(ControlMathTest, CrossSphereLine)
+{
+	/* Testing 9 positions (+) around waypoints (o):
+	 *
+	 * Far             +              +              +
+	 *
+	 * Near            +              +              +
+	 * On trajectory --+----o---------+---------o----+--
+	 *                    prev                curr
+	 *
+	 * Expected targets (1, 2, 3):
+	 * Far             +              +              +
+	 *
+	 *
+	 * On trajectory -------1---------2---------3-------
+	 *
+	 *
+	 * Near            +              +              +
+	 * On trajectory -------o---1---------2-----3-------
+	 *
+	 *
+	 * On trajectory --+----o----1----+--------2/3---+-- */
+	const Vector3f prev = Vector3f(0.f, 0.f, 0.f);
+	const Vector3f curr = Vector3f(0.f, 0.f, 2.f);
+	Vector3f res;
+	bool retval = false;
+
+	// on line, near, before previous waypoint
+	retval = cross_sphere_line(Vector3f(0.f, 0.f, -.5f), 1.f, prev, curr, res);
+	EXPECT_TRUE(retval);
+	EXPECT_EQ(res, Vector3f(0.f, 0.f, 0.5f));
+
+	// on line, near, before target waypoint
+	retval = cross_sphere_line(Vector3f(0.f, 0.f, 1.f), 1.f, prev, curr, res);
+	EXPECT_TRUE(retval);
+	EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f));
+
+	// on line, near, after target waypoint
+	retval = cross_sphere_line(Vector3f(0.f, 0.f, 2.5f), 1.f, prev, curr, res);
+	EXPECT_TRUE(retval);
+	EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f));
+
+	// near, before previous waypoint
+	retval = cross_sphere_line(Vector3f(0.f, .5f, -.5f), 1.f, prev, curr, res);
+	EXPECT_TRUE(retval);
+	EXPECT_EQ(res, Vector3f(0.f, 0.f, .366025388f));
+
+	// near, before target waypoint
+	retval = cross_sphere_line(Vector3f(0.f, .5f, 1.f), 1.f, prev, curr, res);
+	EXPECT_TRUE(retval);
+	EXPECT_EQ(res, Vector3f(0.f, 0.f, 1.866025448f));
+
+	// near, after target waypoint
+	retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, .5f, 2.5f), 1.f, prev, curr, res);
+	EXPECT_TRUE(retval);
+	EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f));
+
+	// far, before previous waypoint
+	retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, -.5f), 1.f, prev, curr, res);
+	EXPECT_FALSE(retval);
+	EXPECT_EQ(res, Vector3f());
+
+	// far, before target waypoint
+	retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, 1.f), 1.f, prev, curr, res);
+	EXPECT_FALSE(retval);
+	EXPECT_EQ(res, Vector3f(0.f, 0.f, 1.f));
+
+	// far, after target waypoint
+	retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, 2.5f), 1.f, prev, curr, res);
+	EXPECT_FALSE(retval);
+	EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f));
+}
+
+TEST(ControlMathTest, addIfNotNan)
+{
+	float v = 1.f;
+	// regular addition
+	ControlMath::addIfNotNan(v, 2.f);
+	EXPECT_EQ(v, 3.f);
+	// addition is NAN and has no influence
+	ControlMath::addIfNotNan(v, NAN);
+	EXPECT_EQ(v, 3.f);
+	v = NAN;
+	// both summands are NAN
+	ControlMath::addIfNotNan(v, NAN);
+	EXPECT_TRUE(std::isnan(v));
+	// regular value gets added to NAN and overwrites it
+	ControlMath::addIfNotNan(v, 3.f);
+	EXPECT_EQ(v, 3.f);
+}
@@ -0,0 +1,368 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <PositionControl.hpp>
+#include <px4_defines.h>
+
+using namespace matrix;
+
+TEST(PositionControlTest, EmptySetpoint)
+{
+	ScPositionControl position_control;
+
+	vehicle_attitude_setpoint_s attitude{};
+	position_control.getAttitudeSetpoint(attitude);
+	EXPECT_FLOAT_EQ(attitude.roll_body, 0.f);
+	EXPECT_FLOAT_EQ(attitude.pitch_body, 0.f);
+	EXPECT_FLOAT_EQ(attitude.yaw_body, 0.f);
+	EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f);
+	EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f));
+	EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f));
+	EXPECT_EQ(attitude.reset_integral, false);
+	EXPECT_EQ(attitude.fw_control_yaw_wheel, false);
+}
+
+class PositionControlBasicTest : public ::testing::Test
+{
+public:
+	PositionControlBasicTest()
+	{
+		_position_control.setPositionGains(Vector3f(1.f, 1.f, 1.f));
+		_position_control.setVelocityGains(Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f));
+		_position_control.setVelocityLimits(1.f, 1.f, 1.f);
+		_position_control.setThrustLimits(0.1f, MAXIMUM_THRUST);
+		_position_control.setHorizontalThrustMargin(HORIZONTAL_THRUST_MARGIN);
+		_position_control.setTiltLimit(1.f);
+		_position_control.setHoverThrust(.5f);
+	}
+
+	bool runController()
+	{
+		_position_control.setInputSetpoint(_input_setpoint);
+		const bool ret = _position_control.update(.1f);
+		_position_control.getAttitudeSetpoint(_attitude);
+		return ret;
+	}
+
+	ScPositionControl _position_control;
+	trajectory_setpoint_s _input_setpoint{PositionControl::empty_trajectory_setpoint};
+	vehicle_local_position_setpoint_s _output_setpoint{};
+	vehicle_attitude_setpoint_s _attitude{};
+
+	static constexpr float MAXIMUM_THRUST = 0.9f;
+	static constexpr float HORIZONTAL_THRUST_MARGIN = 0.3f;
+};
+
+class PositionControlBasicDirectionTest : public PositionControlBasicTest
+{
+public:
+	void checkDirection()
+	{
+		Vector3f thrust(_output_setpoint.thrust);
+		EXPECT_GT(thrust(0), 0.f);
+		EXPECT_GT(thrust(1), 0.f);
+		EXPECT_LT(thrust(2), 0.f);
+
+		Vector3f body_z = Quatf(_attitude.q_d).dcm_z();
+		EXPECT_LT(body_z(0), 0.f);
+		EXPECT_LT(body_z(1), 0.f);
+		EXPECT_GT(body_z(2), 0.f);
+	}
+};
+
+TEST_F(PositionControlBasicDirectionTest, PositionDirection)
+{
+	Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.position);
+	EXPECT_TRUE(runController());
+	checkDirection();
+}
+
+TEST_F(PositionControlBasicDirectionTest, VelocityDirection)
+{
+	Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.velocity);
+	EXPECT_TRUE(runController());
+	checkDirection();
+}
+
+TEST_F(PositionControlBasicTest, TiltLimit)
+{
+	Vector3f(10.f, 10.f, 0.f).copyTo(_input_setpoint.position);
+
+	EXPECT_TRUE(runController());
+	Vector3f body_z = Quatf(_attitude.q_d).dcm_z();
+	float angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f)));
+	EXPECT_GT(angle, 0.f);
+	EXPECT_LE(angle, 1.f);
+
+	_position_control.setTiltLimit(0.5f);
+	EXPECT_TRUE(runController());
+	body_z = Quatf(_attitude.q_d).dcm_z();
+	angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f)));
+	EXPECT_GT(angle, 0.f);
+	EXPECT_LE(angle, .50001f);
+
+	_position_control.setTiltLimit(1.f);  // restore original
+}
+
+TEST_F(PositionControlBasicTest, VelocityLimit)
+{
+	Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position);
+
+	EXPECT_TRUE(runController());
+	Vector2f velocity_xy(_output_setpoint.vx, _output_setpoint.vy);
+	EXPECT_LE(velocity_xy.norm(), 1.f);
+	EXPECT_LE(abs(_output_setpoint.vz), 1.f);
+}
+
+TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit)
+{
+	// Given a setpoint that drives the controller into vertical and horizontal saturation
+	Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position);
+
+	// When you run it for one iteration
+	runController();
+	Vector3f thrust(_output_setpoint.thrust);
+
+	// Then the thrust vector length is limited by the maximum
+	EXPECT_FLOAT_EQ(thrust.norm(), MAXIMUM_THRUST);
+
+	// Then the horizontal thrust is limited by its margin
+	EXPECT_FLOAT_EQ(thrust(0), HORIZONTAL_THRUST_MARGIN / sqrt(2.f));
+	EXPECT_FLOAT_EQ(thrust(1), HORIZONTAL_THRUST_MARGIN / sqrt(2.f));
+	EXPECT_FLOAT_EQ(thrust(2),
+			-sqrt(MAXIMUM_THRUST * MAXIMUM_THRUST - HORIZONTAL_THRUST_MARGIN * HORIZONTAL_THRUST_MARGIN));
+	thrust.print();
+
+	// Then the collective thrust is limited by the maximum
+	EXPECT_EQ(_attitude.thrust_body[0], 0.f);
+	EXPECT_EQ(_attitude.thrust_body[1], 0.f);
+	EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -MAXIMUM_THRUST);
+
+	// Then the horizontal margin results in a tilt with the ratio of: horizontal margin / maximum thrust
+	EXPECT_FLOAT_EQ(_attitude.roll_body, asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST));
+	// TODO: add this line back once attitude setpoint generation strategy does not align body yaw with heading all the time anymore
+	// EXPECT_FLOAT_EQ(_attitude.pitch_body, -asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST));
+}
+
+TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit)
+{
+	Vector3f(10.f, 0.f, 10.f).copyTo(_input_setpoint.position);
+
+	runController();
+	Vector3f thrust(_output_setpoint.thrust);
+	EXPECT_FLOAT_EQ(thrust.length(), 0.1f);
+
+	EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.1f);
+
+	EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f);
+	EXPECT_FLOAT_EQ(_attitude.pitch_body, -1.f);
+}
+
+TEST_F(PositionControlBasicTest, FailsafeInput)
+{
+	_input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f;
+	_input_setpoint.velocity[2] = .1f;
+
+	EXPECT_TRUE(runController());
+	EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f);
+	EXPECT_FLOAT_EQ(_attitude.thrust_body[1], 0.f);
+	EXPECT_LT(_output_setpoint.thrust[2], -.1f);
+	EXPECT_GT(_output_setpoint.thrust[2], -.5f);
+	EXPECT_GT(_attitude.thrust_body[2], -.5f);
+	EXPECT_LE(_attitude.thrust_body[2], -.1f);
+}
+
+TEST_F(PositionControlBasicTest, IdleThrustInput)
+{
+	// High downwards acceleration to make sure there's no thrust
+	Vector3f(0.f, 0.f, 100.f).copyTo(_input_setpoint.acceleration);
+
+	EXPECT_TRUE(runController());
+	EXPECT_FLOAT_EQ(_output_setpoint.thrust[0], 0.f);
+	EXPECT_FLOAT_EQ(_output_setpoint.thrust[1], 0.f);
+	EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f); // minimum thrust
+}
+
+TEST_F(PositionControlBasicTest, InputCombinationsPosition)
+{
+	Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position);
+
+	EXPECT_TRUE(runController());
+	EXPECT_FLOAT_EQ(_output_setpoint.x, .1f);
+	EXPECT_FLOAT_EQ(_output_setpoint.y, .2f);
+	EXPECT_FLOAT_EQ(_output_setpoint.z, .3f);
+	EXPECT_FALSE(isnan(_output_setpoint.vx));
+	EXPECT_FALSE(isnan(_output_setpoint.vy));
+	EXPECT_FALSE(isnan(_output_setpoint.vz));
+	EXPECT_FALSE(isnan(_output_setpoint.thrust[0]));
+	EXPECT_FALSE(isnan(_output_setpoint.thrust[1]));
+	EXPECT_FALSE(isnan(_output_setpoint.thrust[2]));
+}
+
+TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity)
+{
+	_input_setpoint.velocity[0] = .1f;
+	_input_setpoint.velocity[1] = .2f;
+	_input_setpoint.position[2] = .3f; // altitude
+
+	EXPECT_TRUE(runController());
+	EXPECT_TRUE(isnan(_output_setpoint.x));
+	EXPECT_TRUE(isnan(_output_setpoint.y));
+	EXPECT_FLOAT_EQ(_output_setpoint.z, .3f);
+	EXPECT_FLOAT_EQ(_output_setpoint.vx, .1f);
+	EXPECT_FLOAT_EQ(_output_setpoint.vy, .2f);
+	EXPECT_FALSE(isnan(_output_setpoint.vz));
+	EXPECT_FALSE(isnan(_output_setpoint.thrust[0]));
+	EXPECT_FALSE(isnan(_output_setpoint.thrust[1]));
+	EXPECT_FALSE(isnan(_output_setpoint.thrust[2]));
+}
+
+TEST_F(PositionControlBasicTest, SetpointValiditySimple)
+{
+	EXPECT_FALSE(runController());
+	_input_setpoint.position[0] = .1f;
+	EXPECT_FALSE(runController());
+	_input_setpoint.position[1] = .2f;
+	EXPECT_FALSE(runController());
+	_input_setpoint.acceleration[2] = .3f;
+	EXPECT_TRUE(runController());
+}
+
+TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
+{
+	// This test runs any combination of set and unset (NAN) setpoints and checks if it gets accepted or rejected correctly
+	float *const setpoint_loop_access_map[] = {&_input_setpoint.position[0], &_input_setpoint.velocity[0], &_input_setpoint.acceleration[0],
+						   &_input_setpoint.position[1], &_input_setpoint.velocity[1], &_input_setpoint.acceleration[1],
+						   &_input_setpoint.position[2], &_input_setpoint.velocity[2], &_input_setpoint.acceleration[2]
+						  };
+
+	for (int combination = 0; combination < 512; combination++) {
+		_input_setpoint = PositionControl::empty_trajectory_setpoint;
+
+		for (int j = 0; j < 9; j++) {
+			if (combination & (1 << j)) {
+				// Set arbitrary finite value, some values clearly hit the limits to check these corner case combinations
+				*(setpoint_loop_access_map[j]) = static_cast<float>(combination) / static_cast<float>(j + 1);
+			}
+		}
+
+		// Expect at least one setpoint per axis
+		const bool has_x_setpoint = ((combination & 7) != 0);
+		const bool has_y_setpoint = (((combination >> 3) & 7) != 0);
+		const bool has_z_setpoint = (((combination >> 6) & 7) != 0);
+		// Expect xy setpoints to come in pairs
+		const bool has_xy_pairs = (combination & 7) == ((combination >> 3) & 7);
+		const bool expected_result = has_x_setpoint && has_y_setpoint && has_z_setpoint && has_xy_pairs;
+
+		EXPECT_EQ(runController(), expected_result) << "combination " << combination << std::endl
+				<< "input" << std::endl
+				<< "position     " << _input_setpoint.position[0] << ", "
+				<< _input_setpoint.position[1] << ", " << _input_setpoint.position[2] << std::endl
+				<< "velocity     " << _input_setpoint.velocity[0] << ", "
+				<< _input_setpoint.velocity[1] << ", " << _input_setpoint.velocity[2] << std::endl
+				<< "acceleration " << _input_setpoint.acceleration[0] << ", "
+				<< _input_setpoint.acceleration[1] << ", " << _input_setpoint.acceleration[2] << std::endl
+				<< "output" << std::endl
+				<< "position     " << _output_setpoint.x << ", " << _output_setpoint.y << ", " << _output_setpoint.z << std::endl
+				<< "velocity     " << _output_setpoint.vx << ", " << _output_setpoint.vy << ", " << _output_setpoint.vz << std::endl
+				<< "acceleration " << _output_setpoint.acceleration[0] << ", "
+				<< _output_setpoint.acceleration[1] << ", " << _output_setpoint.acceleration[2] << std::endl;
+	}
+}
+
+TEST_F(PositionControlBasicTest, InvalidState)
+{
+	Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position);
+
+	PositionControlStates states{};
+	states.position(0) = NAN;
+	_position_control.setState(states);
+	EXPECT_FALSE(runController());
+
+	states.velocity(0) = NAN;
+	_position_control.setState(states);
+	EXPECT_FALSE(runController());
+
+	states.position(0) = 0.f;
+	_position_control.setState(states);
+	EXPECT_FALSE(runController());
+
+	states.velocity(0) = 0.f;
+	states.acceleration(1) = NAN;
+	_position_control.setState(states);
+	EXPECT_FALSE(runController());
+}
+
+
+TEST_F(PositionControlBasicTest, UpdateHoverThrust)
+{
+	// GIVEN: some hover thrust and 0 velocity change
+	const float hover_thrust = 0.6f;
+	_position_control.setHoverThrust(hover_thrust);
+
+	Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity);
+
+	// WHEN: we run the controller
+	EXPECT_TRUE(runController());
+
+	// THEN: the output thrust equals the hover thrust
+	EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust);
+
+	// HOWEVER WHEN: we set a new hover thrust through the update function
+	const float hover_thrust_new = 0.7f;
+	_position_control.updateHoverThrust(hover_thrust_new);
+	EXPECT_TRUE(runController());
+
+	// THEN: the integral is updated to avoid discontinuities and
+	// the output is still the same
+	EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust);
+}
+
+TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint)
+{
+	// GIVEN: the controller was ran with an invalid setpoint containing some valid values
+	_input_setpoint.position[0] = .1f;
+	_input_setpoint.position[1] = .2f;
+	// all z-axis setpoints stay NAN
+	EXPECT_FALSE(runController());
+
+	// WHEN: we run the controller with a valid setpoint
+	_input_setpoint = PositionControl::empty_trajectory_setpoint;
+	Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity);
+	EXPECT_TRUE(runController());
+
+	// THEN: the integral did not wind up and produce unexpected deviation
+	EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f);
+	EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f);
+}
@@ -0,0 +1,401 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2020 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.
+ *
+ ****************************************************************************/
+
+#include "SpacecraftPositionControl.hpp"
+
+#include <float.h>
+#include <px4_platform_common/events.h>
+#include "PositionControl/ControlMath.hpp"
+
+using namespace matrix;
+
+SpacecraftPositionControl::SpacecraftPositionControl(ModuleParams *parent) : ModuleParams(parent),
+	_vehicle_attitude_setpoint_pub(ORB_ID(vehicle_attitude_setpoint))
+{
+	updateParams();
+}
+
+void SpacecraftPositionControl::updateParams()
+{
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		// update parameters from storage
+		ModuleParams::updateParams();
+
+		int num_changed = 0;
+
+		if (_param_sys_vehicle_resp.get() >= 0.f) {
+			// make it less sensitive at the lower end
+			float responsiveness = _param_sys_vehicle_resp.get() * _param_sys_vehicle_resp.get();
+
+			num_changed += _param_mpc_acc.commit_no_notification(math::lerp(1.f, 15.f, responsiveness));
+			num_changed += _param_mpc_acc_max.commit_no_notification(math::lerp(2.f, 15.f, responsiveness));
+			num_changed += _param_mpc_man_y_max.commit_no_notification(math::lerp(80.f, 450.f, responsiveness));
+
+			if (responsiveness > 0.6f) {
+				num_changed += _param_mpc_man_y_tau.commit_no_notification(0.f);
+
+			} else {
+				num_changed += _param_mpc_man_y_tau.commit_no_notification(math::lerp(0.5f, 0.f, responsiveness / 0.6f));
+			}
+
+			num_changed += _param_mpc_jerk_max.commit_no_notification(math::lerp(2.f, 50.f, responsiveness));
+			num_changed += _param_mpc_jerk_auto.commit_no_notification(math::lerp(1.f, 25.f, responsiveness));
+		}
+
+		if (_param_mpc_vel_all.get() >= 0.f) {
+			float all_vel = _param_mpc_vel_all.get();
+			num_changed += _param_mpc_vel_manual.commit_no_notification(all_vel);
+			num_changed += _param_mpc_vel_cruise.commit_no_notification(all_vel);
+			num_changed += _param_mpc_vel_max.commit_no_notification(all_vel);
+		}
+
+		if (num_changed > 0) {
+			param_notify_changes();
+		}
+
+		// Set PI and PID gains, as well as anti-windup limits
+		_control.setPositionGains(
+			Vector3f(_param_mpc_pos_p.get(), _param_mpc_pos_p.get(), _param_mpc_pos_p.get()),
+			Vector3f(_param_mpc_pos_i.get(), _param_mpc_pos_i.get(), _param_mpc_pos_i.get()));
+		_control.setPositionIntegralLimits(_param_mpc_pos_i_lim.get());
+		_control.setVelocityGains(
+			Vector3f(_param_mpc_vel_p_acc.get(), _param_mpc_vel_p_acc.get(), _param_mpc_vel_p_acc.get()),
+			Vector3f(_param_mpc_vel_i_acc.get(), _param_mpc_vel_i_acc.get(), _param_mpc_vel_i_acc.get()),
+			Vector3f(_param_mpc_vel_d_acc.get(), _param_mpc_vel_d_acc.get(), _param_mpc_vel_d_acc.get()));
+		_control.setVelocityIntegralLimits(_param_mpc_vel_i_lim.get());
+
+		// Check that the design parameters are inside the absolute maximum constraints
+		if (_param_mpc_vel_cruise.get() > _param_mpc_vel_max.get()) {
+			_param_mpc_vel_cruise.set(_param_mpc_vel_max.get());
+			_param_mpc_vel_cruise.commit();
+			mavlink_log_critical(&_mavlink_log_pub, "Cruise speed has been constrained by max speed\t");
+			/* EVENT
+			 * @description <param>SPC_VEL_CRUISE</param> is set to {1:.0}.
+			 */
+			events::send<float>(events::ID("sc_pos_ctrl_cruise_set"), events::Log::Warning,
+					    "Cruise speed has been constrained by maximum speed", _param_mpc_vel_max.get());
+		}
+
+		if (_param_mpc_vel_manual.get() > _param_mpc_vel_max.get()) {
+			_param_mpc_vel_manual.set(_param_mpc_vel_max.get());
+			_param_mpc_vel_manual.commit();
+			mavlink_log_critical(&_mavlink_log_pub, "Manual speed has been constrained by max speed\t");
+			/* EVENT
+			 * @description <param>SPC_VEL_MANUAL</param> is set to {1:.0}.
+			 */
+			events::send<float>(events::ID("sc_pos_ctrl_man_vel_set"), events::Log::Warning,
+					    "Manual speed has been constrained by maximum speed", _param_mpc_vel_max.get());
+		}
+
+		yaw_rate = math::radians(_param_mpc_man_y_max.get());
+	}
+}
+
+PositionControlStates SpacecraftPositionControl::set_vehicle_states(const vehicle_local_position_s
+		&vehicle_local_position, const vehicle_attitude_s &vehicle_attitude)
+{
+	PositionControlStates states;
+
+	const Vector2f position_xy(vehicle_local_position.x, vehicle_local_position.y);
+
+	// only set position states if valid and finite
+	if (vehicle_local_position.xy_valid && position_xy.isAllFinite()) {
+		states.position.xy() = position_xy;
+
+	} else {
+		states.position(0) = states.position(1) = NAN;
+	}
+
+	if (PX4_ISFINITE(vehicle_local_position.z) && vehicle_local_position.z_valid) {
+		states.position(2) = vehicle_local_position.z;
+
+	} else {
+		states.position(2) = NAN;
+	}
+
+	const Vector2f velocity_xy(vehicle_local_position.vx, vehicle_local_position.vy);
+
+	if (vehicle_local_position.v_xy_valid && velocity_xy.isAllFinite()) {
+		states.velocity.xy() = velocity_xy;
+
+	} else {
+		states.velocity(0) = states.velocity(1) = NAN;
+	}
+
+	if (PX4_ISFINITE(vehicle_local_position.vz) && vehicle_local_position.v_z_valid) {
+		states.velocity(2) = vehicle_local_position.vz;
+
+	} else {
+		states.velocity(2) = NAN;
+	}
+
+	if (PX4_ISFINITE(vehicle_attitude.q[0]) && PX4_ISFINITE(vehicle_attitude.q[1]) && PX4_ISFINITE(vehicle_attitude.q[2])
+	    && PX4_ISFINITE(vehicle_attitude.q[3])) {
+		states.quaternion = Quatf(vehicle_attitude.q);
+
+	} else {
+		states.quaternion = Quatf();
+	}
+
+	return states;
+}
+
+void SpacecraftPositionControl::updatePositionControl()
+{
+	vehicle_local_position_s vehicle_local_position;
+	vehicle_attitude_s v_att;
+
+	if (_local_pos_sub.update(&vehicle_local_position)) {
+		const float dt =
+			math::constrain(((vehicle_local_position.timestamp_sample - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f);
+		_time_stamp_last_loop = vehicle_local_position.timestamp_sample;
+
+		if (_vehicle_control_mode_sub.updated()) {
+			const bool previous_position_control_enabled = _vehicle_control_mode.flag_control_position_enabled;
+
+			if (_vehicle_control_mode_sub.update(&_vehicle_control_mode)) {
+				if (!previous_position_control_enabled && _vehicle_control_mode.flag_control_position_enabled) {
+					_time_position_control_enabled = _vehicle_control_mode.timestamp;
+
+				} else if (previous_position_control_enabled && !_vehicle_control_mode.flag_control_position_enabled) {
+					// clear existing setpoint when controller is no longer active
+					_setpoint = ScPositionControl::empty_trajectory_setpoint;
+				}
+			}
+		}
+
+		// TODO: check if setpoint is different than the previous one and reset integral then
+		// 		 _control.resetIntegral();
+		_trajectory_setpoint_sub.update(&_setpoint);
+		_vehicle_attitude_sub.update(&v_att);
+
+		// adjust existing (or older) setpoint with any EKF reset deltas
+		if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) {
+			if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
+				_setpoint.velocity[0] += vehicle_local_position.delta_vxy[0];
+				_setpoint.velocity[1] += vehicle_local_position.delta_vxy[1];
+			}
+
+			if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
+				_setpoint.velocity[2] += vehicle_local_position.delta_vz;
+			}
+
+			if (vehicle_local_position.xy_reset_counter != _xy_reset_counter) {
+				_setpoint.position[0] += vehicle_local_position.delta_xy[0];
+				_setpoint.position[1] += vehicle_local_position.delta_xy[1];
+			}
+
+			if (vehicle_local_position.z_reset_counter != _z_reset_counter) {
+				_setpoint.position[2] += vehicle_local_position.delta_z;
+			}
+
+			if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) {
+				// Set proper attitude setpoint with quaternion
+				// _setpoint.yaw = wrap_pi(_setpoint.yaw + vehicle_local_position.delta_heading);
+			}
+		}
+
+		// save latest reset counters
+		_vxy_reset_counter = vehicle_local_position.vxy_reset_counter;
+		_vz_reset_counter = vehicle_local_position.vz_reset_counter;
+		_xy_reset_counter = vehicle_local_position.xy_reset_counter;
+		_z_reset_counter = vehicle_local_position.z_reset_counter;
+		_heading_reset_counter = vehicle_local_position.heading_reset_counter;
+
+		PositionControlStates states{set_vehicle_states(vehicle_local_position, v_att)};
+
+		poll_manual_setpoint(dt, vehicle_local_position, v_att);
+
+		if (_vehicle_control_mode.flag_control_position_enabled) {
+			// set failsafe setpoint if there hasn't been a new
+			// trajectory setpoint since position control started
+			if ((_setpoint.timestamp < _time_position_control_enabled)
+			    && (vehicle_local_position.timestamp_sample > _time_position_control_enabled)) {
+				PX4_INFO("Setpoint time: %f, Vehicle local pos time: %f, Pos Control Enabled time: %f",
+					 (double)_setpoint.timestamp, (double)vehicle_local_position.timestamp_sample,
+					 (double)_time_position_control_enabled);
+				_setpoint = generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, false);
+			}
+		}
+
+		if (_vehicle_control_mode.flag_control_position_enabled
+		    && (_setpoint.timestamp >= _time_position_control_enabled)) {
+
+			_control.setThrustLimit(_param_mpc_thr_max.get());
+
+			_control.setVelocityLimits(_param_mpc_vel_max.get());
+
+			_control.setInputSetpoint(_setpoint);
+
+			_control.setState(states);
+
+			// Run position control
+			if (!_control.update(dt)) {
+				_control.setInputSetpoint(generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, true));
+				_control.setVelocityLimits(_param_mpc_vel_max.get());
+				_control.update(dt);
+			}
+
+			// Publish attitude setpoint output
+			vehicle_attitude_setpoint_s attitude_setpoint{};
+			_control.getAttitudeSetpoint(attitude_setpoint, v_att);
+			// PX4_INFO("States: %f %f %f / %f %f %f", (double)states.position(0), (double)states.position(1),
+			// 	 (double)states.position(2), (double)states.velocity(0), (double)states.velocity(1),
+			// 	 (double)states.velocity(2));
+			// PX4_INFO("Setpoint: %f %f %f / %f %f %f", (double)_setpoint.position[0], (double)_setpoint.position[1],
+			// 	 (double)_setpoint.position[2], (double)_setpoint.velocity[0], (double)_setpoint.velocity[1],
+			// 	 (double)_setpoint.velocity[2]);
+			// PX4_INFO("Control input: %f %f %f / %f %f %f %f", (double)attitude_setpoint.thrust_body[0], (double)attitude_setpoint.thrust_body[1],
+			// 	(double)attitude_setpoint.thrust_body[2], (double)attitude_setpoint.q_d[0], (double)attitude_setpoint.q_d[1],
+			// 	(double)attitude_setpoint.q_d[2], (double)attitude_setpoint.q_d[3]);
+			attitude_setpoint.timestamp = hrt_absolute_time();
+			_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
+
+			// publish setpoint
+			publishLocalPositionSetpoint(attitude_setpoint);
+		}
+	}
+}
+
+void SpacecraftPositionControl::publishLocalPositionSetpoint(vehicle_attitude_setpoint_s &_att_sp)
+{
+	// complete the setpoint data structure
+	vehicle_local_position_setpoint_s local_position_setpoint{};
+	local_position_setpoint.timestamp = hrt_absolute_time();
+
+	local_position_setpoint.x = _setpoint.position[0];
+	local_position_setpoint.y = _setpoint.position[1];
+	local_position_setpoint.z = _setpoint.position[2];
+	local_position_setpoint.vx = _setpoint.velocity[0];
+	local_position_setpoint.vy = _setpoint.velocity[1];
+	local_position_setpoint.vz = _setpoint.velocity[2];
+	local_position_setpoint.acceleration[0] = _setpoint.acceleration[0];
+	local_position_setpoint.acceleration[1] = _setpoint.acceleration[1];
+	local_position_setpoint.acceleration[2] = _setpoint.acceleration[2];
+	local_position_setpoint.thrust[0] = _att_sp.thrust_body[0];
+	local_position_setpoint.thrust[1] = _att_sp.thrust_body[1];
+	local_position_setpoint.thrust[2] = _att_sp.thrust_body[2];
+	_local_pos_sp_pub.publish(local_position_setpoint);
+}
+
+void SpacecraftPositionControl::poll_manual_setpoint(const float dt,
+		const vehicle_local_position_s &vehicle_local_position,
+		const vehicle_attitude_s &_vehicle_att)
+{
+	if (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_armed) {
+		if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) {
+			if (!_vehicle_control_mode.flag_control_offboard_enabled) {
+				if (_vehicle_control_mode.flag_control_attitude_enabled &&
+				    _vehicle_control_mode.flag_control_position_enabled) {
+					// We are in Stabilized mode
+					// Generate position setpoints
+					if (!stabilized_pos_sp_initialized) {
+						// Initialize position setpoint
+						target_pos_sp = Vector3f(vehicle_local_position.x, vehicle_local_position.y,
+									 vehicle_local_position.z);
+
+						const float vehicle_yaw = Eulerf(Quatf(_vehicle_att.q)).psi();
+						_manual_yaw_sp = vehicle_yaw;
+						stabilized_pos_sp_initialized = true;
+					}
+
+					// Update velocity setpoint
+					Vector3f target_vel_sp = Vector3f(_manual_control_setpoint.pitch, _manual_control_setpoint.roll, 0.0);
+					target_pos_sp = target_pos_sp + target_vel_sp * dt;
+
+					// Update _setpoint
+					_setpoint.position[0] = target_pos_sp(0);
+					_setpoint.position[1] = target_pos_sp(1);
+					_setpoint.position[2] = target_pos_sp(2);
+
+					_setpoint.velocity[0] = target_vel_sp(0);
+					_setpoint.velocity[1] = target_vel_sp(1);
+					_setpoint.velocity[2] = target_vel_sp(2);
+
+					// Generate attitude setpoints
+					float yaw_sp_move_rate = 0.0;
+
+					if (_manual_control_setpoint.throttle > -0.9f) {
+						yaw_sp_move_rate = _manual_control_setpoint.yaw * yaw_rate;
+					}
+
+					_manual_yaw_sp = wrap_pi(_manual_yaw_sp + yaw_sp_move_rate * dt);
+					const float roll_body = 0.0;
+					const float pitch_body = 0.0;
+
+					Quatf q_sp(Eulerf(roll_body, pitch_body, _manual_yaw_sp));
+					q_sp.copyTo(_setpoint.quaternion);
+
+					_setpoint.timestamp = hrt_absolute_time();
+
+				} else {
+					// We are in Manual mode
+					stabilized_pos_sp_initialized = false;
+				}
+
+			} else {
+				stabilized_pos_sp_initialized = false;
+			}
+
+			_manual_setpoint_last_called = hrt_absolute_time();
+		}
+	}
+}
+
+trajectory_setpoint6dof_s SpacecraftPositionControl::generateFailsafeSetpoint(const hrt_abstime &now,
+		const PositionControlStates &states, bool warn)
+{
+	// rate limit the warnings
+	warn = warn && (now - _last_warn) > 2_s;
+
+	if (warn) {
+		PX4_WARN("invalid setpoints");
+		_last_warn = now;
+	}
+
+	trajectory_setpoint6dof_s failsafe_setpoint = ScPositionControl::empty_trajectory_setpoint;
+	failsafe_setpoint.timestamp = now;
+
+	failsafe_setpoint.velocity[0] = failsafe_setpoint.velocity[1] = failsafe_setpoint.velocity[2] = 0.f;
+
+	if (warn) {
+		PX4_WARN("Failsafe: stop and wait");
+	}
+
+	return failsafe_setpoint;
+}
@@ -0,0 +1,177 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2020 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Multicopter position controller.
+ */
+
+#pragma once
+
+#include "PositionControl/PositionControl.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <lib/controllib/blocks.hpp>
+#include <lib/perf/perf_counter.h>
+#include <lib/slew_rate/SlewRateYaw.hpp>
+#include <lib/systemlib/mavlink_log.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/matrix/matrix/math.hpp>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/tasks.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/trajectory_setpoint6dof.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+
+using namespace time_literals;
+
+class SpacecraftPositionControl : public ModuleParams
+{
+public:
+	SpacecraftPositionControl(ModuleParams *parent);
+	~SpacecraftPositionControl() = default;
+
+	void updatePositionControl();
+
+protected:
+	/**
+	 * Update our local parameter cache.
+	 */
+	void updateParams();
+
+private:
+
+	orb_advert_t _mavlink_log_pub{nullptr};
+
+	uORB::Publication<vehicle_attitude_setpoint_s>	     _vehicle_attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)};
+	uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};	/**< vehicle local position setpoint publication */
+
+	uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)};	/**< vehicle local position */
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; 	/**< notification of manual control updates */
+
+	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint6dof)};
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+
+	hrt_abstime _time_stamp_last_loop{0};		/**< time stamp of last loop iteration */
+	hrt_abstime _time_position_control_enabled{0};
+	hrt_abstime _manual_setpoint_last_called{0};
+
+	trajectory_setpoint6dof_s 	_setpoint{ScPositionControl::empty_trajectory_setpoint};
+	vehicle_control_mode_s 		_vehicle_control_mode{};
+	manual_control_setpoint_s	_manual_control_setpoint{};			    /**< r/c channel data */
+
+	DEFINE_PARAMETERS(
+		// Position Control
+		(ParamFloat<px4::params::SPC_POS_P>)        _param_mpc_pos_p,
+		(ParamFloat<px4::params::SPC_POS_I>)        _param_mpc_pos_i,
+		(ParamFloat<px4::params::SPC_POS_I_LIM>)    _param_mpc_pos_i_lim,
+		(ParamFloat<px4::params::SPC_VEL_P>) 		_param_mpc_vel_p_acc,
+		(ParamFloat<px4::params::SPC_VEL_I>) 		_param_mpc_vel_i_acc,
+		(ParamFloat<px4::params::SPC_VEL_I_LIM>)    _param_mpc_vel_i_lim,
+		(ParamFloat<px4::params::SPC_VEL_D>) 		_param_mpc_vel_d_acc,
+		(ParamFloat<px4::params::SPC_VEL_ALL>)   	_param_mpc_vel_all,
+		(ParamFloat<px4::params::SPC_VEL_MAX>)   	_param_mpc_vel_max,
+		(ParamFloat<px4::params::SPC_VEL_CRUISE>)   _param_mpc_vel_cruise,
+		(ParamFloat<px4::params::SPC_VEL_MANUAL>)   _param_mpc_vel_manual,
+		(ParamFloat<px4::params::SPC_VEHICLE_RESP>) _param_sys_vehicle_resp,
+		(ParamFloat<px4::params::SPC_ACC>)      	_param_mpc_acc,
+		(ParamFloat<px4::params::SPC_ACC_MAX>)      _param_mpc_acc_max,
+		(ParamFloat<px4::params::SPC_MAN_Y_MAX>)    _param_mpc_man_y_max,
+		(ParamFloat<px4::params::SPC_MAN_Y_TAU>)    _param_mpc_man_y_tau,
+		(ParamFloat<px4::params::SPC_JERK_AUTO>)    _param_mpc_jerk_auto,
+		(ParamFloat<px4::params::SPC_JERK_MAX>)     _param_mpc_jerk_max,
+		(ParamFloat<px4::params::SPC_THR_MAX>)      _param_mpc_thr_max
+	);
+
+	matrix::Vector3f target_pos_sp;
+	float yaw_rate;
+	bool stabilized_pos_sp_initialized{false};
+
+	ScPositionControl _control;  /**< class for core PID position control */
+
+	hrt_abstime _last_warn{0}; /**< timer when the last warn message was sent out */
+
+	/** Timeout in us for trajectory data to get considered invalid */
+	static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms;
+
+	uint8_t _vxy_reset_counter{0};
+	uint8_t _vz_reset_counter{0};
+	uint8_t _xy_reset_counter{0};
+	uint8_t _z_reset_counter{0};
+	uint8_t _heading_reset_counter{0};
+
+	// Manual setpoints on yaw and reset
+	bool _reset_yaw_sp{true};
+	float _manual_yaw_sp{0.f};
+	float _throttle_control{0.f};
+	float _yaw_control{0.f};
+
+	/**
+	 * Check for validity of positon/velocity states.
+	 */
+	PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos, const vehicle_attitude_s &att);
+
+	/**
+	 * Check for manual setpoints.
+	 */
+	void poll_manual_setpoint(const float dt, const vehicle_local_position_s
+				  &vehicle_local_position, const vehicle_attitude_s &_vehicle_att);
+
+	/**
+	 * @brief publishes target setpoint.
+	 *
+	 */
+	void publishLocalPositionSetpoint(vehicle_attitude_setpoint_s &_att_sp);
+
+	/**
+	 * Generate setpoint to bridge no executable setpoint being available.
+	 * Used to handle transitions where no proper setpoint was generated yet and when the received setpoint is invalid.
+	 * This should only happen briefly when transitioning and never during mode operation or by design.
+	 */
+	trajectory_setpoint6dof_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states,
+			bool warn);
+};