sf45: refactor how sensor orientation (yaw_cfg) correction is applied…

… to incoming sensor data.

yaw_cfg is now read into the obstacle_distance message as the angle_offset. The offset is computed once at init and applied to each measurement.

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
-				}
+			float scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
 
-				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;
-
-			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()) {
@@ -726,7 +707,7 @@ 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;
 }