fix: removed extra newline

ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d

@@ -20,7 +20,7 @@ param set-default COM_ARM_CHK_ESCS 0  # We don't have ESCs
 param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later?
 
 param set-default CA_AIRFRAME 14
-param set-default MAV_TYPE 99
+param set-default MAV_TYPE 7  # Using Airship
 
 param set-default CA_THRUSTER_CNT 8
 param set-default CA_R_REV 0

ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d

@@ -11,7 +11,7 @@
 . ${R}etc/init.d/rc.sc_defaults
 
 param set-default CA_AIRFRAME 14
-param set-default MAV_TYPE 99
+param set-default MAV_TYPE 7
 
 param set-default CA_THRUSTER_CNT 8
 param set-default CA_R_REV 0

ROMFS/px4fmu_common/init.d/rc.sc_defaults

@@ -5,8 +5,8 @@
 
 set VEHICLE_TYPE sc
 
-# MAV_TYPE_QUADROTOR 2
-#param set-default MAV_TYPE 12
+# MAV_TYPE_SPACECRAFT
+param set-default MAV_TYPE 7
 
 # Set micro-dds-client to use ethernet and IP-address 192.168.0.1
 param set-default UXRCE_DDS_AG_IP -1062731775

ROMFS/px4fmu_common/init.d/rc.vehicle_setup

@@ -32,6 +32,15 @@ then
 	. ${R}etc/init.d/rc.rover_apps
 fi
 
+#
+# Spapcecraft setup.
+#
+if [ $VEHICLE_TYPE = sc ]
+then
+	# Start standard multicopter apps.
+	. ${R}etc/init.d/rc.sc_apps
+fi
+
 #
 # Differential Rover setup.
 #

msg/versioned/VehicleStatus.msg

@@ -92,6 +92,7 @@ uint8 vehicle_type
 uint8 VEHICLE_TYPE_ROTARY_WING = 0
 uint8 VEHICLE_TYPE_FIXED_WING = 1
 uint8 VEHICLE_TYPE_ROVER = 2
+uint8 VEHICLE_TYPE_SPACECRAFT = 7
 
 uint8 FAILSAFE_DEFER_STATE_DISABLED = 0
 uint8 FAILSAFE_DEFER_STATE_ENABLED = 1

src/modules/commander/Commander.cpp

@@ -1734,6 +1734,9 @@ void Commander::updateParameters()
 
 	} else if (is_ground) {
 		_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
+
+	} else if (is_spacecraft(_vehicle_status)) {
+		_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_SPACECRAFT;
 	}
 
 	_vehicle_status.is_vtol = is_vtol(_vehicle_status);

src/modules/commander/commander_helper.cpp

@@ -78,6 +78,7 @@
 #define VEHICLE_TYPE_VTOL_TILTROTOR 21
 #define VEHICLE_TYPE_VTOL_FIXEDROTOR 22 // VTOL standard
 #define VEHICLE_TYPE_VTOL_TAILSITTER 23
+#define VEHICLE_TYPE_SPACECRAFT 7
 
 #define BLINK_MSG_TIME	700000	// 3 fast blinks (in us)
 
@@ -122,6 +123,11 @@ bool is_ground_vehicle(const vehicle_status_s &current_status)
 	return (current_status.system_type == VEHICLE_TYPE_BOAT || current_status.system_type == VEHICLE_TYPE_GROUND_ROVER);
 }
 
+bool is_spacecraft(const vehicle_status_s &current_status)
+{
+	return (current_status.system_type == VEHICLE_TYPE_SPACECRAFT);
+}
+
 // End time for currently blinking LED message, 0 if no blink message
 static hrt_abstime blink_msg_end = 0;
 static int fd_leds{-1};

src/modules/commander/commander_helper.h

@@ -56,6 +56,7 @@ bool is_vtol(const vehicle_status_s &current_status);
 bool is_vtol_tailsitter(const vehicle_status_s &current_status);
 bool is_fixed_wing(const vehicle_status_s &current_status);
 bool is_ground_vehicle(const vehicle_status_s &current_status);
+bool is_spacecraft(const vehicle_status_s &current_status);
 
 int buzzer_init(void);
 void buzzer_deinit(void);

src/modules/spacecraft/CMakeLists.txt

@@ -32,6 +32,9 @@
 ############################################################################
 
 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+add_subdirectory(SpacecraftRateControl)
+add_subdirectory(SpacecraftAttitudeControl)
+add_subdirectory(SpacecraftPositionControl)
 
 px4_add_module(
 	MODULE modules__spacecraft
@@ -48,4 +51,7 @@ px4_add_module(
 	DEPENDS
 		mathlib
 		px4_work_queue
+		SpacecraftRateControl
+		SpacecraftAttitudeControl
+		SpacecraftPositionControl
 )

src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControl.cpp

@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file AttitudeControl.cpp
+ */
+
+#include <AttitudeControl.hpp>
+
+#include <mathlib/math/Functions.hpp>
+
+using namespace matrix;
+
+void ScAttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain)
+{
+	_proportional_gain = proportional_gain;
+}
+
+matrix::Vector3f ScAttitudeControl::update(const Quatf &q) const
+{
+	Quatf qd = _attitude_setpoint_q;
+
+	// calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch
+	const Vector3f e_z = q.dcm_z();
+	const Vector3f e_z_d = qd.dcm_z();
+	Quatf qd_red(e_z, e_z_d);
+
+	if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
+		// In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction,
+		// full attitude control anyways generates no yaw input and directly takes the combination of
+		// roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable.
+		qd_red = qd;
+
+	} else {
+		// transform rotation from current to desired thrust vector into a world frame reduced desired attitude
+		qd_red *= q;
+	}
+
+	// mix full and reduced desired attitude
+	Quatf q_mix = qd_red.inversed() * qd;
+	q_mix.canonicalize();
+
+	// catch numerical problems with the domain of acosf and asinf
+	q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
+	q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);
+	qd = qd_red * Quatf(q_mix(0), 0, 0, q_mix(3));
+
+	// quaternion attitude control law, qe is rotation from q to qd
+	const Quatf qe = q.inversed() * qd;
+
+	// using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)
+	// also taking care of the antipodal unit quaternion ambiguity
+	const Vector3f eq = 2.f * qe.canonical().imag();
+
+	// calculate angular rates setpoint
+	Vector3f rate_setpoint = eq.emult(_proportional_gain);
+
+	// limit rates
+	for (int i = 0; i < 3; i++) {
+		rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
+	}
+
+	return rate_setpoint;
+}

src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControl.hpp

@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file AttitudeControl.hpp
+ *
+ * A quaternion based attitude controller.
+ *
+ * @author Matthias Grob	<maetugr@gmail.com>
+ *
+ * Publication documenting the implemented Quaternion Attitude Control:
+ * Nonlinear Quadrocopter Attitude Control (2013)
+ * by Dario Brescianini, Markus Hehn and Raffaello D'Andrea
+ * Institute for Dynamic Systems and Control (IDSC), ETH Zurich
+ *
+ * https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf
+ */
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+#include <mathlib/math/Limits.hpp>
+
+class ScAttitudeControl
+{
+public:
+	ScAttitudeControl() = default;
+	~ScAttitudeControl() = default;
+
+	/**
+	 * Set proportional attitude control gain
+	 * @param proportional_gain 3D vector containing gains for roll, pitch, yaw
+	 */
+	void setProportionalGain(const matrix::Vector3f &proportional_gain);
+
+	/**
+	 * Set hard limit for output rate setpoints
+	 * @param rate_limit [rad/s] 3D vector containing limits for roll, pitch, yaw
+	 */
+	void setRateLimit(const matrix::Vector3f &rate_limit) { _rate_limit = rate_limit; }
+
+	/**
+	 * Set a new attitude setpoint replacing the one tracked before
+	 * @param qd desired vehicle attitude setpoint
+	 */
+	void setAttitudeSetpoint(const matrix::Quatf &qd)
+	{
+		_attitude_setpoint_q = qd;
+		_attitude_setpoint_q.normalize();
+	}
+
+	/**
+	 * Adjust last known attitude setpoint by a delta rotation
+	 * Optional use to avoid glitches when attitude estimate reference e.g. heading changes.
+	 * @param q_delta delta rotation to apply
+	 */
+	void adaptAttitudeSetpoint(const matrix::Quatf &q_delta)
+	{
+		_attitude_setpoint_q = q_delta * _attitude_setpoint_q;
+		_attitude_setpoint_q.normalize();
+	}
+
+	/**
+	 * Run one control loop cycle calculation
+	 * @param q estimation of the current vehicle attitude unit quaternion
+	 * @return [rad/s] body frame 3D angular rate setpoint vector to be executed by the rate controller
+	 */
+	matrix::Vector3f update(const matrix::Quatf &q) const;
+
+private:
+	matrix::Vector3f _proportional_gain;
+	matrix::Vector3f _rate_limit;
+
+	matrix::Quatf _attitude_setpoint_q; ///< latest known attitude setpoint e.g. from position control
+};

