Commit 74b6617a authored by Your Name's avatar Your Name
Browse files

.

parent 6ff0d32d
......@@ -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
......
No preview for this file type
#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);
}
}
No preview for this file type
#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;
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment