From 6d150197175e889d254e51c7e8934f0047ae6305 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Thu, 12 Jun 2025 21:48:48 +0200 Subject: [PATCH] feat: spacecraft tooling for commander and VehicleStatus (#24716) * feat: spacecraft tooling for commander and VehicleStatus * fix: format * fix: remove iostream * feat: mavlink compliant spacecraft definition * feat: add orbiter to define feat: spacecraft tooling for commander and VehicleStatus fix: format fix: remove iostream feat: mavlink compliant spacecraft definition * feat: add orbiter to define * feat: update mavlink to latest * fix: get away without specifying spacecraft vehicle * fix: removed unnecessary definition * fix: format --- .vscode/cmake-variants.yaml | 5 + .../71001_gazebo-classic_spacecraft_dart | 155 ------------------ .../airframes/71002_gz_spacecraft_2d | 2 +- .../init.d-posix/airframes/CMakeLists.txt | 1 - .../init.d/airframes/70000_spacecraft_2d | 2 +- ROMFS/px4fmu_common/init.d/rc.sc_defaults | 6 +- ROMFS/px4fmu_common/init.d/rc.vehicle_setup | 9 + msg/versioned/VehicleStatus.msg | 1 + src/modules/commander/Commander.cpp | 2 +- src/modules/mavlink/mavlink | 2 +- 10 files changed, 22 insertions(+), 163 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index 741bf8f4e0..bd1a5b7f42 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -6,6 +6,11 @@ CONFIG: buildType: RelWithDebInfo settings: CONFIG: px4_sitl_default + px4_sitl_spacecraft: + short: px4_sitl_spacecraft + buildType: RelWithDebInfo + settings: + CONFIG: px4_sitl_spacecraft px4_sitl_nolockstep: short: px4_sitl_nolockstep buildType: RelWithDebInfo diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart b/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart deleted file mode 100644 index 737ab67b85..0000000000 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart +++ /dev/null @@ -1,155 +0,0 @@ -#!/bin/sh -# -# @name 6DoF Spacecraft Model -# -# @type Freeflyer with 8 thrusters -# -# @maintainer Pedro Roque -# - -. ${R}etc/init.d/rc.sc_defaults - -param set-default CA_AIRFRAME 15 -param set-default MAV_TYPE 99 - -param set-default CA_THRUSTER_CNT 12 -param set-default CA_R_REV 0 - -# param set-default FW_ARSP_MODE 1 - -# Auto to be provided by Custom Airframe -param set-default CA_METHOD 0 - -# disable attitude failure detection -param set-default FD_FAIL_P 0 -param set-default FD_FAIL_R 0 - -# Set proper failsafes -param set-default COM_ACT_FAIL_ACT 0 -param set-default COM_LOW_BAT_ACT 0 -param set-default NAV_DLL_ACT 0 -param set-default GF_ACTION 1 -param set-default NAV_RCL_ACT 1 -param set-default COM_POSCTL_NAVL 2 - -# Set thrusters -param set-default CA_THRUSTER0_PX -0.50 -param set-default CA_THRUSTER0_PY 0.50 -param set-default CA_THRUSTER0_PZ 0.0 -param set-default CA_THRUSTER0_CT 0.237 -param set-default CA_THRUSTER0_AX 0.0 -param set-default CA_THRUSTER0_AY -1.0 -param set-default CA_THRUSTER0_AZ 0.0 - -param set-default CA_THRUSTER1_PX 0.50 -param set-default CA_THRUSTER1_PY 0.50 -param set-default CA_THRUSTER1_PZ 0.0 -param set-default CA_THRUSTER1_CT 0.237 -param set-default CA_THRUSTER1_AX 0.0 -param set-default CA_THRUSTER1_AY -1.0 -param set-default CA_THRUSTER1_AZ 0.0 - -param set-default CA_THRUSTER2_PX 0.50 -param set-default CA_THRUSTER2_PY -0.50 -param set-default CA_THRUSTER2_PZ 0.0 -param set-default CA_THRUSTER2_CT 0.237 -param set-default CA_THRUSTER2_AX 0.0 -param set-default CA_THRUSTER2_AY 1.0 -param set-default CA_THRUSTER2_AZ 0.0 - -param set-default CA_THRUSTER3_PX -0.50 -param set-default CA_THRUSTER3_PY -0.50 -param set-default CA_THRUSTER3_PZ 0.0 -param set-default CA_THRUSTER3_CT 0.237 -param set-default CA_THRUSTER3_AX 0.0 -param set-default CA_THRUSTER3_AY 1.0 -param set-default CA_THRUSTER3_AZ 0.0 - -param set-default CA_THRUSTER4_PX -0.50 -param set-default CA_THRUSTER4_PY 0.0 -param set-default CA_THRUSTER4_PZ -0.50 -param set-default CA_THRUSTER4_CT 0.237 -param set-default CA_THRUSTER4_AX 1.0 -param set-default CA_THRUSTER4_AY 0.0 -param set-default CA_THRUSTER4_AZ 0.0 - -param set-default CA_THRUSTER5_PX 0.50 -param set-default CA_THRUSTER5_PY 0.0 -param set-default CA_THRUSTER5_PZ -0.50 -param set-default CA_THRUSTER5_CT 0.237 -param set-default CA_THRUSTER5_AX -1.0 -param set-default CA_THRUSTER5_AY 0.0 -param set-default CA_THRUSTER5_AZ 0.0 - -param set-default CA_THRUSTER6_PX 0.50 -param set-default CA_THRUSTER6_PY 0.0 -param set-default CA_THRUSTER6_PZ 0.50 -param set-default CA_THRUSTER6_CT 0.237 -param set-default CA_THRUSTER6_AX -1.0 -param set-default CA_THRUSTER6_AY 0.0 -param set-default CA_THRUSTER6_AZ 0.0 - -param set-default CA_THRUSTER7_PX -0.50 -param set-default CA_THRUSTER7_PY 0.0 -param set-default CA_THRUSTER7_PZ 0.50 -param set-default CA_THRUSTER7_CT 0.237 -param set-default CA_THRUSTER7_AX 1.0 -param set-default CA_THRUSTER7_AY 0.0 -param set-default CA_THRUSTER7_AZ 0.0 - -param set-default CA_THRUSTER8_PX 0.0 -param set-default CA_THRUSTER8_PY -0.50 -param set-default CA_THRUSTER8_PZ -0.50 -param set-default CA_THRUSTER8_CT 0.237 -param set-default CA_THRUSTER8_AX 0.0 -param set-default CA_THRUSTER8_AY 0.0 -param set-default CA_THRUSTER8_AZ 1.0 - -param set-default CA_THRUSTER9_PX 0.0 -param set-default CA_THRUSTER9_PY 0.50 -param set-default CA_THRUSTER9_PZ -0.50 -param set-default CA_THRUSTER9_CT 0.237 -param set-default CA_THRUSTER9_AX 0.0 -param set-default CA_THRUSTER9_AY 0.0 -param set-default CA_THRUSTER9_AZ 1.0 - -param set-default CA_THRUSTER10_PX 0.0 -param set-default CA_THRUSTER10_PY 0.50 -param set-default CA_THRUSTER10_PZ 0.50 -param set-default CA_THRUSTER10_CT 0.237 -param set-default CA_THRUSTER10_AX 0.0 -param set-default CA_THRUSTER10_AY 0.0 -param set-default CA_THRUSTER10_AZ -1.0 - -param set-default CA_THRUSTER11_PX 0.0 -param set-default CA_THRUSTER11_PY -0.50 -param set-default CA_THRUSTER11_PZ 0.50 -param set-default CA_THRUSTER11_CT 0.237 -param set-default CA_THRUSTER11_AX 0.0 -param set-default CA_THRUSTER11_AY 0.0 -param set-default CA_THRUSTER11_AZ -1.0 - -param set-default PWM_MAIN_FUNC1 101 -param set-default PWM_MAIN_FUNC2 102 -param set-default PWM_MAIN_FUNC3 103 -param set-default PWM_MAIN_FUNC4 104 -param set-default PWM_MAIN_FUNC5 105 -param set-default PWM_MAIN_FUNC6 106 -param set-default PWM_MAIN_FUNC7 107 -param set-default PWM_MAIN_FUNC8 108 -param set-default PWM_MAIN_FUNC9 109 -param set-default PWM_MAIN_FUNC10 110 -param set-default PWM_MAIN_FUNC11 111 -param set-default PWM_MAIN_FUNC12 112 - -# PWM Simulation -param set PWM_SIM_PWM_MAX 10000 -param set PWM_SIM_PWM_MIN 0 - -# Controller Tunings -param set-default SC_ROLLRATE_P 0.14 -param set-default SC_PITCHRATE_P 0.14 -param set-default SC_ROLLRATE_I 0.3 -param set-default SC_PITCHRATE_I 0.3 -param set-default SC_ROLLRATE_D 0.004 -param set-default SC_PITCHRATE_D 0.004 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d index 4cc0a02e41..6f2860a598 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d +++ b/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 45 param set-default CA_THRUSTER_CNT 8 param set-default CA_R_REV 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index ff72e5e468..8a3cc924be 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -114,7 +114,6 @@ px4_add_romfs_files( 17001_flightgear_tf-g1 17002_flightgear_tf-g2 - 71001_gazebo-classic_spacecraft_dart 71002_gz_spacecraft_2d # [22000, 22999] Reserve for custom models diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d b/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d index c0f55737f5..7ecf3b666b 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d +++ b/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 45 param set-default CA_THRUSTER_CNT 8 param set-default CA_R_REV 0 diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_defaults b/ROMFS/px4fmu_common/init.d/rc.sc_defaults index 8dc8867b22..0caa831cef 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sc_defaults +++ b/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_ORBITTER +param set-default MAV_TYPE 45 # Set micro-dds-client to use ethernet and IP-address 192.168.0.1 param set-default UXRCE_DDS_AG_IP -1062731775 diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index ae34645ad8..43e97b3785 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -68,6 +68,15 @@ then . ${R}etc/init.d/rc.vtol_apps fi +# +# Spapcecraft setup. +# +if [ $VEHICLE_TYPE = spacecraft ] +then + # Start standard multicopter apps. + . ${R}etc/init.d/rc.sc_apps +fi + # # Airship setup. # diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index 0a94fde86c..f433d564e9 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -89,6 +89,7 @@ uint8 HIL_STATE_ON = 1 # Current vehicle locomotion method. A vehicle can have different methods (e.g. VTOL transitions from RW to FW method) uint8 vehicle_type +uint8 VEHICLE_TYPE_UNSPECIFIED = 0 uint8 VEHICLE_TYPE_ROTARY_WING = 1 uint8 VEHICLE_TYPE_FIXED_WING = 2 uint8 VEHICLE_TYPE_ROVER = 3 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 0e183f4ff9..df445166ab 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -688,7 +688,7 @@ Commander::Commander() : _vehicle_status.system_id = 1; _vehicle_status.component_id = 1; _vehicle_status.system_type = 0; - _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNSPECIFIED; _vehicle_status.nav_state = _user_mode_intention.get(); _vehicle_status.nav_state_user_intention = _user_mode_intention.get(); _vehicle_status.nav_state_timestamp = hrt_absolute_time(); diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index 5bfd76d802..067abb83cd 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit 5bfd76d80281f6027134e854aafe6cb3dbfbe9e1 +Subproject commit 067abb83cd2755f0e49491032d63438c64fd0aed