src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControlMath.hpp

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 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 AttitudeControlMath.hpp
+ */
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+
+namespace AttitudeControlMath
+{
+/**
+ * Rotate a tilt quaternion (without yaw rotation) such that when rotated by a yaw setpoint
+ * the resulting tilt is the same as if it was rotated by the current yaw of the vehicle
+ * @param q_sp_tilt pure tilt quaternion (yaw = 0) that needs to be corrected
+ * @param q_att current attitude of the vehicle
+ * @param q_sp_yaw pure yaw quaternion of the desired yaw setpoint
+ */
+void inline correctTiltSetpointForYawError(matrix::Quatf &q_sp_tilt, const matrix::Quatf &q_att,
+		const matrix::Quatf &q_sp_yaw)
+{
+	const matrix::Vector3f z_unit(0.f, 0.f, 1.f);
+
+	// Extract yaw from the current attitude
+	const matrix::Vector3f att_z = q_att.dcm_z();
+	const matrix::Quatf q_tilt(z_unit, att_z);
+	const matrix::Quatf q_yaw = q_tilt.inversed() * q_att; // This is not euler yaw
+
+	// Find the quaternion that creates a tilt aligned with the body frame
+	// when rotated by the yaw setpoint
+	// To do so, solve q_yaw * q_tilt_ne = q_sp_yaw * q_sp_rp_compensated
+	const matrix::Quatf q_sp_rp_compensated = q_sp_yaw.inversed() * q_yaw * q_sp_tilt;
+
+	// Extract the corrected tilt
+	const matrix::Vector3f att_sp_z = q_sp_rp_compensated.dcm_z();
+	q_sp_tilt = matrix::Quatf(z_unit, att_sp_z);
+}
+}

src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/CMakeLists.txt

@@ -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(AttitudeControlLibrary
+	AttitudeControl.cpp
+	AttitudeControl.hpp
+	AttitudeControlMath.hpp
+)
+target_compile_options(AttitudeControlLibrary PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_include_directories(AttitudeControlLibrary PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+# TODO: Add unit tests
+# px4_add_unit_gtest(SRC ScAttitudeControlTest.cpp LINKLIBS SpacecraftAttitudeControl)
+# px4_add_unit_gtest(SRC ScAttitudeControlMathTest.cpp LINKLIBS SpacecraftAttitudeControl)

src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/ScAttitudeControlMathTest.cpp

@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 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 "AttitudeControlMath.hpp"
+
+using namespace matrix;
+using namespace AttitudeControlMath;
+
+static const Vector3f z_unit(0.f, 0.f, 1.f);
+
+TEST(AttitudeControlMath, tiltCorrectionNoError)
+{
+	// GIVEN: some desired (non yaw-rotated) tilt setpoint
+	Quatf q_tilt_sp_ne(z_unit, Vector3f(-0.3, 0.1, 0.7));
+
+	// AND: a desired yaw setpoint
+	const Quatf q_sp_yaw = AxisAnglef(z_unit, -1.23f);
+
+	// WHEN: the current yaw error is zero (regardless of the tilt)
+	const Quatf q = q_sp_yaw * Quatf(z_unit, Vector3f(0.1f, -0.2f, 1.f));
+	const Quatf q_tilt_sp_ne_before = q_tilt_sp_ne;
+	correctTiltSetpointForYawError(q_tilt_sp_ne, q, q_sp_yaw);
+
+	// THEN: the tilt setpoint is unchanged
+	EXPECT_TRUE(isEqual(q_tilt_sp_ne_before, q_tilt_sp_ne));
+}
+
+TEST(AttitudeControlMath, tiltCorrectionYaw180)
+{
+	// GIVEN: some desired (non yaw-rotated) tilt setpoint and a desired yaw setpoint
+	Quatf q_tilt_sp_ne(z_unit, Vector3f(-0.3, 0.1, 0.7));
+	const Quatf q_sp_yaw = AxisAnglef(z_unit, -M_PI_F / 2.f);
+
+	// WHEN: there is a yaw error of 180 degrees
+	const Quatf q_yaw = Quatf(AxisAnglef(z_unit, M_PI_F / 2.f));
+	const Quatf q = q_yaw * Quatf(z_unit, Vector3f(0.1f, -0.2f, 1.f));
+	const Quatf q_tilt_sp_ne_before = q_tilt_sp_ne;
+	correctTiltSetpointForYawError(q_tilt_sp_ne, q, q_sp_yaw);
+
+	// THEN: the tilt is reversed (the corrected tilt angle is the same but the axis of rotation is opposite)
+	EXPECT_FLOAT_EQ(AxisAnglef(q_tilt_sp_ne_before).angle(), AxisAnglef(q_tilt_sp_ne).angle());
+	EXPECT_TRUE(isEqual(AxisAnglef(q_tilt_sp_ne_before).axis(), -AxisAnglef(q_tilt_sp_ne).axis()));
+}
+
+TEST(AttitudeControlMath, tiltCorrection)
+{
+	// GIVEN: some desired (non yaw-rotated) tilt setpoint and a desired yaw setpoint
+	Quatf q_tilt_sp_ne(z_unit, Vector3f(0.5, -0.1, 0.7));
+	const Quatf q_sp_yaw = AxisAnglef(z_unit, -1.23f);
+
+	// WHEN: there is a some yaw error
+	const Quatf q_yaw = Quatf(AxisAnglef(z_unit, 3.1f));
+	const Quatf q = q_yaw * Quatf(z_unit, Vector3f(0.1f, -0.2f, 1.f));
+	const Quatf q_tilt_sp_ne_before = q_tilt_sp_ne;
+	correctTiltSetpointForYawError(q_tilt_sp_ne, q, q_sp_yaw);
+
+	// THEN: the tilt vector obtained by rotating the corrected tilt by the yaw setpoint is the same as
+	// the one obtained by rotating the initial tilt by the current yaw of the vehicle
+	EXPECT_TRUE(isEqual((q_sp_yaw * q_tilt_sp_ne).dcm_z(), (q_yaw * q_tilt_sp_ne_before).dcm_z()));
+}

src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/ScAttitudeControlTest.cpp

@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ *   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 <AttitudeControl.hpp>
+#include <mathlib/math/Functions.hpp>
+
+using namespace matrix;
+
+TEST(ScAttitudeControlTest, AllZeroCase)
+{
+	ScAttitudeControl attitude_control;
+	Vector3f rate_setpoint = attitude_control.update(Quatf());
+	EXPECT_EQ(rate_setpoint, Vector3f());
+}
+
+class ScAttitudeControlConvergenceTest : public ::testing::Test
+{
+public:
+	ScAttitudeControlConvergenceTest()
+	{
+		_attitude_control.setProportionalGain(Vector3f(.5f, .6f, .3f));
+		_attitude_control.setRateLimit(Vector3f(100.f, 100.f, 100.f));
+	}
+
+	void checkConvergence()
+	{
+		int i; // need function scope to check how many steps
+		Vector3f rate_setpoint(1000.f, 1000.f, 1000.f);
+
+		_attitude_control.setAttitudeSetpoint(_quat_goal);
+
+		for (i = 100; i > 0; i--) {
+			// run attitude control to get rate setpoints
+			const Vector3f rate_setpoint_new = _attitude_control.update(_quat_state);
+			// rotate the simulated state quaternion according to the rate setpoint
+			_quat_state = _quat_state * Quatf(AxisAnglef(rate_setpoint_new));
+			_quat_state = -_quat_state; // produce intermittent antipodal quaternion states to test against unwinding problem
+
+			// expect the error and hence also the output to get smaller with each iteration
+			if (rate_setpoint_new.norm() >= rate_setpoint.norm()) {
+				break;
+			}
+
+			rate_setpoint = rate_setpoint_new;
+		}
+
+		EXPECT_EQ(_quat_state.canonical(), _quat_goal.canonical());
+		// it shouldn't have taken longer than an iteration timeout to converge
+		EXPECT_GT(i, 0);
+	}
+
+	ScAttitudeControl _attitude_control;
+	Quatf _quat_state;
+	Quatf _quat_goal;
+};
+
+TEST_F(ScAttitudeControlConvergenceTest, AttitudeControlConvergence)
+{
+	const int inputs = 8;
+
+	const Quatf QArray[inputs] = {
+		Quatf(),
+		Quatf(0, 1, 0, 0),
+		Quatf(0, 0, 1, 0),
+		Quatf(0, 0, 0, 1),
+		Quatf(0.698f, 0.024f, -0.681f, -0.220f),
+		Quatf(-0.820f, -0.313f, 0.225f, -0.423f),
+		Quatf(0.599f, -0.172f, 0.755f, -0.204f),
+		Quatf(0.216f, -0.662f, 0.290f, -0.656f)
+	};
+
+	for (int i = 0; i < inputs; i++) {
+		for (int j = 0; j < inputs; j++) {
+			printf("--- Input combination: %d %d\n", i, j);
+			_quat_state = QArray[i];
+			_quat_goal = QArray[j];
+			_quat_state.normalize();
+			_quat_goal.normalize();
+			checkConvergence();
+		}
+	}
+}
+
+TEST(ScAttitudeControlTest, YawWeightScaling)
+{
+	// GIVEN: default tuning and pure yaw turn command
+	ScAttitudeControl attitude_control;
+	const float yaw_gain = 2.8f;
+	const float yaw_sp = .1f;
+	Quatf pure_yaw_attitude(cosf(yaw_sp / 2.f), 0, 0, sinf(yaw_sp / 2.f));
+	attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain));
+	attitude_control.setRateLimit(Vector3f(1000.f, 1000.f, 1000.f));
+	attitude_control.setAttitudeSetpoint(pure_yaw_attitude);
+
+	// WHEN: we run one iteration of the controller
+	Vector3f rate_setpoint = attitude_control.update(Quatf());
+
+	// THEN: no actuation in roll, pitch
+	EXPECT_EQ(Vector2f(rate_setpoint), Vector2f());
+	// THEN: actuation error * gain in yaw
+	EXPECT_NEAR(rate_setpoint(2), yaw_sp * yaw_gain, 1e-4f);
+
+	// GIVEN: additional corner case of zero yaw weight
+	attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain));
+	// WHEN: we run one iteration of the controller
+	rate_setpoint = attitude_control.update(Quatf());
+	// THEN: no actuation (also no NAN)
+	EXPECT_EQ(rate_setpoint, Vector3f());
+}

