From 0fe271b7f5350aaa613d639a309451b094d968b9 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 10 Sep 2019 17:38:17 -0400
Subject: [PATCH] CollisionPrevention add more distance_sensor orientations

---
 msg/distance_sensor.msg                       | 30 ++++++++----
 .../CollisionPrevention.cpp                   | 46 +++++++++++++++++++
 .../CollisionPrevention.hpp                   | 26 +----------
 3 files changed, 68 insertions(+), 34 deletions(-)

diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg
index 460a47e839..9cadd5ec7b 100644
--- a/msg/distance_sensor.msg
+++ b/msg/distance_sensor.msg
@@ -16,15 +16,27 @@ uint8 MAV_DISTANCE_SENSOR_RADAR = 3
 
 uint8 id			# Onboard ID of the sensor
 
-uint8 orientation		# Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
-uint8 ROTATION_DOWNWARD_FACING = 25 # MAV_SENSOR_ROTATION_PITCH_270
-uint8 ROTATION_UPWARD_FACING   = 24 # MAV_SENSOR_ROTATION_PITCH_90
-uint8 ROTATION_BACKWARD_FACING = 12 # MAV_SENSOR_ROTATION_PITCH_180
-uint8 ROTATION_FORWARD_FACING  = 0  # MAV_SENSOR_ROTATION_NONE
-uint8 ROTATION_LEFT_FACING     = 6  # MAV_SENSOR_ROTATION_YAW_270
-uint8 ROTATION_RIGHT_FACING    = 2  # MAV_SENSOR_ROTATION_YAW_90
-uint8 ROTATION_CUSTOM          =100 # MAV_SENSOR_ROTATION_CUSTOM
-
 float32 h_fov # Sensor horizontal field of view (rad)
 float32 v_fov # Sensor vertical field of view (rad)
 float32[4] q # Quaterion sensor orientation with respect to the vehicle body frame to specify the orientation ROTATION_CUSTOM
+
+uint8 orientation		# Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
+
+uint8 ROTATION_YAW_0		= 0 # MAV_SENSOR_ROTATION_NONE
+uint8 ROTATION_YAW_45		= 1 # MAV_SENSOR_ROTATION_YAW_45
+uint8 ROTATION_YAW_90		= 2 # MAV_SENSOR_ROTATION_YAW_90
+uint8 ROTATION_YAW_135		= 3 # MAV_SENSOR_ROTATION_YAW_135
+uint8 ROTATION_YAW_180		= 4 # MAV_SENSOR_ROTATION_YAW_180
+uint8 ROTATION_YAW_225		= 5 # MAV_SENSOR_ROTATION_YAW_225
+uint8 ROTATION_YAW_270		= 6 # MAV_SENSOR_ROTATION_YAW_270
+uint8 ROTATION_YAW_315		= 7 # MAV_SENSOR_ROTATION_YAW_315
+
+uint8 ROTATION_FORWARD_FACING	= 0 # MAV_SENSOR_ROTATION_NONE
+uint8 ROTATION_RIGHT_FACING	= 2 # MAV_SENSOR_ROTATION_YAW_90
+uint8 ROTATION_BACKWARD_FACING	= 4 # MAV_SENSOR_ROTATION_YAW_180
+uint8 ROTATION_LEFT_FACING	= 6 # MAV_SENSOR_ROTATION_YAW_270
+
+uint8 ROTATION_UPWARD_FACING   = 24 # MAV_SENSOR_ROTATION_PITCH_90
+uint8 ROTATION_DOWNWARD_FACING = 25 # MAV_SENSOR_ROTATION_PITCH_270
+
+uint8 ROTATION_CUSTOM          = 100 # MAV_SENSOR_ROTATION_CUSTOM
diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp
index 4e3af1ec12..a5028d9e27 100644
--- a/src/lib/CollisionPrevention/CollisionPrevention.cpp
+++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp
@@ -323,6 +323,52 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
 	}
 }
 
+float
+CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const
+{
+	float offset = angle_offset > 0.0f ? math::radians(angle_offset) : 0.0f;
+
+	switch (distance_sensor.orientation) {
+	case distance_sensor_s::ROTATION_YAW_0:
+		offset = 0.0f;
+		break;
+
+	case distance_sensor_s::ROTATION_YAW_45:
+		offset = M_PI_F / 4.0f;
+		break;
+
+	case distance_sensor_s::ROTATION_YAW_90:
+		offset = M_PI_F / 2.0f;
+		break;
+
+	case distance_sensor_s::ROTATION_YAW_135:
+		offset = 3.0f * M_PI_F / 4.0f;
+		break;
+
+	case distance_sensor_s::ROTATION_YAW_180:
+		offset = M_PI_F;
+		break;
+
+	case distance_sensor_s::ROTATION_YAW_225:
+		offset = -3.0f * M_PI_F / 4.0f;
+		break;
+
+	case distance_sensor_s::ROTATION_YAW_270:
+		offset = -M_PI_F / 2.0f;
+		break;
+
+	case distance_sensor_s::ROTATION_YAW_315:
+		offset = -M_PI_F / 4.0f;
+		break;
+
+	case distance_sensor_s::ROTATION_CUSTOM:
+		offset = matrix::Eulerf(matrix::Quatf(distance_sensor.q)).psi();
+		break;
+	}
+
+	return offset;
+}
+
 void
 CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos,
 		const Vector2f &curr_vel)
diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp
index 7e8168cb39..af4afe2555 100644
--- a/src/lib/CollisionPrevention/CollisionPrevention.hpp
+++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp
@@ -151,31 +151,7 @@ private:
 	 * @param distance_sensor, distance sensor message
 	 * @param angle_offset, sensor body frame offset
 	 */
-	inline float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset)
-	{
-
-		float offset = angle_offset > 0.f ? math::radians(angle_offset) : 0.0f;
-
-		switch (distance_sensor.orientation) {
-		case distance_sensor_s::ROTATION_RIGHT_FACING:
-			offset = M_PI_F / 2.0f;
-			break;
-
-		case distance_sensor_s::ROTATION_LEFT_FACING:
-			offset = -M_PI_F / 2.0f;
-			break;
-
-		case distance_sensor_s::ROTATION_BACKWARD_FACING:
-			offset = M_PI_F;
-			break;
-
-		case distance_sensor_s::ROTATION_CUSTOM:
-			offset = matrix::Eulerf(matrix::Quatf(distance_sensor.q)).psi();
-			break;
-		}
-
-		return offset;
-	}
+	float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const;
 
 	/**
 	 * Computes collision free setpoints