yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
* 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
3 changed files with 24 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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;
}