src/modules/spacecraft/SpacecraftAttitudeControl/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2015-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.
+#
+############################################################################
+
+add_subdirectory(AttitudeControl)
+
+px4_add_library(SpacecraftAttitudeControl
+SpacecraftAttitudeControl.cpp
+)
+
+target_link_libraries(SpacecraftAttitudeControl PUBLIC mathlib)
+target_link_libraries(SpacecraftAttitudeControl PUBLIC AttitudeControlLibrary)
+target_include_directories(SpacecraftAttitudeControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.cpp

@@ -0,0 +1,238 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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 sc_att_control_main.cpp
+ * Spacecraft attitude controller.
+ * Based off the multicopter attitude controller module.
+ *
+ * @author Pedro Roque, <padr@kth.se>
+ *
+ */
+
+#include "SpacecraftAttitudeControl.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <mathlib/math/Limits.hpp>
+#include <mathlib/math/Functions.hpp>
+
+#include "AttitudeControl/AttitudeControlMath.hpp"
+
+using namespace matrix;
+
+SpacecraftAttitudeControl::SpacecraftAttitudeControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+}
+
+void
+SpacecraftAttitudeControl::updateParams()
+{
+	ModuleParams::updateParams();
+	// Store some of the parameters in a more convenient way & precompute often-used values
+	_attitude_control.setProportionalGain(Vector3f(_param_sc_roll_p.get(), _param_sc_pitch_p.get(), _param_sc_yaw_p.get()));
+
+	// angular rate limits
+	using math::radians;
+	_attitude_control.setRateLimit(Vector3f(radians(_param_sc_rollrate_max.get()), radians(_param_sc_pitchrate_max.get()),
+						radians(_param_sc_yawrate_max.get())));
+	_man_tilt_max = math::radians(_param_sc_man_tilt_max.get());
+}
+
+void
+SpacecraftAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, bool reset_yaw_sp)
+{
+	vehicle_attitude_setpoint_s attitude_setpoint{};
+	const float yaw = Eulerf(q).psi();
+
+	attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.yaw * math::radians(_param_sc_man_y_scale.get());
+
+	// Avoid accumulating absolute yaw error with arming stick gesture in case heading_good_for_control stays true
+	if (_manual_control_setpoint.throttle < -.9f) {
+		reset_yaw_sp = true;
+	}
+
+	// Make sure not absolute heading error builds up
+	if (reset_yaw_sp) {
+		_man_yaw_sp = yaw;
+
+	} else {
+		_man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt);
+	}
+
+	/*
+	 * Input mapping for roll & pitch setpoints
+	 * ----------------------------------------
+	 * We control the following 2 angles:
+	 * - tilt angle, given by sqrt(roll*roll + pitch*pitch)
+	 * - the direction of the maximum tilt in the XY-plane, which also defines the direction of the motion
+	 *
+	 * This allows a simple limitation of the tilt angle, the vehicle flies towards the direction that the stick
+	 * points to, and changes of the stick input are linear.
+	 */
+	_man_roll_input_filter.setParameters(dt, _param_sc_man_tilt_tau.get());
+	_man_pitch_input_filter.setParameters(dt, _param_sc_man_tilt_tau.get());
+
+	Vector2f v = Vector2f(_man_roll_input_filter.update(_manual_control_setpoint.roll * _man_tilt_max),
+			      -_man_pitch_input_filter.update(_manual_control_setpoint.pitch * _man_tilt_max));
+	float v_norm = v.norm(); // the norm of v defines the tilt angle
+
+	if (v_norm > _man_tilt_max) { // limit to the configured maximum tilt angle
+		v *= _man_tilt_max / v_norm;
+	}
+
+	Quatf q_sp_rp = AxisAnglef(v(0), v(1), 0.f);
+	// The axis angle can change the yaw as well (noticeable at higher tilt angles).
+	// This is the formula by how much the yaw changes:
+	//   let a := tilt angle, b := atan(y/x) (direction of maximum tilt)
+	//   yaw = atan(-2 * sin(b) * cos(b) * sin^2(a/2) / (1 - 2 * cos^2(b) * sin^2(a/2))).
+	const Quatf q_sp_yaw(cosf(_man_yaw_sp / 2.f), 0.f, 0.f, sinf(_man_yaw_sp / 2.f));
+
+	// Align the desired tilt with the yaw setpoint
+	Quatf q_sp = q_sp_yaw * q_sp_rp;
+
+	q_sp.copyTo(attitude_setpoint.q_d);
+
+	// Transform to euler angles for logging only
+	// const Eulerf euler_sp(q_sp);
+	// attitude_setpoint.roll_body = euler_sp(0);
+	// attitude_setpoint.pitch_body = euler_sp(1);
+	// attitude_setpoint.yaw_body = euler_sp(2);
+	// attitude_setpoint.q_d[0] = q_sp;
+
+	attitude_setpoint.timestamp = hrt_absolute_time();
+
+	_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
+
+	// update attitude controller setpoint immediately
+	_attitude_control.setAttitudeSetpoint(q_sp);
+	_thrust_setpoint_body = Vector3f(attitude_setpoint.thrust_body);
+	_last_attitude_setpoint = attitude_setpoint.timestamp;
+}
+
+void
+SpacecraftAttitudeControl::updateAttitudeControl()
+{
+	// run controller on attitude updates
+	vehicle_attitude_s v_att;
+
+	if (_vehicle_attitude_sub.update(&v_att)) {
+
+		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+		const float dt = math::constrain(((v_att.timestamp_sample - _last_run) * 1e-6f), 0.0002f, 0.02f);
+		_last_run = v_att.timestamp_sample;
+
+		const Quatf q{v_att.q};
+
+		// Check for new attitude setpoint
+		if (_vehicle_attitude_setpoint_sub.updated()) {
+			vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
+
+			if (_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint)
+			    && (vehicle_attitude_setpoint.timestamp > _last_attitude_setpoint)) {
+				_attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d));
+				_thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body);
+				_last_attitude_setpoint = vehicle_attitude_setpoint.timestamp;
+			}
+		}
+
+		// Check for a heading reset
+		if (_quat_reset_counter != v_att.quat_reset_counter) {
+			const Quatf delta_q_reset(v_att.delta_q_reset);
+
+			// for stabilized attitude generation only extract the heading change from the delta quaternion
+			_man_yaw_sp = wrap_pi(_man_yaw_sp + Eulerf(delta_q_reset).psi());
+
+			if (v_att.timestamp > _last_attitude_setpoint) {
+				// adapt existing attitude setpoint unless it was generated after the current attitude estimate
+				_attitude_control.adaptAttitudeSetpoint(delta_q_reset);
+			}
+
+			_quat_reset_counter = v_att.quat_reset_counter;
+		}
+
+		/* check for updates in other topics */
+		_manual_control_setpoint_sub.update(&_manual_control_setpoint);
+		_vehicle_control_mode_sub.update(&_vehicle_control_mode);
+
+		if (_vehicle_status_sub.updated()) {
+			vehicle_status_s vehicle_status;
+
+			if (_vehicle_status_sub.copy(&vehicle_status)) {
+				_vehicle_type_spacecraft = (vehicle_status.system_type == vehicle_status_s::VEHICLE_TYPE_SPACECRAFT);
+
+			}
+		}
+
+		if (_vehicle_local_position_sub.updated()) {
+			vehicle_local_position_s vehicle_local_position;
+
+			if (_vehicle_local_position_sub.copy(&vehicle_local_position)) {
+				_heading_good_for_control = vehicle_local_position.heading_good_for_control;
+			}
+		}
+
+		bool attitude_setpoint_generated = false;
+
+		if (_vehicle_control_mode.flag_control_attitude_enabled) {
+
+			// Generate the attitude setpoint from stick inputs if we are in Stabilized mode
+			if (_vehicle_control_mode.flag_control_manual_enabled &&
+			    !_vehicle_control_mode.flag_control_altitude_enabled &&
+			    !_vehicle_control_mode.flag_control_velocity_enabled &&
+			    !_vehicle_control_mode.flag_control_position_enabled) {
+				generate_attitude_setpoint(q, dt, _reset_yaw_sp);
+				attitude_setpoint_generated = true;
+
+			} else {
+				_man_roll_input_filter.reset(0.f);
+				_man_pitch_input_filter.reset(0.f);
+			}
+
+			Vector3f rates_sp = _attitude_control.update(q);
+
+			// publish rate setpoint
+			vehicle_rates_setpoint_s rates_setpoint{};
+			rates_setpoint.roll = rates_sp(0);
+			rates_setpoint.pitch = rates_sp(1);
+			rates_setpoint.yaw = rates_sp(2);
+			_thrust_setpoint_body.copyTo(rates_setpoint.thrust_body);
+			rates_setpoint.timestamp = hrt_absolute_time();
+			_vehicle_rates_setpoint_pub.publish(rates_setpoint);
+		}
+
+		// reset yaw setpoint during transitions, tailsitter.cpp generates
+		// attitude setpoint for the transition
+		_reset_yaw_sp = !attitude_setpoint_generated || !_heading_good_for_control;
+	}
+}

