From e10a63ba1512da54434f5d94fba8efe6d87953d0 Mon Sep 17 00:00:00 2001 From: Perre Date: Mon, 2 Dec 2024 17:27:23 +0100 Subject: [PATCH] Add gz model for quadtailsitter (#23943) * Add gazebo airspeed plugin and add a tailsitter model --------- Co-authored-by: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> --- .../airframes/4018_gz_quadtailsitter | 97 +++++++++++++++++++ .../init.d-posix/airframes/CMakeLists.txt | 1 + Tools/simulation/gz | 2 +- src/modules/simulation/gz_bridge/GZBridge.cpp | 15 ++- src/modules/simulation/gz_bridge/GZBridge.hpp | 5 +- 5 files changed, 108 insertions(+), 12 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter new file mode 100644 index 000000000000..9856b1bf273e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter @@ -0,0 +1,97 @@ +#!/bin/sh +# +# @name Quadrotor + Tailsitter +# +# @type VTOL Quad Tailsitter +# + +. ${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:=quadtailsitter} + +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 20 + +param set-default CA_AIRFRAME 4 + +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.15 +param set-default CA_ROTOR0_PY 0.23 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR1_PX -0.15 +param set-default CA_ROTOR1_PY -0.23 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR2_PX 0.15 +param set-default CA_ROTOR2_PY -0.23 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.15 +param set-default CA_ROTOR3_PY 0.23 +param set-default CA_ROTOR3_KM -0.05 + +param set-default CA_SV_CS_COUNT 0 + +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 FD_FAIL_R 70 + +param set-default FW_P_TC 0.6 + +param set-default FW_PR_I 0.3 +param set-default FW_PR_P 0.5 +param set-default FW_PSP_OFF 2 +param set-default FW_RR_FF 0.1 +param set-default FW_RR_I 0.1 +param set-default FW_RR_P 0.2 +param set-default FW_YR_FF 0 # make yaw rate controller very weak, only keep default P +param set-default FW_YR_I 0 +param set-default FW_THR_TRIM 0.35 +param set-default FW_THR_MAX 0.8 +param set-default FW_THR_MIN 0.05 +param set-default FW_T_CLMB_MAX 6 +param set-default FW_T_HRATE_FF 0.5 +param set-default FW_T_SINK_MAX 3 +param set-default FW_T_SINK_MIN 1.6 +param set-default FW_AIRSPD_STALL 10 +param set-default FW_AIRSPD_MIN 14 +param set-default FW_AIRSPD_TRIM 18 +param set-default FW_AIRSPD_MAX 22 + +param set-default MC_AIRMODE 2 +param set-default MAN_ARM_GESTURE 0 # required for yaw airmode +param set-default MC_ROLL_P 3 +param set-default MC_PITCH_P 3 +param set-default MC_ROLLRATE_P 0.3 +param set-default MC_PITCHRATE_P 0.3 + +param set-default VT_ARSP_TRANS 15 +param set-default VT_B_TRANS_DUR 5 +param set-default VT_FW_DIFTHR_EN 7 +param set-default VT_FW_DIFTHR_S_Y 1 +param set-default VT_F_TRANS_DUR 1.5 +param set-default VT_TYPE 0 + +param set-default WV_EN 0 + +param set-default EKF2_FUSE_BETA 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 96e1737e5d53..1987464e319b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -89,6 +89,7 @@ px4_add_romfs_files( 4015_gz_r1_rover_mecanum 4016_gz_x500_lidar_down 4017_gz_x500_lidar_front + 4018_gz_quadtailsitter 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 9e47793f2bc1..019f63e2d507 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 9e47793f2bc18aa7cde39b1fc1c4b7bbc67e04ba +Subproject commit 019f63e2d50763862b8f8d40cce77371b3260c52 diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 95be696835e2..61aa5d0aa4e1 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -226,17 +226,15 @@ int GZBridge::init() PX4_WARN("failed to subscribe to %s", lidar_sensor.c_str()); } -#if 0 // Airspeed: /world/$WORLD/model/$MODEL/link/airspeed_link/sensor/air_speed/air_speed - std::string airpressure_topic = "/world/" + _world_name + "/model/" + _model_name + - "/link/airspeed_link/sensor/air_speed/air_speed"; + std::string airspeed_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/airspeed_link/sensor/air_speed/air_speed"; - if (!_node.Subscribe(airpressure_topic, &GZBridge::airspeedCallback, this)) { - PX4_ERR("failed to subscribe to %s", airpressure_topic.c_str()); + if (!_node.Subscribe(airspeed_topic, &GZBridge::airspeedCallback, this)) { + PX4_ERR("failed to subscribe to %s", airspeed_topic.c_str()); return PX4_ERROR; } -#endif // Air pressure: /world/$WORLD/model/$MODEL/link/base_link/sensor/air_pressure_sensor/air_pressure std::string air_pressure_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/base_link/sensor/air_pressure_sensor/air_pressure"; @@ -449,8 +447,8 @@ void GZBridge::barometerCallback(const gz::msgs::FluidPressure &air_pressure) pthread_mutex_unlock(&_node_mutex); } -#if 0 -void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed) + +void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &air_speed) { if (hrt_absolute_time() == 0) { return; @@ -475,7 +473,6 @@ void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed) pthread_mutex_unlock(&_node_mutex); } -#endif void GZBridge::imuCallback(const gz::msgs::IMU &imu) { diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index f31ecf062f68..e4c6800755e5 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -68,6 +68,7 @@ #include #include +#include #include #include #include @@ -106,7 +107,7 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S void clockCallback(const gz::msgs::Clock &clock); - // void airspeedCallback(const gz::msgs::AirSpeedSensor &air_pressure); + void airspeedCallback(const gz::msgs::AirSpeed &air_speed); void barometerCallback(const gz::msgs::FluidPressure &air_pressure); void imuCallback(const gz::msgs::IMU &imu); void poseInfoCallback(const gz::msgs::Pose_V &pose); @@ -167,8 +168,8 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - //uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::Publication _distance_sensor_pub{ORB_ID(distance_sensor)}; + uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; uORB::Publication _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};