Create LIDAR lib for hadling both rangefinders - SICK and Hokuyo

[eurobot/public.git] / src / lidars / sick-tim3xx / sickd.c

diff --git a/src/sick-tim3xx/sickd.c b/src/lidars/sick-tim3xx/sickd.c
similarity index 97%
rename from src/sick-tim3xx/sickd.c
rename to src/lidars/sick-tim3xx/sickd.c
index 24e356965098bf8a072244398dc2a81ebf291800..e8aa7eaaef9a0d0ea4b586ac1d30aa7f0a47da83 100644 (file)
--- a/src/sick-tim3xx/sickd.c
+++ b/src/lidars/sick-tim3xx/sickd.c
@@ -5,6 +5,9 @@
 #include <robottype.h>
 #include <roboorte_robottype.h>
 #include <signal.h>
+#include <lidar_params.h>
+#include <robodim.h>
+#include <sick.h>
 
 static unsigned short *dists = NULL;   // distance scan
 //static unsigned short *refls = NULL; // reflex instensity scan
@@ -162,15 +165,17 @@ int main(void) {
            
            int j;
            for (j = 0; j < scan_max; j++) {
-               //printf("index: %d, distance: %li, reflex intensity: %li\n", j, dists[j], refls[j]);
+                 //printf("index: %d, distance: %li, reflex intensity: %li\n", j, dists[j], refls[j]);
                 //printf("index: %d, distance: %d\n", j, dists[j]);
-               orte.sick_scan.data[j] = dists[j];
-               /*if (refls) {
-                 orte.sick_scan.data[j + scan_max] = refls[j];
-               }*/  
+                 orte.sick_scan.data[j] = dists[j];
+                 /*if (refls) {
+                   orte.sick_scan.data[j + scan_max] = refls[j];
+                 }*/  
            };
            
-           
+           orte.sick_scan.data_lenght = sick_params.data_lenght;
+      orte.sick_scan.lidar_type = sick_params.type;
+
            // Publish on ORTE
            ORTEPublicationSend(orte.publication_sick_scan);
        }