src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.hpp

@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+#include <perf/perf_counter.h>
+#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/posix.h>
+#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/autotune_attitude_control_status.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_rates_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+
+#include <AttitudeControl.hpp>
+
+class SpacecraftAttitudeControl : public ModuleParams
+{
+public:
+	SpacecraftAttitudeControl(ModuleParams *parent);
+	~SpacecraftAttitudeControl() = default;
+
+	void updateAttitudeControl();
+
+protected:
+
+	/**
+	 * initialize some vectors/matrices from parameters
+	 */
+	void updateParams();
+
+private:
+
+	/**
+	 * Generate & publish an attitude setpoint from stick inputs
+	 */
+	void generate_attitude_setpoint(const matrix::Quatf &q, float dt, bool reset_yaw_sp);
+
+	ScAttitudeControl _attitude_control; /**< class for attitude control calculations */
+
+	// Attitude setpoint and current attitude sub
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
+
+	// Manual control setpoint sub
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+
+	// Vehicle control mode sub and status
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	// Vehicle local position sub
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+
+	// Publish rate setpoint for rate controller and att_control status
+	uORB::Publication<vehicle_rates_setpoint_s>     _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)};    /**< rate setpoint publication */
+	uORB::Publication<vehicle_attitude_setpoint_s>  _vehicle_attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)}; /**< attitude setpoint publication */
+
+	manual_control_setpoint_s       _manual_control_setpoint {};    /**< manual control setpoint */
+	vehicle_control_mode_s          _vehicle_control_mode {};       /**< vehicle control mode */
+
+	matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */
+
+	float _man_yaw_sp{0.f};                 /**< current yaw setpoint in manual mode */
+	float _man_tilt_max;                    /**< maximum tilt allowed for manual flight [rad] */
+
+	AlphaFilter<float> _man_roll_input_filter;
+	AlphaFilter<float> _man_pitch_input_filter;
+
+	hrt_abstime _last_run{0};
+	hrt_abstime _last_attitude_setpoint{0};
+
+	bool _reset_yaw_sp{true};
+	bool _heading_good_for_control{true}; ///< initialized true to have heading lock when local position never published
+	bool _vehicle_type_spacecraft{true};
+
+	uint8_t _quat_reset_counter{0};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::SC_MAN_TILT_TAU>)  _param_sc_man_tilt_tau,
+
+		(ParamFloat<px4::params::SC_ROLL_P>)        _param_sc_roll_p,
+		(ParamFloat<px4::params::SC_PITCH_P>)       _param_sc_pitch_p,
+		(ParamFloat<px4::params::SC_YAW_P>)         _param_sc_yaw_p,
+		(ParamFloat<px4::params::SC_YAW_WEIGHT>)    _param_sc_yaw_weight,
+
+		(ParamFloat<px4::params::SC_ROLLRATE_MAX>)  _param_sc_rollrate_max,
+		(ParamFloat<px4::params::SC_PITCHRATE_MAX>) _param_sc_pitchrate_max,
+		(ParamFloat<px4::params::SC_YAWRATE_MAX>)   _param_sc_yawrate_max,
+		(ParamFloat<px4::params::SC_MAN_TILT_MAX>)  _param_sc_man_tilt_max,
+
+		/* Stabilized mode params */
+		(ParamFloat<px4::params::SC_MAN_Y_SCALE>)    _param_sc_man_y_scale /**< scaling factor from stick to yaw rate */
+	)
+};

src/modules/spacecraft/SpacecraftHandler.cpp

@@ -34,16 +34,77 @@
 /**
  * @file SpacecraftHandler.cpp
  *
- * Control allocator.
+ * Spacecraft control handler.
  *
- * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ * @author Pedro Roque <padr@kth.se>
  */
 
 #include "SpacecraftHandler.hpp"
 
+
+using namespace time_literals;
+
+SpacecraftHandler::SpacecraftHandler() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
+{
+	updateParams();
+}
+
+bool SpacecraftHandler::init()
+{
+	ScheduleOnInterval(4_ms); // 250 Hz
+	return true;
+}
+
+void SpacecraftHandler::updateParams()
+{
+	ModuleParams::updateParams();
+}
+
+void SpacecraftHandler::Run()
+{
+	if (_parameter_update_sub.updated()) {
+		updateParams();
+	}
+
+	const hrt_abstime timestamp_prev = _timestamp;
+	_timestamp = hrt_absolute_time();
+	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+
+	_spacecraft_position_control.updatePositionControl();
+	_spacecraft_attitude_control.updateAttitudeControl();
+	_spacecraft_rate_control.updateRateControl();
+
+	// TODO: Prepare allocation
+	// if (_vehicle_control_mode.flag_armed) {
+	// 	generateActuatorSetpoint();
+
+	// }
+
+}
+
 int SpacecraftHandler::task_spawn(int argc, char *argv[])
 {
-	return 0;
+	SpacecraftHandler *instance = new SpacecraftHandler();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
 }
 
 int SpacecraftHandler::print_status()
@@ -75,6 +136,7 @@ int SpacecraftHandler::print_usage(const char *reason)
 
 	PRINT_MODULE_USAGE_NAME("spacecraft", "controller");
 	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_COMMAND("status");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
 	return 0;

src/modules/spacecraft/SpacecraftHandler.hpp

@@ -62,13 +62,18 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/failure_detector_status.h>
 
+// Local includes
+#include "SpacecraftRateControl/SpacecraftRateControl.hpp"
+#include "SpacecraftAttitudeControl/SpacecraftAttitudeControl.hpp"
+#include "SpacecraftPositionControl/SpacecraftPositionControl.hpp"
+
 class SpacecraftHandler : public ModuleBase<SpacecraftHandler>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
 
 	SpacecraftHandler();
 
