.

lsllidar/Makefile

@@ -5,7 +5,7 @@ CXX=g++
 CC=g++
 
 
-LDLIBS+=-llsl32 -lboost_program_options 
+LDLIBS+=-llsl32 -lboost_program_options -lrt
 #LDLIBS+=-lzmq -lboost_program_options -lpigpio -llsl32
 #LDLIBS+=-lzmq -lpthread -lboost_system -lboost_regex  -lboost_program_options
 #INCLUDE+=RS-232

lsllidar/lsllidar.cc

+#define _POSIX_C_SOURCE 199309L
 #include <linux/types.h>
 #include <cstdio>
 #include <cstdint>
@@ -5,11 +6,18 @@
 #include <lsl_cpp.h>
 #include <boost/program_options.hpp>
 #include <include/lidarlite_v3.h>
+#include <sched.h>
+#include <sys/time.h>
+#include <time.h>
+#include <signal.h>
 
 using namespace std;
 using namespace boost;
 using namespace lsl;
 
+const int sched_policy = SCHED_FIFO;
+const int sched_priority = 99;
+
 LIDARLite_v3 myLidarLite;
 
 stream_outlet *outlet;
@@ -76,18 +84,69 @@ void parse_options(int argc, const char *argv[]) {
         cout << "LSL sourceid is: " << info_sourceid << endl;
 }
 
-int main(int argc, const char *argv[])
+int rtpriority(int n) {
+    struct sched_param sched;
+
+    memset(&sched, 0, sizeof(sched));
+
+    if (n > sched_get_priority_max(sched_policy))
+        sched.sched_priority = sched_get_priority_max(sched_policy);
+    else
+        sched.sched_priority = n;
+
+    return sched_setscheduler(0, sched_policy, &sched);
+}
+
+static sigset_t timer_sig;
+
+void waitfor_periodic_timer(void)
 {
-    int16_t distance;
-    __u8  busyFlag;
+    int dummy;
+    sigwait(&timer_sig, &dummy);
+}
 
+int start_periodic_timer(uint64_t offs, uint64_t period)
+{
+    struct itimerspec t;
+    struct sigevent sigev;
+    timer_t timer;
+    const int signal = SIGRTMIN;
+    int res;
+
+    t.it_value.tv_sec     = offs    / 1000000;
+    t.it_value.tv_nsec    = (offs   % 1000000) * 1000;
+    t.it_interval.tv_sec  = period  / 1000000;
+    t.it_interval.tv_nsec = (period % 1000000) * 1000;
+
+    sigemptyset(&timer_sig);
+    sigaddset(&timer_sig, signal);
+    sigprocmask(SIG_BLOCK, &timer_sig, NULL);
+
+    memset(&sigev, 0, sizeof(struct sigevent));
+    sigev.sigev_notify = SIGEV_SIGNAL;
+    sigev.sigev_signo = signal;
+    res = timer_create(CLOCK_REALTIME, &sigev, &timer);
+    if (res < 0) {
+        perror("Timer Create");
+        exit(-1);
+    }
+    return timer_settime(timer, 0 /*TIMER_ABSTIME*/, &t, NULL);
+}
+
+int main(int argc, const char *argv[])
+{
     parse_options(argc, argv);
 
+    if (rtpriority(sched_priority) < 0) {
+        perror("rtpriority");
+        return 1;
+    }
+
     // Initialize i2c peripheral in the cpu core
     myLidarLite.i2c_init();
 
     // Optionally configure LIDAR-Lite
-    myLidarLite.configure(0);
+    myLidarLite.configure(3);  // 3 = max range
 
     // make a new stream_info and open an outlet with it
     stream_info info(info_name.c_str(), info_type.c_str(), 1,
@@ -95,22 +154,22 @@ int main(int argc, const char *argv[])
                      info_sourceid.c_str());
     outlet = new stream_outlet(info);
 
+    start_periodic_timer(1000000,100000);
+
     while(1)
     {
+        waitfor_periodic_timer();
         // Each time through the loop, check BUSY
-        busyFlag = myLidarLite.getBusyFlag();
-
-        if (busyFlag == 0x00)
-        {
-            // When no longer busy, immediately initialize another measurement
-            // and then read the distance data from the last measurement.
-            // This method will result in faster I2C rep rates.
-            myLidarLite.takeRange();
-            distance = myLidarLite.readDistance();
-
-            outlet->push_sample(&distance);
-            printf("%4d\n", distance);
-        }
+        int busyFlag = myLidarLite.getBusyFlag();
+
+        if (busyFlag != 0x00)
+            continue;
+        
+        int16_t distance;
+        myLidarLite.takeRange();
+        distance = myLidarLite.readDistance();
+        outlet->push_sample(&distance);
+        printf("%4d\n", distance);
     }
 }
 

lsllidar/periodic.cc

+#define _POSIX_C_SOURCE 199309L
+#include <sys/time.h>
+#include <signal.h>
+#include <time.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdio.h>
+
+
+static sigset_t sig;
+
+void wait_periodic_timer(void)
+{
+    int dummy;
+    sigwait(&sig, &dummy);
+}
+
+int start_periodic_timer(uint64_t offs, uint64_t period)
+{
+    struct itimerspec t;
+    struct sigevent sigev;
+    timer_t timer;
+    const int signal = SIGALRM;
+    int res;
+
+    t.it_value.tv_sec     = offs    / 1000000;
+    t.it_value.tv_nsec    = (offs   % 1000000) * 1000;
+    t.it_interval.tv_sec  = period  / 1000000;
+    t.it_interval.tv_nsec = (period % 1000000) * 1000;
+
+    sigemptyset(&sig);
+    sigaddset(&sig, signal);
+    sigprocmask(SIG_BLOCK, &sig, NULL);
+
+    memset(&sigev, 0, sizeof(struct sigevent));
+    sigev.sigev_notify = SIGEV_SIGNAL;
+    sigev.sigev_signo = signal;
+    res = timer_create(CLOCK_REALTIME, &sigev, &timer);
+    if (res < 0) {
+        perror("Timer Create");
+	exit(-1);
+    }
+    return timer_settime(timer, 0 /*TIMER_ABSTIME*/, &t, NULL);
+}
+
+static void void_handler(void)
+{
+    static int cnt;
+    static uint64_t start;
+    uint64_t t;
+    struct timeval tv;
+
+    if (start == 0) {
+        gettimeofday(&tv, NULL);
+	start = tv.tv_sec * 1000ULL + tv.tv_usec / 1000ULL;
+    }
+        
+    gettimeofday(&tv, NULL);
+    t = tv.tv_sec * 1000ULL + tv.tv_usec / 1000ULL;
+    if (cnt && (cnt % 100) == 0) {
+        printf("Avg time: %f\n", (double)(t - start) / (double)cnt);
+    }
+    cnt++;
+}
+
+int main()
+{
+    int res;
+
+    res = start_periodic_timer(10000, 5000);
+    if (res < 0) {
+        perror("Start Periodic Timer");
+
+        return -1;
+    }
+
+    while(1) {
+        wait_periodic_timer();
+        void_handler();
+    }
+
+    return 0;
+}