robofsm: Data from Sick Tim551 is updated in the map

[eurobot/public.git] / src / robofsm / robot_orte.c

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 2e0e41834f0b7489d14dbff7f7b10b88c08938f9..7f5a9a44b4b030b28e28d6c0346fb96cceb764d7 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)
 {