-	virtual ~SpacecraftHandler();
+	~SpacecraftHandler() override = default;
 
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -82,6 +87,33 @@ public:
 	/** @see ModuleBase::print_status() */
 	int print_status() override;
 
-private:		/**< loop duration performance counter */
+	bool init();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	void Run() override;
+
+	// uORB subscriptions
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
+	vehicle_control_mode_s _vehicle_control_mode{};
+
+	// uORB publications
+	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+
+	// Class instances
+	SpacecraftRateControl _spacecraft_rate_control{this};
+	SpacecraftAttitudeControl _spacecraft_attitude_control{this};
+	SpacecraftPositionControl _spacecraft_position_control{this};
+
+	// Variables
+	hrt_abstime _timestamp{0};
+	float _dt{0.f};
 
 };

src/modules/spacecraft/SpacecraftPositionControl/CMakeLists.txt

@@ -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})

src/modules/spacecraft/SpacecraftPositionControl/PositionControl/CMakeLists.txt

@@ -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)

src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.cpp

@@ -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());
+}
+}

src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.hpp

@@ -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);
+}

src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.cpp

@@ -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];
+	}
+}

src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.hpp

@@ -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 */
+};

src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ScControlMathTest.cpp

@@ -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);
+}

src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ScPositionControlTest.cpp

@@ -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);
+}

src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.cpp

@@ -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;
+}

src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.hpp

@@ -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);
+};

src/modules/spacecraft/SpacecraftRateControl/CMakeLists.txt

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2025 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(SpacecraftRateControl
+	SpacecraftRateControl.cpp
+)
+
+target_link_libraries(SpacecraftRateControl PUBLIC RateControl)
+target_link_libraries(SpacecraftRateControl PUBLIC mathlib)
+target_link_libraries(SpacecraftRateControl PUBLIC circuit_breaker)
+target_include_directories(SpacecraftRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.cpp

@@ -0,0 +1,261 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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 "SpacecraftRateControl.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <circuit_breaker/circuit_breaker.h>
+#include <mathlib/math/Limits.hpp>
+#include <mathlib/math/Functions.hpp>
+#include <px4_platform_common/events.h>
+
+using namespace matrix;
+using namespace time_literals;
+using math::radians;
+
+SpacecraftRateControl::SpacecraftRateControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	_controller_status_pub.advertise();
+	updateParams();
+}
+
+void SpacecraftRateControl::updateParams()
+{
+	ModuleParams::updateParams();
+	// rate control parameters
+	// The controller gain K is used to convert the parallel (P + I/s + sD) form
+	// to the ideal (K * [1 + 1/sTi + sTd]) form
+	const Vector3f rate_k = Vector3f(_param_sc_rollrate_k.get(), _param_sc_pitchrate_k.get(), _param_sc_yawrate_k.get());
+
+	_rate_control.setPidGains(
+		rate_k.emult(Vector3f(_param_sc_rollrate_p.get(), _param_sc_pitchrate_p.get(), _param_sc_yawrate_p.get())),
+		rate_k.emult(Vector3f(_param_sc_rollrate_i.get(), _param_sc_pitchrate_i.get(), _param_sc_yawrate_i.get())),
+		rate_k.emult(Vector3f(_param_sc_rollrate_d.get(), _param_sc_pitchrate_d.get(), _param_sc_yawrate_d.get())));
+
+	_rate_control.setIntegratorLimit(
+		Vector3f(_param_sc_rr_int_lim.get(), _param_sc_pr_int_lim.get(), _param_sc_yr_int_lim.get()));
+
+	_rate_control.setFeedForwardGain(
+		Vector3f(_param_sc_rollrate_ff.get(), _param_sc_pitchrate_ff.get(), _param_sc_yawrate_ff.get()));
+
+	// manual rate control acro mode rate limits
+	_acro_rate_max = Vector3f(radians(_param_sc_acro_r_max.get()), radians(_param_sc_acro_p_max.get()),
+				  radians(_param_sc_acro_y_max.get()));
+	_manual_force_max = _param_sc_manual_f_max.get();
+	_manual_torque_max = _param_sc_manual_t_max.get();
+}
+
+void SpacecraftRateControl::updateRateControl()
+{
+	if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
+		const hrt_abstime now = angular_velocity.timestamp_sample;
+
+		// Guard against too small (< 0.125ms) and too large (> 20ms) dt's.
+		const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f);
+		_last_run = now;
+
+		const Vector3f rates{angular_velocity.xyz};
+		const Vector3f angular_accel{angular_velocity.xyz_derivative};
+
+		/* check for updates in other topics */
+		_vehicle_control_mode_sub.update(&_vehicle_control_mode);
+		_vehicle_status_sub.update(&_vehicle_status);
+
+		// use rates setpoint topic
+		vehicle_rates_setpoint_s vehicle_rates_setpoint{};
+
+		if (_vehicle_control_mode.flag_control_manual_enabled &&
+		    !_vehicle_control_mode.flag_control_attitude_enabled) {
+			// Here we can be in: Manual Mode or Acro Mode
+			// generate the rate setpoint from sticks
+			manual_control_setpoint_s manual_control_setpoint;
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				if (_vehicle_control_mode.flag_control_rates_enabled) {
+					// manual rates control - ACRO mode
+					const Vector3f man_rate_sp{manual_control_setpoint.roll,
+								   -manual_control_setpoint.pitch,
+								   manual_control_setpoint.yaw};
+
+					_rates_setpoint = man_rate_sp * 5;
+					_thrust_setpoint(2) = -manual_control_setpoint.throttle;
+					_thrust_setpoint(0) = _thrust_setpoint(1) = 0.f;
+
+					// publish rate setpoint
+					vehicle_rates_setpoint.roll = _rates_setpoint(0);
+					vehicle_rates_setpoint.pitch = _rates_setpoint(1);
+					vehicle_rates_setpoint.yaw = _rates_setpoint(2);
+					_thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body);
+					vehicle_rates_setpoint.timestamp = hrt_absolute_time();
+
+					_vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint);
+
+				} else if (!_vehicle_control_mode.flag_control_rates_enabled) {
+					// Manual/direct control
+					// Yaw stick commands rotational moment, Roll/Pitch stick commands translational forces
+					// All other axis are set as zero (We only have four channels on the manual control inputs)
+					_thrust_setpoint(0) = math::constrain((manual_control_setpoint.pitch * _manual_force_max), -1.f, 1.f);
+					_thrust_setpoint(1) = math::constrain((manual_control_setpoint.roll * _manual_force_max), -1.f, 1.f);
+					_thrust_setpoint(2) = 0.0;
+
+					_torque_setpoint(0) = _torque_setpoint(1) = 0.0;
+					_torque_setpoint(2) = math::constrain((manual_control_setpoint.yaw * _manual_torque_max), -1.f, 1.f);
+
+					// publish thrust and torque setpoints
+					vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
+					vehicle_torque_setpoint_s vehicle_torque_setpoint{};
+
+					_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
+					_torque_setpoint.copyTo(vehicle_torque_setpoint.xyz);
+
+					vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
+					vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
+
+					vehicle_torque_setpoint.timestamp = hrt_absolute_time();
+					vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
+
+					_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
+					_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
+
+					updateActuatorControlsStatus(vehicle_torque_setpoint, dt);
+				}
+			}
+
+		} else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) {
+			// Get rates from other controllers (e.g. position or attitude controller)
+			if (_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) {
+				_rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0);
+				_rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1);
+				_rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2);
+				_thrust_setpoint = Vector3f(vehicle_rates_setpoint.thrust_body);
+			}
+		}
+
+		// run the rate controller
+		if (_vehicle_control_mode.flag_control_rates_enabled) {
+			// reset integral if disarmed
+			if (!_vehicle_control_mode.flag_armed) {
+				_rate_control.resetIntegral();
+			}
+
+			// update saturation status from control allocation feedback
+			control_allocator_status_s control_allocator_status;
+
+			if (_control_allocator_status_sub.update(&control_allocator_status)) {
+				Vector<bool, 3> saturation_positive;
+				Vector<bool, 3> saturation_negative;
+
+				if (!control_allocator_status.torque_setpoint_achieved) {
+					for (size_t i = 0; i < 3; i++) {
+						if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
+							saturation_positive(i) = true;
+
+						} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
+							saturation_negative(i) = true;
+						}
+					}
+				}
+			}
+
+			const Vector3f torque_sp = _rate_control.update(rates, _rates_setpoint, angular_accel, dt, false);
+
+			// publish rate controller status
+			rate_ctrl_status_s rate_ctrl_status{};
+			_rate_control.getRateControlStatus(rate_ctrl_status);
+			rate_ctrl_status.timestamp = hrt_absolute_time();
+			_controller_status_pub.publish(rate_ctrl_status);
+
+			// publish thrust and torque setpoints
+			vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
+			vehicle_torque_setpoint_s vehicle_torque_setpoint{};
+
+			_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
+			vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(torque_sp(0)) ? torque_sp(0) : 0.f;
+			vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(torque_sp(1)) ? torque_sp(1) : 0.f;
+			vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(torque_sp(2)) ? torque_sp(2) : 0.f;
+
+			// scale setpoints by battery status if enabled
+			if (_param_sc_bat_scale_en.get()) {
+				if (_battery_status_sub.updated()) {
+					battery_status_s battery_status;
+
+					if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) {
+						_battery_status_scale = battery_status.scale;
+					}
+				}
+
+				if (_battery_status_scale > 0.f) {
+					for (int i = 0; i < 3; i++) {
+						vehicle_thrust_setpoint.xyz[i] =
+							math::constrain(vehicle_thrust_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f);
+						vehicle_torque_setpoint.xyz[i] =
+							math::constrain(vehicle_torque_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f);
+					}
+				}
+			}
+
+			vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
+			vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
+			_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
+
+			vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
+			vehicle_torque_setpoint.timestamp = hrt_absolute_time();
+			_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
+
+			updateActuatorControlsStatus(vehicle_torque_setpoint, dt);
+		}
+	}
+}
+
+void SpacecraftRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint,
+		float dt)
+{
+	for (int i = 0; i < 3; i++) {
+		_control_energy[i] += vehicle_torque_setpoint.xyz[i] * vehicle_torque_setpoint.xyz[i] * dt;
+	}
+
+	_energy_integration_time += dt;
+
+	if (_energy_integration_time > 500e-3f) {
+		actuator_controls_status_s status;
+		status.timestamp = vehicle_torque_setpoint.timestamp;
+
+		for (int i = 0; i < 3; i++) {
+			status.control_power[i] = _control_energy[i] / _energy_integration_time;
+			_control_energy[i] = 0.f;
+		}
+
+		_actuator_controls_status_pub.publish(status);
+		_energy_integration_time = 0.f;
+	}
+}

