diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71000_gazebo-classic_2d_spacecraft b/ROMFS/px4fmu_common/init.d-posix/airframes/71000_gazebo-classic_2d_spacecraft new file mode 100644 index 000000000000..38d9587dde0c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/71000_gazebo-classic_2d_spacecraft @@ -0,0 +1,127 @@ +#!/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 99 + +param set-default CA_THRUSTER_CNT 8 +param set-default CA_R_REV 255 + +# param set-default FW_ARSP_MODE 1 +param set-default UXRCE_DDS_CFG 1000 +param set-default MAV_2_CONFIG 1000 +param set-default MAV_2_REMOTE_PRT 14550 +param set-default MAV_2_UDP 14550 + +# 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.4 +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.4 +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.4 +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.4 +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.4 +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.4 +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.4 +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.4 +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 SC_YAWRATE_P 3.335 +param set SC_YAWRATE_I 0.87 +param set SC_YAWRATE_D 0.15 +param set SC_YR_INT_LIM 0.2 +param set SC_YAW_P 3.0 + +param set SPC_POS_P 0.20 +param set SPC_VEL_P 6.55 +param set SPC_VEL_I 0.0 +param set SPC_VEL_D 0.0 +param set SPC_VEL_MAX 12.0 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart b/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart new file mode 100644 index 000000000000..4cb64da9540b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart @@ -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 99 + +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/71002_gz_spacecraft_2d b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d new file mode 100644 index 000000000000..342ac7c66718 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d @@ -0,0 +1,149 @@ +#!/bin/sh +# +# @name 3DoF Spacecraft Model +# +# @type 2D Freeflyer with 8 thrusters - Planar motion +# +# @maintainer Pedro Roque +# + +. ${R}etc/init.d/rc.sc_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=spacecraft_2d} + +param set-default SIM_GZ_EN 1 + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 1 +param set-default SENS_EN_MAGSIM 1 +param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs +param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later? + +param set-default CA_AIRFRAME 13 +param set-default MAV_TYPE 99 + +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 3 # 0 is PseudoInverse, 3 is Metric + +# 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.4 +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.4 +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.4 +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.4 +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.4 +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.4 +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.4 +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.4 +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 SIM_GZ_TH_FUNC1 501 +param set-default SIM_GZ_TH_FUNC2 502 +param set-default SIM_GZ_TH_FUNC3 503 +param set-default SIM_GZ_TH_FUNC4 504 +param set-default SIM_GZ_TH_FUNC5 505 +param set-default SIM_GZ_TH_FUNC6 506 +param set-default SIM_GZ_TH_FUNC7 507 +param set-default SIM_GZ_TH_FUNC8 508 + +param set-default SIM_GZ_TH_MIN1 0 +param set-default SIM_GZ_TH_MIN2 0 +param set-default SIM_GZ_TH_MIN3 0 +param set-default SIM_GZ_TH_MIN4 0 +param set-default SIM_GZ_TH_MIN5 0 +param set-default SIM_GZ_TH_MIN6 0 +param set-default SIM_GZ_TH_MIN7 0 +param set-default SIM_GZ_TH_MIN8 0 + +param set-default SIM_GZ_TH_MAX1 10000 +param set-default SIM_GZ_TH_MAX2 10000 +param set-default SIM_GZ_TH_MAX3 10000 +param set-default SIM_GZ_TH_MAX4 10000 +param set-default SIM_GZ_TH_MAX5 10000 +param set-default SIM_GZ_TH_MAX6 10000 +param set-default SIM_GZ_TH_MAX7 10000 +param set-default SIM_GZ_TH_MAX8 10000 + +# Controller Tunings +param set SC_YAWRATE_P 3.335 +param set SC_YAWRATE_I 0.87 +param set SC_YAWRATE_D 0.15 +param set SC_YR_INT_LIM 0.2 +param set SC_YAW_P 3.0 + +param set SPC_POS_P 0.20 +param set SPC_VEL_P 6.55 +param set SPC_VEL_I 0.0 +param set SPC_VEL_D 0.0 +param set SPC_VEL_MAX 12.0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index da741d2a64db..06ab8cb3dcf2 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -113,5 +113,9 @@ px4_add_romfs_files( 17001_flightgear_tf-g1 17002_flightgear_tf-g2 + 71000_gazebo-classic_2d_spacecraft + 71001_gazebo-classic_spacecraft_dart + 71002_gz_spacecraft_2d + # [22000, 22999] Reserve for custom models ) diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 488b41575a59..62914fa67fff 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -92,6 +92,13 @@ if(CONFIG_MODULES_ROVER_ACKERMANN) ) endif() +if(true) + px4_add_romfs_files( + rc.sc_apps + rc.sc_defaults + ) +endif() + if(CONFIG_MODULES_ROVER_MECANUM) px4_add_romfs_files( rc.rover_mecanum_apps diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d b/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d new file mode 100644 index 000000000000..0df09f8a297f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d @@ -0,0 +1,151 @@ +#!/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 99 + +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 + +# Set Mocap Vision frame +param set EKF2_EV_CTRL 15 +param set EKF2_HGT_REF 3 + + +# 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.4 +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.4 +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.4 +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.4 +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.4 +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.4 +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.4 +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.4 +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/rc.sc_apps b/ROMFS/px4fmu_common/init.d/rc.sc_apps new file mode 100644 index 000000000000..77b94520446d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.sc_apps @@ -0,0 +1,33 @@ +#!/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 +# +# sc_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 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..6ce7016844aa --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.sc_defaults @@ -0,0 +1,28 @@ +#!/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_AG_IP -1062731775 + +# Disable preflight disarm to not interfere with external launching +param set-default COM_DISARM_PRFLT -1 +param set-default CBRK_SUPPLY_CHK 894281 +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 +