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.
53 lines
1017 B
C++
53 lines
1017 B
C++
#include "p_serial_bus.h"
|
|
#include "p_serial_packet.h"
|
|
#include <iostream>
|
|
#include <signal.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string>
|
|
#include <zmq.hpp>
|
|
|
|
#define SERIAL_DEV ("/dev/ttyAMA1")
|
|
#define ZMQ_IPC_ADDR ("ipc:///penguinator/pubsub")
|
|
|
|
int x = 0;
|
|
int y = 0;
|
|
|
|
void sigint_handler(int param)
|
|
{
|
|
std::cout << "Exiting..." << std::endl;
|
|
exit(1);
|
|
}
|
|
|
|
int main()
|
|
{
|
|
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);
|
|
|
|
for (;;)
|
|
{
|
|
zmq::message_t msg;
|
|
int32_t x = 0;
|
|
int32_t y = 0;
|
|
if (sub.recv(&msg, 0))
|
|
{
|
|
uint8_t xy[64];
|
|
memset(xy, 0, 64);
|
|
memcpy(xy, msg.data(), msg.size());
|
|
sscanf((const char*)xy, "X:%" PRId32 " Y:%" PRId32 "\n", &x, &y);
|
|
ps_packet xy_packet = ps_packet::from_xy(x, y);
|
|
for (int ind = 0; ind < xy_packet.msg_len; ind++)
|
|
{
|
|
printf("[%d]: %02x ", ind, xy_packet.data[ind]);
|
|
}
|
|
printf("\n");
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|