src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.hpp

@@ -0,0 +1,160 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <lib/rate_control/rate_control.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <lib/systemlib/mavlink_log.h>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/actuator_controls_status.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/control_allocator_status.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/rate_ctrl_status.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_thrust_setpoint.h>
+#include <uORB/topics/vehicle_torque_setpoint.h>
+
+using namespace time_literals;
+
+class SpacecraftRateControl : public ModuleParams
+{
+public:
+	SpacecraftRateControl(ModuleParams *parent);
+	~SpacecraftRateControl() = default;
+
+	/**
+	 * @brief Update rate controller.
+	 */
+	void updateRateControl();
+
+protected:
+	void updateParams() override;
+
+private:
+	void updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt);
+
+	RateControl _rate_control; ///< class for rate control calculations
+
+	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
+	uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::Publication<actuator_controls_status_s>	_actuator_controls_status_pub{ORB_ID(actuator_controls_status_0)};
+	uORB::PublicationMulti<rate_ctrl_status_s>	_controller_status_pub{ORB_ID(rate_ctrl_status)};
+	uORB::Publication<vehicle_rates_setpoint_s>	_vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)};
+	uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
+	uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
+
+	vehicle_control_mode_s		_vehicle_control_mode{};
+	vehicle_status_s		_vehicle_status{};
+	vehicle_angular_velocity_s 	angular_velocity{};
+
+	bool _landed{true};
+	bool _maybe_landed{true};
+
+	hrt_abstime _last_run{0};
+
+	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
+
+	// keep setpoint values between updates
+	matrix::Vector3f _acro_rate_max;		/**< max attitude rates in acro mode */
+	matrix::Vector3f _rates_setpoint{};
+	float _manual_torque_max{1.0};
+	float _manual_force_max{1.0};
+
+	float _battery_status_scale{0.0f};
+	matrix::Vector3f _thrust_setpoint{};
+	matrix::Vector3f _torque_setpoint{};
+
+	float _energy_integration_time{0.0f};
+	float _control_energy[4] {};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::SC_ROLLRATE_P>) _param_sc_rollrate_p,
+		(ParamFloat<px4::params::SC_ROLLRATE_I>) _param_sc_rollrate_i,
+		(ParamFloat<px4::params::SC_RR_INT_LIM>) _param_sc_rr_int_lim,
+		(ParamFloat<px4::params::SC_ROLLRATE_D>) _param_sc_rollrate_d,
+		(ParamFloat<px4::params::SC_ROLLRATE_FF>) _param_sc_rollrate_ff,
+		(ParamFloat<px4::params::SC_ROLLRATE_K>) _param_sc_rollrate_k,
+
+		(ParamFloat<px4::params::SC_PITCHRATE_P>) _param_sc_pitchrate_p,
+		(ParamFloat<px4::params::SC_PITCHRATE_I>) _param_sc_pitchrate_i,
+		(ParamFloat<px4::params::SC_PR_INT_LIM>) _param_sc_pr_int_lim,
+		(ParamFloat<px4::params::SC_PITCHRATE_D>) _param_sc_pitchrate_d,
+		(ParamFloat<px4::params::SC_PITCHRATE_FF>) _param_sc_pitchrate_ff,
+		(ParamFloat<px4::params::SC_PITCHRATE_K>) _param_sc_pitchrate_k,
+
+		(ParamFloat<px4::params::SC_YAWRATE_P>) _param_sc_yawrate_p,
+		(ParamFloat<px4::params::SC_YAWRATE_I>) _param_sc_yawrate_i,
+		(ParamFloat<px4::params::SC_YR_INT_LIM>) _param_sc_yr_int_lim,
+		(ParamFloat<px4::params::SC_YAWRATE_D>) _param_sc_yawrate_d,
+		(ParamFloat<px4::params::SC_YAWRATE_FF>) _param_sc_yawrate_ff,
+		(ParamFloat<px4::params::SC_YAWRATE_K>) _param_sc_yawrate_k,
+
+		(ParamFloat<px4::params::SC_ACRO_R_MAX>) _param_sc_acro_r_max,
+		(ParamFloat<px4::params::SC_ACRO_P_MAX>) _param_sc_acro_p_max,
+		(ParamFloat<px4::params::SC_ACRO_Y_MAX>) _param_sc_acro_y_max,
+		(ParamFloat<px4::params::SC_ACRO_EXPO>) _param_sc_acro_expo,			/**< expo stick curve shape (roll & pitch) */
+		(ParamFloat<px4::params::SC_ACRO_EXPO_Y>) _param_sc_acro_expo_y,				/**< expo stick curve shape (yaw) */
+		(ParamFloat<px4::params::SC_ACRO_SUPEXPO>) _param_sc_acro_supexpo,		/**< superexpo stick curve shape (roll & pitch) */
+		(ParamFloat<px4::params::SC_ACRO_SUPEXPOY>) _param_sc_acro_supexpoy,		/**< superexpo stick curve shape (yaw) */
+
+		(ParamFloat<px4::params::SC_MAN_F_MAX>) _param_sc_manual_f_max,
+		(ParamFloat<px4::params::SC_MAN_T_MAX>) _param_sc_manual_t_max,
+
+		(ParamBool<px4::params::SC_BAT_SCALE_EN>) _param_sc_bat_scale_en
+	)
+};

src/modules/spacecraft/module.yaml

@@ -1,9 +1,8 @@
 __max_num_mc_motors: &max_num_mc_motors 12
 __max_num_thrusters: &max_num_thrusters 12
 __max_num_servos: &max_num_servos 8
-__max_num_tilts: &max_num_tilts 4
 
-module_name: Control Allocation
+module_name: Spacecraft
 
 parameters:
     - group: Geometry
@@ -172,6 +171,8 @@ parameters:
             max: 100
             default: 6.5
 
+
+
 # Mixer
 mixer:
     actuator_types:

src/modules/spacecraft/spacecraft_attitude_params.c

