fix: remove extra line

ROMFS/px4fmu_common/init.d/rc.sc_defaults

@@ -3,7 +3,7 @@
 # NOTE: Script variables are declared/initialized/unset in the rcS script.
 #
 
-set VEHICLE_TYPE sc
+set VEHICLE_TYPE spacecraft
 
 # MAV_TYPE_SPACECRAFT
 param set-default MAV_TYPE 7

ROMFS/px4fmu_common/init.d/rc.vehicle_setup

@@ -35,7 +35,7 @@ fi
 #
 # Spapcecraft setup.
 #
-if [ $VEHICLE_TYPE = sc ]
+if [ $VEHICLE_TYPE = spacecraft ]
 then
 	# Start standard multicopter apps.
 	. ${R}etc/init.d/rc.sc_apps

src/modules/spacecraft/CMakeLists.txt

@@ -33,6 +33,7 @@
 
 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
 add_subdirectory(SpacecraftRateControl)
+add_subdirectory(SpacecraftAttitudeControl)
 
 px4_add_module(
 	MODULE modules__spacecraft
@@ -50,4 +51,5 @@ px4_add_module(
 		mathlib
 		px4_work_queue
 		SpacecraftRateControl
+		SpacecraftAttitudeControl
 )

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

@@ -72,6 +72,7 @@ void SpacecraftHandler::Run()
 	_timestamp = hrt_absolute_time();
 	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
 
+	_spacecraft_attitude_control.updateAttitudeControl();
 	_spacecraft_rate_control.updateRateControl();
 
 	// TODO: Prepare allocation

src/modules/spacecraft/SpacecraftHandler.hpp

@@ -64,6 +64,7 @@
 
 // Local includes
 #include "SpacecraftRateControl/SpacecraftRateControl.hpp"
+#include "SpacecraftAttitudeControl/SpacecraftAttitudeControl.hpp"
 
 class SpacecraftHandler : public ModuleBase<SpacecraftHandler>, public ModuleParams, public px4::ScheduledWorkItem
 {
@@ -107,6 +108,7 @@ private:
 
 	// Class instances
 	SpacecraftRateControl _spacecraft_rate_control{this};
+	SpacecraftAttitudeControl _spacecraft_attitude_control{this};
 
 	// Variables
 	hrt_abstime _timestamp{0};

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