From ebcb99da985e0fbaa4f55f6226f4db64c6d276cd Mon Sep 17 00:00:00 2001 From: Pernilla Date: Fri, 15 Nov 2024 15:10:48 +0100 Subject: [PATCH 1/2] Adding tiltrotor model --- .../init.d-posix/airframes/4030_gz_tiltrotor | 112 ++++++++++++++++++ .../init.d-posix/airframes/CMakeLists.txt | 1 + Tools/simulation/gz | 2 +- 3 files changed, 114 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4030_gz_tiltrotor diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4030_gz_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/4030_gz_tiltrotor new file mode 100644 index 000000000000..7d5ebf3a873f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4030_gz_tiltrotor @@ -0,0 +1,112 @@ +#!/bin/sh +# +# @name VTOL Tiltrotor +# +# @type VTOL Tiltrotor +# + +. ${R}etc/init.d/rc.vtol_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=tiltrotor} + +param set-default SIM_GZ_EN 1 # Gazebo bridge + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 0 +param set-default SENS_EN_MAGSIM 1 +param set-default SENS_EN_ARSPDSIM 0 + +param set-default MAV_TYPE 21 + +param set-default CA_AIRFRAME 3 + +param set-default CA_ROTOR_COUNT 4 +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_ROTOR0_TILT 1 +param set-default CA_ROTOR2_TILT 2 +param set-default CA_SV_TL0_MAXA 90 +param set-default CA_SV_TL0_MINA 0 +param set-default CA_SV_TL0_TD 0 +param set-default CA_SV_TL0_CT 1 +param set-default CA_SV_TL1_MAXA 90 +param set-default CA_SV_TL1_MINA 0 +param set-default CA_SV_TL1_TD 0 +param set-default CA_SV_TL1_CT 1 + +param set-default CA_SV_CS0_TRQ_R -0.5 +param set-default CA_SV_CS0_TYPE 1 +param set-default CA_SV_CS1_TRQ_R 0.5 +param set-default CA_SV_CS1_TYPE 2 +param set-default CA_SV_CS2_TRQ_P 1 +param set-default CA_SV_CS2_TYPE 3 +param set-default CA_SV_CS_COUNT 3 +param set-default CA_SV_TL_COUNT 2 + +param set-default SIM_GZ_EC_FUNC1 101 +param set-default SIM_GZ_EC_FUNC2 102 +param set-default SIM_GZ_EC_FUNC3 103 +param set-default SIM_GZ_EC_FUNC4 104 + +param set-default SIM_GZ_EC_MIN1 10 +param set-default SIM_GZ_EC_MIN2 10 +param set-default SIM_GZ_EC_MIN3 10 +param set-default SIM_GZ_EC_MIN4 10 + +param set-default SIM_GZ_EC_MAX1 1500 +param set-default SIM_GZ_EC_MAX2 1500 +param set-default SIM_GZ_EC_MAX3 1500 +param set-default SIM_GZ_EC_MAX4 1500 + +param set-default SIM_GZ_SV_FUNC1 201 +param set-default SIM_GZ_SV_FUNC2 202 +param set-default SIM_GZ_SV_FUNC3 203 +param set-default SIM_GZ_SV_FUNC4 204 +param set-default SIM_GZ_SV_FUNC5 205 + +param set-default SIM_GZ_SV_MAX4 27000 +param set-default SIM_GZ_SV_MIN4 18000 +param set-default SIM_GZ_SV_MAX5 27000 +param set-default SIM_GZ_SV_MIN5 18000 + +param set-default NPFG_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 +param set-default FW_P_LIM_MIN -15 +param set-default FW_RR_FF 0.1 +param set-default FW_RR_P 0.3 +param set-default FW_THR_TRIM 0.38 +param set-default FW_THR_MAX 0.6 +param set-default FW_THR_MIN 0.05 +param set-default FW_T_CLMB_MAX 8 +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_YAWRATE_P 0.4 +param set-default MC_YAWRATE_I 0.1 + +param set-default MPC_XY_VEL_P_ACC 3 +param set-default MPC_XY_VEL_I_ACC 4 +param set-default MPC_XY_VEL_D_ACC 0.1 + +# param set-default MIS_TAKEOFF_ALT 10 + +param set-default VT_FWD_THRUST_EN 4 +param set-default VT_FWD_THRUST_SC 0.6 +param set-default VT_TILT_TRANS 0.6 +param set-default VT_TYPE 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 1987464e319b..ac228f84c349 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -90,6 +90,7 @@ px4_add_romfs_files( 4016_gz_x500_lidar_down 4017_gz_x500_lidar_front 4018_gz_quadtailsitter + 4030_gz_tiltrotor 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 018f335e7210..7c9cf6080d22 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 018f335e7210f244667d03672c4defd4e9bfeb9d +Subproject commit 7c9cf6080d2220c1f2df2b0dcbdf5df47bfe0f95 From 4ec9990577ddc4f1376729d7a41d3884fd99a0cf Mon Sep 17 00:00:00 2001 From: Pernilla Date: Thu, 28 Nov 2024 18:46:30 +0100 Subject: [PATCH 2/2] Send angles in actual range instead of hardcoded angles --- .../init.d-posix/airframes/4011_gz_lawnmower | 12 ++++++------ .../init.d-posix/airframes/4012_gz_rover_ackermann | 2 ++ .../init.d-posix/airframes/4030_gz_tiltrotor | 8 ++++---- Tools/module_config/generate_params.py | 4 ++++ .../simulation/gz_bridge/GZMixingInterfaceServo.cpp | 4 ++-- src/modules/simulation/gz_bridge/module.yaml | 8 ++++---- 6 files changed, 22 insertions(+), 16 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower index 5f2b635238f7..8181d0052306 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -62,17 +62,17 @@ param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels # Cutter deck blades clutch, PCA9685 servo channel 3, "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203): param set-default SIM_GZ_SV_FUNC3 203 -param set-default SIM_GZ_SV_MIN3 0 -param set-default SIM_GZ_SV_MAX3 1000 -param set-default SIM_GZ_SV_DIS3 500 +param set-default SIM_GZ_SV_MIN3 900 +param set-default SIM_GZ_SV_MAX3 2700 +param set-default SIM_GZ_SV_DIS3 1800 param set-default SIM_GZ_SV_FAIL3 500 # Gas engine throttle, PCA9685 servo channel 4, "RC AUX1" (407) - left knob, or "Servo 4" (204): # - on minimum when disarmed or failed: param set-default SIM_GZ_SV_FUNC4 204 -param set-default SIM_GZ_SV_MIN4 0 -param set-default SIM_GZ_SV_MAX4 1000 -param set-default SIM_GZ_SV_DIS4 500 +param set-default SIM_GZ_SV_MIN4 900 +param set-default SIM_GZ_SV_MAX4 2700 +param set-default SIM_GZ_SV_DIS4 1800 param set-default SIM_GZ_SV_FAIL4 500 # Controlling PCA9685 servos 5,6,7,8 directly via "Servo 5..8" setting, by publishing actuator_servos.control[]: diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann index 9e54c4015909..d38799e299f1 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann @@ -49,3 +49,5 @@ param set-default SIM_GZ_WH_DIS1 100 # Steering param set-default SIM_GZ_SV_FUNC1 201 param set-default SIM_GZ_SV_REV 1 +param set-default SIM_GZ_SV_MIN1 1500 +param set-default SIM_GZ_SV_MAX1 2100 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4030_gz_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/4030_gz_tiltrotor index 7d5ebf3a873f..72752e9daa8d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4030_gz_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4030_gz_tiltrotor @@ -77,10 +77,10 @@ param set-default SIM_GZ_SV_FUNC3 203 param set-default SIM_GZ_SV_FUNC4 204 param set-default SIM_GZ_SV_FUNC5 205 -param set-default SIM_GZ_SV_MAX4 27000 -param set-default SIM_GZ_SV_MIN4 18000 -param set-default SIM_GZ_SV_MAX5 27000 -param set-default SIM_GZ_SV_MIN5 18000 +param set-default SIM_GZ_SV_MAX4 2700 +param set-default SIM_GZ_SV_MIN4 1800 +param set-default SIM_GZ_SV_MAX5 2700 +param set-default SIM_GZ_SV_MIN5 1800 param set-default NPFG_PERIOD 12 param set-default FW_PR_FF 0.2 diff --git a/Tools/module_config/generate_params.py b/Tools/module_config/generate_params.py index 92b25936568e..8defa0615c71 100755 --- a/Tools/module_config/generate_params.py +++ b/Tools/module_config/generate_params.py @@ -284,9 +284,13 @@ def add_local_param(param_name, param_def): ''' minimum_description = \ '''Minimum output value (when not disarmed). +Servo output represents angles [-180,180] with an offset of 180 deg, and a scaling of 10. +Ex. ailerons of -45 deg would be 1350. (Must be coherent with joint limits in the airframe/model.sdf) ''' maximum_description = \ '''Maxmimum output value (when not disarmed). +Servo output represents angles [-180,180] deg with an offset of 180 deg, and a scaling of 10. +Ex. ailerons of 45 deg would be 2250. (Must be coherent with joint limits in the airframe/model.sdf) ''' failsafe_description = \ '''This is the output value that is set when in failsafe mode. diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp index 9c7969b2dc1e..1b81ef27d076 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp @@ -64,8 +64,8 @@ bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MA for (auto &servo_pub : _servos_pub) { if (_mixing_output.isFunctionSet(i)) { gz::msgs::Double servo_output; - ///TODO: Normalize output data - double output = (outputs[i] - 500) / 500.0; + double output = math::radians((double)outputs[i] / 10. - 180.);; + // std::cout << "outputs[" << i << "]: " << outputs[i] << std::endl; // std::cout << " output: " << output << std::endl; servo_output.set_data(output); diff --git a/src/modules/simulation/gz_bridge/module.yaml b/src/modules/simulation/gz_bridge/module.yaml index 1b38e13d9883..bd810369929a 100644 --- a/src/modules/simulation/gz_bridge/module.yaml +++ b/src/modules/simulation/gz_bridge/module.yaml @@ -20,10 +20,10 @@ actuator_output: group_label: 'Servos' channel_label: 'Servo' standard_params: - disarmed: { min: 0, max: 1000, default: 500 } - min: { min: 0, max: 1000, default: 0 } - max: { min: 0, max: 1000, default: 1000 } - failsafe: { min: 0, max: 1000 } + disarmed: { min: 0, max: 3600, default: 1800 } + min: { min: 0, max: 3600, default: 1350 } + max: { min: 0, max: 3600, default: 2250 } + failsafe: { min: 0, max: 3600 } num_channels: 8 - param_prefix: SIM_GZ_WH group_label: 'Wheels'