forked from Archive/PX4-Autopilot
Simulation Gazebo: Use Gazebo Airpressure sensor (#21176)
* Simulation Gazebo: Use Gazebo Airpressure sensor Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com> * Fixed build Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com> * Added feedback Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com> * make linters happy Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com> --------- Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
This commit is contained in:
Alejandro Hernández Cordero
GitHub
parent
06dfd1726f
commit
ea6814d258
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -84,6 +84,18 @@
|
|||
<always_on>1</always_on>
|
||||
<update_rate>250</update_rate>
|
||||
</sensor>
|
||||
<sensor name="air_pressure_sensor" type="air_pressure">
|
||||
<always_on>1</always_on>
|
||||
<update_rate>50</update_rate>
|
||||
<air_pressure>
|
||||
<pressure>
|
||||
<noise type="gaussian">
|
||||
<mean>0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</pressure>
|
||||
</air_pressure>
|
||||
</sensor>
|
||||
</link>
|
||||
<link name="airspeed">
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
|
|
|
|||
|
|
@ -134,6 +134,18 @@
|
|||
<always_on>1</always_on>
|
||||
<update_rate>250</update_rate>
|
||||
</sensor>
|
||||
<sensor name="air_pressure_sensor" type="air_pressure">
|
||||
<always_on>1</always_on>
|
||||
<update_rate>50</update_rate>
|
||||
<air_pressure>
|
||||
<pressure>
|
||||
<noise type="gaussian">
|
||||
<mean>0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</pressure>
|
||||
</air_pressure>
|
||||
</sensor>
|
||||
</link>
|
||||
<link name='rotor_0'>
|
||||
<pose>0.35 -0.35 0.07 0 0 0</pose>
|
||||
|
|
|
|||
|
|
@ -215,6 +215,18 @@
|
|||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<sensor name="air_pressure_sensor" type="air_pressure">
|
||||
<always_on>1</always_on>
|
||||
<update_rate>50</update_rate>
|
||||
<air_pressure>
|
||||
<pressure>
|
||||
<noise type="gaussian">
|
||||
<mean>0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</pressure>
|
||||
</air_pressure>
|
||||
</sensor>
|
||||
<sensor name="imu_sensor" type="imu">
|
||||
<always_on>1</always_on>
|
||||
<update_rate>250</update_rate>
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@
|
|||
<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::AirPressure' filename='ignition-gazebo-air-pressure-system'/>
|
||||
<plugin name='ignition::gazebo::systems::Sensors' filename='ignition-gazebo-sensors-system'>
|
||||
<render_engine>ogre2</render_engine>
|
||||
</plugin>
|
||||
|
|
|
|||
|
|
@ -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<float>(air_pressure_value); // hPa to Pa;
|
||||
report.temperature = static_cast<float>(air_pressure.variance()) + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // K to C
|
||||
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(&_node_mutex);
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -54,13 +54,14 @@
|
|||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/sensor_baro.h>
|
||||
|
||||
#include <gz/math.hh>
|
||||
#include <gz/msgs.hh>
|
||||
#include <gz/transport.hh>
|
||||
|
||||
// #include <gz/msgs/fluid_pressure.pb.h>
|
||||
#include <gz/msgs/imu.pb.h>
|
||||
#include <gz/msgs/fluid_pressure.pb.h>
|
||||
|
||||
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<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::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
|
||||
|
||||
uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
|
||||
uORB::PublicationMulti<sensor_gyro_s> _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(
|
||||
|
|
|
|||
Loading…
Reference in New Issue