@@ -0,0 +1,183 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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 sc_att_control_params.c
+ * Parameters for spacecraft attitude controller.
+ *
+ * @author Pedro Roque, <padr@kth.se>
+ */
+
+/**
+ * Roll P gain
+ *
+ * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @min 0.0
+ * @max 12
+ * @decimal 2
+ * @increment 0.1
+ * @group Spacecraft Attitude Control
+ */
+PARAM_DEFINE_FLOAT(SC_ROLL_P, 6.5f);
+
+/**
+ * Pitch P gain
+ *
+ * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @min 0.0
+ * @max 12
+ * @decimal 2
+ * @increment 0.1
+ * @group Spacecraft Attitude Control
+ */
+PARAM_DEFINE_FLOAT(SC_PITCH_P, 6.5f);
+
+/**
+ * Yaw P gain
+ *
+ * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @min 0.0
+ * @max 5
+ * @decimal 2
+ * @increment 0.1
+ * @group Spacecraft Attitude Control
+ */
+PARAM_DEFINE_FLOAT(SC_YAW_P, 2.8f);
+
+/**
+ * Yaw weight
+ *
+ * A fraction [0,1] deprioritizing yaw compared to roll and pitch in non-linear attitude control.
+ * Deprioritizing yaw is necessary because multicopters have much less control authority
+ * in yaw compared to the other axes and it makes sense because yaw is not critical for
+ * stable hovering or 3D navigation.
+ *
+ * For yaw control tuning use SC_YAW_P. This ratio has no impact on the yaw gain.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.1
+ * @group Spacecraft Attitude Control
+ */
+PARAM_DEFINE_FLOAT(SC_YAW_WEIGHT, 0.4f);
+
+/**
+ * Max roll rate
+ *
+ * Limit for roll rate in manual and auto modes (except acro).
+ * Has effect for large rotations in autonomous mode, to avoid large control
+ * output and mixer saturation.
+ *
+ * This is not only limited by the vehicle's properties, but also by the maximum
+ * measurement rate of the gyro.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 1800.0
+ * @decimal 1
+ * @increment 5
+ * @group Spacecraft Attitude Control
+ */
+PARAM_DEFINE_FLOAT(SC_ROLLRATE_MAX, 220.0f);
+
+/**
+ * Max pitch rate
+ *
+ * Limit for pitch rate in manual and auto modes (except acro).
+ * Has effect for large rotations in autonomous mode, to avoid large control
+ * output and mixer saturation.
+ *
+ * This is not only limited by the vehicle's properties, but also by the maximum
+ * measurement rate of the gyro.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 1800.0
+ * @decimal 1
+ * @increment 5
+ * @group Spacecraft Attitude Control
+ */
+PARAM_DEFINE_FLOAT(SC_PITCHRATE_MAX, 220.0f);
+
+/**
+ * Max yaw rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 1800.0
+ * @decimal 1
+ * @increment 5
+ * @group Spacecraft Attitude Control
+ */
+PARAM_DEFINE_FLOAT(SC_YAWRATE_MAX, 200.0f);
+
+/**
+ * Manual tilt input filter time constant
+ *
+ * Setting this parameter to 0 disables the filter
+ *
+ * @unit s
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SC_MAN_TILT_TAU, 0.0f);
+
+/**
+ * Max manual yaw rate for Stabilized, Altitude, Position mode
+ *
+ * @unit deg/s
+ * @min 0
+ * @max 400
+ * @decimal 0
+ * @increment 10
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SC_MAN_Y_SCALE, 150.f);
+
+/**
+ * Maximal tilt angle in Stabilized or Manual mode
+ *
+ * @unit deg
+ * @min 0
+ * @max 90
+ * @decimal 0
+ * @increment 1
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(SC_MAN_TILT_MAX, 90.f);

src/modules/spacecraft/spacecraft_position_params.c

@@ -0,0 +1,293 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Proportional gain for position error
+ *
+ * Defined as corrective velocity in m/s per m position error
+ *
+ * @min 0
+ * @max 2
+ * @decimal 2
+ * @increment 0.1
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_POS_P, 0.2f);
+
+/**
+ * Integral gain for position error
+ *
+ * Defined as corrective velocity in m/s per m velocity error
+ *
+ * @min 0
+ * @max 15
+ * @decimal 2
+ * @increment 0.1
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_POS_I, 0.f);
+
+/**
+ * Integral limit for position error
+ *
+ * Defined as corrective velocity in m/s per m velocity error
+ *
+ * @min 0
+ * @max 5
+ * @decimal 2
+ * @increment 0.01
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_POS_I_LIM, 1.f);
+
+/**
+ * Proportional gain for velocity error
+ *
+ * Defined as corrective acceleration in m/s^2 per m/s velocity error
+ *
+ * @min 0
+ * @max 15
+ * @decimal 2
+ * @increment 0.1
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_VEL_P, 6.55f);
+
+/**
+ * Integral gain for velocity error
+ *
+ * Defined as corrective acceleration in m/s^2 per m/s velocity error
+ *
+ * @min 0
+ * @max 15
+ * @decimal 2
+ * @increment 0.1
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_VEL_I, 0.f);
+
+/**
+ * Integral limit for velocity error
+ *
+ * Defined as corrective acceleration in m/s^2 per m/s velocity error
+ *
+ * @min 0
+ * @max 5
+ * @decimal 2
+ * @increment 0.1
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_VEL_I_LIM, 1.f);
+
+/**
+ * Derivative gain for velocity error
+ *
+ * Defined as corrective acceleration in m/s^2 per m/s velocity error
+ *
+ * @min 0.0
+ * @max 15
+ * @decimal 2
+ * @increment 0.1
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_VEL_D, 0.0f);
+
+/**
+ * Maximum velocity
+ *
+ * Absolute maximum for all velocity controlled modes.
+ * Any higher value is truncated.
+ *
+ * @unit m/s
+ * @min 0
+ * @max 20
+ * @decimal 1
+ * @increment 1
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_VEL_MAX, 12.f);
+
+/**
+ * Overall Velocity Limit
+ *
+ * If set to a value greater than zero, other parameters are automatically set (such as
+ * MPC_VEL_MAX or MPC_VEL_MANUAL).
+ * If set to a negative value, the existing individual parameters are used.
+ *
+ * @min -20
+ * @max 20
+ * @decimal 1
+ * @increment 1
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_VEL_ALL, -10.f);
+
+/**
+ * Cruising elocity setpoint in autonomous modes
+ *
+ * @unit m/s
+ * @min 3
+ * @max 20
+ * @increment 1
+ * @decimal 1
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_VEL_CRUISE, 10.f);
+
+/**
+ * Maximum velocity setpoint in Position mode
+ *
+ * @unit m/s
+ * @min 3
+ * @max 20
+ * @increment 1
+ * @decimal 1
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_VEL_MANUAL, 10.f);
+
+/**
+ * Maximum collective thrust
+ *
+ * Limit allowed thrust
+ *
+ * @unit norm
+ * @min 0
+ * @max 1
+ * @decimal 2
+ * @increment 0.05
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_THR_MAX, 1.f);
+
+/**
+ * Acceleration for autonomous and for manual modes
+ *
+ * When piloting manually, this parameter is only used in MPC_POS_MODE 4.
+ *
+ * @unit m/s^2
+ * @min 2
+ * @max 15
+ * @decimal 1
+ * @increment 1
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_ACC, 3.f);
+
+/**
+ * Maximum accelaration in autonomous modes
+ *
+ *
+ * @unit m/s^2
+ * @min 2
+ * @max 15
+ * @decimal 1
+ * @increment 1
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_ACC_MAX, 5.f);
+
+/**
+ * Jerk limit in autonomous modes
+ *
+ * Limit the maximum jerk of the vehicle (how fast the acceleration can change).
+ * A lower value leads to smoother vehicle motions but also limited agility.
+ *
+ * @unit m/s^3
+ * @min 1
+ * @max 80
+ * @decimal 1
+ * @increment 1
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_JERK_AUTO, 4.f);
+
+PARAM_DEFINE_FLOAT(SPC_VEHICLE_RESP, 0.5f);
+/**
+ * Maximum jerk in Position/Altitude mode
+ *
+ * Limit the maximum jerk of the vehicle (how fast the acceleration can change).
+ * A lower value leads to smoother motions but limits agility
+ * (how fast it can change directions or break).
+ *
+ * Setting this to the maximum value essentially disables the limit.
+ *
+ * Only used with smooth MPC_POS_MODE 3 and 4.
+ *
+ * @unit m/s^3
+ * @min 0.5
+ * @max 500
+ * @decimal 0
+ * @increment 1
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_JERK_MAX, 8.f);
+
+/**
+ * Max manual yaw rate for Stabilized, Altitude, Position mode
+ *
+ * @unit deg/s
+ * @min 0
+ * @max 400
+ * @decimal 0
+ * @increment 10
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_MAN_Y_MAX, 150.f);
+
+/**
+ * Manual yaw rate input filter time constant
+ *
+ * Not used in Stabilized mode
+ * Setting this parameter to 0 disables the filter
+ *
+ * @unit s
+ * @min 0
+ * @max 5
+ * @decimal 2
+ * @increment 0.01
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_MAN_Y_TAU, 0.08f);
+
+/**
+ * Numerical velocity derivative low pass cutoff frequency
+ *
+ * @unit Hz
+ * @min 0
+ * @max 10
+ * @decimal 1
+ * @increment 0.5
+ * @group Spacecraft Position Control
+ */
+PARAM_DEFINE_FLOAT(SPC_VELD_LP, 5.0f);

