Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Biophysics Labs
Firmware
Commits
74b6617a
Commit
74b6617a
authored
Jul 30, 2020
by
Your Name
Browse files
.
parent
6ff0d32d
Changes
6
Hide whitespace changes
Inline
Side-by-side
lsllidar/Makefile
View file @
74b6617a
...
...
@@ -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
View file @
74b6617a
No preview for this file type
lsllidar/lsllidar.cc
View file @
74b6617a
#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/lsllidar.o
View file @
74b6617a
No preview for this file type
lsllidar/periodic
0 → 100755
View file @
74b6617a
File added
lsllidar/periodic.cc
0 → 100644
View file @
74b6617a
#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
;
}
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment