SF45: Adjust _handle_missed_bins() logic

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp

@@ -92,6 +92,11 @@ int SF45LaserSerial::init()
 	param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg);
 	param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg);
 
+	// set the sensor orientation
+	const float yaw_cfg_angle = ObstacleMath::sensor_orientation_to_yaw_offset(static_cast<ObstacleMath::SensorOrientation>
+				    (_yaw_cfg));
+	_obstacle_distance.angle_offset = math::degrees(matrix::wrap_2pi(yaw_cfg_angle));
+
 	start();
 	return PX4_OK;
 }
@@ -594,7 +599,6 @@ void SF45LaserSerial::sf45_process_replies()
 	case SF_DISTANCE_DATA_CM: {
 			const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8);
 			int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8));
-			int16_t scaled_yaw = 0;
 
 			// The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float
 			if (raw_yaw > 32000) {
@@ -607,33 +611,10 @@ void SF45LaserSerial::sf45_process_replies()
 			}
 
 			// SF45/B product guide {Data output bit: 8 Description: "Yaw angle [1/100 deg] size: int16}"
-			scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
-
-			switch (_yaw_cfg) {
-			case ROTATION_FORWARD_FACING:
-				break;
-
-			case ROTATION_BACKWARD_FACING:
-				if (scaled_yaw > 180) {
-					scaled_yaw = scaled_yaw - 180;
-
-				} else {
-					scaled_yaw = scaled_yaw + 180; // rotation facing aft
-				}
-
-				break;
-
-			case ROTATION_RIGHT_FACING:
-				scaled_yaw = scaled_yaw + 90; // rotation facing right
-				break;
-
-			case ROTATION_LEFT_FACING:
-				scaled_yaw = scaled_yaw - 90; // rotation facing left
-				break;
+			float scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
 
-			default:
-				break;
-			}
+			// Adjust for sensor orientation
+			scaled_yaw = sf45_wrap_360(scaled_yaw + _obstacle_distance.angle_offset);
 
 			// Convert to meters for the debug message
 			float distance_m = raw_distance * SF45_SCALE_FACTOR;
@@ -642,7 +623,7 @@ void SF45LaserSerial::sf45_process_replies()
 			uint8_t current_bin = sf45_convert_angle(scaled_yaw);
 
 			if (current_bin != _previous_bin) {
-				PX4_DEBUG("scaled_yaw: \t %d, \t current_bin: \t %d, \t distance: \t %8.4f\n", scaled_yaw, current_bin,
+				PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw, current_bin,
 					  (double)distance_m);
 
 				if (_vehicle_attitude_sub.updated()) {
@@ -664,6 +645,7 @@ void SF45LaserSerial::sf45_process_replies()
 
 				hrt_abstime now = hrt_absolute_time();
 
+				_obstacle_distance.distances[current_bin] = _current_bin_dist;
 				_handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now);
 
 				_publish_obstacle_msg(now);
@@ -701,48 +683,31 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_
 {
 	// 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.
-	uint8_t start;
-	uint8_t end;
-
-	if (abs(current_bin - previous_bin) > BIN_COUNT / 4) {
-		// wrap-around case is assumed to have happend when the distance between the bins is larger than 1/4 of all Bins
-		// THis is simplyfied as we are not considering the scaning direction
-		start = math::max(previous_bin, current_bin);
-		end = math::min(previous_bin, current_bin);
-
-	} else if (previous_bin < current_bin) {	// Scanning clockwise
-		start = previous_bin + 1;
-		end = current_bin;
-
-	} else { 					// scanning counter-clockwise
-		start = current_bin;
-		end = previous_bin - 1;
-	}
-
-	if (start <= end) {
-		for (uint8_t i = start; i <= end; i++) {
-			_obstacle_distance.distances[i] = measurement;
-			_data_timestamps[i] = now;
-		}
 
-	} else { // wrap-around case
-		for (uint8_t i = start; i < BIN_COUNT; i++) {
-			_obstacle_distance.distances[i] = measurement;
-			_data_timestamps[i] = now;
-		}
-
-		for (uint8_t i = 0; i <= end; i++) {
-			_obstacle_distance.distances[i] = measurement;
-			_data_timestamps[i] = now;
-		}
+	// Shift bin indices such that we can never have the wrap-around case.
+	const float    fov_offset_angle    = 360.0f - SF45_FIELDOF_VIEW / 2.f;
+	const uint16_t current_bin_offset  = ObstacleMath::get_offset_bin_index(current_bin,  _obstacle_distance.increment,
+					     fov_offset_angle);
+	const uint16_t previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, _obstacle_distance.increment,
+					     fov_offset_angle);
+
+	const uint16_t start = math::min(current_bin_offset, previous_bin_offset) + 1;
+	const uint16_t end   = math::max(current_bin_offset, previous_bin_offset);
+
+	// populate the missed bins with the measurement
+	for (uint16_t i = start; i < end; i++) {
+		uint16_t bin_index = ObstacleMath::get_offset_bin_index(i, _obstacle_distance.increment, -fov_offset_angle);
+		_obstacle_distance.distances[bin_index] = measurement;
+		_data_timestamps[bin_index] = now;
 	}
 }
 
+
 uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
 {
 	uint8_t mapped_sector = 0;
 	float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_distance.angle_offset);
-	mapped_sector = round(adjusted_yaw / _obstacle_distance.increment);
+	mapped_sector = floor(adjusted_yaw / _obstacle_distance.increment);
 
 	return mapped_sector;
 }

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp

@@ -105,6 +105,7 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
 				obstacle_distance_s::distances[0]);
 	static constexpr uint64_t 	SF45_MEAS_TIMEOUT{100_ms};
 	static constexpr float 		SF45_SCALE_FACTOR = 0.01f;
+	static constexpr float		SF45_FIELDOF_VIEW = 320.f; // degrees
 
 	void				start();
 	void				stop();

src/lib/collision_prevention/ObstacleMath.cpp

@@ -51,4 +51,62 @@ void project_distance_on_horizontal_plane(float &distance, const float yaw, cons
 	distance *= horizontal_projection_scale;
 }
 
+int get_bin_at_angle(float bin_width, float angle)
+{
+	int bin_at_angle = (int)round(matrix::wrap(angle, 0.f, 360.f) / bin_width);
+	return wrap_bin(bin_at_angle, 360 / bin_width);
+}
+
+int get_offset_bin_index(int bin, float bin_width, float angle_offset)
+{
+	int offset = get_bin_at_angle(bin_width, angle_offset);
+	return wrap_bin(bin - offset, 360 / bin_width);
+}
+
+float sensor_orientation_to_yaw_offset(const SensorOrientation orientation)
+{
+	float offset = 0.0f;
+
+	switch (orientation) {
+	case SensorOrientation::ROTATION_YAW_0:
+		offset = 0.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_45:
+		offset = M_PI_F / 4.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_90:
+		offset = M_PI_F / 2.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_135:
+		offset = 3.0f * M_PI_F / 4.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_180:
+		offset = M_PI_F;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_225:
+		offset = -3.0f * M_PI_F / 4.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_270:
+		offset = -M_PI_F / 2.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_315:
+		offset = -M_PI_F / 4.0f;
+		break;
+	}
+
+	return offset;
+}
+
+int wrap_bin(int bin, int bin_count)
+{
+	return (bin + bin_count) % bin_count;
+}
+
 } // ObstacleMath

src/lib/collision_prevention/ObstacleMath.hpp

@@ -36,6 +36,28 @@
 namespace ObstacleMath
 {
 
+enum SensorOrientation {
+	ROTATION_YAW_0   = 0,     // MAV_SENSOR_ROTATION_NONE
+	ROTATION_YAW_45  = 1,	  // MAV_SENSOR_ROTATION_YAW_45
+	ROTATION_YAW_90  = 2,	  // MAV_SENSOR_ROTATION_YAW_90
+	ROTATION_YAW_135 = 3,	  // MAV_SENSOR_ROTATION_YAW_135
+	ROTATION_YAW_180 = 4,	  // MAV_SENSOR_ROTATION_YAW_180
+	ROTATION_YAW_225 = 5,	  // MAV_SENSOR_ROTATION_YAW_225
+	ROTATION_YAW_270 = 6,	  // MAV_SENSOR_ROTATION_YAW_270
+	ROTATION_YAW_315 = 7,	  // MAV_SENSOR_ROTATION_YAW_315
+
+	ROTATION_FORWARD_FACING  = 0, // MAV_SENSOR_ROTATION_NONE
+	ROTATION_RIGHT_FACING    = 2, // MAV_SENSOR_ROTATION_YAW_90
+	ROTATION_BACKWARD_FACING = 4, // MAV_SENSOR_ROTATION_YAW_180
+	ROTATION_LEFT_FACING     = 6  // MAV_SENSOR_ROTATION_YAW_270
+};
+
+/**
+ * Converts a sensor orientation to a yaw offset
+ * @param orientation sensor orientation
+ */
+float sensor_orientation_to_yaw_offset(const SensorOrientation orientation);
+
 /**
  * Scales a distance measurement taken in the vehicle body horizontal plane onto the world horizontal plane
  * @param distance measurement which is scaled down
@@ -44,4 +66,26 @@ namespace ObstacleMath
  */
 void project_distance_on_horizontal_plane(float &distance, const float yaw, const matrix::Quatf &q_world_vehicle);
 
+/**
+ * Returns bin index at a given angle from the 0th bin
+ * @param bin_width width of a bin in degrees
+ * @param angle clockwise angle from start bin in degrees
+ */
+int get_bin_at_angle(float bin_width, float angle);
+
+/**
+ * Returns bin index for the current bin after an angle offset
+ * @param bin current bin index
+ * @param bin_width width of a bin in degrees
+ * @param angle_offset clockwise angle offset in degrees
+ */
+int get_offset_bin_index(int bin, float bin_width, float angle_offset);
+
+/**
+ * Wraps a bin index to the range [0, bin_count)
+ * @param bin bin index
+ * @param bin_count number of bins
+ */
+int wrap_bin(int bin, int bin_count);
+
 } // ObstacleMath