Gz bridge add mag air pressure

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -154,6 +154,15 @@ int GZBridge::init()
 		return PX4_ERROR;
 	}
 
+	// MAG: /world/$WORLD/model/$MODEL/link/base_link/sensor/magnetometer_sensor/magnetometer
+	std::string mag_topic = "/world/" + _world_name + "/model/" + _model_name +
+				"/link/base_link/sensor/magnetometer_sensor/magnetometer";
+
+	if (!_node.Subscribe(mag_topic, &GZBridge::magnetometerCallback, this)) {
+		PX4_ERR("failed to subscribe to %s", mag_topic.c_str());
+		return PX4_ERROR;
+	}
+
 	// GPS: /world/$WORLD/model/$MODEL/link/gps1_link/sensor/gps/navsat
 	std::string gps_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/gps1_link/sensor/gps/navsat";
 
@@ -162,6 +171,27 @@ int GZBridge::init()
 		return PX4_ERROR;
 	}
 
+	// 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";
+
+	if (!_node.Subscribe(air_pressure_topic, &GZBridge::barometerCallback, this)) {
+		PX4_ERR("failed to subscribe to %s", air_pressure_topic.c_str());
+		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";
+
+	if (!_node.Subscribe(airpressure_topic, &GZBridge::airspeedCallback, this)) {
+		PX4_ERR("failed to subscribe to %s", airpressure_topic.c_str());
+		return PX4_ERROR;
+	}
+
+#endif
+
 	// ESC feedback: /x500/command/motor_speed
 	if (_vehicle_type != "rover") {
 		std::string motor_speed_topic = "/" + _model_name + "/command/motor_speed";
@@ -429,6 +459,50 @@ void GZBridge::imuCallback(const gz::msgs::IMU &imu)
 	pthread_mutex_unlock(&_mutex);
 }
 
+void GZBridge::magnetometerCallback(const gz::msgs::Magnetometer &magnetometer)
+{
+	if (hrt_absolute_time() == 0) {
+		return;
+	}
+
+	pthread_mutex_lock(&_mutex);
+
+	const uint64_t time_us = (magnetometer.header().stamp().sec() * 1000000) + (magnetometer.header().stamp().nsec() /
+				 1000);
+
+	if (time_us > _world_time_us.load()) {
+		updateClock(magnetometer.header().stamp().sec(), magnetometer.header().stamp().nsec());
+	}
+
+	// FLU -> FRD
+	static const auto q_FLU_to_FRD = gz::math::Quaterniond(0, 1, 0, 0);
+
+	gz::math::Vector3d mag_b = q_FLU_to_FRD.RotateVector(gz::math::Vector3d(
+					   magnetometer.field_tesla().x(),
+					   magnetometer.field_tesla().y(),
+					   magnetometer.field_tesla().z()));
+
+	// publish mag
+	sensor_mag_s sensor_mag{};
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	sensor_mag.timestamp_sample = time_us;
+	sensor_mag.timestamp = time_us;
+#else
+	sensor_mag.timestamp_sample = hrt_absolute_time();
+	sensor_mag.timestamp = hrt_absolute_time();
+#endif
+	sensor_mag.device_id = 197388; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+	sensor_mag.x = mag_b.X();
+	sensor_mag.y = mag_b.Y();
+	sensor_mag.z = mag_b.Z();
+	sensor_mag.temperature = NAN;
+	sensor_mag.error_count = 0;
+
+	_sensor_mag_pub.publish(sensor_mag);
+
+	pthread_mutex_unlock(&_mutex);
+}
+
 void GZBridge::gpsCallback(const gz::msgs::NavSat &gps)
 {
 	if (hrt_absolute_time() == 0) {
@@ -504,6 +578,57 @@ void GZBridge::gpsCallback(const gz::msgs::NavSat &gps)
 	pthread_mutex_unlock(&_mutex);
 }
 
+void GZBridge::barometerCallback(const gz::msgs::FluidPressure &air_pressure)
+{
+	if (hrt_absolute_time() == 0) {
+		return;
+	}
+
+	pthread_mutex_lock(&_mutex);
+
+	const uint64_t time_us = (air_pressure.header().stamp().sec() * 1000000)
+				 + (air_pressure.header().stamp().nsec() / 1000);
+
+	// publish
+	sensor_baro_s sensor_baro{};
+	sensor_baro.timestamp_sample = time_us;
+	sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
+	sensor_baro.pressure = air_pressure.pressure();
+	sensor_baro.temperature = this->_temperature;
+	sensor_baro.timestamp = hrt_absolute_time();
+	_sensor_baro_pub.publish(sensor_baro);
+
+	pthread_mutex_unlock(&_mutex);
+}
+
+#if 0
+void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed)
+{
+	if (hrt_absolute_time() == 0) {
+		return;
+	}
+
+	pthread_mutex_lock(&_mutex);
+
+	const uint64_t time_us = (air_speed.header().stamp().sec() * 1000000)
+				 + (air_speed.header().stamp().nsec() / 1000);
+
+	double air_speed_value = air_speed.diff_pressure();
+
+	differential_pressure_s report{};
+	report.timestamp_sample = time_us;
+	report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
+	report.differential_pressure_pa = static_cast<float>(air_speed_value); // hPa to Pa;
+	report.temperature = static_cast<float>(air_speed.temperature()) + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // K to C
+	report.timestamp = hrt_absolute_time();;
+	_differential_pressure_pub.publish(report);
+
+	this->_temperature = report.temperature;
+
+	pthread_mutex_unlock(&_mutex);
+}
+#endif
+
 void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)
 {
 	if (hrt_absolute_time() == 0) {

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -46,9 +46,12 @@
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -59,6 +62,7 @@
 #include <gz/transport.hh>
 #include <gz/math.hh>
 #include <gz/msgs/imu.pb.h>
+#include <gz/msgs/fluid_pressure.pb.h>
 
 using namespace time_literals;
 
@@ -95,7 +99,10 @@ class GZBridge : public ModuleBase<GZBridge>, public OutputModuleInterface
 
 	void clockCallback(const gz::msgs::Clock &clock);
 	void imuCallback(const gz::msgs::IMU &imu);
+	void magnetometerCallback(const gz::msgs::Magnetometer &mag);
 	void gpsCallback(const gz::msgs::NavSat &gps);
+	void barometerCallback(const gz::msgs::FluidPressure &air_pressure);
+
 	void poseInfoCallback(const gz::msgs::Pose_V &pose);
 	void motorSpeedCallback(const gz::msgs::Actuators &actuators);
 	void updateCmdVel();
@@ -109,10 +116,14 @@ class GZBridge : public ModuleBase<GZBridge>, public OutputModuleInterface
 	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)};
 	uORB::Publication<vehicle_local_position_s>   _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)};
+	//uORB::Publication<differential_pressure_s>    _differential_pressure_pub{ORB_ID(differential_pressure)};
 
 	uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
 	uORB::PublicationMulti<sensor_gyro_s>  _sensor_gyro_pub{ORB_ID(sensor_gyro)};
+	uORB::PublicationMulti<sensor_mag_s>  _sensor_mag_pub{ORB_ID(sensor_mag)};
 	uORB::PublicationMulti<sensor_gps_s>  _sensor_gps_pub{ORB_ID(sensor_gps)};
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
+
 
 	px4::atomic<uint64_t> _world_time_us{0};
 
@@ -133,6 +144,8 @@ class GZBridge : public ModuleBase<GZBridge>, public OutputModuleInterface
 
 	MixingOutput _mixing_output{"SIM_GZ", 8, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 
+	float _temperature{288.15};  // 15 degrees
+
 	gz::transport::Node _node;
 	gz::transport::Node::Publisher _actuators_pub;
 	gz::transport::Node::Publisher _cmd_vel_pub;