diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 index a8c34815a7..bb6bedd386 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 @@ -14,7 +14,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500} param set-default SIM_GZ_EN 1 param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 1 +param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default CA_AIRFRAME 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4002_gz_x500_depth b/ROMFS/px4fmu_common/init.d-posix/airframes/4002_gz_x500_depth index 6600c451d4..f3cce38f39 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4002_gz_x500_depth +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4002_gz_x500_depth @@ -14,7 +14,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_depth} param set-default SIM_GZ_EN 1 param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 1 +param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default CA_AIRFRAME 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna index 9a266bdc2a..bac1f86032 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna @@ -11,7 +11,7 @@ PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} PX4_SIM_MODEL=${PX4_SIM_MODEL:=rc_cessna} param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 1 +param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 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 index 0cbd8cc1bd..9061f39a5d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol @@ -12,7 +12,7 @@ PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol} param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 1 +param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 diff --git a/Tools/simulation/gz/models/rc_cessna/model.sdf b/Tools/simulation/gz/models/rc_cessna/model.sdf index 3f302399cb..a9a0e2f1d4 100644 --- a/Tools/simulation/gz/models/rc_cessna/model.sdf +++ b/Tools/simulation/gz/models/rc_cessna/model.sdf @@ -84,6 +84,18 @@ 1 250 + + 1 + 50 + + + + 0 + 0.01 + + + + 0 0 0 0 0 0 diff --git a/Tools/simulation/gz/models/standard_vtol/model.sdf b/Tools/simulation/gz/models/standard_vtol/model.sdf index 769fb314c3..1c8da54308 100644 --- a/Tools/simulation/gz/models/standard_vtol/model.sdf +++ b/Tools/simulation/gz/models/standard_vtol/model.sdf @@ -134,6 +134,18 @@ 1 250 + + 1 + 50 + + + + 0 + 0.01 + + + + 0.35 -0.35 0.07 0 0 0 diff --git a/Tools/simulation/gz/models/x500/model.sdf b/Tools/simulation/gz/models/x500/model.sdf index 719352b7a4..39269e00c9 100644 --- a/Tools/simulation/gz/models/x500/model.sdf +++ b/Tools/simulation/gz/models/x500/model.sdf @@ -215,6 +215,18 @@ + + 1 + 50 + + + + 0 + 0.01 + + + + 1 250 diff --git a/Tools/simulation/gz/worlds/default.sdf b/Tools/simulation/gz/worlds/default.sdf index ca2ef31634..31cb3b63ed 100644 --- a/Tools/simulation/gz/worlds/default.sdf +++ b/Tools/simulation/gz/worlds/default.sdf @@ -10,6 +10,7 @@ + ogre2 diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index db8bc6c69b..885ae5f273 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -157,12 +157,20 @@ int GZBridge::init() std::string airpressure_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/airspeed_link/sensor/air_speed/air_speed"; - if (!_node.Subscribe(airpressure_topic, &GZBridge::airpressureCallback, this)) { + if (!_node.Subscribe(airpressure_topic, &GZBridge::airspeedCallback, this)) { PX4_ERR("failed to subscribe to %s", airpressure_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"; + + 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 (!_mixing_interface_esc.init(_model_name)) { PX4_ERR("failed to init ESC output"); @@ -185,6 +193,7 @@ int GZBridge::task_spawn(int argc, char *argv[]) const char *model_pose = nullptr; const char *model_sim = nullptr; const char *px4_instance = nullptr; + std::string model_name_std; bool error_flag = false; @@ -248,7 +257,7 @@ int GZBridge::task_spawn(int argc, char *argv[]) } } else if (!model_name) { - std::string model_name_std = std::string(model_sim) + "_" + std::string(px4_instance); + model_name_std = std::string(model_sim) + "_" + std::string(px4_instance); model_name = model_name_std.c_str(); } @@ -329,28 +338,53 @@ void GZBridge::clockCallback(const gz::msgs::Clock &clock) pthread_mutex_unlock(&_node_mutex); } -#if 0 -void GZBridge::airpressureCallback(const gz::msgs::FluidPressure &air_pressure) +void GZBridge::barometerCallback(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); - double air_pressure_value = air_pressure.pressure(); + // 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(&_node_mutex); +} + +#if 0 +void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed) +{ + if (hrt_absolute_time() == 0) { + return; + } + + pthread_mutex_lock(&_node_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(air_pressure_value); // hPa to Pa; - report.temperature = static_cast(air_pressure.variance()) + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // K to C + report.differential_pressure_pa = static_cast(air_speed_value); // hPa to Pa; + report.temperature = static_cast(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(&_node_mutex); } #endif diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 6a8a0d435e..42ac2ae828 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -54,13 +54,14 @@ #include #include #include +#include #include #include #include -// #include #include +#include using namespace time_literals; @@ -94,7 +95,8 @@ private: void clockCallback(const gz::msgs::Clock &clock); - //void airpressureCallback(const gz::msgs::FluidPressure &air_pressure); + // void airspeedCallback(const gz::msgs::AirSpeedSensor &air_pressure); + void barometerCallback(const gz::msgs::FluidPressure &air_pressure); void imuCallback(const gz::msgs::IMU &imu); void poseInfoCallback(const gz::msgs::Pose_V &pose); @@ -106,6 +108,7 @@ private: uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; @@ -129,6 +132,8 @@ private: const std::string _model_sim; const std::string _model_pose; + float _temperature{288.15}; // 15 degrees + gz::transport::Node _node; DEFINE_PARAMETERS(