From 29e1e0df910003cd76d0ce8ff1009bdbaecd1b04 Mon Sep 17 00:00:00 2001
From: Benjamin Perseghetti <bperseghetti@rudislabs.com>
Date: Wed, 25 Jan 2023 12:20:32 -0500
Subject: [PATCH] Fix for sensor airspeed enable.

Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>
---
 .../init.d-posix/px4-rc.simulator              |  2 +-
 .../gz/models/standard_vtol/model.sdf          | 18 ++----------------
 Tools/simulation/gz/worlds/default.sdf         | 12 ++++++------
 .../simulation/gz_bridge/CMakeLists.txt        |  1 -
 src/modules/simulation/gz_bridge/GZBridge.cpp  |  9 ++++-----
 src/modules/simulation/gz_bridge/GZBridge.hpp  |  6 +++---
 6 files changed, 16 insertions(+), 32 deletions(-)

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 @@
         <update_rate>250</update_rate>
       </sensor>
     </link>
-    <link name="airspeed">
+    <link name="airspeed_link">
       <pose>0 0 0 0 0 0</pose>
       <inertial>
         <pose>0 0 0 0 0 0</pose>
@@ -177,23 +177,9 @@
       </sensor>
     </link>
     <joint name='airspeed_joint' type='fixed'>
-      <child>airspeed</child>
+      <child>airspeed_link</child>
       <parent>base_link</parent>
     </joint>
-    <!-- <include>
-      <uri>model://airspeed</uri>
-      <pose>0 0 0 0 0 0</pose>
-      <name>airspeed</name>
-    </include>
-    <include>
-      <uri>model://magnetometer</uri>
-      <pose>0 0 0 0 0 0</pose>
-      <name>mag</name>
-    </include>
-    <joint name='mag_joint' type='fixed'>
-      <child>mag::link</child>
-      <parent>base_link</parent>
-    </joint> -->
     <link name='rotor_0'>
       <pose>0.35 -0.35 0.07 0 0 0</pose>
       <inertial>
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 @@
       <real_time_factor>1.0</real_time_factor>
       <real_time_update_rate>250</real_time_update_rate>
     </physics>
-    <plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
-    <plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
-    <plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
-    <plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
-    <plugin name='ignition::gazebo::systems::Imu' filename='ignition-gazebo-imu-system'/>
-    <plugin name='ignition::gazebo::systems::Sensors' filename='ignition-gazebo-sensors-system'>
+    <plugin name='gz::sim::systems::Physics' filename='gz-sim-physics-system'/>
+    <plugin name='gz::sim::systems::UserCommands' filename='gz-sim-user-commands-system'/>
+    <plugin name='gz::sim::systems::SceneBroadcaster' filename='gz-sim-scene-broadcaster-system'/>
+    <plugin name='gz::sim::systems::Contact' filename='gz-sim-contact-system'/>
+    <plugin name='gz::sim::systems::Imu' filename='gz-sim-imu-system'/>
+    <plugin name='gz::sim::systems::Sensors' filename='gz-sim-sensors-system'>
       <render_engine>ogre2</render_engine>
     </plugin>
     <gui fullscreen='false'>
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 <gz/msgs.hh>
 #include <gz/transport.hh>
 
-// #include <gz/msgs/fluid_pressure.pb.h>
+#include <gz/msgs/fluid_pressure.pb.h>
 #include <gz/msgs/imu.pb.h>
 
 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_s>    _differential_pressure_pub{ORB_ID(differential_pressure)};
+	uORB::Publication<differential_pressure_s>    _differential_pressure_pub{ORB_ID(differential_pressure)};
 	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)};
 	uORB::Publication<vehicle_global_position_s>  _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};