Implemented code for Slamtec rplidar C1

1 files changed, 185 insertions, 0 deletions

diff --git a/LIDAR/sdk/src/arch/linux/thread.hpp b/LIDAR/sdk/src/arch/linux/thread.hpp
new file mode 100644
index 0000000..6a9107b
--- /dev/null
+++ b/LIDAR/sdk/src/arch/linux/thread.hpp
@@ -0,0 +1,185 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "arch/linux/arch_linux.h"
+
+#include <sched.h>
+#include <sys/types.h>
+#include <sys/syscall.h>
+#include <sys/time.h>
+#include <sys/resource.h>
+
+namespace rp{ namespace hal{
+
+Thread Thread::create(thread_proc_t proc, void * data)
+{
+    Thread newborn(proc, data);
+    
+    // tricky code, we assume pthread_t is not a structure but a word size value
+    assert( sizeof(newborn._handle) >= sizeof(pthread_t));
+
+    pthread_create((pthread_t *)&newborn._handle, NULL, (void * (*)(void *))proc, data);
+
+    return newborn;
+}
+
+u_result Thread::terminate()
+{
+    if (!this->_handle) return RESULT_OK;
+    
+    return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL;
+}
+
+u_result Thread::SetSelfPriority( priority_val_t p)
+{
+
+    pid_t selfTid = syscall(SYS_gettid);
+
+        // check whether current schedule policy supports priority levels
+    int current_policy = SCHED_OTHER;
+    struct sched_param current_param;
+    int nice = 0;
+    int ans;
+
+    if (sched_getparam(selfTid, &current_param))
+    {
+        // cannot retreieve values
+        return RESULT_OPERATION_FAIL;
+    }   
+
+    int pthread_priority_min;
+
+#if 1
+    pthread_priority_min = sched_get_priority_min(SCHED_RR);
+#else
+    pthread_priority_min = 1;
+#endif
+	int pthread_priority = 0 ;
+
+	switch(p)
+	{
+	case PRIORITY_REALTIME:
+		//pthread_priority = pthread_priority_max;
+        current_policy = SCHED_RR;
+        pthread_priority = pthread_priority_min + 1;
+        nice = 0;
+		break;
+	case PRIORITY_HIGH:
+		//pthread_priority = (pthread_priority_max + pthread_priority_min)/2;
+        current_policy = SCHED_RR;
+        pthread_priority = pthread_priority_min;
+        nice = 0;
+		break;
+	case PRIORITY_NORMAL:
+        pthread_priority = 0;
+        current_policy = SCHED_OTHER;
+        nice = 0;
+        break;
+	case PRIORITY_LOW:
+        pthread_priority = 0;
+        current_policy = SCHED_OTHER;
+        nice = 10;
+        break;
+	case PRIORITY_IDLE:
+		pthread_priority = 0;
+        current_policy = SCHED_IDLE;
+        nice = 0;
+		break;
+	}
+    // change the inhertiable behavior
+    current_policy |= SCHED_RESET_ON_FORK;
+
+    current_param.__sched_priority = pthread_priority;
+
+  
+
+    
+    // do not use pthread version as it will make the priority be inherited by a thread child
+	if ( (ans = sched_setscheduler(selfTid, current_policy , &current_param)) )
+	{
+        if (ans == EPERM)
+        {
+            //DBG_PRINT("warning, current process hasn't the right permission to set threads priority\n");
+        }
+		return RESULT_OPERATION_FAIL;
+	}
+
+
+    if ((current_policy == SCHED_OTHER) || (current_policy == SCHED_BATCH))
+    {
+        if (setpriority(PRIO_PROCESS, selfTid, nice)) {
+            return RESULT_OPERATION_FAIL;
+        }
+    }
+    
+
+	return  RESULT_OK;
+}
+
+Thread::priority_val_t Thread::getPriority()
+{
+	if (!this->_handle) return PRIORITY_NORMAL;
+
+    int current_policy;
+    struct sched_param current_param;
+    if (pthread_getschedparam( (pthread_t) this->_handle, &current_policy, &current_param))
+    {
+        // cannot retreieve values
+        return PRIORITY_NORMAL;
+    }   
+
+    int pthread_priority_max = sched_get_priority_max(SCHED_RR);
+    int pthread_priority_min = sched_get_priority_min(SCHED_RR);
+
+    if (current_param.__sched_priority ==(pthread_priority_max ))
+	{
+		return PRIORITY_REALTIME;
+	}
+	if (current_param.__sched_priority >=(pthread_priority_max + pthread_priority_min)/2)
+	{
+		return PRIORITY_HIGH;
+	}
+	return PRIORITY_NORMAL;
+}
+
+u_result Thread::join(unsigned long timeout)
+{
+    if (!this->_handle) return RESULT_OK;
+    
+    pthread_join((pthread_t)(this->_handle), NULL);
+    this->_handle = 0;
+    return RESULT_OK;
+}
+
+}}