dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

simulator: add support for multi-distance_sensor instances (#14143) 

* simulator: add support for multi-distance_sensor instances

* update submodule Tools/sitl_gazebo

* sitl_gazebo: add updated iris_obs_avoid; simulator: cleanup distance_sensor topic init

* update submodule Tools/sitl_gazebo

* simulator: delete _dist_pubs uORB::PublicationMulti<distance_sensor_s> in simulator destructor
This commit is contained in:
Nuno Marques 2020-02-14 10:32:28 +00:00 committed by GitHub
parent abd2bb4eb7
commit 7cafbc824e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 24 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/sitl_gazebo

@ -1 +1 @@
Subproject commit 045156da19ac6518455c9b0e6a5a63b10388d4d7
Subproject commit 44181fd63ebdcb9eaeb56557a3f807d501f58012

9
src/modules/simulator/simulator.h
View File

@ -45,6 +45,7 @@
#include <battery/battery.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_range_finder.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
@ -111,6 +112,10 @@ private:
perf_free(_perf_sim_delay);
perf_free(_perf_sim_interval);
for (size_t i = 0; i < sizeof(_dist_pubs) / sizeof(_dist_pubs[0]); i++) {
delete _dist_pubs[i];
}
_instance = nullptr;
}
@ -134,11 +139,13 @@ private:
// uORB publisher handlers
uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::PublicationMulti<distance_sensor_s> _dist_pub{ORB_ID(distance_sensor)};
uORB::PublicationMulti<optical_flow_s> _flow_pub{ORB_ID(optical_flow)};
uORB::Publication<irlock_report_s> _irlock_report_pub{ORB_ID(irlock_report)};
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
uORB::PublicationMulti<distance_sensor_s> *_dist_pubs[RANGE_FINDER_MAX_SENSORS] {};
uint8_t _dist_sensor_ids[RANGE_FINDER_MAX_SENSORS] {};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
unsigned int _port{14560};

17
src/modules/simulator/simulator_mavlink.cpp
View File

@ -1124,7 +1124,6 @@ int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavl
dist.orientation = dist_mavlink->orientation;
dist.variance = dist_mavlink->covariance * 1e-4f; // cm^2 to m^2
dist.signal_quality = -1;
dist.h_fov = dist_mavlink->horizontal_fov;
dist.v_fov = dist_mavlink->vertical_fov;
dist.q[0] = dist_mavlink->quaternion[0];
@ -1132,7 +1131,21 @@ int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavl
dist.q[2] = dist_mavlink->quaternion[2];
dist.q[3] = dist_mavlink->quaternion[3];
_dist_pub.publish(dist);
// New publishers will be created based on the sensor ID's being different or not
for (size_t i = 0; i < sizeof(_dist_sensor_ids) / sizeof(_dist_sensor_ids[0]); i++) {
if (_dist_pubs[i] && _dist_sensor_ids[i] == dist.id) {
_dist_pubs[i]->publish(dist);
break;
}
if (_dist_pubs[i] == nullptr) {
_dist_pubs[i] = new uORB::PublicationMulti<distance_sensor_s> {ORB_ID(distance_sensor)};
_dist_sensor_ids[i] = dist.id;
_dist_pubs[i]->publish(dist);
break;
}
}
return PX4_OK;
}