Skip to content

Commit

Permalink
update to gazebo harmonic message
Browse files Browse the repository at this point in the history
  • Loading branch information
Perrrewi committed Nov 22, 2024
1 parent 73d5b13 commit 29f1151
Show file tree
Hide file tree
Showing 2 changed files with 7 additions and 7 deletions.
10 changes: 5 additions & 5 deletions src/modules/simulation/gz_bridge/GZBridge.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -448,24 +448,24 @@ void GZBridge::barometerCallback(const gz::msgs::FluidPressure &air_pressure)
}


void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed_sensor)
void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &air_speed)
{
if (hrt_absolute_time() == 0) {
return;
}

pthread_mutex_lock(&_node_mutex);

const uint64_t time_us = (air_speed_sensor.header().stamp().sec() * 1000000)
+ (air_speed_sensor.header().stamp().nsec() / 1000);
const uint64_t time_us = (air_speed.header().stamp().sec() * 1000000)
+ (air_speed.header().stamp().nsec() / 1000);

double air_speed_value = air_speed_sensor.diff_pressure();
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_sensor.temperature()) + atmosphere::kAbsoluteNullCelsius; // K to C
report.temperature = static_cast<float>(air_speed.temperature()) + atmosphere::kAbsoluteNullCelsius; // K to C
report.timestamp = hrt_absolute_time();;
_differential_pressure_pub.publish(report);

Expand Down
4 changes: 2 additions & 2 deletions src/modules/simulation/gz_bridge/GZBridge.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@

#include <gz/msgs/imu.pb.h>
#include <gz/msgs/fluid_pressure.pb.h>
#include <gz/msgs/air_speed_sensor.pb.h>
#include <gz/msgs/air_speed.pb.h>
#include <gz/msgs/model.pb.h>
#include <gz/msgs/odometry_with_covariance.pb.h>
#include <gz/msgs/laserscan.pb.h>
Expand Down Expand Up @@ -107,7 +107,7 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S

void clockCallback(const gz::msgs::Clock &clock);

void airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed_sensor);
void airspeedCallback(const gz::msgs::AirSpeed &air_speed);
void barometerCallback(const gz::msgs::FluidPressure &air_pressure);
void imuCallback(const gz::msgs::IMU &imu);
void poseInfoCallback(const gz::msgs::Pose_V &pose);
Expand Down

0 comments on commit 29f1151

Please sign in to comment.