src/modules/spacecraft/spacecraft_rate_params.c

@@ -0,0 +1,421 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2025 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 spacecraft_params.c
+ * Parameters for spacecraft vehicle type.
+ *
+ * @author Pedro Roque <padr@kth.se>
+ */
+
+/**
+ * Roll rate P gain
+ *
+ * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.01
+ * @max 0.5
+ * @decimal 3
+ * @increment 0.01
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_ROLLRATE_P, 0.15f);
+
+/**
+ * Roll rate I gain
+ *
+ * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_ROLLRATE_I, 0.2f);
+
+/**
+ * Roll rate integrator limit
+ *
+ * Roll rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large roll moment trim changes.
+ *
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_RR_INT_LIM, 0.30f);
+
+/**
+ * Roll rate D gain
+ *
+ * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @max 0.01
+ * @decimal 4
+ * @increment 0.0005
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_ROLLRATE_D, 0.003f);
+
+/**
+ * Roll rate feedforward
+ *
+ * Improves tracking performance.
+ *
+ * @min 0.0
+ * @decimal 4
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_ROLLRATE_FF, 0.0f);
+
+/**
+ * Roll rate controller gain
+ *
+ * Global gain of the controller.
+ *
+ * This gain scales the P, I and D terms of the controller:
+ * output = SC_ROLLRATE_K * (SC_ROLLRATE_P * error
+ * 			     + SC_ROLLRATE_I * error_integral
+ * 			     + SC_ROLLRATE_D * error_derivative)
+ * Set SC_ROLLRATE_P=1 to implement a PID in the ideal form.
+ * Set SC_ROLLRATE_K=1 to implement a PID in the parallel form.
+ *
+ * @min 0.01
+ * @max 5.0
+ * @decimal 4
+ * @increment 0.0005
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_ROLLRATE_K, 1.0f);
+
+/**
+ * Pitch rate P gain
+ *
+ * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.01
+ * @max 0.6
+ * @decimal 3
+ * @increment 0.01
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_PITCHRATE_P, 0.15f);
+
+/**
+ * Pitch rate I gain
+ *
+ * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.01
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_PITCHRATE_I, 0.2f);
+
+/**
+ * Pitch rate integrator limit
+ *
+ * Pitch rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large pitch moment trim changes.
+ *
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_PR_INT_LIM, 0.30f);
+
+/**
+ * Pitch rate D gain
+ *
+ * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @decimal 4
+ * @increment 0.0005
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_PITCHRATE_D, 0.003f);
+
+/**
+ * Pitch rate feedforward
+ *
+ * Improves tracking performance.
+ *
+ * @min 0.0
+ * @decimal 4
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_PITCHRATE_FF, 0.0f);
+
+/**
+ * Pitch rate controller gain
+ *
+ * Global gain of the controller.
+ *
+ * This gain scales the P, I and D terms of the controller:
+ * output = SC_PITCHRATE_K * (SC_PITCHRATE_P * error
+ * 			     + SC_PITCHRATE_I * error_integral
+ * 			     + SC_PITCHRATE_D * error_derivative)
+ * Set SC_PITCHRATE_P=1 to implement a PID in the ideal form.
+ * Set SC_PITCHRATE_K=1 to implement a PID in the parallel form.
+ *
+ * @min 0.01
+ * @max 5.0
+ * @decimal 4
+ * @increment 0.0005
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_PITCHRATE_K, 1.0f);
+
+/**
+ * Yaw rate P gain
+ *
+ * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_YAWRATE_P, 10.0f);
+
+/**
+ * Yaw rate I gain
+ *
+ * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_YAWRATE_I, 0.865f);
+
+/**
+ * Yaw rate integrator limit
+ *
+ * Yaw rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large yaw moment trim changes.
+ *
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_YR_INT_LIM, 0.2f);
+
+/**
+ * Yaw rate D gain
+ *
+ * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.01
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_YAWRATE_D, 0.0f);
+
+/**
+ * Yaw rate feedforward
+ *
+ * Improves tracking performance.
+ *
+ * @min 0.0
+ * @decimal 4
+ * @increment 0.01
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_YAWRATE_FF, 0.0f);
+
+/**
+ * Yaw rate controller gain
+ *
+ * Global gain of the controller.
+ *
+ * This gain scales the P, I and D terms of the controller:
+ * output = SC_YAWRATE_K * (SC_YAWRATE_P * error
+ * 			     + SC_YAWRATE_I * error_integral
+ * 			     + SC_YAWRATE_D * error_derivative)
+ * Set SC_YAWRATE_P=1 to implement a PID in the ideal form.
+ * Set SC_YAWRATE_K=1 to implement a PID in the parallel form.
+ *
+ * @min 0.0
+ * @max 5.0
+ * @decimal 4
+ * @increment 0.0005
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_YAWRATE_K, 1.0f);
+
+/**
+ * Max acro roll rate
+ *
+ * default: 2 turns per second
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 1800.0
+ * @decimal 1
+ * @increment 5
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_ACRO_R_MAX, 720.0f);
+
+/**
+ * Max acro pitch rate
+ *
+ * default: 2 turns per second
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 1800.0
+ * @decimal 1
+ * @increment 5
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_ACRO_P_MAX, 720.0f);
+
+/**
+ * Max acro yaw rate
+ *
+ * default 1.5 turns per second
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 1800.0
+ * @decimal 1
+ * @increment 5
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_ACRO_Y_MAX, 540.0f);
+
+/**
+ * Acro mode Expo factor for Roll and Pitch.
+ *
+ * Exponential factor for tuning the input curve shape.
+ *
+ * 0 Purely linear input curve
+ * 1 Purely cubic input curve
+ *
+ * @min 0
+ * @max 1
+ * @decimal 2
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_ACRO_EXPO, 0.69f);
+
+/**
+ * Acro mode Expo factor for Yaw.
+ *
+ * Exponential factor for tuning the input curve shape.
+ *
+ * 0 Purely linear input curve
+ * 1 Purely cubic input curve
+ *
+ * @min 0
+ * @max 1
+ * @decimal 2
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_ACRO_EXPO_Y, 0.69f);
+
+/**
+ * Acro mode SuperExpo factor for Roll and Pitch.
+ *
+ * SuperExpo factor for refining the input curve shape tuned using SC_ACRO_EXPO.
+ *
+ * 0 Pure Expo function
+ * 0.7 reasonable shape enhancement for intuitive stick feel
+ * 0.95 very strong bent input curve only near maxima have effect
+ *
+ * @min 0
+ * @max 0.95
+ * @decimal 2
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_ACRO_SUPEXPO, 0.7f);
+
+/**
+ * Acro mode SuperExpo factor for Yaw.
+ *
+ * SuperExpo factor for refining the input curve shape tuned using SC_ACRO_EXPO_Y.
+ *
+ * 0 Pure Expo function
+ * 0.7 reasonable shape enhancement for intuitive stick feel
+ * 0.95 very strong bent input curve only near maxima have effect
+ *
+ * @min 0
+ * @max 0.95
+ * @decimal 2
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_ACRO_SUPEXPOY, 0.7f);
+
+/**
+ * Battery power level scaler
+ *
+ * This compensates for voltage drop of the battery over time by attempting to
+ * normalize performance across the operating range of the battery. The copter
+ * should constantly behave as if it was fully charged with reduced max acceleration
+ * at lower battery percentages. i.e. if hover is at 0.5 throttle at 100% battery,
+ * it will still be 0.5 at 60% battery.
+ *
+ * @boolean
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_INT32(SC_BAT_SCALE_EN, 0);
+
+/**
+ * Manual mode maximum force.
+ *
+ * *
+ * @min 0
+ * @max 1.0
+ * @decimal 2
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_MAN_F_MAX, 1.0f);
+
+/**
+ * Manual mode maximum torque.
+ *
+ * *
+ * @min 0
+ * @max 1.0
+ * @decimal 2
+ * @group Spacecraft Rate Control
+ */
+PARAM_DEFINE_FLOAT(SC_MAN_T_MAX, 1.0f);