diff --git a/CMakeLists.txt b/CMakeLists.txt index 82771613c9ba..2fd9ae728d80 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -493,3 +493,28 @@ add_custom_target(install_python_requirements 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) +if(RES_LEN GREATER 0) + install(DIRECTORY build/ + DESTINATION share/${PROJECT_NAME} + PATTERN ".svn" EXCLUDE) + # Install ROMFS + install(DIRECTORY ROMFS/ + DESTINATION share/${PROJECT_NAME} + PATTERN ".svn" EXCLUDE) +else() + message(STATUS "No PX4 build files to install.") +endif() + diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter index 36bed6a03bc6..1a7118e2ccfc 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter @@ -7,7 +7,7 @@ # @maintainer Jaeyoung Lim # -. ${R}etc/init.d/rc.mc_defaults +. ${R}etc/init.d/rc.mc_apps param set-default CA_AIRFRAME 0 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 new file mode 100644 index 000000000000..5d17a2239996 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9000_gazebo-classic_2d_spacecraft @@ -0,0 +1,118 @@ +#!/bin/sh +# +# @name 3DoF Spacecraft Model +# +# @type 2D Freeflyer with 8 thrusters - Planar motion +# +# @maintainer Pedro Roque +# + +. ${R}etc/init.d/rc.sc_defaults + +param set-default CA_AIRFRAME 13 +param set-default MAV_TYPE 24 + +param set-default CA_THRUSTER_CNT 8 +param set-default CA_R_REV 255 + +# param set-default FW_ARSP_MODE 1 + +# Auto to be provided by Custom Airframe +param set-default CA_METHOD 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 + +# disable attitude failure detection +param set-default FD_FAIL_P 0 +param set-default FD_FAIL_R 0 + +param set-default CA_THRUSTER0_PX -0.12 +param set-default CA_THRUSTER0_PY -0.12 +param set-default CA_THRUSTER0_PZ 0.0 +param set-default CA_THRUSTER0_CT 1.75 +param set-default CA_THRUSTER0_AX 1.0 +param set-default CA_THRUSTER0_AY 0.0 +param set-default CA_THRUSTER0_AZ 0.0 + +param set-default CA_THRUSTER1_PX 0.12 +param set-default CA_THRUSTER1_PY -0.12 +param set-default CA_THRUSTER1_PZ 0.0 +param set-default CA_THRUSTER1_CT 1.75 +param set-default CA_THRUSTER1_AX -1.0 +param set-default CA_THRUSTER1_AY 0.0 +param set-default CA_THRUSTER1_AZ 0.0 + +param set-default CA_THRUSTER2_PX -0.12 +param set-default CA_THRUSTER2_PY 0.12 +param set-default CA_THRUSTER2_PZ 0.0 +param set-default CA_THRUSTER2_CT 1.75 +param set-default CA_THRUSTER2_AX 1.0 +param set-default CA_THRUSTER2_AY 0.0 +param set-default CA_THRUSTER2_AZ 0.0 + +param set-default CA_THRUSTER3_PX 0.12 +param set-default CA_THRUSTER3_PY 0.12 +param set-default CA_THRUSTER3_PZ 0.0 +param set-default CA_THRUSTER3_CT 1.75 +param set-default CA_THRUSTER3_AX -1.0 +param set-default CA_THRUSTER3_AY 0.0 +param set-default CA_THRUSTER3_AZ 0.0 + +param set-default CA_THRUSTER4_PX 0.12 +param set-default CA_THRUSTER4_PY -0.12 +param set-default CA_THRUSTER4_PZ 0.0 +param set-default CA_THRUSTER4_CT 1.75 +param set-default CA_THRUSTER4_AX 0.0 +param set-default CA_THRUSTER4_AY 1.0 +param set-default CA_THRUSTER4_AZ 0.0 + +param set-default CA_THRUSTER5_PX 0.12 +param set-default CA_THRUSTER5_PY 0.12 +param set-default CA_THRUSTER5_PZ 0.0 +param set-default CA_THRUSTER5_CT 1.75 +param set-default CA_THRUSTER5_AX 0.0 +param set-default CA_THRUSTER5_AY -1.0 +param set-default CA_THRUSTER5_AZ 0.0 + +param set-default CA_THRUSTER6_PX -0.12 +param set-default CA_THRUSTER6_PY -0.12 +param set-default CA_THRUSTER6_PZ 0.0 +param set-default CA_THRUSTER6_CT 1.75 +param set-default CA_THRUSTER6_AX 0.0 +param set-default CA_THRUSTER6_AY 1.0 +param set-default CA_THRUSTER6_AZ 0.0 + +param set-default CA_THRUSTER7_PX -0.12 +param set-default CA_THRUSTER7_PY 0.12 +param set-default CA_THRUSTER7_PZ 0.0 +param set-default CA_THRUSTER7_CT 1.75 +param set-default CA_THRUSTER7_AX 0.0 +param set-default CA_THRUSTER7_AY -1.0 +param set-default CA_THRUSTER7_AZ 0.0 + +param set-default PWM_MAIN_FUNC1 501 +param set-default PWM_MAIN_FUNC2 502 +param set-default PWM_MAIN_FUNC3 503 +param set-default PWM_MAIN_FUNC4 504 +param set-default PWM_MAIN_FUNC5 505 +param set-default PWM_MAIN_FUNC6 506 +param set-default PWM_MAIN_FUNC7 507 +param set-default PWM_MAIN_FUNC8 508 + +# 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/9001_gazebo-classic_3d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft new file mode 100644 index 000000000000..a0ef1317ccb1 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/9001_gazebo-classic_3d_spacecraft @@ -0,0 +1,155 @@ +#!/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 14 +param set-default MAV_TYPE 25 + +param set-default CA_THRUSTER_CNT 12 +param set-default CA_R_REV 255 + +# 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 501 +param set-default PWM_MAIN_FUNC2 502 +param set-default PWM_MAIN_FUNC3 503 +param set-default PWM_MAIN_FUNC4 504 +param set-default PWM_MAIN_FUNC5 505 +param set-default PWM_MAIN_FUNC6 506 +param set-default PWM_MAIN_FUNC7 507 +param set-default PWM_MAIN_FUNC8 508 +param set-default PWM_MAIN_FUNC9 509 +param set-default PWM_MAIN_FUNC10 510 +param set-default PWM_MAIN_FUNC11 511 +param set-default PWM_MAIN_FUNC12 512 + +# 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 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 9277ad303947..7fb8a5d3b84f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -88,6 +88,8 @@ px4_add_romfs_files( 6011_gazebo-classic_typhoon_h480.post 8011_gz_omnicopter + 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 0fd28a0f13ca..3cfbed5d849f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -220,6 +220,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" ] @@ -307,6 +308,10 @@ then uxrce_dds_port="$PX4_UXRCE_DDS_PORT" fi +# Refresh uxrce_dds_client if already running +uxrce_dds_client stop + +# Start it for SITL setup uxrce_dds_client start -t udp -h 127.0.0.1 -p $uxrce_dds_port $uxrce_dds_ns if param greater -s MNT_MODE_IN -1 diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 34b255e2555f..969f4d4b9af5 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -68,6 +68,13 @@ if(CONFIG_MODULES_MC_RATE_CONTROL) ) endif() +if(CONFIG_MODULES_SC_RATE_CONTROL) + px4_add_romfs_files( + rc.sc_apps + rc.sc_defaults + ) +endif() + if(CONFIG_MODULES_ROVER_POS_CONTROL) px4_add_romfs_files( rc.rover_apps diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot new file mode 100644 index 000000000000..595b4370521d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/70000_kth_space_robot @@ -0,0 +1,146 @@ +#!/bin/sh +# +# @name KTH Space Robot +# +# @type Space Robot +# @class 2D Space Robot +# +# @maintainer DISCOWER +# + +. ${R}etc/init.d/rc.sc_defaults + +param set-default CA_AIRFRAME 13 +param set-default MAV_TYPE 24 + +param set-default CA_THRUSTER_CNT 8 +param set-default CA_R_REV 255 + +# Auto to be provided by Custom Airframe +param set-default CA_METHOD 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 + +# disable attitude failure detection +param set-default FD_FAIL_P 0 +param set-default FD_FAIL_R 0 + +param set-default CA_THRUSTER0_PX -0.12 +param set-default CA_THRUSTER0_PY -0.12 +param set-default CA_THRUSTER0_PZ 0.0 +param set-default CA_THRUSTER0_CT 1.75 +param set-default CA_THRUSTER0_AX 1.0 +param set-default CA_THRUSTER0_AY 0.0 +param set-default CA_THRUSTER0_AZ 0.0 + +param set-default CA_THRUSTER1_PX 0.12 +param set-default CA_THRUSTER1_PY -0.12 +param set-default CA_THRUSTER1_PZ 0.0 +param set-default CA_THRUSTER1_CT 1.75 +param set-default CA_THRUSTER1_AX -1.0 +param set-default CA_THRUSTER1_AY 0.0 +param set-default CA_THRUSTER1_AZ 0.0 + +param set-default CA_THRUSTER2_PX -0.12 +param set-default CA_THRUSTER2_PY 0.12 +param set-default CA_THRUSTER2_PZ 0.0 +param set-default CA_THRUSTER2_CT 1.75 +param set-default CA_THRUSTER2_AX 1.0 +param set-default CA_THRUSTER2_AY 0.0 +param set-default CA_THRUSTER2_AZ 0.0 + +param set-default CA_THRUSTER3_PX 0.12 +param set-default CA_THRUSTER3_PY 0.12 +param set-default CA_THRUSTER3_PZ 0.0 +param set-default CA_THRUSTER3_CT 1.75 +param set-default CA_THRUSTER3_AX -1.0 +param set-default CA_THRUSTER3_AY 0.0 +param set-default CA_THRUSTER3_AZ 0.0 + +param set-default CA_THRUSTER4_PX 0.12 +param set-default CA_THRUSTER4_PY -0.12 +param set-default CA_THRUSTER4_PZ 0.0 +param set-default CA_THRUSTER4_CT 1.75 +param set-default CA_THRUSTER4_AX 0.0 +param set-default CA_THRUSTER4_AY 1.0 +param set-default CA_THRUSTER4_AZ 0.0 + +param set-default CA_THRUSTER5_PX 0.12 +param set-default CA_THRUSTER5_PY 0.12 +param set-default CA_THRUSTER5_PZ 0.0 +param set-default CA_THRUSTER5_CT 1.75 +param set-default CA_THRUSTER5_AX 0.0 +param set-default CA_THRUSTER5_AY -1.0 +param set-default CA_THRUSTER5_AZ 0.0 + +param set-default CA_THRUSTER6_PX -0.12 +param set-default CA_THRUSTER6_PY -0.12 +param set-default CA_THRUSTER6_PZ 0.0 +param set-default CA_THRUSTER6_CT 1.75 +param set-default CA_THRUSTER6_AX 0.0 +param set-default CA_THRUSTER6_AY 1.0 +param set-default CA_THRUSTER6_AZ 0.0 + +param set-default CA_THRUSTER7_PX -0.12 +param set-default CA_THRUSTER7_PY 0.12 +param set-default CA_THRUSTER7_PZ 0.0 +param set-default CA_THRUSTER7_CT 1.75 +param set-default CA_THRUSTER7_AX 0.0 +param set-default CA_THRUSTER7_AY -1.0 +param set-default CA_THRUSTER7_AZ 0.0 + + +param set-default PWM_AUX_TIM0 10 +param set-default PWM_AUX_TIM1 10 +param set-default PWM_AUX_TIM2 10 + +param set-default PWM_AUX_FUNC1 101 +param set-default PWM_AUX_FUNC2 102 +param set-default PWM_AUX_FUNC3 103 +param set-default PWM_AUX_FUNC4 104 +param set-default PWM_AUX_FUNC5 105 +param set-default PWM_AUX_FUNC6 106 +param set-default PWM_AUX_FUNC7 107 +param set-default PWM_AUX_FUNC8 108 + +param set-default PWM_AUX_DIS1 0 +param set-default PWM_AUX_DIS2 0 +param set-default PWM_AUX_DIS3 0 +param set-default PWM_AUX_DIS4 0 +param set-default PWM_AUX_DIS5 0 +param set-default PWM_AUX_DIS6 0 +param set-default PWM_AUX_DIS7 0 +param set-default PWM_AUX_DIS8 0 + +param set-default PWM_AUX_MIN1 0 +param set-default PWM_AUX_MIN2 0 +param set-default PWM_AUX_MIN3 0 +param set-default PWM_AUX_MIN4 0 +param set-default PWM_AUX_MIN5 0 +param set-default PWM_AUX_MIN6 0 +param set-default PWM_AUX_MIN7 0 +param set-default PWM_AUX_MIN8 0 + +# BOARD_PWM_FREQ is downscaled by 10, thus PWM value is given in 10s of usec +param set-default PWM_AUX_MAX1 10000 +param set-default PWM_AUX_MAX2 10000 +param set-default PWM_AUX_MAX3 10000 +param set-default PWM_AUX_MAX4 10000 +param set-default PWM_AUX_MAX5 10000 +param set-default PWM_AUX_MAX6 10000 +param set-default PWM_AUX_MAX7 10000 +param set-default PWM_AUX_MAX8 10000 + +# 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/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 89498206c3e9..5f0980deb3b0 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -38,6 +38,7 @@ px4_add_romfs_files( 18001_TF-B1 # [22000, 22999] Reserve for custom models + ) if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM) @@ -157,3 +158,10 @@ if(CONFIG_MODULES_UUV_ATT_CONTROL) 60002_uuv_bluerov2_heavy ) endif() + +if(CONFIG_MODULES_SC_RATE_CONTROL) + px4_add_romfs_files( + # [70000, 79999] Spacecraft-type platforms + 70000_kth_space_robot + ) +endif() diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_apps b/ROMFS/px4fmu_common/init.d/rc.sc_apps new file mode 100644 index 000000000000..3961a613dbab --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.sc_apps @@ -0,0 +1,37 @@ +#!/bin/sh +# +# Standard apps for sr. Attitude/Position estimator, Attitude/Position control. +# +# NOTE: Script variables are declared/initialized/unset in the rcS script. +# + +# Estimator Group Selection +ekf2 start & + +# Start MicroDDS Client +# 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 + +# +# Start Spacecraft Rate Controller. +# +sc_rate_control start + +# +# Start Spacecraft Attitude Controller. +# +sc_att_control start + +# +# Start Spacecraft Position Controller. +# +sc_pos_control start + +# Start Space Robot Thruster Controller +sc_thruster_controller start + diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_defaults b/ROMFS/px4fmu_common/init.d/rc.sc_defaults new file mode 100644 index 000000000000..a2963f689a40 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.sc_defaults @@ -0,0 +1,36 @@ +#!/bin/sh +# +# NOTE: Script variables are declared/initialized/unset in the rcS script. +# + +set VEHICLE_TYPE sc + +# MAV_TYPE_QUADROTOR 2 +#param set-default MAV_TYPE 12 + +# Set micro-dds-client to use ethernet and IP-address 192.168.0.1 +param set-default UXRCE_DDS_CFG 1000 +param set-default UXRCE_DDS_AG_IP -1062731775 + +# MAVLink +param set-default MAV_2_CONFIG 1000 +param set-default MAV_2_REMOTE_PRT 14550 +param set-default MAV_2_UDP 14550 + +# Disable preflight disarm to not interfere with external launching +param set-default COM_DISARM_PRFLT -1 +param set-default CBRK_AIRSPD_CHK 162128 +param set-default CBRK_SUPPLY_CHK 894281 +param set-default COM_ARM_ARSP_EN 0 +param set-default COM_ARM_HFLT_CHK 0 + +#Missing params +param set-default CP_DIST -1.0 + +# Default to MoCap fusion +param set-default ATT_EXT_HDG_M 2 +param set-default EKF2_EV_CTRL 15 +param set-default EKF2_EV_DELAY 5 +param set-default EKF2_GPS_CTRL 0 +param set-default EKF2_HGT_REF 3 + diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index 42b9c7e876d7..57645162740f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -68,7 +68,14 @@ then . ${R}etc/init.d/rc.uuv_apps fi - +# +# Spacecraft setup +# +if [ $VEHICLE_TYPE = sc ] +then + # Start standard sr apps. + . ${R}etc/init.d/rc.sc_apps +fi # # Generic setup (autostart ID not found). diff --git a/Tools/module_config/generate_actuators_metadata.py b/Tools/module_config/generate_actuators_metadata.py index 196119259baa..bf9096e3823b 100755 --- a/Tools/module_config/generate_actuators_metadata.py +++ b/Tools/module_config/generate_actuators_metadata.py @@ -364,6 +364,8 @@ def get_mixers(yaml_config, output_functions, verbose): actuator['group-label'] = 'Motors' elif actuator_conf['actuator_type'] == 'servo': actuator['group-label'] = 'Servos' + elif actuator_conf['actuator_type'] == 'thruster': + actuator['group-label'] = 'Thrusters' else: raise Exception('Missing group label for actuator type "{}"'.format(actuator_conf['actuator_type'])) diff --git a/Tools/px4airframes/srcparser.py b/Tools/px4airframes/srcparser.py index 56c3ce52be16..a07e695c1c82 100644 --- a/Tools/px4airframes/srcparser.py +++ b/Tools/px4airframes/srcparser.py @@ -107,6 +107,8 @@ def GetImageName(self): return "Balloon" elif (self.type == "Vectored 6 DOF UUV"): return "Vectored6DofUUV" + elif (self.type == "Space Robot"): + return "SpaceRobot" return "AirframeUnknown" def GetAirframes(self): diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index f509ced4eb1a..e3c8059b71eb 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -13,6 +13,7 @@ set -e INSTALL_NUTTX="true" INSTALL_SIM="true" +INSTALL_GZ_CLASSIC="false" INSTALL_ARCH=`uname -m` # Parse arguments @@ -25,6 +26,10 @@ do if [[ $arg == "--no-sim-tools" ]]; then INSTALL_SIM="false" fi + + if [[ $arg == "--gazebo-classic" ]]; then + INSTALL_GZ_CLASSIC="true" + fi done # detect if running in docker @@ -217,19 +222,29 @@ if [[ $INSTALL_SIM == "true" ]]; then ; # Set Java 11 as default - sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version") + sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version"); # Gazebo / Gazebo classic installation if [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then - echo "Gazebo (Garden) will be installed" - echo "Earlier versions will be removed" - # Add Gazebo binary repository - sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg - echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null - sudo apt-get update -y --quiet - - # Install Gazebo - gazebo_packages="gz-garden" + if [[ $INSTALL_GZ_CLASSIC == "true" ]]; then + echo "Gazebo 11 will be installed" + sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' + wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - + # Update list, since new gazebo-stable.list has been added + sudo apt-get update -y --quiet + # gazebo_classic_version=11 + gazebo_packages="gazebo libgazebo-dev libgazebo11" + else + echo "Gazebo (Garden) will be installed" + echo "Earlier versions will be removed" + # Add Gazebo binary repository + sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null + sudo apt-get update -y --quiet + + # Install Gazebo + gazebo_packages="gz-garden" + fi else sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index da7206e05770..09f233501607 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit da7206e057703cc645770f02437013358b71e1c0 +Subproject commit 09f2335016074e4706724ce7c44b15c11bf91ce5 diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 4e21f0fd0870..699a2896c3fd 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -43,20 +43,13 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_UAVCAN=y CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 -CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_BATTERY_STATUS=y -CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y -CONFIG_MODULES_FLIGHT_MODE_MANAGER=y -CONFIG_MODULES_FW_ATT_CONTROL=y -CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_FW_POS_CONTROL=y -CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_LAND_DETECTOR=y @@ -66,18 +59,13 @@ CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y -CONFIG_MODULES_MC_ATT_CONTROL=y -CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y -CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y -CONFIG_MODULES_MC_POS_CONTROL=y -CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SC_THRUSTER_CONTROLLER=y CONFIG_MODULES_SENSORS=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y -CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BSONDUMP=y CONFIG_SYSTEMCMDS_DMESG=y diff --git a/boards/px4/fmu-v6x/spacecraft.px4board b/boards/px4/fmu-v6x/spacecraft.px4board new file mode 100644 index 000000000000..c4a125c80496 --- /dev/null +++ b/boards/px4/fmu-v6x/spacecraft.px4board @@ -0,0 +1,19 @@ +CONFIG_MODULES_AIRSPEED_SELECTOR=n +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_MODULES_DIFFERENTIAL_DRIVE=n +CONFIG_EKF2_AUX_GLOBAL_POSITION=y +CONFIG_MODULES_SC_THRUSTER_CONTROLLER=y +CONFIG_MODULES_SC_RATE_CONTROL=y +CONFIG_MODULES_SC_ATT_CONTROL=y +CONFIG_MODULES_SC_POS_CONTROL=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 7dd7ce481a3f..39e6479edfdd 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -77,3 +77,7 @@ CONFIG_EXAMPLES_HELLO=y CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y CONFIG_EXAMPLES_PX4_SIMPLE_APP=y CONFIG_EXAMPLES_WORK_ITEM=y +CONFIG_MODULES_SC_THRUSTER_CONTROLLER=y +CONFIG_MODULES_SC_RATE_CONTROL=y +CONFIG_MODULES_SC_ATT_CONTROL=y +CONFIG_MODULES_SC_POS_CONTROL=y diff --git a/msg/ActuatorMotors.msg b/msg/ActuatorMotors.msg index e74566f1f75e..bbc908c074a7 100644 --- a/msg/ActuatorMotors.msg +++ b/msg/ActuatorMotors.msg @@ -5,6 +5,7 @@ uint64 timestamp_sample # the timestamp the data this control response is ba uint16 reversible_flags # bitset which motors are configured to be reversible uint8 ACTUATOR_FUNCTION_MOTOR1 = 101 +uint16 ACTUATOR_FUNCTION_THRUSTER1 = 501 uint8 NUM_CONTROLS = 12 float32[12] control # range: [-1, 1], where 1 means maximum positive thrust, diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index c53402bd0f35..86324d5c5d8e 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -206,6 +206,7 @@ set(msg_files TaskStackInfo.msg TecsStatus.msg TelemetryStatus.msg + ThrusterCommand.msg TiltrotorExtraControls.msg TimesyncStatus.msg TrajectoryBezier.msg diff --git a/msg/ThrusterCommand.msg b/msg/ThrusterCommand.msg new file mode 100644 index 000000000000..74a25f0f769c --- /dev/null +++ b/msg/ThrusterCommand.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) + +float32 x1 # duty cycle of thruster-pair x1 [-1;1] +float32 x2 # duty cycle of thruster-pair x2 [-1;1] +float32 y1 # duty cycle of thruster-pair y1 [-1;1] +float32 y2 # duty cycle of thruster-pair y2 [-1;1] diff --git a/msg/TrajectorySetpoint.msg b/msg/TrajectorySetpoint.msg index 4a88c86769a6..565b14de561e 100644 --- a/msg/TrajectorySetpoint.msg +++ b/msg/TrajectorySetpoint.msg @@ -10,6 +10,9 @@ float32[3] position # in meters float32[3] velocity # in meters/second float32[3] acceleration # in meters/second^2 float32[3] jerk # in meters/second^3 (for logging only) +float32[4] attitude # in quaternions +float32[3] angular_velocity # in rad/s^2 float32 yaw # euler angle of desired attitude in radians -PI..+PI float32 yawspeed # angular velocity around NED frame z-axis in radians/second + diff --git a/msg/VehicleStatus.msg b/msg/VehicleStatus.msg index 4c711b9763e1..97ebb09ca487 100644 --- a/msg/VehicleStatus.msg +++ b/msg/VehicleStatus.msg @@ -92,6 +92,7 @@ uint8 VEHICLE_TYPE_ROTARY_WING = 1 uint8 VEHICLE_TYPE_FIXED_WING = 2 uint8 VEHICLE_TYPE_ROVER = 3 uint8 VEHICLE_TYPE_AIRSHIP = 4 +uint8 VEHICLE_TYPE_SPACECRAFT = 5 uint8 FAILSAFE_DEFER_STATE_DISABLED = 0 uint8 FAILSAFE_DEFER_STATE_ENABLED = 1 diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index 4246ea3585e5..a8430b5cfb13 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -86,8 +86,11 @@ static int io_timer_handler7(int irq, void *context, void *arg); * We also allow for overrides here but all timer register usage need to be * taken into account */ -#if !defined(BOARD_PWM_FREQ) +#if !defined(BOARD_PWM_FREQ) && !defined(CONFIG_MODULES_SC_RATE_CONTROL) #define BOARD_PWM_FREQ 1000000 +/* Downscaling by 10 */ +#elif !defined(BOARD_PWM_FREQ) && defined(CONFIG_MODULES_SC_RATE_CONTROL) +#define BOARD_PWM_FREQ 100000 #endif #if !defined(BOARD_ONESHOT_FREQ) @@ -663,7 +666,8 @@ int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode) * default to updating at 50Hz */ - timer_set_rate(timer, 50); + timer_set_rate(timer, 10); + // timer_set_rate(timer, 50); /* * Note that the timer is left disabled with IRQ subs installed diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt index 783dfe3ddef9..c0b58ce45e4c 100644 --- a/src/lib/mixer_module/CMakeLists.txt +++ b/src/lib/mixer_module/CMakeLists.txt @@ -51,6 +51,7 @@ px4_add_library(mixer_module functions/FunctionLandingGear.hpp functions/FunctionManualRC.hpp functions/FunctionMotors.hpp + functions/FunctionThrusters.hpp functions/FunctionParachute.hpp functions/FunctionServos.hpp diff --git a/src/lib/mixer_module/functions/FunctionThrusters.hpp b/src/lib/mixer_module/functions/FunctionThrusters.hpp new file mode 100644 index 000000000000..61449c021fb5 --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionThrusters.hpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (c) 2021-2022 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 "FunctionProviderBase.hpp" + +/** + * Functions: Thruster1 ... ThrusterMax + */ +class FunctionThrusters : public FunctionProviderBase +{ +public: + static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::ThrusterMax - (int)OutputFunction::Thruster1 + 1, + "Unexpected num thrusters"); + + static_assert(actuator_motors_s::ACTUATOR_FUNCTION_THRUSTER1 == (int)OutputFunction::Thruster1, + "Unexpected thruster idx"); + + FunctionThrusters(const Context &context) : + _topic(&context.work_item, ORB_ID(actuator_motors)), + _thrust_factor(context.thrust_factor) + { + for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) { + _data.control[i] = NAN; + } + } + + static FunctionProviderBase *allocate(const Context &context) { return new FunctionThrusters(context); } + + void update() override + { + if (_topic.update(&_data)) { + updateValues(_data.reversible_flags, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS); + } + } + + float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Thruster1]; } + + bool allowPrearmControl() const override { return false; } + + uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; } + + bool getLatestSampleTimestamp(hrt_abstime &t) const override { t = _data.timestamp_sample; return t != 0; } + + static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values) + { + // TODO(@Pedro-Roque): for now bypass this + /*if (thrust_factor > 0.f && thrust_factor <= 1.f) { + + // thrust factor + // rel_thrust = factor * x^2 + (1-factor) * x, + const float a = thrust_factor; + const float b = (1.f - thrust_factor); + + // don't recompute for all values (ax^2+bx+c=0) + const float tmp1 = b / (2.f * a); + const float tmp2 = b * b / (4.f * a * a); + + for (int i = 0; i < num_values; ++i) { + float control = values[i]; + + if (control > 0.f) { + values[i] = -tmp1 + sqrtf(tmp2 + (control / a)); + + } else if (control < -0.f) { + values[i] = tmp1 - sqrtf(tmp2 - (control / a)); + + } else { + values[i] = 0.f; + } + } + } + + for (int i = 0; i < num_values; ++i) { + if ((reversible & (1u << i)) == 0) { + if (values[i] < -FLT_EPSILON) { + values[i] = NAN; + + } else { + // remap from [0, 1] to [-1, 1] + values[i] = values[i] * 2.f - 1.f; + } + } + }*/ + } + + bool reversible(OutputFunction func) const override { return false; } + +private: + uORB::SubscriptionCallbackWorkItem _topic; + actuator_motors_s _data{}; + const float &_thrust_factor; +}; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index ee75bcf9fa0e..3a471770ea42 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -56,6 +56,7 @@ static const FunctionProvider all_function_providers[] = { {OutputFunction::Constant_Min, &FunctionConstantMin::allocate}, {OutputFunction::Constant_Max, &FunctionConstantMax::allocate}, {OutputFunction::Motor1, OutputFunction::MotorMax, &FunctionMotors::allocate}, + {OutputFunction::Thruster1, OutputFunction::ThrusterMax, &FunctionThrusters::allocate}, {OutputFunction::Servo1, OutputFunction::ServoMax, &FunctionServos::allocate}, {OutputFunction::Peripheral_via_Actuator_Set1, OutputFunction::Peripheral_via_Actuator_Set6, &FunctionActuatorSet::allocate}, {OutputFunction::Landing_Gear, &FunctionLandingGear::allocate}, @@ -237,7 +238,8 @@ bool MixingOutput::updateSubscriptions(bool allow_wq_switch) int32_t function; if (_param_handles[i].function != PARAM_INVALID && param_get(_param_handles[i].function, &function) == 0) { - if (function >= (int32_t)OutputFunction::Motor1 && function <= (int32_t)OutputFunction::MotorMax) { + if ((function >= (int32_t)OutputFunction::Motor1 && function <= (int32_t)OutputFunction::MotorMax) || + (function >= (int32_t)OutputFunction::Thruster1 && function <= (int32_t)OutputFunction::ThrusterMax)) { switch_requested = true; } } @@ -531,10 +533,16 @@ uint16_t MixingOutput::output_limit_calc_single(int i, float value) const value = -1.f * value; } - const float output = math::interpolate(value, -1.f, 1.f, + // Thruster is a non-revertible output, so we need to map the value to the correct range + if(_function_assignment[i] >= OutputFunction::Thruster1 && _function_assignment[i] <= OutputFunction::ThrusterMax) { + const float output = math::interpolate(value, 0.f, 1.f, static_cast(_min_value[i]), static_cast(_max_value[i])); - - return math::constrain(lroundf(output), 0L, static_cast(UINT16_MAX)); + return math::constrain(lroundf(output), 0L, static_cast(UINT16_MAX)); + } else { + const float output = math::interpolate(value, -1.f, 1.f, + static_cast(_min_value[i]), static_cast(_max_value[i])); + return math::constrain(lroundf(output), 0L, static_cast(UINT16_MAX)); + } } void diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 3deaa54aa42f..bb9be22a6d20 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -44,6 +44,7 @@ #include "functions/FunctionLandingGearWheel.hpp" #include "functions/FunctionManualRC.hpp" #include "functions/FunctionMotors.hpp" +#include "functions/FunctionThrusters.hpp" #include "functions/FunctionParachute.hpp" #include "functions/FunctionServos.hpp" diff --git a/src/lib/mixer_module/output_functions.yaml b/src/lib/mixer_module/output_functions.yaml index 7543ed388bce..4dcc425dbf01 100644 --- a/src/lib/mixer_module/output_functions.yaml +++ b/src/lib/mixer_module/output_functions.yaml @@ -10,6 +10,9 @@ functions: Motor: start: 101 count: 12 + Thruster: + start: 501 + count: 12 Servo: start: 201 count: 8 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index c4e3d331e45d..7a20ea0a0911 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1707,6 +1707,7 @@ void Commander::updateParameters() const bool is_fixed = is_fixed_wing(_vehicle_status) || (is_vtol(_vehicle_status) && _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); const bool is_ground = is_ground_vehicle(_vehicle_status); + const bool is_space = is_spacecraft(_vehicle_status); /* disable manual override for all systems that rely on electronic stabilization */ if (is_rotary) { @@ -1717,6 +1718,9 @@ void Commander::updateParameters() } else if (is_ground) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER; + + } else if (is_space) { + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; } _vehicle_status.is_vtol = is_vtol(_vehicle_status); diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index fbebc7b93d8b..83c698d2bd4b 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/src/modules/commander/ModeUtil/control_mode.cpp @@ -39,7 +39,13 @@ namespace mode_util static bool stabilization_required(uint8_t vehicle_type) { - return vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + return vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + || vehicle_type == vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; +} + +static bool if_not_spacecraft(uint8_t vehicle_type) +{ + return vehicle_type != vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; } void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, @@ -64,8 +70,8 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, case vehicle_status_s::NAVIGATION_STATE_ALTCTL: vehicle_control_mode.flag_control_manual_enabled = true; - vehicle_control_mode.flag_control_altitude_enabled = true; - vehicle_control_mode.flag_control_climb_rate_enabled = true; + vehicle_control_mode.flag_control_altitude_enabled = if_not_spacecraft(vehicle_type); + vehicle_control_mode.flag_control_climb_rate_enabled = if_not_spacecraft(vehicle_type); vehicle_control_mode.flag_control_attitude_enabled = true; vehicle_control_mode.flag_control_rates_enabled = true; vehicle_control_mode.flag_control_allocation_enabled = true; @@ -76,8 +82,8 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, vehicle_control_mode.flag_control_manual_enabled = true; vehicle_control_mode.flag_control_position_enabled = true; vehicle_control_mode.flag_control_velocity_enabled = true; - vehicle_control_mode.flag_control_altitude_enabled = true; - vehicle_control_mode.flag_control_climb_rate_enabled = true; + vehicle_control_mode.flag_control_altitude_enabled = if_not_spacecraft(vehicle_type); + vehicle_control_mode.flag_control_climb_rate_enabled = if_not_spacecraft(vehicle_type); vehicle_control_mode.flag_control_attitude_enabled = true; vehicle_control_mode.flag_control_rates_enabled = true; vehicle_control_mode.flag_control_allocation_enabled = true; @@ -125,8 +131,8 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, if (offboard_control_mode.position) { vehicle_control_mode.flag_control_position_enabled = true; vehicle_control_mode.flag_control_velocity_enabled = true; - vehicle_control_mode.flag_control_altitude_enabled = true; - vehicle_control_mode.flag_control_climb_rate_enabled = true; + vehicle_control_mode.flag_control_altitude_enabled = if_not_spacecraft(vehicle_type); + vehicle_control_mode.flag_control_climb_rate_enabled = if_not_spacecraft(vehicle_type); vehicle_control_mode.flag_control_acceleration_enabled = true; vehicle_control_mode.flag_control_attitude_enabled = true; vehicle_control_mode.flag_control_rates_enabled = true; @@ -134,8 +140,8 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, } else if (offboard_control_mode.velocity) { vehicle_control_mode.flag_control_velocity_enabled = true; - vehicle_control_mode.flag_control_altitude_enabled = true; - vehicle_control_mode.flag_control_climb_rate_enabled = true; + vehicle_control_mode.flag_control_altitude_enabled = if_not_spacecraft(vehicle_type); + vehicle_control_mode.flag_control_climb_rate_enabled = if_not_spacecraft(vehicle_type); vehicle_control_mode.flag_control_acceleration_enabled = true; vehicle_control_mode.flag_control_attitude_enabled = true; vehicle_control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 48c93982495a..559cd4a76c18 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -78,6 +78,8 @@ #define VEHICLE_TYPE_VTOL_TILTROTOR 21 #define VEHICLE_TYPE_VTOL_FIXEDROTOR 22 // VTOL standard #define VEHICLE_TYPE_VTOL_TAILSITTER 23 +#define VEHICLE_TYPE_SPACECRAFT_2D 24 +#define VEHICLE_TYPE_SPACECRAFT_3D 25 #define BLINK_MSG_TIME 700000 // 3 fast blinks (in us) @@ -122,6 +124,12 @@ bool is_ground_vehicle(const vehicle_status_s ¤t_status) return (current_status.system_type == VEHICLE_TYPE_BOAT || current_status.system_type == VEHICLE_TYPE_GROUND_ROVER); } +bool is_spacecraft(const vehicle_status_s ¤t_status) +{ + return (current_status.system_type == VEHICLE_TYPE_SPACECRAFT_2D + || current_status.system_type == VEHICLE_TYPE_SPACECRAFT_3D); +} + // End time for currently blinking LED message, 0 if no blink message static hrt_abstime blink_msg_end = 0; static int fd_leds{-1}; diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 3e96a636e575..631025817f88 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -56,6 +56,7 @@ bool is_vtol(const vehicle_status_s ¤t_status); bool is_vtol_tailsitter(const vehicle_status_s ¤t_status); bool is_fixed_wing(const vehicle_status_s ¤t_status); bool is_ground_vehicle(const vehicle_status_s ¤t_status); +bool is_spacecraft(const vehicle_status_s ¤t_status); int buzzer_init(void); void buzzer_deinit(void); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 8f2c3d51ced0..41c7bcb6ba66 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -504,7 +504,8 @@ PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0); * * @value 0 Altitude/Manual * @value 1 Land/Descend - * + * @value 2 Manual + * * @group Commander */ PARAM_DEFINE_INT32(COM_POSCTL_NAVL, 0); diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 9a9b1e7cad2c..ee12d156e430 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -609,6 +609,17 @@ FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_ } } + break; + + case position_control_navigation_loss_response::Manual: // Land/Terminate + + // PosCtrl -> Stabilized + if (user_intended_mode == vehicle_status_s::NAVIGATION_STATE_POSCTL + && !modeCanRun(status_flags, user_intended_mode)) { + action = Action::FallbackStab; + user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB; + } + break; } diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index bb81dd02baeb..8ef224d8388f 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -81,6 +81,7 @@ class Failsafe : public FailsafeBase enum class position_control_navigation_loss_response : int32_t { Altitude_Manual = 0, Land_Descend = 1, + Manual = 2, }; enum class actuator_failure_failsafe_mode : int32_t { diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 0ddd4988f335..5f797f55e0aa 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -55,6 +55,7 @@ enum class AllocationMethod { enum class ActuatorType { MOTORS = 0, + THRUSTERS, SERVOS, COUNT diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp new file mode 100644 index 000000000000..ff440dadea4a --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 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 "ActuatorEffectivenessSpacecraft.hpp" + +using namespace matrix; + +ActuatorEffectivenessSpacecraft::ActuatorEffectivenessSpacecraft(ModuleParams *parent) + : ModuleParams(parent), + _sc_thrusters(this) +{ +} + +bool +ActuatorEffectivenessSpacecraft::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // Thrusters + const bool thrusters_added_successfully = _sc_thrusters.addActuators(configuration); + + return thrusters_added_successfully; +} + +void ActuatorEffectivenessSpacecraft::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + // TODO(@E-Krantz): Here is where we can add the nonlinearity of multiple thrusters open + // and how to adjust thrust in that time + return; +} \ No newline at end of file diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp new file mode 100644 index 000000000000..97bc6c8007d3 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessSpacecraft.hpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 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 "ActuatorEffectiveness.hpp" +#include "ActuatorEffectivenessThrusters.hpp" + +class ActuatorEffectivenessSpacecraft : public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessSpacecraft(ModuleParams *parent); + virtual ~ActuatorEffectivenessSpacecraft() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override + { + // TODO(@Jaeyoung-Lim): to be updated + allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION; + } + + void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override + { + // TODO(@Jaeyoung-Lim): to be updated + normalize[0] = true; + } + + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + + const char *name() const override { return "Spacecraft"; } + +protected: + ActuatorEffectivenessThrusters _sc_thrusters; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp new file mode 100644 index 000000000000..d70dcf047f44 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.cpp @@ -0,0 +1,205 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 ActuatorEffectivenessThrusters.hpp + * + * Actuator effectiveness computed from thrusters position and orientation + * + * @author Pedro Roque, + */ + +#include "ActuatorEffectivenessThrusters.hpp" + +using namespace matrix; + +ActuatorEffectivenessThrusters::ActuatorEffectivenessThrusters(ModuleParams *parent, AxisConfiguration axis_config, + bool tilt_support) + : ModuleParams(parent), _axis_config(axis_config), _tilt_support(tilt_support) +{ + + for (int i = 0; i < NUM_THRUSTERS_MAX; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PX", i); + _param_handles[i].position_x = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PY", i); + _param_handles[i].position_y = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PZ", i); + _param_handles[i].position_z = param_find(buffer); + + if (_axis_config == AxisConfiguration::Configurable) { + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AX", i); + _param_handles[i].axis_x = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AY", i); + _param_handles[i].axis_y = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AZ", i); + _param_handles[i].axis_z = param_find(buffer); + } + + snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_CT", i); + _param_handles[i].thrust_coef = param_find(buffer); + + if (_tilt_support) { + PX4_ERR("Tilt support not implemented"); + } + } + + _count_handle = param_find("CA_THRUSTER_CNT"); + + updateParams(); +} + +void ActuatorEffectivenessThrusters::updateParams() +{ + ModuleParams::updateParams(); + + int32_t count = 0; + + if (param_get(_count_handle, &count) != 0) { + PX4_ERR("param_get failed"); + return; + } + + _geometry.num_thrusters = math::min(NUM_THRUSTERS_MAX, (int)count); + + for (int i = 0; i < _geometry.num_thrusters; ++i) { + Vector3f &position = _geometry.thrusters[i].position; + param_get(_param_handles[i].position_x, &position(0)); + param_get(_param_handles[i].position_y, &position(1)); + param_get(_param_handles[i].position_z, &position(2)); + + Vector3f &axis = _geometry.thrusters[i].axis; + + switch (_axis_config) { + case AxisConfiguration::Configurable: + param_get(_param_handles[i].axis_x, &axis(0)); + param_get(_param_handles[i].axis_y, &axis(1)); + param_get(_param_handles[i].axis_z, &axis(2)); + break; + } + + param_get(_param_handles[i].thrust_coef, &_geometry.thrusters[i].thrust_coef); + } +} + +bool +ActuatorEffectivenessThrusters::addActuators(Configuration &configuration) +{ + if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) { + PX4_ERR("Wrong actuator ordering: servos need to be after motors"); + return false; + } + + int num_actuators = computeEffectivenessMatrix(_geometry, + configuration.effectiveness_matrices[configuration.selected_matrix], + configuration.num_actuators_matrix[configuration.selected_matrix]); + configuration.actuatorsAdded(ActuatorType::THRUSTERS, num_actuators); + return true; +} + +int +ActuatorEffectivenessThrusters::computeEffectivenessMatrix(const Geometry &geometry, + EffectivenessMatrix &effectiveness, int actuator_start_index) +{ + int num_actuators = 0; + + for (int i = 0; i < geometry.num_thrusters; i++) { + + if (i + actuator_start_index >= NUM_ACTUATORS) { + break; + } + + ++num_actuators; + + // Get rotor axis + Vector3f axis = geometry.thrusters[i].axis; + + // Normalize axis + float axis_norm = axis.norm(); + + if (axis_norm > FLT_EPSILON) { + axis /= axis_norm; + + } else { + // Bad axis definition, ignore this rotor + continue; + } + + // Get rotor position + const Vector3f &position = geometry.thrusters[i].position; + + // Get coefficients + float ct = geometry.thrusters[i].thrust_coef; + + if (fabsf(ct) < FLT_EPSILON) { + continue; + } + + // Compute thrust generated by this rotor + matrix::Vector3f thrust = ct * axis; + + // Compute moment generated by this rotor + matrix::Vector3f moment = ct * position.cross(axis); + + // Fill corresponding items in effectiveness matrix + for (size_t j = 0; j < 3; j++) { + effectiveness(j, i + actuator_start_index) = moment(j); + effectiveness(j + 3, i + actuator_start_index) = thrust(j); + } + } + + return num_actuators; +} + + +uint32_t ActuatorEffectivenessThrusters::getThrusters() const +{ + uint32_t thrusters = 0; + + for (int i = 0; i < _geometry.num_thrusters; ++i) { + thrusters |= 1u << i; + } + + return thrusters; +} + +bool +ActuatorEffectivenessThrusters::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + return addActuators(configuration); +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp new file mode 100644 index 000000000000..be112059c26d --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrusters.hpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 ActuatorEffectivenessThrusters.hpp + * + * Actuator effectiveness computed from thrusters position and orientation + * + * @author Pedro Roque, + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +#include +#include +#include + +class ActuatorEffectivenessTilts; + +using namespace time_literals; + +class ActuatorEffectivenessThrusters : public ModuleParams, public ActuatorEffectiveness +{ +public: + enum class AxisConfiguration { + Configurable, ///< axis can be configured + }; + + static constexpr int NUM_THRUSTERS_MAX = 12; + + struct ThrusterGeometry { + matrix::Vector3f position; + matrix::Vector3f axis; + float thrust_coef; + }; + + struct Geometry { + ThrusterGeometry thrusters[NUM_THRUSTERS_MAX]; + int num_thrusters{0}; + }; + + ActuatorEffectivenessThrusters(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable, + bool tilt_support = false); + virtual ~ActuatorEffectivenessThrusters() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override + { + allocation_method_out[0] = + AllocationMethod::SEQUENTIAL_DESATURATION; // TODO(@Jaeyoung-Lim): needs to be updated based on metric mixer + } + + void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override + { + // TODO(@Jaeyoung-Lim): needs to be updated based on metric mixer + normalize[0] = true; + } + + static int computeEffectivenessMatrix(const Geometry &geometry, + EffectivenessMatrix &effectiveness, int actuator_start_index = 0); + + bool addActuators(Configuration &configuration); + + const char *name() const override { return "Thrusters"; } + + const Geometry &geometry() const { return _geometry; } + + uint32_t getThrusters() const; + +private: + void updateParams() override; + const AxisConfiguration _axis_config; + const bool _tilt_support; ///< if true, tilt servo assignment params are loaded + + struct ParamHandles { + param_t position_x; + param_t position_y; + param_t position_z; + param_t axis_x; + param_t axis_y; + param_t axis_z; + param_t thrust_coef; + }; + ParamHandles _param_handles[NUM_THRUSTERS_MAX]; + param_t _count_handle; + + Geometry _geometry{}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrustersTest.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrustersTest.cpp new file mode 100644 index 000000000000..603248ab9d9b --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessThrustersTest.cpp @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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 ActuatorEffectivenessRotors.cpp + * + * Tests for Control Allocation Algorithms + * + * @author Julien Lecoeur + */ + +#include +#include "ActuatorEffectivenessRotors.hpp" + +using namespace matrix; + +TEST(ActuatorEffectivenessRotors, AllZeroCase) +{ + // Quad wide geometry + ActuatorEffectivenessRotors::Geometry geometry = {}; + geometry.rotors[0].position(0) = 1.0f; + geometry.rotors[0].position(1) = 1.0f; + geometry.rotors[0].position(2) = 0.0f; + geometry.rotors[0].axis(0) = 0.0f; + geometry.rotors[0].axis(1) = 0.0f; + geometry.rotors[0].axis(2) = -1.0f; + geometry.rotors[0].thrust_coef = 1.0f; + geometry.rotors[0].moment_ratio = 0.05f; + + geometry.rotors[1].position(0) = -1.0f; + geometry.rotors[1].position(1) = -1.0f; + geometry.rotors[1].position(2) = 0.0f; + geometry.rotors[1].axis(0) = 0.0f; + geometry.rotors[1].axis(1) = 0.0f; + geometry.rotors[1].axis(2) = -1.0f; + geometry.rotors[1].thrust_coef = 1.0f; + geometry.rotors[1].moment_ratio = 0.05f; + + geometry.rotors[2].position(0) = 1.0f; + geometry.rotors[2].position(1) = -1.0f; + geometry.rotors[2].position(2) = 0.0f; + geometry.rotors[2].axis(0) = 0.0f; + geometry.rotors[2].axis(1) = 0.0f; + geometry.rotors[2].axis(2) = -1.0f; + geometry.rotors[2].thrust_coef = 1.0f; + geometry.rotors[2].moment_ratio = -0.05f; + + geometry.rotors[3].position(0) = -1.0f; + geometry.rotors[3].position(1) = 1.0f; + geometry.rotors[3].position(2) = 0.0f; + geometry.rotors[3].axis(0) = 0.0f; + geometry.rotors[3].axis(1) = 0.0f; + geometry.rotors[3].axis(2) = -1.0f; + geometry.rotors[3].thrust_coef = 1.0f; + geometry.rotors[3].moment_ratio = -0.05f; + + geometry.num_rotors = 4; + + ActuatorEffectiveness::EffectivenessMatrix effectiveness; + effectiveness.setZero(); + ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness); + + const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = { + {-1.0f, 1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 1.0f, -1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.05f, 0.05f, -0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-1.0f, -1.0f, -1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + ActuatorEffectiveness::EffectivenessMatrix effectiveness_expected(expected); + + EXPECT_EQ(effectiveness, effectiveness_expected); +} + +TEST(ActuatorEffectivenessRotors, Tilt) +{ + Vector3f axis_expected{0.f, 0.f, -1.f}; + Vector3f axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{1.f, 0.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{1.f / sqrtf(2.f), 0.f, -1.f / sqrtf(2.f)}; + axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f / 2.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{-1.f, 0.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, 0.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{0.f, 0.f, -1.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, M_PI_F / 2.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{0.f, 1.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, M_PI_F / 2.f); + EXPECT_EQ(axis, axis_expected); + + axis_expected = Vector3f{0.f, -1.f, 0.f}; + axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, M_PI_F / 2.f); + EXPECT_EQ(axis, axis_expected); +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 2406b81bf80b..8aa62fa7556d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -54,6 +54,10 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessTilts.hpp ActuatorEffectivenessRotors.cpp ActuatorEffectivenessRotors.hpp + ActuatorEffectivenessThrusters.cpp + ActuatorEffectivenessThrusters.hpp + ActuatorEffectivenessSpacecraft.cpp + ActuatorEffectivenessSpacecraft.hpp ActuatorEffectivenessStandardVTOL.cpp ActuatorEffectivenessStandardVTOL.hpp ActuatorEffectivenessTiltrotorVTOL.cpp @@ -73,3 +77,4 @@ target_link_libraries(ActuatorEffectiveness px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness) px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness) +px4_add_functional_gtest(SRC ActuatorEffectivenessThrustersTest.cpp LINKLIBS ActuatorEffectiveness) \ No newline at end of file diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index ee1d2dceb2c3..bb8281c1cf73 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -40,6 +40,7 @@ */ #include "ControlAllocationPseudoInverse.hpp" +#include void ControlAllocationPseudoInverse::setEffectivenessMatrix( diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 2ead22c57079..6ccebceccd90 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -270,6 +270,14 @@ ControlAllocator::update_effectiveness_source() tmp = new ActuatorEffectivenessHelicopterCoaxial(this); break; + case EffectivenessSource::SPACECRAFT_2D: + tmp = new ActuatorEffectivenessSpacecraft(this); + break; + + case EffectivenessSource::SPACECRAFT_3D: + tmp = new ActuatorEffectivenessSpacecraft(this); + break; + default: PX4_ERR("Unknown airframe"); break; @@ -519,6 +527,16 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso slew_rate[selected_matrix](actuator_idx_matrix[selected_matrix]) = _params.slew_rate_motors[actuator_type_idx]; + } else if ((ActuatorType)actuator_type == ActuatorType::THRUSTERS) { + if (actuator_type_idx >= MAX_NUM_THRUSTERS) { + PX4_ERR("Too many thrusters"); + _num_actuators[actuator_type] = 0; + break; + } + + minimum[selected_matrix](actuator_idx_matrix[selected_matrix]) = 0.f; + + } else if ((ActuatorType)actuator_type == ActuatorType::SERVOS) { if (actuator_type_idx >= MAX_NUM_SERVOS) { PX4_ERR("Too many servos"); @@ -672,10 +690,18 @@ ControlAllocator::publish_actuator_controls() uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors() | _handled_motor_failure_bitmask; + // Actuator setpoints are shared between Motors or Thrusters. For now, we assume we have only either of them + int actuator_type = 0; + + if (_num_actuators[(int)ActuatorType::THRUSTERS] > 0) { + actuator_type = (int)ActuatorType::THRUSTERS; + } + // motors int motors_idx; - for (motors_idx = 0; motors_idx < _num_actuators[0] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) { + for (motors_idx = 0; motors_idx < _num_actuators[actuator_type] + && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) { int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); actuator_motors.control[motors_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; @@ -695,10 +721,11 @@ ControlAllocator::publish_actuator_controls() _actuator_motors_pub.publish(actuator_motors); // servos - if (_num_actuators[1] > 0) { + if (_num_actuators[(int)ActuatorType::SERVOS] > 0) { int servos_idx; - for (servos_idx = 0; servos_idx < _num_actuators[1] && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) { + for (servos_idx = 0; servos_idx < _num_actuators[(int)ActuatorType::SERVOS] + && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) { int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); actuator_servos.control[servos_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 303316f1ef52..0331117546ac 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -86,6 +87,7 @@ class ControlAllocator : public ModuleBase, public ModuleParam static constexpr int NUM_AXES = ControlAllocation::NUM_AXES; static constexpr int MAX_NUM_MOTORS = actuator_motors_s::NUM_CONTROLS; + static constexpr int MAX_NUM_THRUSTERS = actuator_motors_s::NUM_CONTROLS; static constexpr int MAX_NUM_SERVOS = actuator_servos_s::NUM_CONTROLS; using ActuatorVector = ActuatorEffectiveness::ActuatorVector; @@ -158,6 +160,8 @@ class ControlAllocator : public ModuleBase, public ModuleParam HELICOPTER_TAIL_ESC = 10, HELICOPTER_TAIL_SERVO = 11, HELICOPTER_COAXIAL = 12, + SPACECRAFT_2D = 13, + SPACECRAFT_3D = 14, }; enum class FailureMode { diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 8683d7477d95..a5d39dbe156f 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -1,4 +1,5 @@ __max_num_mc_motors: &max_num_mc_motors 12 +__max_num_mc_motors: &max_num_sc_thrusters 12 __max_num_servos: &max_num_servos 8 __max_num_tilts: &max_num_tilts 4 @@ -30,6 +31,8 @@ parameters: 10: Helicopter (tail ESC) 11: Helicopter (tail Servo) 12: Helicopter (Coaxial) + 13: Spacecraft 2D + 14: Spacecraft 3D default: 0 CA_METHOD: @@ -236,6 +239,108 @@ parameters: instance_start: 0 default: 0 + # (SC) Thrusters + CA_THRUSTER_CNT: + description: + short: Total number of thrusters + type: enum + values: + 0: '0' + 1: '1' + 2: '2' + 3: '3' + 4: '4' + 5: '5' + 6: '6' + 7: '7' + 8: '8' + 9: '9' + 10: '10' + 11: '11' + 12: '12' + default: 0 + CA_THRUSTER${i}_PX: + description: + short: Position of thruster ${i} along X body axis + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_sc_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_PY: + description: + short: Position of thruster ${i} along Y body axis + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_sc_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_PZ: + description: + short: Position of thruster ${i} along Z body axis + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_sc_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_AX: + description: + short: Axis of thruster ${i} thrust vector, X body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_sc_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_AY: + description: + short: Axis of thruster ${i} thrust vector, Y body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_sc_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_AZ: + description: + short: Axis of thruster ${i} thrust vector, Z body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_sc_thrusters + min: -100 + max: 100 + default: -1.0 + + CA_THRUSTER${i}_CT: + description: + short: Thrust coefficient of rotor ${i} + long: | + The thrust coefficient if defined as Thrust = CT * u^2, + where u (with value between actuator minimum and maximum) + is the output signal sent to the motor controller. + type: float + decimal: 1 + increment: 1 + num_instances: *max_num_sc_thrusters + min: 0 + max: 100 + default: 6.5 + # control surfaces CA_SV_CS_COUNT: description: @@ -560,6 +665,12 @@ mixer: label: 'Slew Rate' index_offset: 0 advanced: true + thruster: + functions: 'Thruster' + actuator_testing_values: + min: 0 + max: 1 + default_is_nan: true servo: functions: 'Servo' actuator_testing_values: @@ -1140,3 +1251,48 @@ mixer: parameters: - label: 'Throttle spoolup time' name: COM_SPOOLUP_TIME + + 13: # Spacecraft 2D + type: 'spacecraft_2d' + title: 'Spacecraft 2D' + actuators: + - actuator_type: 'thruster' + count: 'CA_THRUSTER_CNT' # count would be too long for 16 max size + per_item_parameters: + standard: + position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ] + extra: + - name: 'CA_THRUSTER${i}_AX' + label: 'Axis X' + function: 'axisx' + advanced: true + - name: 'CA_THRUSTER${i}_AY' + label: 'Axis Y' + function: 'axisy' + advanced: true + - name: 'CA_THRUSTER${i}_AZ' + label: 'Axis Z' + function: 'axisz' + advanced: true + + 14: # Sapcecraft 3D + title: 'Spacecraft 3D' + actuators: + - actuator_type: 'thruster' + count: 'CA_THRUSTER_CNT' + per_item_parameters: + standard: + position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ] + extra: + - name: 'CA_THRUSTER${i}_AX' + label: 'Axis X' + function: 'axisx' + advanced: true + - name: 'CA_THRUSTER${i}_AY' + label: 'Axis Y' + function: 'axisy' + advanced: true + - name: 'CA_THRUSTER${i}_AZ' + label: 'Axis Z' + function: 'axisz' + advanced: true diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml index 9f2f83519637..48af836d8f0b 100644 --- a/src/modules/ekf2/module.yaml +++ b/src/modules/ekf2/module.yaml @@ -1350,3 +1350,13 @@ parameters: min: 1.0 unit: SD decimal: 1 + EKF2_GRAVITY: + description: + short: Gravity constant + long: Sets the gravity acceleration to be used in the EKF modules. + type: float + default: 9.80665 + min: 0.0 + max: 20.0 + unit: m/s^2 + decimal: 5 \ No newline at end of file diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index 063b78dcf6c6..0f82e72e710d 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -3,7 +3,7 @@ #include constexpr uint64_t FlightTask::_timeout; -const trajectory_setpoint_s FlightTask::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; +const trajectory_setpoint_s FlightTask::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, false, {}}; const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}}; diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 0cf632c04359..008ac07569d0 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -44,7 +44,7 @@ using namespace matrix; -const trajectory_setpoint_s PositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; +const trajectory_setpoint_s PositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) { diff --git a/src/modules/sc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/sc_att_control/AttitudeControl/AttitudeControl.cpp new file mode 100644 index 000000000000..7c335485ea40 --- /dev/null +++ b/src/modules/sc_att_control/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 + +#include + +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; +} diff --git a/src/modules/sc_att_control/AttitudeControl/AttitudeControl.hpp b/src/modules/sc_att_control/AttitudeControl/AttitudeControl.hpp new file mode 100644 index 000000000000..3d702055b83d --- /dev/null +++ b/src/modules/sc_att_control/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 + * + * 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 +#include + +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 +}; diff --git a/src/modules/sc_att_control/AttitudeControl/AttitudeControlMath.hpp b/src/modules/sc_att_control/AttitudeControl/AttitudeControlMath.hpp new file mode 100644 index 000000000000..fe0f760ed49c --- /dev/null +++ b/src/modules/sc_att_control/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 + +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); +} +} diff --git a/src/modules/sc_att_control/AttitudeControl/CMakeLists.txt b/src/modules/sc_att_control/AttitudeControl/CMakeLists.txt new file mode 100644 index 000000000000..ab68cd5a74e4 --- /dev/null +++ b/src/modules/sc_att_control/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(SpacecraftAttitudeControl + AttitudeControl.cpp + AttitudeControl.hpp + AttitudeControlMath.hpp +) +target_compile_options(SpacecraftAttitudeControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(SpacecraftAttitudeControl 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) diff --git a/src/modules/sc_att_control/AttitudeControl/ScAttitudeControlMathTest.cpp b/src/modules/sc_att_control/AttitudeControl/ScAttitudeControlMathTest.cpp new file mode 100644 index 000000000000..b247fce0745e --- /dev/null +++ b/src/modules/sc_att_control/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 + +#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())); +} diff --git a/src/modules/sc_att_control/AttitudeControl/ScAttitudeControlTest.cpp b/src/modules/sc_att_control/AttitudeControl/ScAttitudeControlTest.cpp new file mode 100644 index 000000000000..c7c20c5a62e0 --- /dev/null +++ b/src/modules/sc_att_control/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 +#include +#include + +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()); +} diff --git a/src/modules/sc_att_control/CMakeLists.txt b/src/modules/sc_att_control/CMakeLists.txt new file mode 100644 index 000000000000..d180741b13b3 --- /dev/null +++ b/src/modules/sc_att_control/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# 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_module( + MODULE modules__sc_att_control + MAIN sc_att_control + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + SRCS + sc_att_control_main.cpp + sc_att_control.hpp + DEPENDS + SpacecraftAttitudeControl + mathlib + px4_work_queue + ) diff --git a/src/modules/sc_att_control/Kconfig b/src/modules/sc_att_control/Kconfig new file mode 100644 index 000000000000..9391e461d326 --- /dev/null +++ b/src/modules/sc_att_control/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_SC_ATT_CONTROL + bool "sc_att_control" + default n + ---help--- + Enable support for sc_att_control + +menuconfig USER_SC_ATT_CONTROL + bool "sc_att_control running as userspace module" + default n + depends on BOARD_PROTECTED && MODULES_SC_ATT_CONTROL + ---help--- + Put sc_att_control in userspace memory diff --git a/src/modules/sc_att_control/sc_att_control.hpp b/src/modules/sc_att_control/sc_att_control.hpp new file mode 100644 index 000000000000..9767e8b4f73e --- /dev/null +++ b/src/modules/sc_att_control/sc_att_control.hpp @@ -0,0 +1,156 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace time_literals; + +class SpacecraftAttitudeControl : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + SpacecraftAttitudeControl(); + ~SpacecraftAttitudeControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + /** + * initialize some vectors/matrices from parameters + */ + void parameters_updated(); + + float throttle_curve(float throttle_stick_input); + + /** + * 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 */ + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + // Attitude setpoint and current attitude sub + uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, 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_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ + uORB::Publication _vehicle_attitude_setpoint_pub; + + manual_control_setpoint_s _manual_control_setpoint {}; /**< manual control setpoint */ + vehicle_control_mode_s _vehicle_control_mode {}; /**< vehicle control mode */ + + perf_counter_t _loop_perf; /**< loop duration performance counter */ + + 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 _man_roll_input_filter; + AlphaFilter _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) _param_sc_man_tilt_tau, + + (ParamFloat) _param_sc_roll_p, + (ParamFloat) _param_sc_pitch_p, + (ParamFloat) _param_sc_yaw_p, + (ParamFloat) _param_sc_yaw_weight, + + (ParamFloat) _param_sc_rollrate_max, + (ParamFloat) _param_sc_pitchrate_max, + (ParamFloat) _param_sc_yawrate_max, + (ParamFloat) _param_sc_man_tilt_max, + + /* Stabilized mode params */ + (ParamFloat) _param_sc_man_y_scale /**< scaling factor from stick to yaw rate */ + ) +}; + diff --git a/src/modules/sc_att_control/sc_att_control_main.cpp b/src/modules/sc_att_control/sc_att_control_main.cpp new file mode 100644 index 000000000000..7256657b8832 --- /dev/null +++ b/src/modules/sc_att_control/sc_att_control_main.cpp @@ -0,0 +1,355 @@ +/**************************************************************************** + * + * 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, + * + */ + +#include "sc_att_control.hpp" + +#include +#include +#include + +#include "AttitudeControl/AttitudeControlMath.hpp" + +using namespace matrix; + +SpacecraftAttitudeControl::SpacecraftAttitudeControl() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _vehicle_attitude_setpoint_pub(ORB_ID(vehicle_attitude_setpoint)), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + + parameters_updated(); +} + +SpacecraftAttitudeControl::~SpacecraftAttitudeControl() +{ + perf_free(_loop_perf); +} + +bool +SpacecraftAttitudeControl::init() +{ + if (!_vehicle_attitude_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +void +SpacecraftAttitudeControl::parameters_updated() +{ + // 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.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::Run() +{ + if (should_exit()) { + _vehicle_attitude_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + parameters_updated(); + } + + // 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)) { + /* Removing print + PX4_INFO("Current setpoint: %f %f %f %f", (double)vehicle_attitude_setpoint.q_d[0], + (double)vehicle_attitude_setpoint.q_d[1], (double)vehicle_attitude_setpoint.q_d[2], + (double)vehicle_attitude_setpoint.q_d[3]);*/ + + _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 Manual/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; + } + + perf_end(_loop_perf); +} + +int SpacecraftAttitudeControl::task_spawn(int argc, char *argv[]) +{ + if (argc > 1) { + PX4_INFO("Ignoring extra parameters"); + } + + SpacecraftAttitudeControl *instance = new SpacecraftAttitudeControl(); + + 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 SpacecraftAttitudeControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int SpacecraftAttitudeControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements the spacecraft attitude controller. It takes attitude +setpoints (`vehicle_attitude_setpoint`) as inputs and outputs a rate setpoint. + +The controller has a P loop for angular error + +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 + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sc_att_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + + +/** + * Multicopter attitude control app start / stop handling function + */ +extern "C" __EXPORT int sc_att_control_main(int argc, char *argv[]) +{ + return SpacecraftAttitudeControl::main(argc, argv); +} diff --git a/src/modules/sc_att_control/sc_att_control_params.c b/src/modules/sc_att_control/sc_att_control_params.c new file mode 100644 index 000000000000..440edc3a16dd --- /dev/null +++ b/src/modules/sc_att_control/sc_att_control_params.c @@ -0,0 +1,185 @@ +/**************************************************************************** + * + * 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 Lorenz Meier + * @author Anton Babushkin + * @author Pedro Roque, + */ + +/** + * 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); \ No newline at end of file diff --git a/src/modules/sc_pos_control/CMakeLists.txt b/src/modules/sc_pos_control/CMakeLists.txt new file mode 100644 index 000000000000..c54e9132a94e --- /dev/null +++ b/src/modules/sc_pos_control/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2015-2020 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(PositionControl) + +px4_add_module( + MODULE modules__sc_pos_control + MAIN sc_pos_control + COMPILE_FLAGS + SRCS + SpacecraftPositionControl.cpp + SpacecraftPositionControl.hpp + DEPENDS + SpacecraftPositionControl + controllib + geo + SlewRate + ) diff --git a/src/modules/sc_pos_control/Kconfig b/src/modules/sc_pos_control/Kconfig new file mode 100644 index 000000000000..57f36b74b126 --- /dev/null +++ b/src/modules/sc_pos_control/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_SC_POS_CONTROL + bool "sc_pos_control" + default n + ---help--- + Enable support for sc_pos_control + +menuconfig USER_SC_POS_CONTROL + bool "sc_pos_control running as userspace module" + default y + depends on BOARD_PROTECTED && MODULES_SC_POS_CONTROL + ---help--- + Put sc_pos_control in userspace memory diff --git a/src/modules/sc_pos_control/PositionControl/CMakeLists.txt b/src/modules/sc_pos_control/PositionControl/CMakeLists.txt new file mode 100644 index 000000000000..9b197ae5b185 --- /dev/null +++ b/src/modules/sc_pos_control/PositionControl/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(SpacecraftPositionControl + ControlMath.cpp + ControlMath.hpp + PositionControl.cpp + PositionControl.hpp +) +target_include_directories(SpacecraftPositionControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +# TODO: add unit tests +# px4_add_unit_gtest(SRC ScControlMathTest.cpp LINKLIBS SpacecraftPositionControl) +# px4_add_unit_gtest(SRC ScPositionControlTest.cpp LINKLIBS SpacecraftPositionControl) diff --git a/src/modules/sc_pos_control/PositionControl/ControlMath.cpp b/src/modules/sc_pos_control/PositionControl/ControlMath.cpp new file mode 100644 index 000000000000..d2228d559743 --- /dev/null +++ b/src/modules/sc_pos_control/PositionControl/ControlMath.cpp @@ -0,0 +1,260 @@ +/**************************************************************************** + * + * Copyright (C) 2018 - 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 ControlMath.cpp + */ + +#include "ControlMath.hpp" +#include +#include +#include + +using namespace matrix; + +namespace ControlMath +{ +void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp) +{ + bodyzToAttitude(-thr_sp, yaw_sp, att_sp); + att_sp.thrust_body[2] = -thr_sp.length(); +} + +void limitTilt(Vector3f &body_unit, const Vector3f &world_unit, const float max_angle) +{ + // determine tilt + const float dot_product_unit = body_unit.dot(world_unit); + float angle = acosf(dot_product_unit); + // limit tilt + angle = math::min(angle, max_angle); + Vector3f rejection = body_unit - (dot_product_unit * world_unit); + + // corner case exactly parallel vectors + if (rejection.norm_squared() < FLT_EPSILON) { + rejection(0) = 1.f; + } + + body_unit = cosf(angle) * world_unit + sinf(angle) * rejection.unit(); +} + +void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp) +{ + // zero vector, no direction, set safe level value + if (body_z.norm_squared() < FLT_EPSILON) { + body_z(2) = 1.f; + } + + body_z.normalize(); + + // vector of desired yaw direction in XY plane, rotated by PI/2 + const Vector3f y_C{-sinf(yaw_sp), cosf(yaw_sp), 0.f}; + + // desired body_x axis, orthogonal to body_z + Vector3f body_x = y_C % body_z; + + // keep nose to front while inverted upside down + if (body_z(2) < 0.f) { + body_x = -body_x; + } + + if (fabsf(body_z(2)) < 0.000001f) { + // desired thrust is in XY plane, set X downside to construct correct matrix, + // but yaw component will not be used actually + body_x.zero(); + body_x(2) = 1.f; + } + + body_x.normalize(); + + // desired body_y axis + const Vector3f body_y = body_z % body_x; + + Dcmf R_sp; + + // fill rotation matrix + for (int i = 0; i < 3; i++) { + R_sp(i, 0) = body_x(i); + R_sp(i, 1) = body_y(i); + R_sp(i, 2) = body_z(i); + } + + // copy quaternion setpoint to attitude setpoint topic + const Quatf q_sp{R_sp}; + q_sp.copyTo(att_sp.q_d); + + // calculate euler angles, for logging only, must not be used for control + const Eulerf euler{R_sp}; + att_sp.roll_body = euler.phi(); + att_sp.pitch_body = euler.theta(); + att_sp.yaw_body = euler.psi(); +} + +Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max) +{ + if (Vector2f(v0 + v1).norm() <= max) { + // vector does not exceed maximum magnitude + return v0 + v1; + + } else if (v0.length() >= max) { + // the magnitude along v0, which has priority, already exceeds maximum. + return v0.normalized() * max; + + } else if (fabsf(Vector2f(v1 - v0).norm()) < 0.001f) { + // the two vectors are equal + return v0.normalized() * max; + + } else if (v0.length() < 0.001f) { + // the first vector is 0. + return v1.normalized() * max; + + } else { + // vf = final vector with ||vf|| <= max + // s = scaling factor + // u1 = unit of v1 + // vf = v0 + v1 = v0 + s * u1 + // constraint: ||vf|| <= max + // + // solve for s: ||vf|| = ||v0 + s * u1|| <= max + // + // Derivation: + // For simplicity, replace v0 -> v, u1 -> u + // v0(0/1/2) -> v0/1/2 + // u1(0/1/2) -> u0/1/2 + // + // ||v + s * u||^2 = (v0+s*u0)^2+(v1+s*u1)^2+(v2+s*u2)^2 = max^2 + // v0^2+2*s*u0*v0+s^2*u0^2 + v1^2+2*s*u1*v1+s^2*u1^2 + v2^2+2*s*u2*v2+s^2*u2^2 = max^2 + // s^2*(u0^2+u1^2+u2^2) + s*2*(u0*v0+u1*v1+u2*v2) + (v0^2+v1^2+v2^2-max^2) = 0 + // + // quadratic equation: + // -> s^2*a + s*b + c = 0 with solution: s1/2 = (-b +- sqrt(b^2 - 4*a*c))/(2*a) + // + // b = 2 * u.dot(v) + // a = 1 (because u is normalized) + // c = (v0^2+v1^2+v2^2-max^2) = -max^2 + ||v||^2 + // + // sqrt(b^2 - 4*a*c) = + // sqrt(4*u.dot(v)^2 - 4*(||v||^2 - max^2)) = 2*sqrt(u.dot(v)^2 +- (||v||^2 -max^2)) + // + // s1/2 = ( -2*u.dot(v) +- 2*sqrt(u.dot(v)^2 - (||v||^2 -max^2)) / 2 + // = -u.dot(v) +- sqrt(u.dot(v)^2 - (||v||^2 -max^2)) + // m = u.dot(v) + // s = -m + sqrt(m^2 - c) + // + // + // + // notes: + // - s (=scaling factor) needs to be positive + // - (max - ||v||) always larger than zero, otherwise it never entered this if-statement + Vector2f u1 = v1.normalized(); + float m = u1.dot(v0); + float c = v0.dot(v0) - max * max; + float s = -m + sqrtf(m * m - c); + return v0 + u1 * s; + } +} + +bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r, + const Vector3f &line_a, const Vector3f &line_b, Vector3f &res) +{ + // project center of sphere on line normalized AB + Vector3f ab_norm = line_b - line_a; + + if (ab_norm.length() < 0.01f) { + return true; + } + + ab_norm.normalize(); + Vector3f d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm); + float cd_len = (sphere_c - d).length(); + + if (sphere_r > cd_len) { + // we have triangle CDX with known CD and CX = R, find DX + float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); + + if ((sphere_c - line_b) * ab_norm > 0.f) { + // target waypoint is already behind us + res = line_b; + + } else { + // target is in front of us + res = d + ab_norm * dx_len; // vector A->B on line + } + + return true; + + } else { + + // have no roots, return D + res = d; // go directly to line + + // previous waypoint is still in front of us + if ((sphere_c - line_a) * ab_norm < 0.f) { + res = line_a; + } + + // target waypoint is already behind us + if ((sphere_c - line_b) * ab_norm > 0.f) { + res = line_b; + } + + return false; + } +} + +void addIfNotNan(float &setpoint, const float addition) +{ + if (PX4_ISFINITE(setpoint) && PX4_ISFINITE(addition)) { + // No NAN, add to the setpoint + setpoint += addition; + + } else if (!PX4_ISFINITE(setpoint)) { + // Setpoint NAN, take addition + setpoint = addition; + } + + // Addition is NAN or both are NAN, nothing to do +} + +void addIfNotNanVector3f(Vector3f &setpoint, const Vector3f &addition) +{ + for (int i = 0; i < 3; i++) { + addIfNotNan(setpoint(i), addition(i)); + } +} + +void setZeroIfNanVector3f(Vector3f &vector) +{ + // Adding zero vector overwrites elements that are NaN with zero + addIfNotNanVector3f(vector, Vector3f()); +} +} \ No newline at end of file diff --git a/src/modules/sc_pos_control/PositionControl/ControlMath.hpp b/src/modules/sc_pos_control/PositionControl/ControlMath.hpp new file mode 100644 index 000000000000..c2d9b8b7f8bb --- /dev/null +++ b/src/modules/sc_pos_control/PositionControl/ControlMath.hpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (C) 2018 - 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 ControlMath.hpp + * + * Simple functions for vector manipulation that do not fit into matrix lib. + * These functions are specific for controls. + */ + +#pragma once + +#include +#include + +namespace ControlMath +{ +/** + * Converts thrust vector and yaw set-point to a desired attitude. + * @param thr_sp desired 3D thrust vector + * @param yaw_sp the desired yaw + * @param att_sp attitude setpoint to fill + */ +void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp); + +/** + * Limits the tilt angle between two unit vectors + * @param body_unit unit vector that will get adjusted if angle is too big + * @param world_unit fixed vector to measure the angle against + * @param max_angle maximum tilt angle between vectors in radians + */ +void limitTilt(matrix::Vector3f &body_unit, const matrix::Vector3f &world_unit, const float max_angle); + +/** + * Converts a body z vector and yaw set-point to a desired attitude. + * @param body_z a world frame 3D vector in direction of the desired body z axis + * @param yaw_sp the desired yaw setpoint + * @param att_sp attitude setpoint to fill + */ +void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp); + +/** + * Outputs the sum of two vectors but respecting the limits and priority. + * The sum of two vectors are constraint such that v0 has priority over v1. + * This means that if the length of (v0+v1) exceeds max, then it is constraint such + * that v0 has priority. + * + * @param v0 a 2D vector that has priority given the maximum available magnitude. + * @param v1 a 2D vector that less priority given the maximum available magnitude. + * @return 2D vector + */ +matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f &v1, const float &max); + +/** + * This method was used for smoothing the corners along two lines. + * + * @param sphere_c + * @param sphere_r + * @param line_a + * @param line_b + * @param res + * return boolean + * + * Note: this method is not used anywhere and first requires review before usage. + */ +bool cross_sphere_line(const matrix::Vector3f &sphere_c, const float sphere_r, const matrix::Vector3f &line_a, + const matrix::Vector3f &line_b, matrix::Vector3f &res); + +/** + * Adds e.g. feed-forward to the setpoint making sure existing or added NANs have no influence on control. + * This function is udeful to support all the different setpoint combinations of position, velocity, acceleration with NAN representing an uncommitted value. + * @param setpoint existing possibly NAN setpoint to add to + * @param addition value/NAN to add to the setpoint + */ +void addIfNotNan(float &setpoint, const float addition); + +/** + * _addIfNotNan for Vector3f treating each element individually + * @see _addIfNotNan + */ +void addIfNotNanVector3f(matrix::Vector3f &setpoint, const matrix::Vector3f &addition); + +/** + * Overwrites elements of a Vector3f which are NaN with zero + * @param vector possibly containing NAN elements + */ +void setZeroIfNanVector3f(matrix::Vector3f &vector); +} diff --git a/src/modules/sc_pos_control/PositionControl/PositionControl.cpp b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp new file mode 100644 index 000000000000..3bee760fd2fc --- /dev/null +++ b/src/modules/sc_pos_control/PositionControl/PositionControl.cpp @@ -0,0 +1,185 @@ +/**************************************************************************** + * + * Copyright (c) 2018 - 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 PositionControl.cpp + */ + +#include "PositionControl.hpp" +#include "ControlMath.hpp" +#include +#include +#include +#include +#include + +using namespace matrix; + +const trajectory_setpoint_s ScPositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; + +void ScPositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) +{ + _gain_vel_p = P; + _gain_vel_i = I; + _gain_vel_d = D; +} + +void ScPositionControl::setVelocityLimits(const float vel_limit) +{ + _lim_vel = vel_limit; +} + +void ScPositionControl::setThrustLimit(const float max) +{ + _lim_thr_max = max; +} + +void ScPositionControl::setState(const PositionControlStates &states) +{ + _pos = states.position; + _vel = states.velocity; + _vel_dot = states.acceleration; + _att_q = states.quaternion; +} + +void ScPositionControl::setInputSetpoint(const trajectory_setpoint_s &setpoint) +{ + _pos_sp = Vector3f(setpoint.position); + _vel_sp = Vector3f(setpoint.velocity); + _acc_sp = Vector3f(setpoint.acceleration); + _quat_sp = Quatf(setpoint.attitude); +} + +bool ScPositionControl::update(const float dt) +{ + bool valid = _inputValid(); + + if (valid) { + _positionControl(); + _velocityControl(dt); + } + + // There has to be a valid output acceleration and thrust setpoint otherwise something went wrong + return valid && _acc_sp.isAllFinite() && _thr_sp.isAllFinite(); +} + +void ScPositionControl::_positionControl() +{ + // P-position controller + Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p); + // Position and feed-forward velocity setpoints or position states being NAN results in them not having an influence + ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position); + // make sure there are no NAN elements for further reference while constraining + ControlMath::setZeroIfNanVector3f(vel_sp_position); + + // Constrain velocity setpoints + _vel_sp(0) = math::constrain(_vel_sp(0), -_lim_vel, _lim_vel); + _vel_sp(1) = math::constrain(_vel_sp(1), -_lim_vel, _lim_vel); + _vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel, _lim_vel); +} + +void ScPositionControl::_velocityControl(const float dt) +{ + // Constrain vertical velocity integral + _vel_int(2) = math::constrain(_vel_int(2), -CONSTANTS_ONE_G, CONSTANTS_ONE_G); + + // PID velocity control + Vector3f vel_error = _vel_sp - _vel; + Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d); + + // No control input from setpoints or corresponding states which are NAN + ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity); + + // Accelaration to Thrust + _thr_sp = _acc_sp; + _thr_sp(0) = math::constrain(_thr_sp(0), -_lim_thr_max, _lim_thr_max); + _thr_sp(1) = math::constrain(_thr_sp(1), -_lim_thr_max, _lim_thr_max); + _thr_sp(2) = math::constrain(_thr_sp(2), -_lim_thr_max, _lim_thr_max); + + // Make sure integral doesn't get NAN + ControlMath::setZeroIfNanVector3f(vel_error); + + // Update integral part of velocity control + _vel_int += vel_error.emult(_gain_vel_i) * dt; +} + +bool ScPositionControl::_inputValid() +{ + bool valid = true; + + // Every axis x, y, z needs to have some setpoint + for (int i = 0; i <= 2; i++) { + valid = valid && (PX4_ISFINITE(_pos_sp(i)) || PX4_ISFINITE(_vel_sp(i)) || PX4_ISFINITE(_acc_sp(i))); + } + + // x and y input setpoints always have to come in pairs + valid = valid && (PX4_ISFINITE(_pos_sp(0)) == PX4_ISFINITE(_pos_sp(1))); + valid = valid && (PX4_ISFINITE(_vel_sp(0)) == PX4_ISFINITE(_vel_sp(1))); + valid = valid && (PX4_ISFINITE(_acc_sp(0)) == PX4_ISFINITE(_acc_sp(1))); + + // For each controlled state the estimate has to be valid + for (int i = 0; i <= 2; i++) { + if (PX4_ISFINITE(_pos_sp(i))) { + valid = valid && PX4_ISFINITE(_pos(i)); + } + + if (PX4_ISFINITE(_vel_sp(i))) { + valid = valid && PX4_ISFINITE(_vel(i)) && PX4_ISFINITE(_vel_dot(i)); + } + } + + return valid; +} + +void ScPositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, + vehicle_attitude_s &v_att) const +{ + // Set thrust setpoint + attitude_setpoint.thrust_body[0] = _thr_sp(0); + attitude_setpoint.thrust_body[1] = _thr_sp(1); + attitude_setpoint.thrust_body[2] = _thr_sp(2); + + // Bypass attitude control by giving same attitude setpoint to att control + if (PX4_ISFINITE(_quat_sp(0)) && PX4_ISFINITE(_quat_sp(1)) && PX4_ISFINITE(_quat_sp(2)) && PX4_ISFINITE(_quat_sp(3))) { + attitude_setpoint.q_d[0] = _quat_sp(0); + attitude_setpoint.q_d[1] = _quat_sp(1); + attitude_setpoint.q_d[2] = _quat_sp(2); + attitude_setpoint.q_d[3] = _quat_sp(3); + + } else { + attitude_setpoint.q_d[0] = v_att.q[0]; + attitude_setpoint.q_d[1] = v_att.q[1]; + attitude_setpoint.q_d[2] = v_att.q[2]; + attitude_setpoint.q_d[3] = v_att.q[3]; + } +} diff --git a/src/modules/sc_pos_control/PositionControl/PositionControl.hpp b/src/modules/sc_pos_control/PositionControl/PositionControl.hpp new file mode 100644 index 000000000000..62e4b75258cd --- /dev/null +++ b/src/modules/sc_pos_control/PositionControl/PositionControl.hpp @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * Copyright (c) 2018 - 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 PositionControl.hpp + * + * A cascaded position controller for position/velocity control only. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +struct PositionControlStates { + matrix::Vector3f position; + matrix::Vector3f velocity; + matrix::Vector3f acceleration; + matrix::Quatf quaternion; // bypassed to attitude controller +}; + +/** + * Core Position-Control for spacecrafts. + * This class contains P-controller for position and + * PID-controller for velocity. + * + * Inputs: + * vehicle position/velocity/attitude + * desired set-point position/velocity/thrust/attitude + * Output + * thrust vector and quaternion for attitude control + * + * A setpoint that is NAN is considered as not set. + * If there is a position/velocity- and thrust-setpoint present, then + * the thrust-setpoint is ommitted and recomputed from position-velocity-PID-loop. + */ +class ScPositionControl +{ +public: + + ScPositionControl() = default; + ~ScPositionControl() = default; + + /** + * Set the position control gains + * @param P 3D vector of proportional gains for x,y,z axis + */ + void setPositionGains(const matrix::Vector3f &P) { _gain_pos_p = P; } + + /** + * Set the velocity control gains + * @param P 3D vector of proportional gains for x,y,z axis + * @param I 3D vector of integral gains + * @param D 3D vector of derivative gains + */ + void setVelocityGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D); + + /** + * Set the maximum velocity to execute with feed forward and position control + * @param vel_limit velocity limit + */ + void setVelocityLimits(const float vel_limit); + + /** + * Set the minimum and maximum collective normalized thrust [0,1] that can be output by the controller + * @param min minimum thrust e.g. 0.1 or 0 + * @param max maximum thrust e.g. 0.9 or 1 + */ + void setThrustLimit(const float max); + + /** + * Pass the current vehicle state to the controller + * @param PositionControlStates structure + */ + void setState(const PositionControlStates &states); + + /** + * Pass the desired setpoints + * Note: NAN value means no feed forward/leave state uncontrolled if there's no higher order setpoint. + * @param setpoint setpoints including feed-forwards to execute in update() + */ + void setInputSetpoint(const trajectory_setpoint_s &setpoint); + + /** + * Apply P-position and PID-velocity controller that updates the member + * thrust, yaw- and yawspeed-setpoints. + * @see _thr_sp + * @see _yaw_sp + * @see _yawspeed_sp + * @param dt time in seconds since last iteration + * @return true if update succeeded and output setpoint is executable, false if not + */ + bool update(const float dt); + + /** + * Set the integral term in xy to 0. + * @see _vel_int + */ + void resetIntegral() { _vel_int.setZero(); } + + /** + * Get the controllers output attitude setpoint + * This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control. + * It needs to be executed by the attitude controller to achieve velocity and position tracking. + * @param attitude_setpoint reference to struct to fill up + */ + void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, vehicle_attitude_s &v_att) const; + + /** + * All setpoints are set to NAN (uncontrolled). Timestampt zero. + */ + static const trajectory_setpoint_s empty_trajectory_setpoint; + +private: + // The range limits of the hover thrust configuration/estimate + static constexpr float HOVER_THRUST_MIN = 0.05f; + static constexpr float HOVER_THRUST_MAX = 0.9f; + + bool _inputValid(); + + void _positionControl(); ///< Position proportional control + void _velocityControl(const float dt); ///< Velocity PID control + + // Gains + matrix::Vector3f _gain_pos_p; ///< Position control proportional gain + matrix::Vector3f _gain_vel_p; ///< Velocity control proportional gain + matrix::Vector3f _gain_vel_i; ///< Velocity control integral gain + matrix::Vector3f _gain_vel_d; ///< Velocity control derivative gain + + // Limits + float _lim_vel{}; ///< Horizontal velocity limit with feed forward and position control + float _lim_thr_min{}; ///< Minimum collective thrust allowed as output [-1,0] e.g. -0.9 + float _lim_thr_max{}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1 + + // States + matrix::Vector3f _pos; /**< current position */ + matrix::Vector3f _vel; /**< current velocity */ + matrix::Vector3f _vel_dot; /**< velocity derivative (replacement for acceleration estimate) */ + matrix::Vector3f _vel_int; /**< integral term of the velocity controller */ + matrix::Quatf _att_q; /**< current attitude */ + float _yaw{}; /**< current heading */ + + // Setpoints + matrix::Vector3f _pos_sp; /**< desired position */ + matrix::Vector3f _vel_sp; /**< desired velocity */ + matrix::Vector3f _acc_sp; /**< desired acceleration */ + matrix::Vector3f _thr_sp; /**< desired thrust */ + matrix::Quatf _quat_sp; /**< desired attitude */ +}; diff --git a/src/modules/sc_pos_control/PositionControl/ScControlMathTest.cpp b/src/modules/sc_pos_control/PositionControl/ScControlMathTest.cpp new file mode 100644 index 000000000000..e9b6fba12fb4 --- /dev/null +++ b/src/modules/sc_pos_control/PositionControl/ScControlMathTest.cpp @@ -0,0 +1,258 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include + +using namespace matrix; +using namespace ControlMath; + +TEST(ControlMathTest, LimitTiltUnchanged) +{ + Vector3f body = Vector3f(0.f, 0.f, 1.f).normalized(); + Vector3f body_before = body; + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, body_before); + + body = Vector3f(0.f, .1f, 1.f).normalized(); + body_before = body; + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, body_before); +} + +TEST(ControlMathTest, LimitTiltOpposite) +{ + Vector3f body = Vector3f(0.f, 0.f, -1.f).normalized(); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 45.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); +} + +TEST(ControlMathTest, LimitTiltAlmostOpposite) +{ + // This case doesn't trigger corner case handling but is very close to it + Vector3f body = Vector3f(0.001f, 0.f, -1.f).normalized(); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 45.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); +} + +TEST(ControlMathTest, LimitTilt45degree) +{ + Vector3f body = Vector3f(1.f, 0.f, 0.f); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, Vector3f(M_SQRT1_2_F, 0, M_SQRT1_2_F)); + + body = Vector3f(0.f, 1.f, 0.f); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, Vector3f(0.f, M_SQRT1_2_F, M_SQRT1_2_F)); +} + +TEST(ControlMathTest, LimitTilt10degree) +{ + Vector3f body = Vector3f(1.f, 1.f, .1f).normalized(); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 10.f); + float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 10.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); + EXPECT_FLOAT_EQ(body(0), body(1)); + + body = Vector3f(1, 2, .2f); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 10.f); + angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 10.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); + EXPECT_FLOAT_EQ(2.f * body(0), body(1)); +} + +TEST(ControlMathTest, ThrottleAttitudeMapping) +{ + /* expected: zero roll, zero pitch, zero yaw, full thr mag + * reason: thrust pointing full upward */ + Vector3f thr{0.f, 0.f, -1.f}; + float yaw = 0.f; + vehicle_attitude_setpoint_s att{}; + thrustToAttitude(thr, yaw, att); + EXPECT_FLOAT_EQ(att.roll_body, 0.f); + EXPECT_FLOAT_EQ(att.pitch_body, 0.f); + EXPECT_FLOAT_EQ(att.yaw_body, 0.f); + EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); + + /* expected: same as before but with 90 yaw + * reason: only yaw changed */ + yaw = M_PI_2_F; + thrustToAttitude(thr, yaw, att); + EXPECT_FLOAT_EQ(att.roll_body, 0.f); + EXPECT_FLOAT_EQ(att.pitch_body, 0.f); + EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); + EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); + + /* expected: same as before but roll 180 + * reason: thrust points straight down and order Euler + * order is: 1. roll, 2. pitch, 3. yaw */ + thr = Vector3f(0.f, 0.f, 1.f); + thrustToAttitude(thr, yaw, att); + EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F); + EXPECT_FLOAT_EQ(att.pitch_body, 0.f); + EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); + EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); +} + +TEST(ControlMathTest, ConstrainXYPriorities) +{ + const float max = 5.f; + // v0 already at max + Vector2f v0(max, 0.f); + Vector2f v1(v0(1), -v0(0)); + + Vector2f v_r = constrainXY(v0, v1, max); + EXPECT_FLOAT_EQ(v_r(0), max); + EXPECT_FLOAT_EQ(v_r(1), 0.f); + + // norm of v1 exceeds max but v0 is zero + v0.zero(); + v_r = constrainXY(v0, v1, max); + EXPECT_FLOAT_EQ(v_r(1), -max); + EXPECT_FLOAT_EQ(v_r(0), 0.f); + + v0 = Vector2f(.5f, .5f); + v1 = Vector2f(.5f, -.5f); + v_r = constrainXY(v0, v1, max); + const float diff = Vector2f(v_r - (v0 + v1)).length(); + EXPECT_FLOAT_EQ(diff, 0.f); + + // v0 and v1 exceed max and are perpendicular + v0 = Vector2f(4.f, 0.f); + v1 = Vector2f(0.f, -4.f); + v_r = constrainXY(v0, v1, max); + EXPECT_FLOAT_EQ(v_r(0), v0(0)); + EXPECT_GT(v_r(0), 0.f); + const float remaining = sqrtf(max * max - (v0(0) * v0(0))); + EXPECT_FLOAT_EQ(v_r(1), -remaining); +} + +TEST(ControlMathTest, CrossSphereLine) +{ + /* Testing 9 positions (+) around waypoints (o): + * + * Far + + + + * + * Near + + + + * On trajectory --+----o---------+---------o----+-- + * prev curr + * + * Expected targets (1, 2, 3): + * Far + + + + * + * + * On trajectory -------1---------2---------3------- + * + * + * Near + + + + * On trajectory -------o---1---------2-----3------- + * + * + * On trajectory --+----o----1----+--------2/3---+-- */ + const Vector3f prev = Vector3f(0.f, 0.f, 0.f); + const Vector3f curr = Vector3f(0.f, 0.f, 2.f); + Vector3f res; + bool retval = false; + + // on line, near, before previous waypoint + retval = cross_sphere_line(Vector3f(0.f, 0.f, -.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 0.5f)); + + // on line, near, before target waypoint + retval = cross_sphere_line(Vector3f(0.f, 0.f, 1.f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); + + // on line, near, after target waypoint + retval = cross_sphere_line(Vector3f(0.f, 0.f, 2.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); + + // near, before previous waypoint + retval = cross_sphere_line(Vector3f(0.f, .5f, -.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, .366025388f)); + + // near, before target waypoint + retval = cross_sphere_line(Vector3f(0.f, .5f, 1.f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 1.866025448f)); + + // near, after target waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, .5f, 2.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); + + // far, before previous waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, -.5f), 1.f, prev, curr, res); + EXPECT_FALSE(retval); + EXPECT_EQ(res, Vector3f()); + + // far, before target waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, 1.f), 1.f, prev, curr, res); + EXPECT_FALSE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 1.f)); + + // far, after target waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, 2.5f), 1.f, prev, curr, res); + EXPECT_FALSE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); +} + +TEST(ControlMathTest, addIfNotNan) +{ + float v = 1.f; + // regular addition + ControlMath::addIfNotNan(v, 2.f); + EXPECT_EQ(v, 3.f); + // addition is NAN and has no influence + ControlMath::addIfNotNan(v, NAN); + EXPECT_EQ(v, 3.f); + v = NAN; + // both summands are NAN + ControlMath::addIfNotNan(v, NAN); + EXPECT_TRUE(std::isnan(v)); + // regular value gets added to NAN and overwrites it + ControlMath::addIfNotNan(v, 3.f); + EXPECT_EQ(v, 3.f); +} diff --git a/src/modules/sc_pos_control/PositionControl/ScPositionControlTest.cpp b/src/modules/sc_pos_control/PositionControl/ScPositionControlTest.cpp new file mode 100644 index 000000000000..a9ce422ed580 --- /dev/null +++ b/src/modules/sc_pos_control/PositionControl/ScPositionControlTest.cpp @@ -0,0 +1,368 @@ +/**************************************************************************** + * + * 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 +#include +#include + +using namespace matrix; + +TEST(PositionControlTest, EmptySetpoint) +{ + ScPositionControl position_control; + + vehicle_attitude_setpoint_s attitude{}; + position_control.getAttitudeSetpoint(attitude); + EXPECT_FLOAT_EQ(attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(attitude.pitch_body, 0.f); + EXPECT_FLOAT_EQ(attitude.yaw_body, 0.f); + EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f); + EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f)); + EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f)); + EXPECT_EQ(attitude.reset_integral, false); + EXPECT_EQ(attitude.fw_control_yaw_wheel, false); +} + +class PositionControlBasicTest : public ::testing::Test +{ +public: + PositionControlBasicTest() + { + _position_control.setPositionGains(Vector3f(1.f, 1.f, 1.f)); + _position_control.setVelocityGains(Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f)); + _position_control.setVelocityLimits(1.f, 1.f, 1.f); + _position_control.setThrustLimits(0.1f, MAXIMUM_THRUST); + _position_control.setHorizontalThrustMargin(HORIZONTAL_THRUST_MARGIN); + _position_control.setTiltLimit(1.f); + _position_control.setHoverThrust(.5f); + } + + bool runController() + { + _position_control.setInputSetpoint(_input_setpoint); + const bool ret = _position_control.update(.1f); + _position_control.getAttitudeSetpoint(_attitude); + return ret; + } + + ScPositionControl _position_control; + trajectory_setpoint_s _input_setpoint{PositionControl::empty_trajectory_setpoint}; + vehicle_local_position_setpoint_s _output_setpoint{}; + vehicle_attitude_setpoint_s _attitude{}; + + static constexpr float MAXIMUM_THRUST = 0.9f; + static constexpr float HORIZONTAL_THRUST_MARGIN = 0.3f; +}; + +class PositionControlBasicDirectionTest : public PositionControlBasicTest +{ +public: + void checkDirection() + { + Vector3f thrust(_output_setpoint.thrust); + EXPECT_GT(thrust(0), 0.f); + EXPECT_GT(thrust(1), 0.f); + EXPECT_LT(thrust(2), 0.f); + + Vector3f body_z = Quatf(_attitude.q_d).dcm_z(); + EXPECT_LT(body_z(0), 0.f); + EXPECT_LT(body_z(1), 0.f); + EXPECT_GT(body_z(2), 0.f); + } +}; + +TEST_F(PositionControlBasicDirectionTest, PositionDirection) +{ + Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.position); + EXPECT_TRUE(runController()); + checkDirection(); +} + +TEST_F(PositionControlBasicDirectionTest, VelocityDirection) +{ + Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.velocity); + EXPECT_TRUE(runController()); + checkDirection(); +} + +TEST_F(PositionControlBasicTest, TiltLimit) +{ + Vector3f(10.f, 10.f, 0.f).copyTo(_input_setpoint.position); + + EXPECT_TRUE(runController()); + Vector3f body_z = Quatf(_attitude.q_d).dcm_z(); + float angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_GT(angle, 0.f); + EXPECT_LE(angle, 1.f); + + _position_control.setTiltLimit(0.5f); + EXPECT_TRUE(runController()); + body_z = Quatf(_attitude.q_d).dcm_z(); + angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_GT(angle, 0.f); + EXPECT_LE(angle, .50001f); + + _position_control.setTiltLimit(1.f); // restore original +} + +TEST_F(PositionControlBasicTest, VelocityLimit) +{ + Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position); + + EXPECT_TRUE(runController()); + Vector2f velocity_xy(_output_setpoint.vx, _output_setpoint.vy); + EXPECT_LE(velocity_xy.norm(), 1.f); + EXPECT_LE(abs(_output_setpoint.vz), 1.f); +} + +TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit) +{ + // Given a setpoint that drives the controller into vertical and horizontal saturation + Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position); + + // When you run it for one iteration + runController(); + Vector3f thrust(_output_setpoint.thrust); + + // Then the thrust vector length is limited by the maximum + EXPECT_FLOAT_EQ(thrust.norm(), MAXIMUM_THRUST); + + // Then the horizontal thrust is limited by its margin + EXPECT_FLOAT_EQ(thrust(0), HORIZONTAL_THRUST_MARGIN / sqrt(2.f)); + EXPECT_FLOAT_EQ(thrust(1), HORIZONTAL_THRUST_MARGIN / sqrt(2.f)); + EXPECT_FLOAT_EQ(thrust(2), + -sqrt(MAXIMUM_THRUST * MAXIMUM_THRUST - HORIZONTAL_THRUST_MARGIN * HORIZONTAL_THRUST_MARGIN)); + thrust.print(); + + // Then the collective thrust is limited by the maximum + EXPECT_EQ(_attitude.thrust_body[0], 0.f); + EXPECT_EQ(_attitude.thrust_body[1], 0.f); + EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -MAXIMUM_THRUST); + + // Then the horizontal margin results in a tilt with the ratio of: horizontal margin / maximum thrust + EXPECT_FLOAT_EQ(_attitude.roll_body, asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); + // TODO: add this line back once attitude setpoint generation strategy does not align body yaw with heading all the time anymore + // EXPECT_FLOAT_EQ(_attitude.pitch_body, -asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); +} + +TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit) +{ + Vector3f(10.f, 0.f, 10.f).copyTo(_input_setpoint.position); + + runController(); + Vector3f thrust(_output_setpoint.thrust); + EXPECT_FLOAT_EQ(thrust.length(), 0.1f); + + EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.1f); + + EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(_attitude.pitch_body, -1.f); +} + +TEST_F(PositionControlBasicTest, FailsafeInput) +{ + _input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f; + _input_setpoint.velocity[2] = .1f; + + EXPECT_TRUE(runController()); + EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f); + EXPECT_FLOAT_EQ(_attitude.thrust_body[1], 0.f); + EXPECT_LT(_output_setpoint.thrust[2], -.1f); + EXPECT_GT(_output_setpoint.thrust[2], -.5f); + EXPECT_GT(_attitude.thrust_body[2], -.5f); + EXPECT_LE(_attitude.thrust_body[2], -.1f); +} + +TEST_F(PositionControlBasicTest, IdleThrustInput) +{ + // High downwards acceleration to make sure there's no thrust + Vector3f(0.f, 0.f, 100.f).copyTo(_input_setpoint.acceleration); + + EXPECT_TRUE(runController()); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[0], 0.f); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[1], 0.f); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f); // minimum thrust +} + +TEST_F(PositionControlBasicTest, InputCombinationsPosition) +{ + Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position); + + EXPECT_TRUE(runController()); + EXPECT_FLOAT_EQ(_output_setpoint.x, .1f); + EXPECT_FLOAT_EQ(_output_setpoint.y, .2f); + EXPECT_FLOAT_EQ(_output_setpoint.z, .3f); + EXPECT_FALSE(isnan(_output_setpoint.vx)); + EXPECT_FALSE(isnan(_output_setpoint.vy)); + EXPECT_FALSE(isnan(_output_setpoint.vz)); + EXPECT_FALSE(isnan(_output_setpoint.thrust[0])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[1])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[2])); +} + +TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity) +{ + _input_setpoint.velocity[0] = .1f; + _input_setpoint.velocity[1] = .2f; + _input_setpoint.position[2] = .3f; // altitude + + EXPECT_TRUE(runController()); + EXPECT_TRUE(isnan(_output_setpoint.x)); + EXPECT_TRUE(isnan(_output_setpoint.y)); + EXPECT_FLOAT_EQ(_output_setpoint.z, .3f); + EXPECT_FLOAT_EQ(_output_setpoint.vx, .1f); + EXPECT_FLOAT_EQ(_output_setpoint.vy, .2f); + EXPECT_FALSE(isnan(_output_setpoint.vz)); + EXPECT_FALSE(isnan(_output_setpoint.thrust[0])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[1])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[2])); +} + +TEST_F(PositionControlBasicTest, SetpointValiditySimple) +{ + EXPECT_FALSE(runController()); + _input_setpoint.position[0] = .1f; + EXPECT_FALSE(runController()); + _input_setpoint.position[1] = .2f; + EXPECT_FALSE(runController()); + _input_setpoint.acceleration[2] = .3f; + EXPECT_TRUE(runController()); +} + +TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations) +{ + // This test runs any combination of set and unset (NAN) setpoints and checks if it gets accepted or rejected correctly + float *const setpoint_loop_access_map[] = {&_input_setpoint.position[0], &_input_setpoint.velocity[0], &_input_setpoint.acceleration[0], + &_input_setpoint.position[1], &_input_setpoint.velocity[1], &_input_setpoint.acceleration[1], + &_input_setpoint.position[2], &_input_setpoint.velocity[2], &_input_setpoint.acceleration[2] + }; + + for (int combination = 0; combination < 512; combination++) { + _input_setpoint = PositionControl::empty_trajectory_setpoint; + + for (int j = 0; j < 9; j++) { + if (combination & (1 << j)) { + // Set arbitrary finite value, some values clearly hit the limits to check these corner case combinations + *(setpoint_loop_access_map[j]) = static_cast(combination) / static_cast(j + 1); + } + } + + // Expect at least one setpoint per axis + const bool has_x_setpoint = ((combination & 7) != 0); + const bool has_y_setpoint = (((combination >> 3) & 7) != 0); + const bool has_z_setpoint = (((combination >> 6) & 7) != 0); + // Expect xy setpoints to come in pairs + const bool has_xy_pairs = (combination & 7) == ((combination >> 3) & 7); + const bool expected_result = has_x_setpoint && has_y_setpoint && has_z_setpoint && has_xy_pairs; + + EXPECT_EQ(runController(), expected_result) << "combination " << combination << std::endl + << "input" << std::endl + << "position " << _input_setpoint.position[0] << ", " + << _input_setpoint.position[1] << ", " << _input_setpoint.position[2] << std::endl + << "velocity " << _input_setpoint.velocity[0] << ", " + << _input_setpoint.velocity[1] << ", " << _input_setpoint.velocity[2] << std::endl + << "acceleration " << _input_setpoint.acceleration[0] << ", " + << _input_setpoint.acceleration[1] << ", " << _input_setpoint.acceleration[2] << std::endl + << "output" << std::endl + << "position " << _output_setpoint.x << ", " << _output_setpoint.y << ", " << _output_setpoint.z << std::endl + << "velocity " << _output_setpoint.vx << ", " << _output_setpoint.vy << ", " << _output_setpoint.vz << std::endl + << "acceleration " << _output_setpoint.acceleration[0] << ", " + << _output_setpoint.acceleration[1] << ", " << _output_setpoint.acceleration[2] << std::endl; + } +} + +TEST_F(PositionControlBasicTest, InvalidState) +{ + Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position); + + PositionControlStates states{}; + states.position(0) = NAN; + _position_control.setState(states); + EXPECT_FALSE(runController()); + + states.velocity(0) = NAN; + _position_control.setState(states); + EXPECT_FALSE(runController()); + + states.position(0) = 0.f; + _position_control.setState(states); + EXPECT_FALSE(runController()); + + states.velocity(0) = 0.f; + states.acceleration(1) = NAN; + _position_control.setState(states); + EXPECT_FALSE(runController()); +} + + +TEST_F(PositionControlBasicTest, UpdateHoverThrust) +{ + // GIVEN: some hover thrust and 0 velocity change + const float hover_thrust = 0.6f; + _position_control.setHoverThrust(hover_thrust); + + Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity); + + // WHEN: we run the controller + EXPECT_TRUE(runController()); + + // THEN: the output thrust equals the hover thrust + EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust); + + // HOWEVER WHEN: we set a new hover thrust through the update function + const float hover_thrust_new = 0.7f; + _position_control.updateHoverThrust(hover_thrust_new); + EXPECT_TRUE(runController()); + + // THEN: the integral is updated to avoid discontinuities and + // the output is still the same + EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust); +} + +TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint) +{ + // GIVEN: the controller was ran with an invalid setpoint containing some valid values + _input_setpoint.position[0] = .1f; + _input_setpoint.position[1] = .2f; + // all z-axis setpoints stay NAN + EXPECT_FALSE(runController()); + + // WHEN: we run the controller with a valid setpoint + _input_setpoint = PositionControl::empty_trajectory_setpoint; + Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity); + EXPECT_TRUE(runController()); + + // THEN: the integral did not wind up and produce unexpected deviation + EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f); +} diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.cpp b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp new file mode 100644 index 000000000000..8b7a9ff16ed6 --- /dev/null +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.cpp @@ -0,0 +1,486 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2020 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 "SpacecraftPositionControl.hpp" + +#include +#include +#include "PositionControl/ControlMath.hpp" + +using namespace matrix; + +SpacecraftPositionControl::SpacecraftPositionControl() : + SuperBlock(nullptr, "SPC"), + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _vehicle_attitude_setpoint_pub(ORB_ID(vehicle_attitude_setpoint)), + _vel_x_deriv(this, "VELD"), + _vel_y_deriv(this, "VELD"), + _vel_z_deriv(this, "VELD") +{ + parameters_update(true); +} + +SpacecraftPositionControl::~SpacecraftPositionControl() +{ + perf_free(_cycle_perf); +} + +bool SpacecraftPositionControl::init() +{ + if (!_local_pos_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + _time_stamp_last_loop = hrt_absolute_time(); + ScheduleNow(); + + return true; +} + +void SpacecraftPositionControl::parameters_update(bool force) +{ + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + ModuleParams::updateParams(); + SuperBlock::updateParams(); + + int num_changed = 0; + + if (_param_sys_vehicle_resp.get() >= 0.f) { + // make it less sensitive at the lower end + float responsiveness = _param_sys_vehicle_resp.get() * _param_sys_vehicle_resp.get(); + + num_changed += _param_mpc_acc.commit_no_notification(math::lerp(1.f, 15.f, responsiveness)); + num_changed += _param_mpc_acc_max.commit_no_notification(math::lerp(2.f, 15.f, responsiveness)); + num_changed += _param_mpc_man_y_max.commit_no_notification(math::lerp(80.f, 450.f, responsiveness)); + + if (responsiveness > 0.6f) { + num_changed += _param_mpc_man_y_tau.commit_no_notification(0.f); + + } else { + num_changed += _param_mpc_man_y_tau.commit_no_notification(math::lerp(0.5f, 0.f, responsiveness / 0.6f)); + } + + num_changed += _param_mpc_jerk_max.commit_no_notification(math::lerp(2.f, 50.f, responsiveness)); + num_changed += _param_mpc_jerk_auto.commit_no_notification(math::lerp(1.f, 25.f, responsiveness)); + } + + if (_param_mpc_vel_all.get() >= 0.f) { + float all_vel = _param_mpc_vel_all.get(); + num_changed += _param_mpc_vel_manual.commit_no_notification(all_vel); + num_changed += _param_mpc_vel_cruise.commit_no_notification(all_vel); + num_changed += _param_mpc_vel_max.commit_no_notification(all_vel); + } + + if (num_changed > 0) { + param_notify_changes(); + } + + _control.setPositionGains(Vector3f(_param_mpc_pos_p.get(), _param_mpc_pos_p.get(), _param_mpc_pos_p.get())); + _control.setVelocityGains( + Vector3f(_param_mpc_vel_p_acc.get(), _param_mpc_vel_p_acc.get(), _param_mpc_vel_p_acc.get()), + Vector3f(_param_mpc_vel_i_acc.get(), _param_mpc_vel_i_acc.get(), _param_mpc_vel_i_acc.get()), + Vector3f(_param_mpc_vel_d_acc.get(), _param_mpc_vel_d_acc.get(), _param_mpc_vel_d_acc.get())); + + // Check that the design parameters are inside the absolute maximum constraints + if (_param_mpc_vel_cruise.get() > _param_mpc_vel_max.get()) { + _param_mpc_vel_cruise.set(_param_mpc_vel_max.get()); + _param_mpc_vel_cruise.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Cruise speed has been constrained by max speed\t"); + /* EVENT + * @description SPC_VEL_CRUISE is set to {1:.0}. + */ + events::send(events::ID("sc_pos_ctrl_cruise_set"), events::Log::Warning, + "Cruise speed has been constrained by maximum speed", _param_mpc_vel_max.get()); + } + + if (_param_mpc_vel_manual.get() > _param_mpc_vel_max.get()) { + _param_mpc_vel_manual.set(_param_mpc_vel_max.get()); + _param_mpc_vel_manual.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Manual speed has been constrained by max speed\t"); + /* EVENT + * @description SPC_VEL_MANUAL is set to {1:.0}. + */ + events::send(events::ID("sc_pos_ctrl_man_vel_set"), events::Log::Warning, + "Manual speed has been constrained by maximum speed", _param_mpc_vel_max.get()); + } + + yaw_rate = math::radians(_param_mpc_man_y_max.get()); + } +} + +PositionControlStates SpacecraftPositionControl::set_vehicle_states(const vehicle_local_position_s + &vehicle_local_position, const vehicle_attitude_s &vehicle_attitude) +{ + PositionControlStates states; + + const Vector2f position_xy(vehicle_local_position.x, vehicle_local_position.y); + + // only set position states if valid and finite + if (vehicle_local_position.xy_valid && position_xy.isAllFinite()) { + states.position.xy() = position_xy; + + } else { + states.position(0) = states.position(1) = NAN; + } + + if (PX4_ISFINITE(vehicle_local_position.z) && vehicle_local_position.z_valid) { + states.position(2) = vehicle_local_position.z; + + } else { + states.position(2) = NAN; + } + + const Vector2f velocity_xy(vehicle_local_position.vx, vehicle_local_position.vy); + + if (vehicle_local_position.v_xy_valid && velocity_xy.isAllFinite()) { + states.velocity.xy() = velocity_xy; + states.acceleration(0) = _vel_x_deriv.update(velocity_xy(0)); + states.acceleration(1) = _vel_y_deriv.update(velocity_xy(1)); + + } else { + states.velocity(0) = states.velocity(1) = NAN; + states.acceleration(0) = states.acceleration(1) = NAN; + + // reset derivatives to prevent acceleration spikes when regaining velocity + _vel_x_deriv.reset(); + _vel_y_deriv.reset(); + } + + if (PX4_ISFINITE(vehicle_local_position.vz) && vehicle_local_position.v_z_valid) { + states.velocity(2) = vehicle_local_position.vz; + states.acceleration(2) = _vel_z_deriv.update(states.velocity(2)); + + } else { + states.velocity(2) = NAN; + states.acceleration(2) = NAN; + + // reset derivative to prevent acceleration spikes when regaining velocity + _vel_z_deriv.reset(); + } + + if (PX4_ISFINITE(vehicle_attitude.q[0]) && PX4_ISFINITE(vehicle_attitude.q[1]) && PX4_ISFINITE(vehicle_attitude.q[2]) + && PX4_ISFINITE(vehicle_attitude.q[3])) { + states.quaternion = Quatf(vehicle_attitude.q); + + } else { + states.quaternion = Quatf(); + } + + return states; +} + +void SpacecraftPositionControl::Run() +{ + if (should_exit()) { + _local_pos_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + // reschedule backup + ScheduleDelayed(100_ms); + + parameters_update(false); + + perf_begin(_cycle_perf); + vehicle_local_position_s vehicle_local_position; + vehicle_attitude_s v_att; + + if (_local_pos_sub.update(&vehicle_local_position)) { + const float dt = + math::constrain(((vehicle_local_position.timestamp_sample - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f); + _time_stamp_last_loop = vehicle_local_position.timestamp_sample; + + // set _dt in controllib Block for BlockDerivative + setDt(dt); + + if (_vehicle_control_mode_sub.updated()) { + const bool previous_position_control_enabled = _vehicle_control_mode.flag_control_position_enabled; + + if (_vehicle_control_mode_sub.update(&_vehicle_control_mode)) { + if (!previous_position_control_enabled && _vehicle_control_mode.flag_control_position_enabled) { + _time_position_control_enabled = _vehicle_control_mode.timestamp; + + } else if (previous_position_control_enabled && !_vehicle_control_mode.flag_control_position_enabled) { + // clear existing setpoint when controller is no longer active + _setpoint = ScPositionControl::empty_trajectory_setpoint; + } + } + } + + // TODO: check if setpoint is different than the previous one and reset integral then + // _control.resetIntegral(); + _trajectory_setpoint_sub.update(&_setpoint); + _vehicle_attitude_sub.update(&v_att); + + // adjust existing (or older) setpoint with any EKF reset deltas + if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) { + if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) { + _setpoint.velocity[0] += vehicle_local_position.delta_vxy[0]; + _setpoint.velocity[1] += vehicle_local_position.delta_vxy[1]; + } + + if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) { + _setpoint.velocity[2] += vehicle_local_position.delta_vz; + } + + if (vehicle_local_position.xy_reset_counter != _xy_reset_counter) { + _setpoint.position[0] += vehicle_local_position.delta_xy[0]; + _setpoint.position[1] += vehicle_local_position.delta_xy[1]; + } + + if (vehicle_local_position.z_reset_counter != _z_reset_counter) { + _setpoint.position[2] += vehicle_local_position.delta_z; + } + + if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) { + _setpoint.yaw = wrap_pi(_setpoint.yaw + vehicle_local_position.delta_heading); + } + } + + if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) { + _vel_x_deriv.reset(); + _vel_y_deriv.reset(); + } + + if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) { + _vel_z_deriv.reset(); + } + + // save latest reset counters + _vxy_reset_counter = vehicle_local_position.vxy_reset_counter; + _vz_reset_counter = vehicle_local_position.vz_reset_counter; + _xy_reset_counter = vehicle_local_position.xy_reset_counter; + _z_reset_counter = vehicle_local_position.z_reset_counter; + _heading_reset_counter = vehicle_local_position.heading_reset_counter; + + PositionControlStates states{set_vehicle_states(vehicle_local_position, v_att)}; + + poll_manual_setpoint(dt, vehicle_local_position, v_att); + + if (_vehicle_control_mode.flag_control_position_enabled) { + // set failsafe setpoint if there hasn't been a new + // trajectory setpoint since position control started + if ((_setpoint.timestamp < _time_position_control_enabled) + && (vehicle_local_position.timestamp_sample > _time_position_control_enabled)) { + PX4_INFO("Setpoint time: %f, Vehicle local pos time: %f, Pos Control Enabled time: %f", + (double)_setpoint.timestamp, (double)vehicle_local_position.timestamp_sample, + (double)_time_position_control_enabled); + _setpoint = generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, false); + } + } + + if (_vehicle_control_mode.flag_control_position_enabled + && (_setpoint.timestamp >= _time_position_control_enabled)) { + + _control.setThrustLimit(_param_mpc_thr_max.get()); + + _control.setVelocityLimits(_param_mpc_vel_max.get()); + + _control.setInputSetpoint(_setpoint); + + _control.setState(states); + + // Run position control + if (!_control.update(dt)) { + _control.setInputSetpoint(generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, true)); + _control.setVelocityLimits(_param_mpc_vel_max.get()); + _control.update(dt); + } + + // Publish attitude setpoint output + vehicle_attitude_setpoint_s attitude_setpoint{}; + _control.getAttitudeSetpoint(attitude_setpoint, v_att); + attitude_setpoint.timestamp = hrt_absolute_time(); + _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); + + } + } + + perf_end(_cycle_perf); +} + +void SpacecraftPositionControl::poll_manual_setpoint(const float dt, + const vehicle_local_position_s &vehicle_local_position, + const vehicle_attitude_s &_vehicle_att) +{ + if (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_armed) { + if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { + + if (!_vehicle_control_mode.flag_control_climb_rate_enabled && + !_vehicle_control_mode.flag_control_offboard_enabled) { + + if (_vehicle_control_mode.flag_control_attitude_enabled) { + // We are in Stabilized mode + // Generate position setpoints + if (!stabilized_pos_sp_initialized) { + // Initialize position setpoint + target_pos_sp = Vector3f(vehicle_local_position.x, vehicle_local_position.y, + vehicle_local_position.z); + + const float vehicle_yaw = Eulerf(Quatf(_vehicle_att.q)).psi(); + _manual_yaw_sp = vehicle_yaw; + stabilized_pos_sp_initialized = true; + } + + // Update velocity setpoint + Vector3f target_vel_sp = Vector3f(_manual_control_setpoint.pitch, _manual_control_setpoint.roll, 0.0); + // TODO(@Pedro-Roque): probably need to move velocity to inertial frame + target_pos_sp = target_pos_sp + target_vel_sp * dt; + + // Update _setpoint + _setpoint.position[0] = target_pos_sp(0); + _setpoint.position[1] = target_pos_sp(1); + _setpoint.position[2] = target_pos_sp(2); + + _setpoint.velocity[0] = target_vel_sp(0); + _setpoint.velocity[1] = target_vel_sp(1); + _setpoint.velocity[2] = target_vel_sp(2); + + // Generate attitude setpoints + float yaw_sp_move_rate = 0.0; + + if (_manual_control_setpoint.throttle > -0.9f) { + yaw_sp_move_rate = _manual_control_setpoint.yaw * yaw_rate; + } + + _manual_yaw_sp = wrap_pi(_manual_yaw_sp + yaw_sp_move_rate * dt); + const float roll_body = 0.0; + const float pitch_body = 0.0; + + Quatf q_sp(Eulerf(roll_body, pitch_body, _manual_yaw_sp)); + q_sp.copyTo(_setpoint.attitude); + + _setpoint.timestamp = hrt_absolute_time(); + + } else { + // We are in Manual mode + stabilized_pos_sp_initialized = false; + } + + } else { + stabilized_pos_sp_initialized = false; + } + + _manual_setpoint_last_called = hrt_absolute_time(); + } + } +} + +trajectory_setpoint_s SpacecraftPositionControl::generateFailsafeSetpoint(const hrt_abstime &now, + const PositionControlStates &states, bool warn) +{ + // rate limit the warnings + warn = warn && (now - _last_warn) > 2_s; + + if (warn) { + PX4_WARN("invalid setpoints"); + _last_warn = now; + } + + trajectory_setpoint_s failsafe_setpoint = ScPositionControl::empty_trajectory_setpoint; + failsafe_setpoint.timestamp = now; + + failsafe_setpoint.velocity[0] = failsafe_setpoint.velocity[1] = failsafe_setpoint.velocity[2] = 0.f; + + if (warn) { + PX4_WARN("Failsafe: stop and wait"); + } + + return failsafe_setpoint; +} + +int SpacecraftPositionControl::task_spawn(int argc, char *argv[]) +{ + SpacecraftPositionControl *instance = new SpacecraftPositionControl(); + + 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 SpacecraftPositionControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int SpacecraftPositionControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( + ### Description + The controller has two loops: a P loop for position error and a PID loop for velocity error. + Output of the velocity controller is thrust vector in the body frame, and the same target attitude + received on the trajectory setpoint as quaternion. + + The controller doesn't use Euler angles for its work, they are generated only for more human-friendly control and + logging. + )DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sc_pos_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int sc_pos_control_main(int argc, char *argv[]) +{ + return SpacecraftPositionControl::main(argc, argv); +} diff --git a/src/modules/sc_pos_control/SpacecraftPositionControl.hpp b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp new file mode 100644 index 000000000000..4d15b83bc79f --- /dev/null +++ b/src/modules/sc_pos_control/SpacecraftPositionControl.hpp @@ -0,0 +1,185 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2020 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. + * + ****************************************************************************/ + +/** + * Multicopter position controller. + */ + +#pragma once + +#include "PositionControl/PositionControl.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class SpacecraftPositionControl : public ModuleBase, public control::SuperBlock, + public ModuleParams, public px4::ScheduledWorkItem +{ +public: + SpacecraftPositionControl(); + ~SpacecraftPositionControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + orb_advert_t _mavlink_log_pub{nullptr}; + + uORB::Publication _vehicle_attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ + + uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ + + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + + hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ + hrt_abstime _time_position_control_enabled{0}; + hrt_abstime _manual_setpoint_last_called{0}; + + trajectory_setpoint_s _setpoint{ScPositionControl::empty_trajectory_setpoint}; + vehicle_control_mode_s _vehicle_control_mode{}; + manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */ + + DEFINE_PARAMETERS( + // Position Control + (ParamFloat) _param_mpc_pos_p, + (ParamFloat) _param_mpc_vel_p_acc, + (ParamFloat) _param_mpc_vel_i_acc, + (ParamFloat) _param_mpc_vel_d_acc, + (ParamFloat) _param_mpc_vel_all, + (ParamFloat) _param_mpc_vel_max, + (ParamFloat) _param_mpc_vel_cruise, + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_sys_vehicle_resp, + (ParamFloat) _param_mpc_acc, + (ParamFloat) _param_mpc_acc_max, + (ParamFloat) _param_mpc_man_y_max, + (ParamFloat) _param_mpc_man_y_tau, + (ParamFloat) _param_mpc_jerk_auto, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_thr_max + ); + + control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ + control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */ + control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */ + + matrix::Vector3f target_pos_sp; + float yaw_rate; + bool stabilized_pos_sp_initialized{false}; + + ScPositionControl _control; /**< class for core PID position control */ + + hrt_abstime _last_warn{0}; /**< timer when the last warn message was sent out */ + + /** Timeout in us for trajectory data to get considered invalid */ + static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; + + uint8_t _vxy_reset_counter{0}; + uint8_t _vz_reset_counter{0}; + uint8_t _xy_reset_counter{0}; + uint8_t _z_reset_counter{0}; + uint8_t _heading_reset_counter{0}; + + // Manual setpoints on yaw and reset + bool _reset_yaw_sp{true}; + float _manual_yaw_sp{0.f}; + float _throttle_control{0.f}; + float _yaw_control{0.f}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; + + /** + * Update our local parameter cache. + * Parameter update can be forced when argument is true. + * @param force forces parameter update. + */ + void parameters_update(bool force); + + /** + * Check for validity of positon/velocity states. + */ + PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos, const vehicle_attitude_s &att); + + /** + * Check for manual setpoints. + */ + void poll_manual_setpoint(const float dt, const vehicle_local_position_s + &vehicle_local_position, const vehicle_attitude_s &_vehicle_att); + + /** + * Generate setpoint to bridge no executable setpoint being available. + * Used to handle transitions where no proper setpoint was generated yet and when the received setpoint is invalid. + * This should only happen briefly when transitioning and never during mode operation or by design. + */ + trajectory_setpoint_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states, bool warn); +}; diff --git a/src/modules/sc_pos_control/sc_pos_control_params.c b/src/modules/sc_pos_control/sc_pos_control_params.c new file mode 100644 index 000000000000..9196b67306f5 --- /dev/null +++ b/src/modules/sc_pos_control/sc_pos_control_params.c @@ -0,0 +1,255 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +/** + * Proportional gain for position error + * + * Defined as corrective velocity in m/s per m position error + * + * @min 0 + * @max 2 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_POS_P, 0.95f); + +/** + * Proportional gain for velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 2 + * @max 15 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_P, 4.f); + +/** + * Integral gain for velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 2 + * @max 15 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_I, 4.f); + +/** + * Derivative gain for velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 2 + * @max 15 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_D, 4.f); + +/** + * Maximum velocity + * + * Absolute maximum for all velocity controlled modes. + * Any higher value is truncated. + * + * @unit m/s + * @min 0 + * @max 20 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_MAX, 12.f); + +/** + * Overall Velocity Limit + * + * If set to a value greater than zero, other parameters are automatically set (such as + * MPC_VEL_MAX or MPC_VEL_MANUAL). + * If set to a negative value, the existing individual parameters are used. + * + * @min -20 + * @max 20 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_ALL, -10.f); + +/** + * Cruising elocity setpoint in autonomous modes + * + * @unit m/s + * @min 3 + * @max 20 + * @increment 1 + * @decimal 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_CRUISE, 10.f); + +/** + * Maximum velocity setpoint in Position mode + * + * @unit m/s + * @min 3 + * @max 20 + * @increment 1 + * @decimal 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_MANUAL, 10.f); + +/** + * Maximum collective thrust + * + * Limit allowed thrust + * + * @unit norm + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.05 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_THR_MAX, 1.f); + +/** + * Acceleration for autonomous and for manual modes + * + * When piloting manually, this parameter is only used in MPC_POS_MODE 4. + * + * @unit m/s^2 + * @min 2 + * @max 15 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_ACC, 3.f); + +/** + * Maximum accelaration in autonomous modes + * + * + * @unit m/s^2 + * @min 2 + * @max 15 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_ACC_MAX, 5.f); + +/** + * Jerk limit in autonomous modes + * + * Limit the maximum jerk of the vehicle (how fast the acceleration can change). + * A lower value leads to smoother vehicle motions but also limited agility. + * + * @unit m/s^3 + * @min 1 + * @max 80 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_JERK_AUTO, 4.f); + +PARAM_DEFINE_FLOAT(SPC_VEHICLE_RESP, 0.5f); +/** + * Maximum jerk in Position/Altitude mode + * + * Limit the maximum jerk of the vehicle (how fast the acceleration can change). + * A lower value leads to smoother motions but limits agility + * (how fast it can change directions or break). + * + * Setting this to the maximum value essentially disables the limit. + * + * Only used with smooth MPC_POS_MODE 3 and 4. + * + * @unit m/s^3 + * @min 0.5 + * @max 500 + * @decimal 0 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_JERK_MAX, 8.f); + +/** + * 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(SPC_MAN_Y_MAX, 150.f); + +/** + * Manual yaw rate input filter time constant + * + * Not used in Stabilized mode + * Setting this parameter to 0 disables the filter + * + * @unit s + * @min 0 + * @max 5 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_MAN_Y_TAU, 0.08f); + +/** + * Numerical velocity derivative low pass cutoff frequency + * + * @unit Hz + * @min 0 + * @max 10 + * @decimal 1 + * @increment 0.5 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VELD_LP, 5.0f); + diff --git a/src/modules/sc_rate_control/CMakeLists.txt b/src/modules/sc_rate_control/CMakeLists.txt new file mode 100644 index 000000000000..d2f197cb7fe4 --- /dev/null +++ b/src/modules/sc_rate_control/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# 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_module( + MODULE modules__sc_rate_control + MAIN sc_rate_control + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + SRCS + SpacecraftRateControl.cpp + SpacecraftRateControl.hpp + DEPENDS + circuit_breaker + mathlib + RateControl + px4_work_queue + ) diff --git a/src/modules/sc_rate_control/Kconfig b/src/modules/sc_rate_control/Kconfig new file mode 100644 index 000000000000..0de7129a2bea --- /dev/null +++ b/src/modules/sc_rate_control/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_SC_RATE_CONTROL + bool "sc_rate_control" + default n + ---help--- + Enable support for sc_rate_control + +menuconfig USER_SC_RATE_CONTROL + bool "sc_rate_control running as userspace module" + default n + depends on BOARD_PROTECTED && MODULES_SC_RATE_CONTROL + ---help--- + Put sc_rate_control in userspace memory diff --git a/src/modules/sc_rate_control/SpacecraftRateControl.cpp b/src/modules/sc_rate_control/SpacecraftRateControl.cpp new file mode 100644 index 000000000000..54ffc4daeaaa --- /dev/null +++ b/src/modules/sc_rate_control/SpacecraftRateControl.cpp @@ -0,0 +1,336 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include + +using namespace matrix; +using namespace time_literals; +using math::radians; + +SpacecraftRateControl::SpacecraftRateControl() + : ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _vehicle_torque_setpoint_pub(ORB_ID(vehicle_torque_setpoint)), + _vehicle_thrust_setpoint_pub(ORB_ID(vehicle_thrust_setpoint)), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME ": cycle")) +{ + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_SPACECRAFT; + + parameters_updated(); + _controller_status_pub.advertise(); +} + +SpacecraftRateControl::~SpacecraftRateControl() { perf_free(_loop_perf); } + +bool SpacecraftRateControl::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +void SpacecraftRateControl::parameters_updated() +{ + // 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())); +} + +void SpacecraftRateControl::Run() +{ + if (should_exit()) { + _vehicle_angular_velocity_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + parameters_updated(); + } + + /* run controller on gyro changes */ + vehicle_angular_velocity_s angular_velocity; + + 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); + + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + _maybe_landed = vehicle_land_detected.maybe_landed; + } + } + + _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) { + // generate the rate setpoint from sticks + manual_control_setpoint_s manual_control_setpoint; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + // manual rates control - ACRO mode + const Vector3f man_rate_sp{ + math::superexpo(manual_control_setpoint.roll, _param_sc_acro_expo.get(), _param_sc_acro_supexpo.get()), + math::superexpo(-manual_control_setpoint.pitch, _param_sc_acro_expo.get(), _param_sc_acro_supexpo.get()), + math::superexpo(manual_control_setpoint.yaw, _param_sc_acro_expo_y.get(), _param_sc_acro_supexpoy.get())}; + + _rates_setpoint = man_rate_sp.emult(_acro_rate_max); + _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_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { + 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 || + _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _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 saturation_positive; + Vector 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; + } + } + } + + // TODO: send the unallocated value directly for better anti-windup + _rate_control.setSaturationStatus(saturation_positive, saturation_negative); + } + + // run rate controller + const Vector3f att_control = + _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed); + + // 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(att_control(0)) ? att_control(0) : 0.f; + vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.f; + vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(att_control(2)) ? att_control(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); + } + } + + perf_end(_loop_perf); +} + +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; + } +} + +int SpacecraftRateControl::task_spawn(int argc, char *argv[]) +{ + + SpacecraftRateControl *instance = new SpacecraftRateControl(); + + 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 SpacecraftRateControl::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); } + +int SpacecraftRateControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements the spacecraft rate controller. It takes rate setpoints (in acro mode +via `manual_control_setpoint` topic) as inputs and outputs torque and thrust setpoints. + +The controller has a PID loop for angular rate error. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sc_rate_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int sc_rate_control_main(int argc, char* argv[]) { + return SpacecraftRateControl::main(argc, argv); +} diff --git a/src/modules/sc_rate_control/SpacecraftRateControl.hpp b/src/modules/sc_rate_control/SpacecraftRateControl.hpp new file mode 100644 index 000000000000..2555814fe784 --- /dev/null +++ b/src/modules/sc_rate_control/SpacecraftRateControl.hpp @@ -0,0 +1,164 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class SpacecraftRateControl : public ModuleBase, public ModuleParams, public px4::WorkItem +{ +public: + SpacecraftRateControl(); + ~SpacecraftRateControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + /** + * initialize some vectors/matrices from parameters + */ + void parameters_updated(); + + 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::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + + uORB::Publication _actuator_controls_status_pub{ORB_ID(actuator_controls_status_0)}; + uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; + uORB::Publication _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _vehicle_torque_setpoint_pub; + uORB::Publication _vehicle_thrust_setpoint_pub; + + vehicle_control_mode_s _vehicle_control_mode{}; + vehicle_status_s _vehicle_status{}; + + 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 _battery_status_scale{0.0f}; + matrix::Vector3f _thrust_setpoint{}; + + float _energy_integration_time{0.0f}; + float _control_energy[4] {}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_sc_rollrate_p, + (ParamFloat) _param_sc_rollrate_i, + (ParamFloat) _param_sc_rr_int_lim, + (ParamFloat) _param_sc_rollrate_d, + (ParamFloat) _param_sc_rollrate_ff, + (ParamFloat) _param_sc_rollrate_k, + + (ParamFloat) _param_sc_pitchrate_p, + (ParamFloat) _param_sc_pitchrate_i, + (ParamFloat) _param_sc_pr_int_lim, + (ParamFloat) _param_sc_pitchrate_d, + (ParamFloat) _param_sc_pitchrate_ff, + (ParamFloat) _param_sc_pitchrate_k, + + (ParamFloat) _param_sc_yawrate_p, + (ParamFloat) _param_sc_yawrate_i, + (ParamFloat) _param_sc_yr_int_lim, + (ParamFloat) _param_sc_yawrate_d, + (ParamFloat) _param_sc_yawrate_ff, + (ParamFloat) _param_sc_yawrate_k, + + (ParamFloat) _param_sc_acro_r_max, + (ParamFloat) _param_sc_acro_p_max, + (ParamFloat) _param_sc_acro_y_max, + (ParamFloat) _param_sc_acro_expo, /**< expo stick curve shape (roll & pitch) */ + (ParamFloat) _param_sc_acro_expo_y, /**< expo stick curve shape (yaw) */ + (ParamFloat) _param_sc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */ + (ParamFloat) _param_sc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */ + + (ParamBool) _param_sc_bat_scale_en + ) +}; diff --git a/src/modules/sc_rate_control/sc_rate_control_params.c b/src/modules/sc_rate_control/sc_rate_control_params.c new file mode 100644 index 000000000000..05366e38e7f7 --- /dev/null +++ b/src/modules/sc_rate_control/sc_rate_control_params.c @@ -0,0 +1,400 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file mc_rate_control_params.c + * Parameters for multicopter attitude controller. + * + * @author Lorenz Meier + * @author Anton Babushkin + */ + +/** + * 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 Multicopter 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 Multicopter 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 Multicopter 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 Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_D, 0.003f); + +/** + * Roll rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @group Multicopter 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 Multicopter 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 Multicopter 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 Multicopter 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 Multicopter 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 Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_D, 0.003f); + +/** + * Pitch rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @group Multicopter 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 Multicopter 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 0.6 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_P, 0.2f); + +/** + * 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 Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_I, 0.1f); + +/** + * 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 Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YR_INT_LIM, 0.30f); + +/** + * 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 Multicopter 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 Multicopter 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 Multicopter 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 Multicopter 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 Multicopter 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 Multicopter 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 Multicopter 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 Multicopter 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 Multicopter 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 Multicopter 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 Multicopter Rate Control + */ +PARAM_DEFINE_INT32(SC_BAT_SCALE_EN, 0); diff --git a/src/modules/sc_thruster_controller/CMakeLists.txt b/src/modules/sc_thruster_controller/CMakeLists.txt new file mode 100644 index 000000000000..00e843dba0cf --- /dev/null +++ b/src/modules/sc_thruster_controller/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 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. +# +############################################################################ + +px4_add_module( + MODULE modules__sc_thruster_controller + MAIN sc_thruster_controller + SRCS + sc_thruster_controller.cpp + ) + diff --git a/src/modules/sc_thruster_controller/Kconfig b/src/modules/sc_thruster_controller/Kconfig new file mode 100644 index 000000000000..1d7760abd20b --- /dev/null +++ b/src/modules/sc_thruster_controller/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_SC_THRUSTER_CONTROLLER + bool "sc_thruster_controller" + default n + ---help--- + Enable support for sc_thruster_controller diff --git a/src/modules/sc_thruster_controller/sc_thruster_controller.cpp b/src/modules/sc_thruster_controller/sc_thruster_controller.cpp new file mode 100644 index 000000000000..b3aa3ac0506c --- /dev/null +++ b/src/modules/sc_thruster_controller/sc_thruster_controller.cpp @@ -0,0 +1,240 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#include "sc_thruster_controller.hpp" + +#include +#include +#include + +#include +#include + +int ScThrusterController::print_status() +{ + PX4_INFO("Running"); + // TODO: print additional runtime information about the state of the module + + return 0; +} + +int ScThrusterController::custom_command(int argc, char *argv[]) +{ + if (!is_running()) { + print_usage("not running"); + return 1; + } + + /* + // additional custom commands can be handled like this: + if (!strcmp(argv[0], "do-something")) { + get_instance()->do_something(); + return 0; + } + */ + + return print_usage("unknown command"); +} + +int ScThrusterController::task_spawn(int argc, char *argv[]) +{ + _task_id = px4_task_spawn_cmd("module", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1024, (px4_main_t)&run_trampoline, + (char *const *)argv); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + + return 0; +} + +ScThrusterController *ScThrusterController::instantiate(int argc, char *argv[]) +{ + int myoptind = 1; + int ch; + bool error_flag = false; + const char *myoptarg = nullptr; + + // parse CLI arguments + _debug_thruster_print = false; + + while ((ch = px4_getopt(argc, argv, "d", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + _debug_thruster_print = true; + PX4_INFO("Debugging enabled"); + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return nullptr; + } + + ScThrusterController *instance = new ScThrusterController(); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + } + + return instance; +} + +ScThrusterController::ScThrusterController() : ModuleParams(nullptr) {} + +void ScThrusterController::run() +{ + // Subscribe to thruster command + int thrustercmd_sub = orb_subscribe(ORB_ID(thruster_command)); + px4_pollfd_struct_t fds[] = { + {.fd = thrustercmd_sub, .events = POLLIN}, + }; + + // Create publisher for PWM values + struct actuator_motors_s actuator_motors_msg; + memset(&actuator_motors_msg, 0, sizeof(actuator_motors_msg)); + orb_advert_t _actuator_motors_pub = orb_advertise(ORB_ID(actuator_motors), &actuator_motors_msg); + + // For publishing debug messages + struct mavlink_log_s my_msg; + memset(&my_msg, 0, sizeof(my_msg)); + orb_advert_t _my_message_pub = orb_advertise(ORB_ID(mavlink_log), &my_msg); + + // Thruster command structure + struct thruster_command_s thruster_cmd; + + // initialize parameters + parameters_update(true); + bool debugPrint = ScThrusterController::_debug_thruster_print; + PX4_INFO("Debugging enabled: %d", debugPrint); + + while (!should_exit()) { + // wait for up to 1000ms for data + int pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 1000); + + if (pret == 0) { + // Timeout: let the loop run anyway, don't do `continue` here + if (debugPrint) { PX4_WARN("Timeout..."); } + + } else if (pret < 0) { + // this is undesirable but not much we can do + PX4_ERR("poll error %d, %d", pret, errno); + px4_usleep(50000); + continue; + + } else if (fds[0].revents & POLLIN) { + /* copy sensors raw data into local buffer */ + orb_copy(ORB_ID(thruster_command), thrustercmd_sub, &thruster_cmd); + + if (debugPrint) + PX4_INFO("x1: %8.4f\t x2: %8.4f\t y1: %8.4f\t y2: %8.4f", (double)thruster_cmd.x1, (double)thruster_cmd.x2, + (double)thruster_cmd.y1, (double)thruster_cmd.y2); + + actuator_motors_msg.control[0] = thruster_cmd.x1 > 0 ? thruster_cmd.x1 : 0; + actuator_motors_msg.control[1] = thruster_cmd.x1 < 0 ? -thruster_cmd.x1 : 0; + actuator_motors_msg.control[2] = thruster_cmd.x2 > 0 ? thruster_cmd.x2 : 0; + actuator_motors_msg.control[3] = thruster_cmd.x2 < 0 ? -thruster_cmd.x2 : 0; + actuator_motors_msg.control[4] = thruster_cmd.y1 > 0 ? thruster_cmd.y1 : 0; + actuator_motors_msg.control[5] = thruster_cmd.y1 < 0 ? -thruster_cmd.y1 : 0; + actuator_motors_msg.control[6] = thruster_cmd.y2 > 0 ? thruster_cmd.y2 : 0; + actuator_motors_msg.control[7] = thruster_cmd.y2 < 0 ? -thruster_cmd.y2 : 0; + + orb_publish(ORB_ID(actuator_motors), _actuator_motors_pub, &actuator_motors_msg); + + if (debugPrint) { + strcpy(my_msg.text, "Thruster command received!"); + orb_publish(ORB_ID(mavlink_log), _my_message_pub, &my_msg); + } + } + + parameters_update(); + } + + orb_unsubscribe(thrustercmd_sub); +} + +void ScThrusterController::parameters_update(bool force) +{ + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s update; + _parameter_update_sub.copy(&update); + + // update parameters from storage + updateParams(); + } +} + +int ScThrusterController::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Section that describes the provided module functionality. + +This is a template for a module running as a task in the background with start/stop/status functionality. + +### Implementation +Section describing the high-level implementation of this module. + +### Examples +CLI usage example: +$ module start -f -p 42 + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("module", "template"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true); + PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1000, "Optional example parameter", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int sc_thruster_controller_main(int argc, char* argv[]) { return ScThrusterController::main(argc, argv); } diff --git a/src/modules/sc_thruster_controller/sc_thruster_controller.hpp b/src/modules/sc_thruster_controller/sc_thruster_controller.hpp new file mode 100644 index 000000000000..49b255f0a4ae --- /dev/null +++ b/src/modules/sc_thruster_controller/sc_thruster_controller.hpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 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_thruster_controller.c + * For controlling solenoid cold gas thrusters in space robotics + * + * @author Elias Krantz + */ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using namespace time_literals; + +extern "C" __EXPORT int sc_thruster_controller_main(int argc, char *argv[]); + +class ScThrusterController : public ModuleBase, public ModuleParams +{ +public: + ScThrusterController(); + + virtual ~ScThrusterController() = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static ScThrusterController *instantiate(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::run() */ + void run() override; + + /** @see ModuleBase::print_status() */ + int print_status() override; + +private: + /** + * Check for parameter changes and update them if needed. + * @param parameter_update_sub uorb subscription to parameter_update + * @param force for a parameter update + */ + void parameters_update(bool force = false); + + DEFINE_PARAMETERS((ParamInt)_param_sys_autostart, /**< example parameter */ + (ParamInt)_param_sys_autoconfig /**< another parameter */ + ) + + // Flags + static bool _debug_thruster_print; + + // Subscriptions + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; +}; + +bool ScThrusterController::_debug_thruster_print = true; diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.cpp b/src/modules/simulation/pwm_out_sim/PWMSim.cpp index c11a5eb365fe..b83c53d6bd78 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.cpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.cpp @@ -44,11 +44,16 @@ PWMSim::PWMSim(bool hil_mode_enabled) : OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) { + // initialize parameters - dynamic instead of static + PWM_SIM_PWM_MAX_MAGIC = _param_pwm_sim_max_magic.get(); + PWM_SIM_PWM_MIN_MAGIC = _param_pwm_sim_min_magic.get(); + PWM_SIM_FAILSAFE_MAGIC = _param_pwm_sim_failsafe_magic.get(); + PWM_SIM_DISARMED_MAGIC = _param_pwm_sim_disarmed_magic.get(); + _mixing_output.setAllDisarmedValues(PWM_SIM_DISARMED_MAGIC); _mixing_output.setAllFailsafeValues(PWM_SIM_FAILSAFE_MAGIC); _mixing_output.setAllMinValues(PWM_SIM_PWM_MIN_MAGIC); _mixing_output.setAllMaxValues(PWM_SIM_PWM_MAX_MAGIC); - _mixing_output.setIgnoreLockdown(hil_mode_enabled); } diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.hpp b/src/modules/simulation/pwm_out_sim/PWMSim.hpp index b16a9fac5e75..4f210254c4dc 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.hpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.hpp @@ -38,7 +38,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -76,10 +78,10 @@ class PWMSim : public ModuleBase, public OutputModuleInterface private: void Run() override; - static constexpr uint16_t PWM_SIM_DISARMED_MAGIC = 900; - static constexpr uint16_t PWM_SIM_FAILSAFE_MAGIC = 600; - static constexpr uint16_t PWM_SIM_PWM_MIN_MAGIC = 1000; - static constexpr uint16_t PWM_SIM_PWM_MAX_MAGIC = 2000; + uint16_t PWM_SIM_DISARMED_MAGIC; + uint16_t PWM_SIM_FAILSAFE_MAGIC; + uint16_t PWM_SIM_PWM_MIN_MAGIC; + uint16_t PWM_SIM_PWM_MAX_MAGIC; MixingOutput _mixing_output{PARAM_PREFIX, MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -88,4 +90,11 @@ class PWMSim : public ModuleBase, public OutputModuleInterface perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + + DEFINE_PARAMETERS( + (ParamInt) _param_pwm_sim_max_magic, + (ParamInt) _param_pwm_sim_min_magic, + (ParamInt) _param_pwm_sim_failsafe_magic, + (ParamInt) _param_pwm_sim_disarmed_magic + ) }; diff --git a/src/modules/simulation/pwm_out_sim/pwm_out_sim_params.c b/src/modules/simulation/pwm_out_sim/pwm_out_sim_params.c new file mode 100644 index 000000000000..e6789ff11446 --- /dev/null +++ b/src/modules/simulation/pwm_out_sim/pwm_out_sim_params.c @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * 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 pwm_out_sim_params.c + * Parameters for PWM output simulator. + * + * @author Pedro Roque, + */ + +/** + * Maximum PWM Value + * + * Highest PWM value that can be output by the simulator. + * + * @min 0 + * @max 10000 + * @decimal 0 + * @increment 1 + * @group PWM Output Simulator + */ +PARAM_DEFINE_INT32(PWM_SIM_PWM_MAX, 2000); + +/** + * Minimum PWM Value + * + * Lowest PWM value that can be output by the simulator. + * + * @min -10000 + * @max 2000 + * @decimal 0 + * @increment 1 + * @group PWM Output Simulator + */ +PARAM_DEFINE_INT32(PWM_SIM_PWM_MIN, 1000); + +/** + * Failsafe PWM Value + * + * Output at Failsafe level. + * + * @min 0 + * @max 10000 + * @decimal 0 + * @increment 1 + * @group PWM Output Simulator + */ +PARAM_DEFINE_INT32(PWM_SIM_FAILSAFE, 600); + +/** + * Disarm PWM Value + * + * Output at Disarmed state. + * + * @min 0 + * @max 10000 + * @decimal 0 + * @increment 1 + * @group PWM Output Simulator + */ +PARAM_DEFINE_INT32(PWM_SIM_DISARM, 900); \ No newline at end of file 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 b67fe546ccd7..0d5ec6ac8d9a 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake @@ -88,6 +88,8 @@ if(gazebo_FOUND) iris_rplidar iris_vision omnicopter + 2d_spacecraft + 3d_spacecraft plane plane_cam plane_catapult @@ -113,10 +115,13 @@ if(gazebo_FOUND) ksql_airport mcmillan_airfield ramped_up_wind + frictionless sonoma_raceway warehouse windy yosemite + space + zero_g ) # find corresponding airframes diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index 255c8e0b5417..c42f8059c86d 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -71,7 +71,30 @@ publications: - topic: /fmu/out/vehicle_trajectory_waypoint_desired type: px4_msgs::msg::VehicleTrajectoryWaypoint -# Create uORB::Publication + - topic: /fmu/out/actuator_motors + type: px4_msgs::msg::ActuatorMotors + + - topic: /fmu/out/actuator_outputs + type: px4_msgs::msg::ActuatorOutputs + + - topic: /fmu/out/vehicle_torque_setpoint + type: px4_msgs::msg::VehicleTorqueSetpoint + + - topic: /fmu/out/vehicle_thrust_setpoint + type: px4_msgs::msg::VehicleThrustSetpoint + + - topic: /fmu/out/vehicle_attitude_setpoint + type: px4_msgs::msg::VehicleAttitudeSetpoint + + - topic: /fmu/out/log_message + type: px4_msgs::msg::LogMessage + + - topic: /fmu/out/mavlink_log + type: px4_msgs::msg::MavlinkLog + + - topic: /fmu/out/rc_channels + type: px4_msgs::msg::RcChannels + subscriptions: - topic: /fmu/in/register_ext_component_request type: px4_msgs::msg::RegisterExtComponentRequest @@ -121,8 +144,8 @@ subscriptions: - topic: /fmu/in/vehicle_attitude_setpoint type: px4_msgs::msg::VehicleAttitudeSetpoint - - topic: /fmu/in/vehicle_mocap_odometry - type: px4_msgs::msg::VehicleOdometry + # - topic: /fmu/in/vehicle_mocap_odometry + # type: px4_msgs::msg::VehicleOdometry - topic: /fmu/in/vehicle_rates_setpoint type: px4_msgs::msg::VehicleRatesSetpoint