robofsm: Data from Sick Tim551 is updated in the map

This code is untested.

diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index f47e23d8c8936caeba3f8efcd90ce458bf292533..209a701234c9f4375706ee4449099115500fab9e 100644 (file)
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -105,6 +105,7 @@ extern struct lock_log robot_lock_log;
 #define __robot_lock_sharps            lock
 #define __robot_lock_hokuyo            lock
 #define __robot_lock_sick               lock
+#define __robot_lock_sick551            lock
 #define __robot_lock_cmu               lock
 #define __robot_lock_bumper            lock
 #define __robot_lock_disp              lock_disp
@@ -131,6 +132,7 @@ enum robot_component {
        COMPONENT_LIFT,
 
        ROBOT_COMPONENT_NUMBER
+        COMPONENT_SICK551,
 };
 
 /* robot's main data structure */
@@ -207,6 +209,9 @@ struct robot {
         struct lidar_scan_type sick;
         bool ignore_sick;
 
+        struct lidar_scan_type sick551;
+        bool ignore_sick551;
+
         /* variables for target detection */
         bool target_loaded;
         bool target_valid;

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index ef3e3c6095449625689bbb61f03ae21104ccc6d5..3709ff5a6fdba5d3495efc779c000b3c9d9b07cf 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -362,6 +362,30 @@ void rcv_sick_scan_cb(const ORTERecvInfo *info, void *vinstance,
        }
 }
 
+void rcv_sick551_scan_cb(const ORTERecvInfo *info, void *vinstance,
+                       void *recvCallBackParam)
+{
+       struct lidar_scan_type *instance = (struct lidar_scan_type *)vinstance;
+
+       switch (info->status) {
+               case NEW_DATA: {
+                       ROBOT_LOCK(sick551);
+                       robot.sick551 = *instance;
+                       robot.status[COMPONENT_SICK551] = STATUS_OK;
+                       ROBOT_UNLOCK(sick551);
+                       if(!robot.ignore_sick551)
+                       {
+                               update_map_lidar(&sick551_params, instance);
+                       }
+                       break;
+               }
+               case DEADLINE:
+                       robot.status[COMPONENT_SICK551] = STATUS_FAILED;
+                       DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
+                       break;
+       }
+}
+
 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
                        void *recvCallBackParam)
 {