fix: remove extra line

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

@@ -3,10 +3,10 @@
 # NOTE: Script variables are declared/initialized/unset in the rcS script.
 #
 
-set VEHICLE_TYPE sc
+set VEHICLE_TYPE spacecraft
 
-# 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 = spacecraft ]
+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,8 @@
 ############################################################################
 
 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+add_subdirectory(SpacecraftRateControl)
+add_subdirectory(SpacecraftAttitudeControl)
 
 px4_add_module(
 	MODULE modules__spacecraft
@@ -48,4 +50,6 @@ px4_add_module(
 	DEPENDS
 		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

@@ -34,16 +34,76 @@
 /**
  * @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_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 +135,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,17 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/failure_detector_status.h>
 
+// Local includes
+#include "SpacecraftRateControl/SpacecraftRateControl.hpp"
+#include "SpacecraftAttitudeControl/SpacecraftAttitudeControl.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 +86,32 @@ 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};
+
+	// Variables
+	hrt_abstime _timestamp{0};
+	float _dt{0.f};
 
 };

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