Commit ffca5ba8 authored by Your Name's avatar Your Name
Browse files

add zmq_trigger_subscriber.cc

parent 44e17e2a
......@@ -10,8 +10,9 @@ LDLIBS+=-lzmq -lpthread -lboost_system -lboost_regex -lboost_program_options
#INCLUDE+=RS-232
CXXFLAGS+=-Wall -std=c++11
all: zmq_trigger zmq_trigger_via_broker playtone
all: zmq_trigger zmq_trigger_via_broker playtone zmq_trigger_subscriber
playtone: playtone.cc
zmq_trigger: zmq_trigger.cc
zmq_trigger_via_broker: zmq_trigger_via_broker.cc
zmq_trigger_subscriber: zmq_trigger_subscriber.cc
//
// zmq_trigger - version using PUB-SUB sockets
// Binds SUB socket to tcp://lsldert00.local:5556
// Expects "Hello" from client, replies with "World"
//
#include <lsl_cpp.h>
#include <memory>
#include <iostream>
#include <iomanip>
#include <azmq/socket.hpp>
#include <boost/asio.hpp>
#include <boost/asio/buffer.hpp>
#include <boost/program_options.hpp>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <pigpio.h>
#include <sched.h>
#include <mutex>
#include <thread>
using namespace std;
using namespace boost;
using namespace lsl;
const int chan_left = 13; // PWM1 audio to jack
const int chan_right = 12; // PWM0 audio to jack
int pwmpin = chan_left;
int doutpin = 8;
int dinpin = 7;
stream_outlet *outlet;
string info_name, info_type, info_sourceid;
std::mutex lsl_mutex;
volatile int run=1 ;
void stop(int signum)
{
run = 0;
}
static int verbose = 0;
void parse_options(int argc, const char* argv[])
{
namespace po = boost::program_options;
// Declare the supported options.
po::options_description desc("Usage:");
desc.add_options()
("help,h", "show this message")
("channel,c", po::value<char>(), "select channel [L or R]")
("lsl-name", po::value<string>(), "set the name of the LSL stream")
("lsl-type", po::value<string>(), "set the type of the LSL stream")
("lsl-sourceid", po::value<string>(), "set the sourceid of the LSL stream")
("verbose,v", "enable verbose output")
;
po::variables_map vm;
po::store(po::parse_command_line(argc, argv, desc), vm);
po::notify(vm);
if (vm.count("help")) {
cout << desc << "\n";
exit(0);
}
if (vm.count("verbose")) {
verbose = 1;
}
if (vm.count("lsl-name"))
info_name = vm["lsl-name"].as<string>();
else
info_name = "Raspberry Pi Digital Triggers";
if (verbose)
cout << "LSL name is: " << info_name << endl;
if (vm.count("lsl-type"))
info_type = vm["lsl-type"].as<string>();
else {
char hostname[255];
gethostname(hostname,255);
ostringstream s;
s << "Digital Triggers @ " << hostname;
info_type = s.str();
}
if (verbose)
cout << "LSL type is: " << info_type << endl;
if (vm.count("lsl-sourceid"))
info_sourceid = vm["lsl-sourceid"].as<string>();
else {
char hostname[255];
gethostname(hostname,255);
ostringstream s;
s << argv[0] << "@" << hostname;
info_sourceid = s.str();
}
if (verbose)
cout << "LSL sourceid is: " << info_sourceid << endl;
if (vm.count("channel")) {
char c = vm["channel"].as<char>();
switch (c) {
case 'l': case 'L':
pwmpin = chan_left;
break;
case 'r': case 'R':
pwmpin = chan_right;
break;
default:
cerr << "channel must be 'L' or 'R'" << endl;
exit(1);
}
}
}
std::string str2hex(const std::string& s)
{
const static char str2hex_lookup[] = "0123456789abcdef";
unsigned int i=0,leng=s.length();
std::stringstream r;
for(i=0; i<leng; i++)
{
r<< str2hex_lookup[ s[i] >> 4 ];
r<< str2hex_lookup[ s[i] & 0x0f ];
}
return r.str();
}
volatile uint32_t last_tick = 0;
uint32_t steady_ticks = 15000;
uint32_t tickdelta(uint32_t t2, uint32_t t1)
{
if (t2 > t1)
return t2-t1;
else
return UINT32_MAX-(t1-t2);
}
string gpioInMarker = "DIN";
void gpioISR(int gpio, int level, uint32_t tick)
{
if (level==2) // timeout call
return;
// check if in debounce period
if ((last_tick != 0) && (tickdelta(tick,last_tick) < steady_ticks))
return;
last_tick = tick;
if (last_tick==0) // treat 0 as a special value, i.e. it is not initialized
last_tick=1; // so we're 1 us off now, but that's not critical
std::lock_guard<std::mutex> guard(lsl_mutex);
outlet->push_sample(&gpioInMarker);
if (verbose)
cout << gpioInMarker << endl;
}
void setdio(int value)
{
int r=gpioWrite(doutpin, value);
if (r < 0)
{
perror("gpioWrite");
exit(1);
}
}
void beep(int freq, int durationmsec)
{
int d=500000;
int r=gpioHardwarePWM(pwmpin, freq, d);
if (r < 0)
{
perror("gpioHardwarePWM");
exit(1);
}
std::this_thread::sleep_for(std::chrono::milliseconds(durationmsec));
r=gpioHardwarePWM(pwmpin, 0, 0);
if (r < 0)
{
perror("gpioHardwarePWM");
exit(1);
}
}
int rtpriority(int n)
{
struct sched_param sched ;
memset (&sched, 0, sizeof(sched)) ;
if (n > sched_get_priority_max (SCHED_RR))
sched.sched_priority = sched_get_priority_max (SCHED_RR) ;
else
sched.sched_priority = n ;
return sched_setscheduler (0, SCHED_RR, &sched) ;
}
int main(int argc, const char* argv[])
{
parse_options(argc, argv);
if (rtpriority(99) < 0) {
perror("rtpriority");
return 1;
}
// Prepare sound output
int r = gpioInitialise(); // use Broadcom pin numbering
if (r < 0)
{
perror("gpioInitialise");
return 1;
}
gpioSetSignalFunc(SIGINT, stop);
gpioSetSignalFunc(SIGABRT, stop);
printf("Press control C to stop.\n");
if (gpioSetMode(chan_right, PI_ALT0) < 0) {
perror("gpioSetMode");
return 1;
}
if (gpioSetMode(chan_left, PI_ALT0) < 0) {
perror("gpioSetMode");
return 1;
}
if (gpioSetMode(doutpin, PI_OUTPUT) < 0) {
perror("gpioSetMode");
return 1;
}
if (gpioSetPullUpDown(doutpin, PI_PUD_UP) < 0) {
perror("gpioSetMode");
return 1;
}
if (gpioSetMode(dinpin, PI_OUTPUT) < 0) {
perror("gpioSetMode");
return 1;
}
if (gpioSetPullUpDown(dinpin, PI_PUD_UP) < 0) {
perror("gpioSetMode");
return 1;
}
if (gpioSetISRFunc(dinpin, FALLING_EDGE, 10000, gpioISR) < 0)
{
perror("gpioSetISRFunc");
return 1;
}
// Prepare our context and socket
asio::io_service ios;
azmq::socket socket(ios, ZMQ_SUB);
socket.bind("tcp://lsldert00.local:5555");
// make a new stream_info and open an outlet with it
stream_info info(
info_name.c_str(),
info_type.c_str(),
1,
lsl::IRREGULAR_RATE,
lsl::cf_string,
info_sourceid.c_str()
);
outlet = new stream_outlet(info);
try {
while (run)
{
std::array<char,4096> request;
request.fill(0);
// Wait for next request from client
socket.receive(asio::buffer(request));
std::string str(request.data());
if (verbose)
cout << "Received: " << str << endl;
// // Return result
// std::string result = str;
// socket.send(asio::buffer(result));
std::istringstream s(str);
std::string cmd;
s >> cmd;
if (verbose)
cout << "cmd=" << str << endl;
if (cmd == "b") { // Legacy command
string marker = "BEEP";
std::lock_guard<std::mutex> guard(lsl_mutex);
outlet->push_sample(&marker);
beep(400, 500);
}
else
if (cmd == "B") {
int freq, duration;
string marker;
s >> freq;
s >> duration;
s >> marker;
if (verbose) {
cout << "freq=" << freq << endl;
cout << "duration=" << duration << endl;
cout << "marker=" << marker << endl;
}
std::lock_guard<std::mutex> guard(lsl_mutex);
outlet->push_sample(&marker);
beep(freq, duration);
}
else
if (cmd == "D") {
int level;
string marker;
s >> level;
s >> marker;
level = (level != 0);
if (verbose) {
cout << "level=" << level << endl;
cout << "marker=" << marker << endl;
}
std::lock_guard<std::mutex> guard(lsl_mutex);
outlet->push_sample(&marker);
setdio(level);
}
else
if (cmd == "M") {
string marker;
s >> marker;
if (verbose) {
cout << "marker=" << marker << endl;
}
std::lock_guard<std::mutex> guard(lsl_mutex);
gpioInMarker = marker;
}
}
}
catch (boost::system::system_error &e)
{
//error_code ec = e.code();
//std::cerr << ec.value() << '\n';
//std::cerr << ec.category().name() << '\n';
}
printf("\nmopping up\n");
r=gpioHardwarePWM(pwmpin, 0, 0);
if (r < 0)
{
perror("gpioHardwarePWM");
return 1;
}
gpioTerminate();
return 0;
}
Supports Markdown
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