Update GZBridge to be able to use gazebo airspeed. Add quadtailsitter. (

#23455) * Update GZBridge to be able to use gazebo airspeed. Add gz quadtailsitter. * Fix formatting --------- Co-authored-by: jmackay2 <[email protected]>
PX4 · Aug 19, 2024 · 7e45f49 · 7e45f49
1 parent e29a36a
commit 7e45f49
Show file tree

Hide file tree

Showing 7 changed files with 103 additions and 16 deletions.
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter
@@ -33,7 +33,7 @@ param set-default PWM_MAIN_FUNC3 103
 param set-default PWM_MAIN_FUNC4 104
 param set-default PWM_MAIN_FUNC5 0
 
-parm set-default FD_FAIL_R 70
+param set-default FD_FAIL_R 70
 
 param set-default FW_P_TC 0.6
 

diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol
@@ -14,9 +14,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol}
 param set-default SIM_GZ_EN 1
 
 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 1
 
 # TODO: Enable motor failure detection when the
 # VTOL no longer reports 0A for all ESCs in SITL

diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_quadtailsitter
@@ -0,0 +1,92 @@
+#!/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
+
+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_MIN1 10
+param set-default SIM_GZ_EC_MAX1 1500
+param set-default SIM_GZ_EC_FUNC2 102
+param set-default SIM_GZ_EC_MIN2 10
+param set-default SIM_GZ_EC_MAX2 1500
+param set-default SIM_GZ_EC_FUNC3 103
+param set-default SIM_GZ_EC_MIN3 10
+param set-default SIM_GZ_EC_MAX3 1500
+param set-default SIM_GZ_EC_FUNC4 104
+param set-default SIM_GZ_EC_MIN4 10
+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
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
@@ -85,6 +85,7 @@ px4_add_romfs_files(
 	4011_gz_lawnmower
 	4012_gz_rover_ackermann
 	4013_gz_x500_lidar
+	4014_gz_quadtailsitter
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp
@@ -203,17 +203,15 @@ int GZBridge::init()
 		PX4_WARN("failed to subscribe to %s", laser_scan_topic.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";
+	// Airspeed: /world/$WORLD/model/$MODEL/link/base_link/sensor/airspeed_sensor/air_speed
+	std::string airspeed_topic = "/world/" + _world_name + "/model/" + _model_name +
+				     "/link/base_link/sensor/airspeed_sensor/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";
@@ -426,8 +424,7 @@ 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;
@@ -452,7 +449,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
@@ -102,7 +102,7 @@ class GZBridge : public ModuleBase<GZBridge>, 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);
@@ -123,7 +123,7 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	// Subscriptions
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-	//uORB::Publication<differential_pressure_s>    _differential_pressure_pub{ORB_ID(differential_pressure)};
+	uORB::Publication<differential_pressure_s>    _differential_pressure_pub{ORB_ID(differential_pressure)};
 	uORB::Publication<obstacle_distance_s>        _obstacle_distance_pub{ORB_ID(obstacle_distance)};
 	uORB::Publication<vehicle_angular_velocity_s> _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
 	uORB::Publication<vehicle_attitude_s>         _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};

diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp
@@ -195,7 +195,7 @@ int SensorAirspeedSim::print_usage(const char *reason)
 
 )DESCR_STR");
 
-	PRINT_MODULE_USAGE_NAME("sensor_arispeed_sim", "system");
+	PRINT_MODULE_USAGE_NAME("sensor_airspeed_sim", "system");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();