sitl_gazebo: add updated iris_obs_avoid; simulator: cleanup distance_…

…sensor topic init
PX4 · Feb 13, 2020 · 7ed4afe · 7ed4afe
1 parent 632d8af
commit 7ed4afe
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 16 deletions.
diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
@@ -139,8 +139,8 @@ class Simulator : public ModuleParams
 	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::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)};
 

diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
@@ -1131,24 +1131,19 @@ 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];
 
-	// Create 1st publisher instance if it doesn't exist
-	if (_dist_pubs[0] == nullptr) {
-		_dist_pubs[0] = new uORB::PublicationMulti<distance_sensor_s> {ORB_ID(distance_sensor)};
-		_dist_sensor_ids[0] = dist.id;
-	}
-
 	// 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_sensor_ids[i] == dist.id) {
+		if (_dist_pubs[i] && _dist_sensor_ids[i] == dist.id) {
 			_dist_pubs[i]->publish(dist);
 			break;
 
-		} else {
-			if (_dist_pubs[i] == nullptr) {
-				_dist_pubs[i] = new uORB::PublicationMulti<distance_sensor_s> {ORB_ID(distance_sensor)};
-				_dist_sensor_ids[i] = dist.id;
-				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;
 		}
 	}