Skip to content

Commit

Permalink
gz bridge: add baro and airspeed sensor
Browse files Browse the repository at this point in the history
  • Loading branch information
haitomatic committed Sep 27, 2023
1 parent cebc591 commit 7f8e72a
Show file tree
Hide file tree
Showing 2 changed files with 90 additions and 6 deletions.
86 changes: 80 additions & 6 deletions src/modules/simulation/gz_bridge/GZBridge.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -154,8 +154,9 @@ 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";
// 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());
Expand All @@ -170,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";
Expand Down Expand Up @@ -445,7 +467,8 @@ void GZBridge::magnetometerCallback(const gz::msgs::Magnetometer &magnetometer)

pthread_mutex_lock(&_mutex);

const uint64_t time_us = (magnetometer.header().stamp().sec() * 1000000) + (magnetometer.header().stamp().nsec() / 1000);
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());
Expand All @@ -455,9 +478,9 @@ void GZBridge::magnetometerCallback(const gz::msgs::Magnetometer &magnetometer)
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()));
magnetometer.field_tesla().x(),
magnetometer.field_tesla().y(),
magnetometer.field_tesla().z()));

// publish mag
sensor_mag_s sensor_mag{};
Expand Down Expand Up @@ -555,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) {
Expand Down
10 changes: 10 additions & 0 deletions src/modules/simulation/gz_bridge/GZBridge.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -46,10 +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>
Expand All @@ -60,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;

Expand Down Expand Up @@ -98,6 +101,8 @@ class GZBridge : public ModuleBase<GZBridge>, public OutputModuleInterface
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();
Expand All @@ -111,11 +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};

Expand All @@ -136,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;
Expand Down

0 comments on commit 7f8e72a

Please sign in to comment.