Implemented Feedback

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp

@@ -67,15 +67,15 @@ SF45LaserSerial::SF45LaserSerial(const char *port) :
 	}
 
 	// populate obstacle map members
-	_obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD;
-	_obstacle_map_msg.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER;
-	_obstacle_map_msg.increment = 5;
-	_obstacle_map_msg.min_distance = 20;
-	_obstacle_map_msg.max_distance = 5000;
-	_obstacle_map_msg.angle_offset = 0;
+	_obstacle_distance.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD;
+	_obstacle_distance.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER;
+	_obstacle_distance.increment = 5;
+	_obstacle_distance.min_distance = 20;
+	_obstacle_distance.max_distance = 5000;
+	_obstacle_distance.angle_offset = 0;
 
 	for (uint32_t i = 0 ; i < BIN_COUNT; i++) {
-		_obstacle_map_msg.distances[i] = UINT16_MAX;
+		_obstacle_distance.distances[i] = UINT16_MAX;
 	}
 }
 
@@ -652,8 +652,8 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 				PX4_DEBUG("scaled_yaw: \t %d, \t current_bin: \t %d, \t distance: \t %8.4f\n", scaled_yaw, current_bin,
 					  (double)*distance_m);
 
-				if (_current_bin_dist > _obstacle_map_msg.max_distance) {
-					_current_bin_dist = _obstacle_map_msg.max_distance + 1; // As per ObstacleDistance.msg definition
+				if (_current_bin_dist > _obstacle_distance.max_distance) {
+					_current_bin_dist = _obstacle_distance.max_distance + 1; // As per ObstacleDistance.msg definition
 				}
 
 				hrt_abstime now = hrt_absolute_time();
@@ -676,23 +676,24 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 }
 void SF45LaserSerial::_publish_obstacle_msg(hrt_abstime now)
 {
-	// This whole logic is here because the sensor has a faster loop time than collision prevention, and if we dont work with a timeout on sensor data
-	// we run into the problem of obstacle_distance messages being missed in collision prevention.
+	// This whole logic is in place because CollisionPrevention, just reads in the last published message on the topic, and not every message,
+	// This can result in messages being misssed if the sensor is publishing at a faster rate than the CollisionPrevention module is reading.
 	for (int i = 0; i < BIN_COUNT; i++) {
 		if (now - _data_timestamps[i] > SF45_MEAS_TIMEOUT) {
-			_obstacle_map_msg.distances[i] = UINT16_MAX;
+			// If the data is older than SF45_MEAS_TIMEOUT, we discard the value (UINT16_MAX means no valid data)
+			_obstacle_distance.distances[i] = UINT16_MAX;
 		}
 	}
 
-	_obstacle_map_msg.timestamp = now;
-	_obstacle_distance_pub.publish(_obstacle_map_msg);
+	_obstacle_distance.timestamp = now;
+	_obstacle_distance_pub.publish(_obstacle_distance);
 }
 
 void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement,
 		hrt_abstime now)
 {
 	// if the sensor has its cycle delay configured for a low value like 5, it can happen that not every bin gets a measurement.
-	// in this case we assume the measurement to be valid for all bins between the previous and the current bin. win
+	// in this case we assume the measurement to be valid for all bins between the previous and the current bin.
 	uint8_t start;
 	uint8_t end;
 
@@ -713,27 +714,27 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_
 
 	if (start <= end) {
 		for (uint8_t i = start; i <= end; i++) {
-			_obstacle_map_msg.distances[i] = measurement;
+			_obstacle_distance.distances[i] = measurement;
 			_data_timestamps[i] = now;
 		}
 
 	} else { // wrap-around case
 		for (uint8_t i = start; i < BIN_COUNT; i++) {
-			_obstacle_map_msg.distances[i] = measurement;
+			_obstacle_distance.distances[i] = measurement;
 			_data_timestamps[i] = now;
 		}
 
 		for (uint8_t i = 0; i <= end; i++) {
-			_obstacle_map_msg.distances[i] = measurement;
+			_obstacle_distance.distances[i] = measurement;
 			_data_timestamps[i] = now;
 		}
 	}
 }
 uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
 {
 	uint8_t mapped_sector = 0;
-	float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_map_msg.angle_offset);
-	mapped_sector = round(adjusted_yaw / _obstacle_map_msg.increment);
+	float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_distance.angle_offset);
+	mapped_sector = round(adjusted_yaw / _obstacle_distance.increment);
 
 	return mapped_sector;
 }

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp

@@ -89,7 +89,7 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
 	float				sf45_wrap_360(float f);
 
 private:
-	obstacle_distance_s 			_obstacle_map_msg{};
+	obstacle_distance_s 			_obstacle_distance{};
 	uORB::Publication<obstacle_distance_s>	_obstacle_distance_pub{ORB_ID(obstacle_distance)};	/**< obstacle_distance publication */
 	static constexpr int BIN_COUNT = sizeof(obstacle_distance_s::distances) / sizeof(obstacle_distance_s::distances[0]);
 	static constexpr uint64_t SF45_MEAS_TIMEOUT{100_ms};

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp

@@ -102,7 +102,6 @@ static int usage()
 
 Serial bus driver for the Lightware SF45/b Laser rangefinder.
 
-Setup/usage information: https://docs.px4.io/main/en/sensor/rangefinders.html
 
 ### Examples