hokuyo: Fix bug in hokuyo transformation

[eurobot/public.git] / src / hokuyo / hokuyo.h

diff --git a/src/hokuyo/hokuyo.h b/src/hokuyo/hokuyo.h
index 5525d9a8478f2cfc09b10a5616fe8da4a408e47b..962d63766eba083f7aff4b78e7af2c198c74065a 100644 (file)
--- a/src/hokuyo/hokuyo.h
+++ b/src/hokuyo/hokuyo.h
@@ -9,8 +9,11 @@
 #define HOKUYO_SPLIT_DIVISION 1024
 #define HOKUYO_INITIAL_MEASUREMENT 44
 #define HOKUYO_FINAL_MEASUREMENT 725
+#define HOKUYO_START_ANGLE (239.77/2)
 
-#define HOKUYO_INDEX_TO_RAD(x) (-113.0+(x)*360.0/HOKUYO_SPLIT_DIVISION)
-#define HOKUYO_CLUSTER_TO_RAD(x) (HOKUYO_CLUSTER_TO_DEG(x)/180.0*M_PI)
+#define HOKUYO_INDEX_TO_DEG(x) ((HOKUYO_START_ANGLE-(x)*360.0/HOKUYO_SPLIT_DIVISION) * HOKUYO_ORIENTATION)
+#define HOKUYO_INDEX_TO_RAD(x) (HOKUYO_INDEX_TO_DEG(x)/180.0*M_PI)
+
+#define HOKUYO_DEG_TO_INDEX(d) ((HOKUYO_START_ANGLE-(d)/HOKUYO_ORIENTATION)/(360.0/HOKUYO_SPLIT_DIVISION))
 
 #endif //HOKUYO_H