From fd529b384f7c2f2346411d15ab9cd60fe8c98c17 Mon Sep 17 00:00:00 2001
From: e5l <leonid.stashevsky@gmail.com>
Date: Mon, 22 Aug 2016 16:09:58 +0300
Subject: [PATCH] sonar angle calibration

---
 car_hw/src/car_sonar.c | 16 +++++++++-------
 1 file changed, 9 insertions(+), 7 deletions(-)

diff --git a/car_hw/src/car_sonar.c b/car_hw/src/car_sonar.c
index 481bb3060a0..6e5098d94ee 100644
--- a/car_hw/src/car_sonar.c
+++ b/car_hw/src/car_sonar.c
@@ -7,15 +7,17 @@
  * http://www.dfrobot.com/wiki/index.php?title=URM37_V4.0_Ultrasonic_Sensor_(SKU:SEN0001)
  */
 
+#define FRONT_ANGLE 1
+#define BACK_ANGLE 40
+#define MAX_DEGREE 180
+
 static uint8_t sonar_degree(uint8_t degree)
 {
-    // urm37 accepts values [0;46]
-    // so we have 47 steps.
-    // But we don't use the last 46 value.
-    // It gives us angle > 180.
-    if (degree > 180)
-        degree = 180;
-    return degree / 4;
+   if (degree > MAX_DEGREE)
+        degree = MAX_DEGREE;
+
+    float factor = 1.0 * (BACK_ANGLE - FRONT_ANGLE) / MAX_DEGREE;
+    return factor * degree + FRONT_ANGLE;
 }
 
 #define SONAR_CMD_GET_DIST 0x22