You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
77 lines
1.3 KiB
C++
77 lines
1.3 KiB
C++
#include "p_serial_bus.h"
|
|
#include "p_serial_packet.h"
|
|
#include "p_util.h"
|
|
#include <iostream>
|
|
#include <signal.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string>
|
|
#include <zmq.hpp>
|
|
|
|
#ifdef RUN_TESTS
|
|
#include "tests.h"
|
|
#endif
|
|
|
|
#define SERIAL_DEV ("/dev/ttyAMA1")
|
|
#define ZMQ_IPC_ADDR ("ipc:///penguinator/pubsub")
|
|
|
|
int x = 0;
|
|
int y = 0;
|
|
|
|
bool b_quit = false;
|
|
|
|
void sigint_handler(int param)
|
|
{
|
|
(void)param;
|
|
b_quit = true;
|
|
std::cout << "Exiting..." << std::endl;
|
|
return;
|
|
}
|
|
|
|
int main()
|
|
{
|
|
#ifdef RUN_TESTS
|
|
run_tests();
|
|
return 0;
|
|
#endif
|
|
signal(SIGINT, sigint_handler);
|
|
|
|
ps_bus sbus(SERIAL_DEV);
|
|
zmq::context_t ctx;
|
|
zmq::socket_t sub(ctx, ZMQ_SUB);
|
|
sub.connect(ZMQ_IPC_ADDR);
|
|
sub.setsockopt(ZMQ_SUBSCRIBE, "", 0);
|
|
|
|
while (!b_quit)
|
|
{
|
|
zmq::message_t msg_recv;
|
|
int32_t x = 0;
|
|
int32_t y = 0;
|
|
try
|
|
{
|
|
if (sub.recv(msg_recv, zmq::recv_flags(0)))
|
|
{
|
|
uint8_t xy[64];
|
|
memset(xy, 0, 64);
|
|
memcpy(xy, msg_recv.data(), msg_recv.size());
|
|
sscanf((const char*)xy, "X:%" PRId32 " Y:%" PRId32 "\n", &x,
|
|
&y);
|
|
ps_packet xy_packet = ps_packet::from_xy(x, y);
|
|
xy_packet.encode();
|
|
// xy_packet.show_packet();
|
|
if (!sbus.send_pkt(&xy_packet))
|
|
{
|
|
printf("Some failure on transmission\n");
|
|
}
|
|
}
|
|
}
|
|
catch (zmq::error_t& e)
|
|
{
|
|
|
|
std::cout << e.what() << std::endl;
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|