From: Matous Pokorny <matous.pokorny@me.com>
Date: Fri, 3 Oct 2014 15:22:57 +0000 (+0200)
Subject: robofsm: Data from Sick Tim551 is updated in the map
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/0cbb7a6e54294e1176bf0932d5a92d9fd0ea2bf3

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 f47e23d8..209a7012 100644
--- 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 ef3e3c60..3709ff5a 100644
--- 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)
 {