From d0d211b26648b91d105b2bf593e147ed9c1c4638 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Thu, 18 Apr 2024 14:41:00 +0200 Subject: [PATCH] featt: moved everything to sc --- CMakeLists.txt | 15 ++- .../9000_gazebo-classic_2d_spacecraft | 2 +- .../9001_gazebo-classic_3d_spacecraft | 2 +- .../init.d-posix/airframes/CMakeLists.txt | 3 +- ROMFS/px4fmu_common/init.d-posix/rcS | 1 + .../init.d/airframes/70000_kth_space_robot | 92 ++++++++++++++----- ROMFS/px4fmu_common/init.d/rc.sc_apps | 7 +- .../sitl_targets_gazebo-classic.cmake | 3 +- 8 files changed, 90 insertions(+), 35 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b75dd570db7b..7aa5b545ee9f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -488,6 +488,15 @@ if(EXISTS "${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/finalize.cmake") include(finalize) endif() +# Clear target folder +set(INSTALL_DIR "${CMAKE_INSTALL_PREFIX}") +file(GLOB RESULT_BUILD ${INSTALL_DIR}) +list(LENGTH RESULT_BUILD BUILD_RES_LEN) +if(BUILD_RES_LEN GREATER 0) + message("Deleting target folder: ${INSTALL_DIR}") + file(REMOVE_RECURSE ${INSTALL_DIR}) +endif() + # Install SITL build files file(GLOB RESULT build/*) list(LENGTH RESULT RES_LEN) @@ -496,9 +505,9 @@ if(RES_LEN GREATER 0) DESTINATION share/${PROJECT_NAME} PATTERN ".svn" EXCLUDE) # Install ROMFS -# install(DIRECTORY ROMFS/ -# DESTINATION share/${PROJECT_NAME} -# PATTERN ".svn" EXCLUDE) + install(DIRECTORY ROMFS/ + DESTINATION share/${PROJECT_NAME} + PATTERN ".svn" EXCLUDE) else() message(STATUS "No PX4 build files to install.") endif() \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft index d8e386413617..45135c7f3dcc 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft @@ -7,7 +7,7 @@ # @maintainer Pedro Roque # -. ${R}etc/init.d/rc.sc_apps +. ${R}etc/init.d/rc.sc_defaults param set-default CA_AIRFRAME 13 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft index 5d108101d434..f4cc53927a94 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft @@ -7,7 +7,7 @@ # @maintainer Pedro Roque # -. ${R}etc/init.d/rc.sc_apps +. ${R}etc/init.d/rc.sc_defaults param set-default CA_AIRFRAME 13 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 0cf65f9cfa87..5893769b229a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -81,7 +81,8 @@ px4_add_romfs_files( 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post - 9000_gazebo-classic_spacecraft + 9000_gazebo-classic_2d_spacecraft + 9001_gazebo-classic_3d_spacecraft 10015_gazebo-classic_iris 10016_none_iris diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 6110215efc0a..faa4a2e62c93 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -224,6 +224,7 @@ done if [ -e "$autostart_file" ] then + echo "INFO [init] using autostart file: $autostart_file" . "$autostart_file" elif [ ! -e "$autostart_file" ] && [ "$SYS_AUTOSTART" -ne "0" ] diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot index 12708843098a..e6b157530260 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot +++ b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot @@ -10,32 +10,74 @@ . ${R}etc/init.d/rc.sc_defaults -# param set-default CA_AIRFRAME 7 -# param set-default CA_ROTOR_COUNT 8 +param set-default CA_AIRFRAME 13 + +param set-default CA_ROTOR_COUNT 8 +param set-default CA_R_REV 255 + +# 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 + +param set-default CA_ROTOR0_PX -0.12 +param set-default CA_ROTOR0_PY -0.12 +param set-default CA_ROTOR0_KM 0.1697 +param set-default CA_ROTOR0_AX 1.0 +param set-default CA_ROTOR0_AY 0.0 +param set-default CA_ROTOR0_AZ 0.0 + +param set-default CA_ROTOR1_PX 0.12 +param set-default CA_ROTOR1_PY -0.12 +param set-default CA_ROTOR1_KM 0.1697 +param set-default CA_ROTOR1_AX -1.0 +param set-default CA_ROTOR1_AY 0.0 +param set-default CA_ROTOR1_AZ 0.0 + +param set-default CA_ROTOR2_PX -0.12 +param set-default CA_ROTOR2_PY 0.12 +param set-default CA_ROTOR2_KM 0.1697 +param set-default CA_ROTOR2_AX 1.0 +param set-default CA_ROTOR2_AY 0.0 +param set-default CA_ROTOR2_AZ 0.0 + +param set-default CA_ROTOR3_PX 0.12 +param set-default CA_ROTOR3_PY 0.12 +param set-default CA_ROTOR3_KM 0.1697 +param set-default CA_ROTOR3_AX -1.0 +param set-default CA_ROTOR3_AY 0.0 +param set-default CA_ROTOR3_AZ 0.0 + +param set-default CA_ROTOR4_PX 0.12 +param set-default CA_ROTOR4_PY -0.12 +param set-default CA_ROTOR4_KM 0.1697 +param set-default CA_ROTOR4_AX 0.0 +param set-default CA_ROTOR4_AY 1.0 +param set-default CA_ROTOR4_AZ 0.0 + +param set-default CA_ROTOR5_PX 0.12 +param set-default CA_ROTOR5_PY 0.12 +param set-default CA_ROTOR5_KM 0.1697 +param set-default CA_ROTOR5_AX 0.0 +param set-default CA_ROTOR5_AY -1.0 +param set-default CA_ROTOR5_AZ 0.0 + +param set-default CA_ROTOR6_PX -0.12 +param set-default CA_ROTOR6_PY -0.12 +param set-default CA_ROTOR6_KM 0.1697 +param set-default CA_ROTOR6_AX 0.0 +param set-default CA_ROTOR6_AY 1.0 +param set-default CA_ROTOR6_AZ 0.0 + +param set-default CA_ROTOR7_PX -0.12 +param set-default CA_ROTOR7_PY 0.12 +param set-default CA_ROTOR7_KM 0.1697 +param set-default CA_ROTOR7_AX 0.0 +param set-default CA_ROTOR7_AY -1.0 +param set-default CA_ROTOR7_AZ 0.0 -# param set-default CA_ROTOR0_PX -0.12 -# param set-default CA_ROTOR0_PY -0.12 - -# param set-default CA_ROTOR1_PX 0.12 -# param set-default CA_ROTOR1_PY -0.12 - -# param set-default CA_ROTOR2_PX -0.12 -# param set-default CA_ROTOR2_PY 0.12 - -# param set-default CA_ROTOR3_PX 0.12 -# param set-default CA_ROTOR3_PY 0.12 - -# param set-default CA_ROTOR4_PX -0.12 -# param set-default CA_ROTOR4_PY 0.12 - -# param set-default CA_ROTOR5_PX -0.12 -# param set-default CA_ROTOR5_PY -0.12 - -# param set-default CA_ROTOR6_PX 0.12 -# param set-default CA_ROTOR6_PY 0.12 - -# param set-default CA_ROTOR7_PX 0.12 -# param set-default CA_ROTOR7_PY -0.12 param set-default PWM_AUX_TIM0 10 param set-default PWM_AUX_TIM1 10 diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_apps b/ROMFS/px4fmu_common/init.d/rc.sc_apps index e6759d101299..bf895cf5e120 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.sc_apps @@ -9,17 +9,18 @@ ekf2 start & # Start MicroDDS Client -uxrce_dds_client start -t udp -h 192.168.0.1 -n spacebot2 +# uxrce_dds_client start -t udp -h 192.168.0.1 -n spacebot2 +# uxrce_dds_client start -t udp -p 8888 # # Start Control Allocator # -# control_allocator start +control_allocator start # # Start Multicopter Rate Controller. # -# sc_rate_control start +sc_rate_control start # Start Space Robot Thruster Controller sc_thruster_controller start diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake index 48ef825843c8..c5ebc862ebde 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake @@ -86,7 +86,8 @@ if(gazebo_FOUND) iris_rplidar iris_vision omnicopter - spacecraft + 2d_spacecraft + 3d_spacecraft plane plane_cam plane_catapult