Spacecraft Attitude Control

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

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

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

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

ROMFS/px4fmu_common/init.d/rc.sc_defaults

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

ROMFS/px4fmu_common/init.d/rc.vehicle_setup

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

msg/versioned/VehicleStatus.msg

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

src/modules/commander/Commander.cpp

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

src/modules/commander/commander_helper.cpp

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

src/modules/commander/commander_helper.h

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

src/modules/spacecraft/CMakeLists.txt

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