diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
index 2ea8171e89..c401a42069 100644
--- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
+++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
@@ -49,7 +49,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
echo "INFO [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
- ${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
+ ${gz_command} ${gz_sub_command} --verbose=4 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
if [ -z "${HEADLESS}" ]; then
# HEADLESS not set, starting gui
diff --git a/Tools/simulation/gz/models/standard_vtol/model.sdf b/Tools/simulation/gz/models/standard_vtol/model.sdf
index d3ed4adbe2..f137e0b6d6 100644
--- a/Tools/simulation/gz/models/standard_vtol/model.sdf
+++ b/Tools/simulation/gz/models/standard_vtol/model.sdf
@@ -135,7 +135,7 @@
250
-
+
0 0 0 0 0 0
0 0 0 0 0 0
@@ -177,23 +177,9 @@
- airspeed
+ airspeed_link
base_link
-
0.35 -0.35 0.07 0 0 0
diff --git a/Tools/simulation/gz/worlds/default.sdf b/Tools/simulation/gz/worlds/default.sdf
index ca2ef31634..fdf0151ea5 100644
--- a/Tools/simulation/gz/worlds/default.sdf
+++ b/Tools/simulation/gz/worlds/default.sdf
@@ -5,12 +5,12 @@
1.0
250
-
-
-
-
-
-
+
+
+
+
+
+
ogre2
diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt
index 8462dec6d3..6a9e3e7f05 100644
--- a/src/modules/simulation/gz_bridge/CMakeLists.txt
+++ b/src/modules/simulation/gz_bridge/CMakeLists.txt
@@ -35,7 +35,6 @@
find_package(gz-transport
#REQUIRED COMPONENTS core
NAMES
- #ignition-transport11 # IGN (Fortress and earlier) no longer supported
gz-transport12
#QUIET
)
diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp
index db8bc6c69b..9af8843411 100644
--- a/src/modules/simulation/gz_bridge/GZBridge.cpp
+++ b/src/modules/simulation/gz_bridge/GZBridge.cpp
@@ -152,7 +152,7 @@ int GZBridge::init()
return PX4_ERROR;
}
-#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";
@@ -162,7 +162,6 @@ int GZBridge::init()
return PX4_ERROR;
}
-#endif
if (!_mixing_interface_esc.init(_model_name)) {
PX4_ERR("failed to init ESC output");
@@ -329,14 +328,14 @@ void GZBridge::clockCallback(const gz::msgs::Clock &clock)
pthread_mutex_unlock(&_node_mutex);
}
-#if 0
+
void GZBridge::airpressureCallback(const gz::msgs::FluidPressure &air_pressure)
{
if (hrt_absolute_time() == 0) {
return;
}
- pthread_mutex_lock(&_mutex);
+ pthread_mutex_lock(&_node_mutex);
const uint64_t time_us = (air_pressure.header().stamp().sec() * 1000000)
+ (air_pressure.header().stamp().nsec() / 1000);
@@ -353,7 +352,7 @@ void GZBridge::airpressureCallback(const gz::msgs::FluidPressure &air_pressure)
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 6a8a0d435e..152d3b6b7a 100644
--- a/src/modules/simulation/gz_bridge/GZBridge.hpp
+++ b/src/modules/simulation/gz_bridge/GZBridge.hpp
@@ -59,7 +59,7 @@
#include
#include
-// #include
+#include
#include
using namespace time_literals;
@@ -94,14 +94,14 @@ private:
void clockCallback(const gz::msgs::Clock &clock);
- //void airpressureCallback(const gz::msgs::FluidPressure &air_pressure);
+ void airpressureCallback(const gz::msgs::FluidPressure &air_pressure);
void imuCallback(const gz::msgs::IMU &imu);
void poseInfoCallback(const gz::msgs::Pose_V &pose);
// Subscriptions
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
- //uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)};
+ uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::Publication _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};