diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4430_gz_ssrc_strivermini b/ROMFS/px4fmu_common/init.d-posix/airframes/4430_gz_ssrc_strivermini index adc67bcd320d..c1a75bd31f7b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4430_gz_ssrc_strivermini +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4430_gz_ssrc_strivermini @@ -142,7 +142,7 @@ param set-default VT_F_TRANS_DUR 6.0 param set-default VT_F_TRANS_THR 1.0 # VTOL takeoff -param set-default VTO_LOITER_ALT 20 +param set-default VTO_LOITER_ALT 20.0 param set-default MC_AIRMODE 0 param set-default MC_ROLLRATE_P 0.14 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4400_ssrc_fog_x b/ROMFS/px4fmu_common/init.d/airframes/4400_ssrc_fog_x index 3708278758d4..035d2d527e15 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4400_ssrc_fog_x +++ b/ROMFS/px4fmu_common/init.d/airframes/4400_ssrc_fog_x @@ -49,12 +49,6 @@ param set-default PWM_MAIN_MIN2 1050 param set-default PWM_MAIN_MIN3 1050 param set-default PWM_MAIN_MIN4 1050 -# HITL PWM functions -param set-default HIL_ACT_FUNC1 101 -param set-default HIL_ACT_FUNC2 102 -param set-default HIL_ACT_FUNC3 103 -param set-default HIL_ACT_FUNC4 104 - # UAVCAN_EC functions param set-default UAVCAN_EC_FUNC1 101 param set-default UAVCAN_EC_FUNC2 102 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4421_ssrc_skywalker b/ROMFS/px4fmu_common/init.d/airframes/4421_ssrc_skywalker index 3175822acbd1..8e6358b641bc 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4421_ssrc_skywalker +++ b/ROMFS/px4fmu_common/init.d/airframes/4421_ssrc_skywalker @@ -22,11 +22,6 @@ param set-default PWM_MAIN_FUNC1 201 param set-default PWM_MAIN_FUNC2 202 param set-default PWM_MAIN_FUNC4 101 -# HITL PWM functions -param set-default HIL_ACT_FUNC1 201 -param set-default HIL_ACT_FUNC2 202 -param set-default HIL_ACT_FUNC4 101 - # Airspeed parameters param set-default SENS_EN_MS4525DO 1 param set-default ASPD_DO_CHECKS 15 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4430_ssrc_strivermini b/ROMFS/px4fmu_common/init.d/airframes/4430_ssrc_strivermini index fb4277b7f215..cb6089a160dd 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4430_ssrc_strivermini +++ b/ROMFS/px4fmu_common/init.d/airframes/4430_ssrc_strivermini @@ -70,18 +70,6 @@ param set-default PWM_AUX_FUNC4 204 param set-default PWM_AUX_FUNC5 205 param set-default PWM_AUX_FUNC6 105 -# HITL PWM functions -param set-default HIL_ACT_FUNC1 101 -param set-default HIL_ACT_FUNC2 102 -param set-default HIL_ACT_FUNC3 103 -param set-default HIL_ACT_FUNC4 104 -param set-default HIL_ACT_FUNC5 201 -param set-default HIL_ACT_FUNC6 202 -param set-default HIL_ACT_FUNC7 203 -param set-default HIL_ACT_FUNC8 204 -param set-default HIL_ACT_FUNC9 205 -param set-default HIL_ACT_FUNC10 105 - # Start airspeed sensor driver param set-default SENS_EN_MS4525DO 1 param set-default SENS_EN_SDP3X 1 diff --git a/packaging/build_px4fw.sh b/packaging/build_px4fw.sh index 80d131d26487..4b1d79aaa082 100755 --- a/packaging/build_px4fw.sh +++ b/packaging/build_px4fw.sh @@ -38,7 +38,7 @@ else fi # Remove old build output - rm -Rf build/${arg} + #rm -Rf build/${arg} # Build make ${arg} diff --git a/packaging/entrypoint_sitl_gzsim.sh b/packaging/entrypoint_sitl_gzsim.sh index b08de552b86a..38c0a9284b50 100755 --- a/packaging/entrypoint_sitl_gzsim.sh +++ b/packaging/entrypoint_sitl_gzsim.sh @@ -13,8 +13,10 @@ case $PX4_VEHICLE_TYPE in export PX4_GZ_MODEL=scout_mini ;; vtol) - export PX4_SYS_AUTOSTART=4430 - export PX4_GZ_MODEL=striver_mini + #export PX4_SYS_AUTOSTART=4430 + #export PX4_GZ_MODEL=striver_mini + export PX4_SYS_AUTOSTART=4004 + export PX4_GZ_MODEL=standard_vtol ;; fw) export PX4_SYS_AUTOSTART=4440 diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.cpp b/src/modules/simulation/pwm_out_sim/PWMSim.cpp index 4dbb6ddb697a..fc70c3be44e9 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.cpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.cpp @@ -44,10 +44,16 @@ PWMSim::PWMSim(bool hil_mode_enabled) : OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) { - _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); + for (int i = 0; i < MAX_ACTUATORS; ++i) { + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "MIN", i + 1); + param_get(param_find(param_name), &_pwm_min[i]); + snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "MAX", i + 1); + param_get(param_find(param_name), &_pwm_max[i]); + snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "DIS", i + 1); + param_get(param_find(param_name), &_pwm_disarmed[i]); + PX4_INFO("PWM %d: min: %d, max: %d, disarmed: %d", i, _pwm_min[i], _pwm_max[i], _pwm_disarmed[i]); + } _mixing_output.setIgnoreLockdown(hil_mode_enabled); } @@ -69,7 +75,7 @@ bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], un const uint32_t reversible_outputs = _mixing_output.reversibleOutputs(); for (int i = 0; i < (int)num_outputs; i++) { - if (outputs[i] != PWM_SIM_DISARMED_MAGIC) { + if (outputs[i] != _pwm_disarmed[i]) { OutputFunction function = _mixing_output.outputFunction(i); bool is_reversible = reversible_outputs & (1u << i); @@ -78,12 +84,12 @@ bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], un if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax) && !is_reversible) { // Scale non-reversible motors to [0, 1] - actuator_outputs.output[i] = (output - PWM_SIM_PWM_MIN_MAGIC) / (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC); + actuator_outputs.output[i] = (output - _pwm_min[i]) / (_pwm_max[i] - _pwm_min[i]); } else { // Scale everything else to [-1, 1] - const float pwm_center = (PWM_SIM_PWM_MAX_MAGIC + PWM_SIM_PWM_MIN_MAGIC) / 2.f; - const float pwm_delta = (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC) / 2.f; + const float pwm_center = (_pwm_max[i] + _pwm_min[i]) / 2.f; + const float pwm_delta = (_pwm_max[i] - _pwm_min[i]) / 2.f; actuator_outputs.output[i] = (output - pwm_center) / pwm_delta; } } diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.hpp b/src/modules/simulation/pwm_out_sim/PWMSim.hpp index b16a9fac5e75..a24ae9b526e3 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.hpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.hpp @@ -76,10 +76,9 @@ 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; + int32_t _pwm_min[MAX_ACTUATORS] {}; + int32_t _pwm_max[MAX_ACTUATORS] {}; + int32_t _pwm_disarmed[MAX_ACTUATORS] {}; MixingOutput _mixing_output{PARAM_PREFIX, MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; diff --git a/src/modules/simulation/pwm_out_sim/module_hil.yaml b/src/modules/simulation/pwm_out_sim/module_hil.yaml index 25b9db39e02b..9ed19eecca4e 100644 --- a/src/modules/simulation/pwm_out_sim/module_hil.yaml +++ b/src/modules/simulation/pwm_out_sim/module_hil.yaml @@ -5,4 +5,9 @@ actuator_output: output_groups: - param_prefix: HIL_ACT channel_label: Channel + standard_params: + disarmed: { min: 0, max: 1000, default: 0 } + min: { min: 0, max: 1000, default: 0 } + max: { min: 0, max: 1000, default: 1000 } + failsafe: { min: 0, max: 1000 } num_channels: 16 diff --git a/ssrc_config/config_hitl_eth_gzsim_fw.txt b/ssrc_config/config_hitl_eth_gzsim_fw.txt new file mode 100644 index 000000000000..e0c7583c6c3b --- /dev/null +++ b/ssrc_config/config_hitl_eth_gzsim_fw.txt @@ -0,0 +1,119 @@ +# Default parameter set for HITL with ethernet Gazebo Sim connection on FW +# [ type: hitl_eth_gzsim ] + +#################################### +# general HITL configuration +#################################### + +# Set HITL related flag +param set SYS_HITL 1 +param set MAV_HITL_SHOME 1 +param set SENS_EN_GPSSIM 1 +param set SENS_EN_BAROSIM 1 +param set SENS_EN_MAGSIM 1 +param set SENS_EN_ARSPDSIM 1 + +# Disable safety switch +param set COM_PREARM_MODE 0 +param set CBRK_IO_SAFETY 22027 + +# Disable RC loss failsafe check +param set NAV_RCL_ACT 0 + +# Disable RC input requirement +param set COM_RC_IN_MODE 1 + +# EKF2 +param set EKF2_MULTI_IMU 3 + +# Sensor +param set CAL_ACC0_ID 1310988 +param set CAL_GYRO0_ID 1310988 +param set CAL_ACC1_ID 1310996 +param set CAL_GYRO1_ID 1310996 +param set CAL_ACC2_ID 1311004 +param set CAL_GYRO2_ID 1311004 +param set CAL_MAG1_ID 197388 +param set CAL_MAG1_PRIO 50 + + +#################################### +# specific fw HITL configuration +#################################### + +# Override airframe defaults +param set-default ASPD_DO_CHECKS 7 +param set-default FW_LAUN_DETCN_ON 0 + +# HITL PWM functions +param set-default HIL_ACT_FUNC1 201 +param set-default HIL_ACT_DIS1 500 +param set-default HIL_ACT_FUNC2 202 +param set-default HIL_ACT_DIS2 500 +param set-default HIL_ACT_FUNC4 101 +param set-default HIL_ACT_MIN4 150 +param set-default HIL_ACT_MAX4 1000 + +# Control allocator parameters +param set-default CA_AIRFRAME 1 +param set-default CA_ROTOR_COUNT 1 +param set-default CA_SV_CS_COUNT 2 +param set-default CA_SV_CS0_TYPE 5 +param set-default CA_SV_CS0_TRQ_P 0.5 +param set-default CA_SV_CS0_TRQ_R -0.5 +param set-default CA_SV_CS1_TYPE 6 +param set-default CA_SV_CS1_TRQ_P 0.5 +param set-default CA_SV_CS1_TRQ_R 0.5 + +# Airspeed parameters +param set-default ASPD_PRIMARY 1 +param set-default FW_AIRSPD_MAX 22.0 +param set-default FW_AIRSPD_MIN 14.0 +param set-default FW_AIRSPD_STALL 12.0 +param set-default FW_AIRSPD_TRIM 18.0 + +# Maximum landing slope angle in deg +param set-default FW_LND_ANG 8.0 + +# RC loss failsafe to HOLD mode +param set-default COM_RC_IN_MODE 1 + +# Maximum manual roll angle +param set-default FW_MAN_R_MAX 60.0 + +# Fixed wing control +# Pitch rate +param set-default FW_PR_P 0.9 +param set-default FW_PR_FF 0.5 +param set-default FW_PR_I 0.5 +param set-default TRIM_PITCH -0.15 +# Pitch angle in deg +param set-default FW_PSP_OFF 0.0 +param set-default FW_P_LIM_MIN -15.0 +# Roll rate +param set-default FW_RR_FF 0.5 +param set-default FW_RR_P 0.3 +param set-default FW_RR_I 0.5 +# Yaw rate +param set-default FW_YR_FF 0.5 +param set-default FW_YR_P 0.6 +param set-default FW_YR_I 0.5 +#Throttle limit +param set-default FW_THR_MAX 0.6 +param set-default FW_THR_MIN 0.05 +param set-default FW_THR_TRIM 0.25 +# Climb and sink rate +param set-default FW_T_CLMB_MAX 8.0 +param set-default FW_T_SINK_MAX 2.7 +param set-default FW_T_SINK_MIN 2.2 + +# Navigation +param set-default NAV_ACC_RAD 15.0 +param set-default NAV_DLL_ACT 2 + +# Misc +param set-default RTL_RETURN_ALT 30.0 +param set-default RTL_DESCEND_ALT 30.0 +param set-default FW_LND_USETER 0 +param set-default RWTO_TKOFF 1 +param set-default FD_ESCS_EN 0 diff --git a/ssrc_config/config_hitl_eth_gzsim_mc.txt b/ssrc_config/config_hitl_eth_gzsim_mc.txt new file mode 100644 index 000000000000..8721a6fe4b67 --- /dev/null +++ b/ssrc_config/config_hitl_eth_gzsim_mc.txt @@ -0,0 +1,80 @@ +# Default parameter set for HITL with ethernet Gazebo Sim connection on MC +# [ type: hitl_eth_gzsim ] + +#################################### +# general HITL configuration +#################################### + +# Set HITL related flag +param set SYS_HITL 1 +param set MAV_HITL_SHOME 1 +param set SENS_EN_GPSSIM 1 +param set SENS_EN_BAROSIM 1 +param set SENS_EN_MAGSIM 1 + +# Disable safety switch +param set COM_PREARM_MODE 0 +param set CBRK_IO_SAFETY 22027 + +# Disable RC loss failsafe check +param set NAV_RCL_ACT 0 + +# Disable RC input requirement +param set COM_RC_IN_MODE 1 + +# EKF2 +param set EKF2_MULTI_IMU 3 + +# Sensor +param set CAL_ACC0_ID 1310988 +param set CAL_GYRO0_ID 1310988 +param set CAL_ACC1_ID 1310996 +param set CAL_GYRO1_ID 1310996 +param set CAL_ACC2_ID 1311004 +param set CAL_GYRO2_ID 1311004 +param set CAL_MAG1_ID 197388 +param set CAL_MAG1_PRIO 50 + + +#################################### +# specific mc HITL configuration +#################################### + +# HITL PWM functions +param set-default HIL_ACT_FUNC1 101 +param set-default HIL_ACT_FUNC2 102 +param set-default HIL_ACT_FUNC3 103 +param set-default HIL_ACT_FUNC4 104 + +param set-default CA_ROTOR0_PX 0.175 +param set-default CA_ROTOR0_PY 0.175 +param set-default CA_ROTOR0_KM 0.05 + +param set-default CA_ROTOR1_PX -0.175 +param set-default CA_ROTOR1_PY -0.175 +param set-default CA_ROTOR1_KM 0.05 + +param set-default CA_ROTOR2_PX 0.175 +param set-default CA_ROTOR2_PY -0.175 +param set-default CA_ROTOR2_KM -0.05 + +param set-default CA_ROTOR3_PX -0.175 +param set-default CA_ROTOR3_PY 0.175 +param set-default CA_ROTOR3_KM -0.05 + +# extra +param set-default MPC_THR_HOVER 0.60 +param set COM_RCL_EXCEPT 4 +param set NAV_DLL_ACT 0 +param set NAV_RCL_ACT 0 +param set MAV_0_BROADCAST 1 +param set IMU_GYRO_CUTOFF 60 +param set IMU_DGYRO_CUTOFF 30 +param set MC_ROLLRATE_P 0.14 +param set MC_PITCHRATE_P 0.14 +param set MC_ROLLRATE_I 0.3 +param set MC_PITCHRATE_I 0.3 +param set MC_ROLLRATE_D 0.004 +param set MC_PITCHRATE_D 0.004 +param set BAT_N_CELLS 4 +param set SDLOG_MODE 0 \ No newline at end of file diff --git a/ssrc_config/config_hitl_eth_gzsim_rover.txt b/ssrc_config/config_hitl_eth_gzsim_rover.txt new file mode 100644 index 000000000000..6bb0a73bdbca --- /dev/null +++ b/ssrc_config/config_hitl_eth_gzsim_rover.txt @@ -0,0 +1,91 @@ +# Default parameter set for HITL with ethernet Gazebo Sim connection on rover +# [ type: hitl_eth_gzsim ] + +#################################### +# general HITL configuration +#################################### + +# Set HITL related flag +param set SYS_HITL 1 +param set MAV_HITL_SHOME 1 +param set SENS_EN_GPSSIM 1 +param set SENS_EN_BAROSIM 1 +param set SENS_EN_MAGSIM 1 + +# Disable safety switch +param set COM_PREARM_MODE 0 +param set CBRK_IO_SAFETY 22027 + +# Disable RC loss failsafe check +param set NAV_RCL_ACT 0 + +# Disable RC input requirement +param set COM_RC_IN_MODE 1 + +# EKF2 +param set EKF2_MULTI_IMU 3 + +# Sensor +param set CAL_ACC0_ID 1310988 +param set CAL_GYRO0_ID 1310988 +param set CAL_ACC1_ID 1310996 +param set CAL_GYRO1_ID 1310996 +param set CAL_ACC2_ID 1311004 +param set CAL_GYRO2_ID 1311004 +param set CAL_MAG1_ID 197388 +param set CAL_MAG1_PRIO 50 + + +#################################### +# specific rover HITL configuration +#################################### + +param set IMU_GYRO_CUTOFF 60 +param set IMU_DGYRO_CUTOFF 30 + +param set-default SYS_HAS_BARO 0 + +param set-default BAT1_N_CELLS 4 + +param set-default MIS_TAKEOFF_ALT 0.01 + +param set-default NAV_ACC_RAD 0.5 # reached when within 0.5m of waypoint + +# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor +param set-default CBRK_AIRSPD_CHK 162128 + +# EKF2 +param set-default EKF2_GBIAS_INIT 0.01 +param set-default EKF2_ANGERR_INIT 0.01 +param set-default EKF2_MAG_TYPE 1 +param set-default EKF2_REQ_SACC 1.0 +param set-default EKF2_REQ_VDRIFT 0.4 +param set-default EKF2_REQ_HDRIFT 0.2 + + +# Rover Position Control Module +param set-default GND_SP_CTRL_MODE 1 +param set-default GND_L1_DIST 5 +param set-default GND_L1_PERIOD 3 +param set-default GND_THR_CRUISE 1 +param set-default GND_THR_MAX 1 + +# Because this is differential drive, it can make a turn with radius 0. +# This corresponds to a turn angle of pi radians. +# If a special case is made for differential-drive, this will need to change. +param set-default GND_MAX_ANG 3.142 +param set-default GND_WHEEL_BASE 0.45 + +# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase +# to support negative throttle. +param set-default GND_THR_MIN 0 +param set-default GND_SPEED_P 0.4 +param set-default GND_SPEED_I 1 +param set-default GND_SPEED_D 0.001 +param set-default GND_SPEED_MAX 3.0 +param set-default GND_SPEED_TRIM 3.0 +param set-default GND_SPEED_THR_SC 1.0 +param set-default GND_VEL_CTRL 1 +param set-default GND_ANG_VEL_CTRL 1 +param set-default GND_ACC_LIMIT 10 +param set-default GND_DEC_LIMIT 50 diff --git a/ssrc_config/config_hitl_eth_gzsim_vtol.txt b/ssrc_config/config_hitl_eth_gzsim_vtol.txt new file mode 100644 index 000000000000..8ca49398e7cb --- /dev/null +++ b/ssrc_config/config_hitl_eth_gzsim_vtol.txt @@ -0,0 +1,124 @@ +# Default parameter set for HITL with ethernet Gazebo Sim connection on VTOL +# [ type: hitl_eth_gzsim ] + +#################################### +# general HITL configuration +#################################### + +# Set HITL related flag +param set SYS_HITL 1 +param set MAV_HITL_SHOME 1 +param set SENS_EN_GPSSIM 1 +param set SENS_EN_BAROSIM 1 +param set SENS_EN_MAGSIM 1 +param set SENS_EN_ARSPDSIM 1 + +# Disable safety switch +param set COM_PREARM_MODE 0 +param set CBRK_IO_SAFETY 22027 + +# Disable RC loss failsafe check +param set NAV_RCL_ACT 0 + +# Disable RC input requirement +param set COM_RC_IN_MODE 1 + +# EKF2 +param set EKF2_MULTI_IMU 3 + +# Sensor +param set CAL_ACC0_ID 1310988 +param set CAL_GYRO0_ID 1310988 +param set CAL_ACC1_ID 1310996 +param set CAL_GYRO1_ID 1310996 +param set CAL_ACC2_ID 1311004 +param set CAL_GYRO2_ID 1311004 +param set CAL_MAG1_ID 197388 +param set CAL_MAG1_PRIO 50 + + +#################################### +# specific vtol HITL configuration +#################################### + +# HITL PWM functions +param set-default HIL_ACT_FUNC1 101 +param set-default HIL_ACT_MIN1 10 +param set-default HIL_ACT_MAX1 1500 +param set-default HIL_ACT_FUNC2 102 +param set-default HIL_ACT_MIN2 10 +param set-default HIL_ACT_MAX2 1500 +param set-default HIL_ACT_FUNC3 103 +param set-default HIL_ACT_MIN3 10 +param set-default HIL_ACT_MAX3 1500 +param set-default HIL_ACT_FUNC4 104 +param set-default HIL_ACT_MIN4 10 +param set-default HIL_ACT_MAX4 1500 +param set-default HIL_ACT_FUNC5 105 +param set-default HIL_ACT_MIN5 0 +param set-default HIL_ACT_MAX5 3500 +param set-default HIL_ACT_FUNC6 201 +param set-default HIL_ACT_DIS6 500 +param set-default HIL_ACT_FUNC7 202 +param set-default HIL_ACT_DIS7 500 +param set-default HIL_ACT_FUNC8 203 +param set-default HIL_ACT_DIS8 500 + + +# Control allocator parameters +param set-default CA_ROTOR_COUNT 5 +param set-default CA_ROTOR0_PX 0.1515 +param set-default CA_ROTOR0_PY 0.245 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR1_PX -0.1515 +param set-default CA_ROTOR1_PY -0.1875 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR2_PX 0.1515 +param set-default CA_ROTOR2_PY -0.245 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.1515 +param set-default CA_ROTOR3_PY 0.1875 +param set-default CA_ROTOR3_KM -0.05 +param set-default CA_ROTOR4_AX 1.0 +param set-default CA_ROTOR4_AZ 0.0 +param set-default CA_ROTOR4_PX 0.2 + +param set-default CA_SV_CS_COUNT 3 +param set-default CA_SV_CS0_TYPE 1 +param set-default CA_SV_CS0_TRQ_R -0.5 +param set-default CA_SV_CS1_TYPE 2 +param set-default CA_SV_CS1_TRQ_R 0.5 +param set-default CA_SV_CS2_TYPE 3 +param set-default CA_SV_CS2_TRQ_P 1.0 + +param set-default FW_L1_PERIOD 12 +param set-default FW_PR_FF 0.2 +param set-default FW_PR_P 0.9 +param set-default FW_PSP_OFF 2.0 +param set-default FW_P_LIM_MIN -15.0 +param set-default FW_RR_FF 0.1 +param set-default FW_RR_P 0.3 +param set-default FW_THR_TRIM 0.25 +param set-default FW_THR_MAX 0.6 +param set-default FW_THR_MIN 0.05 +param set-default FW_T_CLMB_MAX 8.0 +param set-default FW_T_SINK_MAX 2.7 +param set-default FW_T_SINK_MIN 2.2 + +param set-default MC_AIRMODE 1 +param set-default MC_ROLLRATE_P 0.3 +param set-default MC_YAW_P 1.6 + +param set-default MIS_TAKEOFF_ALT 10.0 + +param set-default MPC_XY_P 0.8 +param set-default MPC_XY_VEL_P_ACC 3.0 +param set-default MPC_XY_VEL_I_ACC 4.0 +param set-default MPC_XY_VEL_D_ACC 0.1 + +param set-default NAV_ACC_RAD 5.0 + +param set-default VT_FWD_THRUST_EN 4 +param set-default VT_F_TRANS_THR 0.75 +param set-default VT_TYPE 2 +param set-default FD_ESCS_EN 0 \ No newline at end of file diff --git a/ssrc_config/config_hitl_eth_gzsim_vtol_sm.txt b/ssrc_config/config_hitl_eth_gzsim_vtol_sm.txt new file mode 100644 index 000000000000..cad677b6a612 --- /dev/null +++ b/ssrc_config/config_hitl_eth_gzsim_vtol_sm.txt @@ -0,0 +1,184 @@ +# Default parameter set for HITL with ethernet Gazebo Sim connection on VTOL +# [ type: hitl_eth_gzsim ] + +#################################### +# general HITL configuration +#################################### + +# Set HITL related flag +param set SYS_HITL 1 +param set MAV_HITL_SHOME 1 +param set SENS_EN_GPSSIM 1 +param set SENS_EN_BAROSIM 1 +param set SENS_EN_MAGSIM 1 +param set SENS_EN_ARSPDSIM 1 + +# Disable safety switch +param set COM_PREARM_MODE 0 +param set CBRK_IO_SAFETY 22027 + +# Disable RC loss failsafe check +param set NAV_RCL_ACT 0 + +# Disable RC input requirement +param set COM_RC_IN_MODE 1 + +# EKF2 +param set EKF2_MULTI_IMU 3 + +# Sensor +param set CAL_ACC0_ID 1310988 +param set CAL_GYRO0_ID 1310988 +param set CAL_ACC1_ID 1310996 +param set CAL_GYRO1_ID 1310996 +param set CAL_ACC2_ID 1311004 +param set CAL_GYRO2_ID 1311004 +param set CAL_MAG1_ID 197388 +param set CAL_MAG1_PRIO 50 + + +#################################### +# specific vtol HITL configuration +#################################### + +# HITL PWM functions +param set-default HIL_ACT_FUNC1 101 +param set-default HIL_ACT_MIN1 10 +param set-default HIL_ACT_MAX1 1500 +param set-default HIL_ACT_FUNC2 102 +param set-default HIL_ACT_MIN2 10 +param set-default HIL_ACT_MAX2 1500 +param set-default HIL_ACT_FUNC3 103 +param set-default HIL_ACT_MIN3 10 +param set-default HIL_ACT_MAX3 1500 +param set-default HIL_ACT_FUNC4 104 +param set-default HIL_ACT_MIN4 10 +param set-default HIL_ACT_MAX4 1500 +param set-default HIL_ACT_FUNC5 201 +param set-default HIL_ACT_DIS5 500 +param set-default HIL_ACT_FUNC6 202 +param set-default HIL_ACT_DIS6 500 +param set-default HIL_ACT_FUNC7 203 +param set-default HIL_ACT_DIS7 500 +param set-default HIL_ACT_FUNC8 204 +param set-default HIL_ACT_DIS8 500 +param set-default HIL_ACT_FUNC9 205 +param set-default HIL_ACT_DIS9 500 +param set-default HIL_ACT_FUNC10 105 +param set-default HIL_ACT_MIN10 0 +param set-default HIL_ACT_MAX10 3500 + +# Control allocator parameters +param set-default CA_AIRFRAME 2 +param set-default CA_ROTOR_COUNT 5 +param set-default CA_ROTOR0_PX 0.37 +param set-default CA_ROTOR0_PY 0.42 +param set-default CA_ROTOR1_PX -0.41 +param set-default CA_ROTOR1_PY -0.42 +param set-default CA_ROTOR2_PX 0.37 +param set-default CA_ROTOR2_PY -0.42 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.41 +param set-default CA_ROTOR3_PY 0.42 +param set-default CA_ROTOR3_KM -0.05 +param set-default CA_ROTOR4_AX 1.0 +param set-default CA_ROTOR4_AZ 0.0 +param set-default CA_ROTOR4_PX 0.0 +param set-default CA_ROTOR4_KM 0.05 + +param set-default CA_SV_CS_COUNT 5 +param set-default CA_SV_CS0_TYPE 1 +param set-default CA_SV_CS0_TRQ_R -0.5 +param set-default CA_SV_CS1_TYPE 2 +param set-default CA_SV_CS1_TRQ_R 0.5 +param set-default CA_SV_CS2_TYPE 3 +param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TRIM 0.1 +param set-default CA_SV_CS3_TYPE 3 +param set-default CA_SV_CS3_TRQ_P 1.0 +param set-default CA_SV_CS3_TRIM 0.1 +param set-default CA_SV_CS4_TYPE 4 +param set-default CA_SV_CS4_TRQ_Y 1.0 + +# Fixed wing specific +param set-default FW_RR_IMAX 0.4000 +param set-default FW_YR_IMAX 0.4000 + +param set-default FW_YR_P 0.2 +param set-default FW_YR_I 0.01 + +param set-default FW_RR_FF 0.5 +param set-default FW_RR_P 0.05 +param set-default FW_RR_I 0.1 +param set-default FW_RR_IMAX 0.4 + +param set-default FW_R_LIM 50.0 +param set-default FW_R_RMAX 70.0 +param set-default FW_R_TC 0.4 + +param set-default FW_PR_FF 0.5 +param set-default FW_PR_I 0.1 +param set-default FW_PR_IMAX 0.4 + +param set-default FW_P_LIM_MAX 30.0 +param set-default FW_P_LIM_MIN -30.0 + +# Airspeed parameters +param set-default ASPD_PRIMARY 1 +param set-default FW_AIRSPD_MAX 25.0 +param set-default FW_AIRSPD_MIN 15.0 +param set-default FW_AIRSPD_STALL 12.0 +param set-default FW_AIRSPD_TRIM 18.0 + +# VTOL specific +# VTOL type +param set-default VT_TYPE 2 + +# Airspeed at which we can start blending both fw and mc controls. +param set-default VT_ARSP_BLEND 8.0 + +# Airspeed at which we can switch to fw mode +param set-default VT_ARSP_TRANS 14.0 + +# Back-transition duration +param set-default VT_B_TRANS_DUR 10.0 +param set-default VT_B_TRANS_RAMP 3.0 + +# Transition duration +param set-default VT_F_TRANS_DUR 6.0 +param set-default VT_F_TRANS_THR 1.0 + +# VTOL takeoff +param set-default MIS_TAKEOFF_ALT 10.0 +param set-default VTO_LOITER_ALT 10.0 + +param set-default MC_AIRMODE 0 +param set-default MC_ROLLRATE_P 0.14 +param set-default MC_ROLLRATE_I 0.2 +param set-default MC_PITCHRATE_P 0.14 +param set-default MC_PITCHRATE_I 0.2 +param set-default MC_YAW_P 2.0 +param set-default MC_YAW_WEIGHT 0.5 +param set-default MC_YAWRATE_P 0.2 +param set-default MC_YAWRATE_I 0.1 +param set-default MC_YAWRATE_MAX 120.0 + +param set-default MPC_XY_P 0.95 +param set-default MPC_XY_VEL_P_ACC 1.8 +param set-default MPC_XY_VEL_I_ACC 0.4 +param set-default MPC_XY_VEL_D_ACC 0.2 +param set-default MPC_YAW_MODE 4 + +param set-default NAV_ACC_RAD 10.0 + +# # QuadChute altitude (transition to quad mode as a failsafe) +# param set-default VT_FW_MIN_ALT 5 + +# # QuadChute angle limits +# param set-default VT_FW_QC_P 35 +# param set-default VT_FW_QC_R 60 + +# Others +param set-default FD_ACT_EN 0 +param set-default FD_ACT_MOT_TOUT 500 +param set-default FD_ESCS_EN 0