Create LIDAR lib for hadling both rangefinders - SICK and Hokuyo

[eurobot/public.git] / src / lidars / lidar / lidar.c

diff --git a/src/lidars/lidar/lidar.c b/src/lidars/lidar/lidar.c
new file mode 100644 (file)
index 0000000..7713b5b
--- /dev/null
+++ b/src/lidars/lidar/lidar.c
@@ -0,0 +1,22 @@
+#include "lidar.h"
+#include <math.h>
+
+double index2deg(const struct lidar_params lidar, int index)
+{
+       return ((lidar.start_angle_deg - index * 360.0/lidar.split_div) * lidar.orientation);
+}
+
+double index2rad(const struct lidar_params lidar, int index)
+{
+       return (index2deg(lidar, index)/180.0 * M_PI);
+}
+
+int deg2index(const struct lidar_params lidar, double deg)
+{
+       return ((lidar.start_angle_deg -(deg)/lidar.orientation)/(360.0/lidar.split_div));
+}
+
+int rad2index(const struct lidar_params lidar, double rad)
+{
+       return deg2index(lidar, (rad * 180)/M_PI);
+}
\ No newline at end of file