Spacecraft restructuring and Rate Controller

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,7 @@
 ############################################################################
 
 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+add_subdirectory(SpacecraftRateControl)
 
 px4_add_module(
 	MODULE modules__spacecraft
@@ -48,4 +49,5 @@ px4_add_module(
 	DEPENDS
 		mathlib
 		px4_work_queue
+		SpacecraftRateControl
 )

src/modules/spacecraft/SpacecraftHandler.cpp

@@ -34,16 +34,75 @@
 /**
  * @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_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 +134,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,16 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/failure_detector_status.h>
 
+// Local includes
+#include "SpacecraftRateControl/SpacecraftRateControl.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 +85,31 @@ class SpacecraftHandler : public ModuleBase<SpacecraftHandler>, public ModulePar
 	/** @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};
+
+	// 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})