diff --git a/groundStation/.gitignore b/groundStation/.gitignore
index 367bf3b3faf0304e135baa9082caab2f6c0ea3cd..497d963050ad05b99b4c2526566eda54fcfaef95 100644
--- a/groundStation/.gitignore
+++ b/groundStation/.gitignore
@@ -40,3 +40,5 @@ src/vrpn/pc_linux64/*
#Exacutables
BlueTooth
logs
+client
+BackEnd
diff --git a/groundStation/Makefile b/groundStation/Makefile
index d62850d9b18b266043e0ab688bc37c907a43ba31..de98ae68a5fda2a1bf1908492edb2bca7e23f836 100644
--- a/groundStation/Makefile
+++ b/groundStation/Makefile
@@ -2,7 +2,7 @@
GCC=gcc
GXX=g++
CFLAGS= -Wall -Wpedantic -Wextra -Werror -std=c99 -g -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-but-set-variable
-CXXFLAGS= -Wall -Wno-reorder -std=c++11 -g
+CXXFLAGS= -Wall -Wpedantic -Wextra -Werror -Wno-reorder -Wno-unused-variable -std=c++0x -g
INCLUDES = $(foreach dir, $(INCDIR), -I$(dir))
# Directories
@@ -11,7 +11,7 @@ INCDIR=inc src src/vrpn src/vrpn/quat src/vrpn/build
OBJDIR=obj
# Final exacutable name
-EXE=BlueTooth
+EXE=BackEnd
# File names
CSOURCES := $(foreach dir, $(SRCDIR), $(wildcard $(dir)/*.c ))
diff --git a/groundStation/README.md b/groundStation/README.md
index f89fed232d356c457ad4729994eecfe3ee4cbd41..e9c7f6c83b5b74cd7514b5d9cf32330a7e99eb6c 100644
--- a/groundStation/README.md
+++ b/groundStation/README.md
@@ -1 +1,19 @@
-# groundStation
\ No newline at end of file
+# groundStation
+
+## Make Process
+
+First, if submodules were not recursevly added through git. Run this command if you have made any attempt to make vrpn manually.
+run
+ 'git submodule update --init --recursive'
+
+Now that you have all of the files necissary.
+
+cd into the groundstation folder.
+ cd groundStation
+ make vrpn
+ make
+
+run the program with sudo privledges
+ sudo ./BackEnd
+
+If you wish to change the way the backend communicates to the quad and vice versa, modify src/config.h .
diff --git a/groundStation/src/config.h b/groundStation/src/config.h
index 0e2077cca5fbe03aee3ee8c89bc9cd5a91ecb16b..74b0bc671b11b02c99f5dddc16e7d7f8d48678f4 100644
--- a/groundStation/src/config.h
+++ b/groundStation/src/config.h
@@ -7,4 +7,13 @@
#define NOQUAD_ENV "UCART_NO_QUAD"
+// If you are planning on using any of these env vars and you have
+// exported them with normal user rights. You will need to run the
+// backend with sudo elevation and with the --preserve-env flag or -E
+#define QUAD_WIFI_ENV "UCART_USE_WIFI"
+#define QUAD_IP_ENV "UCART_QUAD_IP"
+#define QUAD_IP_DEFAULT "192.168.4.1"
+#define QUAD_PORT_ENV "UCART_QUAD_PORT"
+#define QUAD_PORT_DEFAULT 8080
+
#endif
diff --git a/groundStation/src/microcart_cli.c b/groundStation/src/microcart_cli.c
index a439a897a58b347ca43b57ee30663908fbfcfc1a..224875c3420f2037d6e640dfa8c86525e0784e36 100644
--- a/groundStation/src/microcart_cli.c
+++ b/groundStation/src/microcart_cli.c
@@ -4,6 +4,7 @@
*/
#define _GNU_SOURCE
+#define _BSD_SOURCE
//system includes
#include <err.h>
@@ -13,11 +14,16 @@
#include <signal.h>
#include <sys/socket.h>
#include <sys/un.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
#include <sys/select.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/rfcomm.h>
#include <pthread.h>
#include <assert.h>
+#include <errno.h>
+#include <string.h>
+#include <sys/ioctl.h>
//user created includes
#include "communication.h"
@@ -30,16 +36,15 @@
#define QUAD_BT_ADDR "00:06:66:64:61:D6"
#define QUAD_BT_CHANNEL 0x01
#define CMD_MAX_LENGTH 1024
+#define MAX_HASH_SIZE 15
// function prototypes
-void killHandler(int);
void readAndPrint(void);
void sendVrpnPacket(struct ucart_vrpn_TrackerData *);
void sendStartPacket(void);
void getVRPNPacket(struct ucart_vrpn_TrackerData *);
void printVrpnData(struct ucart_vrpn_TrackerData *);
int connectToZybo();
-int startsWith(const char *, const char *);
int safe_fd_set(int , fd_set* , int* );
int safe_fd_clr(int , fd_set* , int* );
static void safe_close_fd(int fd, pthread_mutex_t *mutexLock);
@@ -60,6 +65,14 @@ static int wasDisconnected(int fd);
/* Thread-safe wrappers */
pthread_mutex_t quadSocketMutex;
static ssize_t writeQuad(const char * buf, size_t count);
+static ssize_t readQuad(char * buf, size_t count);
+
+/* Functions for recording Latencies */
+void findTimeDiff(int respID);
+int timeval_subtract (struct timeval *result, struct timeval *x, struct timeval *y);
+//time stamp checking
+static unsigned int currMessageID = 0;
+struct timeval timeArr[MAX_HASH_SIZE];
// global variables
static volatile int keepRunning = 1;
@@ -77,7 +90,7 @@ fd_set rfds_master;
int max_fd = 0;
pthread_mutex_t quadResponseMutex, cliInputMutex ;
-unsigned char *respBuf, *commandBuf;
+char *respBuf, *commandBuf;
int newQuadResponse = 0, newCliInput = 0;
// Structures to be used throughout
@@ -93,20 +106,11 @@ static void cb(struct ucart_vrpn_TrackerData * td)
if(!(count % 10)) {
sendVrpnPacket(td);
- updateLogFile(td);
+ //updateLogFile(td);
}
-
count++;
- // This will print the vrpn data to the terminal if necissary.
- // Commented out because the callback will cover quad log data
- // at the end of flight.
- /**if(!(count % 100)) {
- printVrpnData(td);
- printf("[Info] Received %d tracker updates.\n", count);
- }**/
}
-
int main(int argc, char **argv)
{
int activity;
@@ -153,26 +157,24 @@ int main(int argc, char **argv)
client_buffers[i][0] = '\n';
}
- //signal(SIGINT, killHandler);
-
if (pthread_mutex_lock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
}
- // if ((zyboSocket = connectToZybo()) < 0)
- // {
- // perror("Error connecting to Zybo...");
- // free(respBuf);
- // free(commandBuf);
- // exit(1);
- // }
+ if ((zyboSocket = connectToZybo()) < 0)
+ {
+ perror("Error connecting to Quad...");
+ free(respBuf);
+ free(commandBuf);
+ exit(1);
+ }
if (pthread_mutex_unlock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
}
// create vrpnTracker instance
- //tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
+ tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
// open the log file
if(createLogFile(argc, argv[1]))
@@ -180,30 +182,26 @@ int main(int argc, char **argv)
perror("Error creating log file...");
exit(1);
}
-// writeStringToLog(logHeader);
+ // writeStringToLog(logHeader);
// watch for input from stdin (fd 0) to see when it has input
safe_fd_set(fileno(stdin), &rfds_master, &max_fd);
// watch for input from the zybo socket
- //safe_fd_set(zyboSocket, &rfds_master, &max_fd);
-
- //printf("zyboSocket = %d, max_fd = %d\n", zyboSocket, max_fd);
-
- //tell the quad we are ready to send it vrpn data
- printf("sending Start Packet...\n");
- //sendStartPacket();
+ safe_fd_set(zyboSocket, &rfds_master, &max_fd);
+ // Tell the quad we are ready to send it vrpn data
+ sendStartPacket();
// this function will be called whenever tracker receives data
- // ucart_vrpn_tracker_addCallback(tracker, cb);
-
- // start the prompt
- // fprintf(stdout, "$microcart> ");
+ ucart_vrpn_tracker_addCallback(tracker, cb);
struct timeval timeout = {
.tv_sec = 1,
.tv_usec = 0
};
+
+ respBuf = calloc(CMD_MAX_LENGTH, sizeof(unsigned char));
+ sleep(3);
while(keepRunning)
{
fd_set rfds;
@@ -212,11 +210,11 @@ int main(int argc, char **argv)
if(activity == -1) {
perror("select() ");
} else if (activity) {
- fprintf(stderr, "Select activity = %d\n", activity);
for(int fd = 0; fd <= max_fd; ++fd) {
if (FD_ISSET(fd, &rfds)) {
- if(wasDisconnected(fd))
+ if(wasDisconnected(fd)){
break;
+ }
if (fd == fileno(stdin)) {
// break;
@@ -238,23 +236,43 @@ int main(int argc, char **argv)
// // Write the command to the control_loop socket
// // int n = writeQuad(packet, ((packet[6] << 8) | packet[5]) + 8);
// // if(n < 0) {
- // // fprintf(stdout, "CLI: ERROR writing to socket\n");
+ // // printf("CLI: ERROR writing to socket\n");
// // }
// }
- // fprintf(stdout, "$microcart> ");
+ // printf("$microcart> ");
// memset(userCommand, 0, cmdLen);
} else if (fd == zyboSocket) {
-
+ // Read the response from the control loop
+ int available;
+ ioctl(fd, FIONREAD, &available);
+ if (available < 12) {
+ continue;
+ }
+ int respLen = readQuad(respBuf, 12);
+ if(respLen <= 0) {
+ perror("ERROR reading from quad...\n");
+ }
+ int id = getInt((unsigned char *)respBuf, 7);
+ findTimeDiff(id);
+ // if(respLen == 11) {
+ // int id = getInt((unsigned char *)respBuf, 7);
+ // findTimeDiff(id);
+ // printf("respLen = %d : id = %d'\n", respLen, id);
+ // for(int i = 0; i <= respLen -1; ++i)
+ // printf("%x ", (unsigned char)respBuf[i]);
+ // printf("'\n");
+ // }
+ memset(respBuf, 0, respLen);
} else if (fd == backendSocket) {
int new_fd = 0;
new_fd = accept(backendSocket, NULL, NULL);
if (new_fd < 0) {
warn("accept");
} else {
- fprintf(stderr, "Connection\n");
+ printf("Connection\n");
if (new_client(new_fd)) {
- fprintf(stderr, "Added client\n");
+ printf("Added client\n");
safe_fd_set(new_fd, &rfds_master, &max_fd);
}
}
@@ -269,25 +287,17 @@ int main(int argc, char **argv)
}
}
- // ucart_vrpn_tracker_freeInstance(tracker);
+ ucart_vrpn_tracker_freeInstance(tracker);
safe_close_fd(zyboSocket, &quadSocketMutex);
-
-
closeLogFile();
return 0;
}
-// signal handler to exit while loop of main function
-void killHandler(int dummy) {
- keepRunning = 0;
- printf("\nleaving Bluetooth module\n");
-}
-
void sendStartPacket() {
unsigned char packet[8] = {0};
-
+ currMessageID++;
metadata_t metadata =
{
(char) BEGIN_CHAR,
@@ -300,8 +310,8 @@ void sendStartPacket() {
packet[0] = metadata.begin_char; // BEGIN //PACKET_START_BYTE;
packet[1] = metadata.msg_type; // UPDATE //'U'; // U for vrpn camera update, C for command
packet[2] = metadata.msg_subtype; // BEGIN UPDATE
- packet[3] = 1; // MSG ID(1)
- packet[4] = 0; // MSG ID(2)
+ packet[3] = (currMessageID & 0x000000ff); // MSG ID(1)
+ packet[4] = ((currMessageID >> 8) & 0x000000ff); // MSG ID(2)
packet[5] = 0; // DATALEN(1)
packet[6] = 0; // DATALEN(2)
@@ -319,19 +329,20 @@ void sendStartPacket() {
}else
{
printf("Start packet successfuly sent...\n");
- }
+ }
}
void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
int pSize = sizeof(info) + 8;
int n;
unsigned char packet[pSize];
+ currMessageID++;
packet[0] = 0xBE; // BEGIN //PACKET_START_BYTE;
packet[1] = 0x04; // UPDATE //'U'; // U for vrpn camera update, C for command
packet[2] = 0x00; // N/A
//TODO Figure out Packet ID with this new ucar_vrpn_TrackerData struct
- packet[3] = (0x00 & 0x000000ff); // MSG ID(1)
- packet[4] = ((0x00 >> 8) & 0x000000ff); // MSG ID(2)
+ packet[3] = (currMessageID & 0x000000ff); // MSG ID(1)
+ packet[4] = ((currMessageID >> 8) & 0x000000ff); // MSG ID(2)
packet[5] = (sizeof(info) & 0x000000ff); // DATALEN(1)
packet[6] = ((sizeof(info) >> 8) & 0x00000ff); // DATALEN(2)
memcpy(&packet[7], &info, sizeof(info));
@@ -348,6 +359,9 @@ void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
perror("vrpnhandler: ERROR writing to socket");
keepRunning = 0;
}
+ struct timeval tstart;
+ gettimeofday(&tstart, NULL);
+ timeArr[currMessageID%MAX_HASH_SIZE] = tstart;
}
void getVRPNPacket(struct ucart_vrpn_TrackerData *td) {
@@ -366,27 +380,69 @@ void printVrpnData(struct ucart_vrpn_TrackerData * td) {
int connectToZybo() {
int sock;
- struct sockaddr_rc addr;
+ int status = 0;
- // allocate a socket
- sock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
+ /* Use bluetooth by default */
+ if (!getenv(QUAD_WIFI_ENV)) {
+ printf("Using BT Settings\n");
+ struct sockaddr_rc addr;
- //set the connection params ie. who to connect to
- addr.rc_family = AF_BLUETOOTH;
- addr.rc_channel = (uint8_t) QUAD_BT_CHANNEL;
- str2ba( QUAD_BT_ADDR, &addr.rc_bdaddr );
-
- printf("Attempting to connect to zybo. Please be patient...\n");
- // blocking call to connect to socket sock ie. zybo board
- int status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
+ // allocate a socket
+ sock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
+
+ //set the connection params ie. who to connect to
+ addr.rc_family = AF_BLUETOOTH;
+ addr.rc_channel = (uint8_t) QUAD_BT_CHANNEL;
+ str2ba( QUAD_BT_ADDR, &addr.rc_bdaddr );
+
+ printf("Attempting to connect to zybo. Please be patient...\n");
+ // blocking call to connect to socket sock ie. zybo board
+ status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
+ } else {
+ printf("Using WIFI settings\n");
+ struct sockaddr_in addr;
+
+ addr.sin_family = AF_INET;
+
+ /* Quick and Dirty */
+ if (getenv(QUAD_IP_ENV)) {
+ if (!inet_aton(getenv(QUAD_IP_ENV), &addr.sin_addr)) {
+ printf("Env var %s invalid IP %s\n",
+ QUAD_IP_ENV, getenv(QUAD_IP_ENV));
+ return -1;
+ }
+ } else {
+ if (!inet_aton(QUAD_IP_DEFAULT, &addr.sin_addr)) {
+ printf("Default IP %s is invalid\n",
+ QUAD_IP_DEFAULT);
+ return -1;
+ }
+ }
+
+ if (getenv(QUAD_PORT_ENV)) {
+ /* Quick 'n dirty, oh yeah! */
+ addr.sin_port = htons(atoi(getenv(QUAD_PORT_ENV)));
+ } else {
+ addr.sin_port = htons(QUAD_PORT_DEFAULT);
+ }
+
+ sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
+ if (sock < 0) {
+ perror("socket");
+ return -1;
+ }
+ printf("Connecting to Quad @ %s:%u\n", inet_ntoa(addr.sin_addr), ntohs(addr.sin_port));
+
+ status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
+ }
// connection failed
if(status < 0)
{
close(sock);
- printf("Connection failed!...\n");
+ perror("connect");
return -1;
- }
+ }
else
{
printf("connection successful!...\n");
@@ -394,12 +450,6 @@ int connectToZybo() {
}
}
-int startsWith(const char *pre, const char *str) {
- size_t lenpre = strlen(pre),
- lenstr = strlen(str);
- return lenstr < lenpre ? 0 : (strncmp(pre, str, lenpre) == 0);
-}
-
/* add a fd to fd_set, and update max_fd */
int safe_fd_set(int fd, fd_set* fds, int* max_fd) {
assert(max_fd != NULL);
@@ -424,13 +474,13 @@ int safe_fd_clr(int fd, fd_set* fds, int* max_fd) {
static ssize_t writeQuad(const char * buf, size_t count) {
ssize_t retval;
- if (1) {//getenv(NOQUAD_ENV)) {
+ if (getenv(NOQUAD_ENV)) {
return count;
}
if (pthread_mutex_lock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
}
- retval = writeQuad(buf, count);
+ retval = write(zyboSocket, buf, count);
if (pthread_mutex_unlock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
}
@@ -438,8 +488,19 @@ static ssize_t writeQuad(const char * buf, size_t count) {
return retval;
}
-static int new_client(int fd)
-{
+static ssize_t readQuad(char * buf, size_t count) {
+ ssize_t retval;
+ if (pthread_mutex_lock(&quadSocketMutex)) {
+ err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+ }
+ retval = read(zyboSocket, buf, count);
+ if (pthread_mutex_unlock(&quadSocketMutex)) {
+ err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+ }
+ return retval;
+}
+
+static int new_client(int fd) {
ssize_t new_slot = -1;
for (ssize_t i = 0; i < MAX_CLIENTS; i++) {
if (client_fds[i] < 0) {
@@ -458,8 +519,7 @@ static int new_client(int fd)
return 1;
}
-static ssize_t get_client_index(int fd)
-{
+static ssize_t get_client_index(int fd) {
for (ssize_t i = 0; i < MAX_CLIENTS; i++) {
if (client_fds[i] == fd) {
return i;
@@ -469,8 +529,7 @@ static ssize_t get_client_index(int fd)
return -1;
}
-static char * get_client_buffer(int fd)
-{
+static char * get_client_buffer(int fd) {
ssize_t slot = get_client_index(fd);
if (slot == -1) {
return NULL;
@@ -501,8 +560,7 @@ static void safe_close_fd(int fd, pthread_mutex_t *mutexLock) {
}
}
-static void client_recv(int fd)
-{
+static void client_recv(int fd) {
char * buffer;
ssize_t len_pre;
buffer = get_client_buffer(fd);
@@ -537,11 +595,11 @@ static void client_recv(int fd)
buffer[newline] = '\0';
unsigned char * packet;
- // fprintf(stderr, "newline =%li, Client sent: '%s'\n", newline, buffer);
+ // printf("newline =%li, Client sent: '%s'\n", newline, buffer);
if(formatCommand(buffer, &packet) != -1) {
- fprintf(stdout, "Backend sees as: %f\n", getFloat(packet, 7));
+ printf("Backend sees as: %f\n", getFloat(packet, 7));
} else {
- fprintf(stderr, "Could not recognize command '%s'\n", buffer);
+ printf("Could not recognize command '%s'\n", buffer);
}
writeQuad((char *) packet, len);
//free(packet);
@@ -559,8 +617,38 @@ static int wasDisconnected(int fd) {
{
remove_client(fd);
safe_fd_clr(fd, &rfds_master, &max_fd);
- fprintf(stderr, "fd %d has disconnect and was removed\n", fd);
+ printf("fd %d has disconnect and was removed\n", fd);
return 1;
}
return 0;
}
+
+int timeval_subtract (struct timeval *result, struct timeval *x, struct timeval *y) {
+ /* Perform the carry for the later subtraction by updating y. */
+ if (x->tv_usec < y->tv_usec) {
+ int nsec = (y->tv_usec - x->tv_usec) / 1000000 + 1;
+ y->tv_usec -= 1000000 * nsec;
+ y->tv_sec += nsec;
+ }
+ if (x->tv_usec - y->tv_usec > 1000000) {
+ int nsec = (x->tv_usec - y->tv_usec) / 1000000;
+ y->tv_usec += 1000000 * nsec;
+ y->tv_sec -= nsec;
+ }
+ /* Compute the time remaining to wait.
+ tv_usec is certainly positive. */
+ result->tv_sec = x->tv_sec - y->tv_sec;
+ result->tv_usec = x->tv_usec - y->tv_usec;
+ /* Return 1 if result is negative. */
+ return x->tv_sec < y->tv_sec;
+}
+
+void findTimeDiff(int respID) {
+ struct timeval result, tend;
+ gettimeofday(&tend, NULL);
+ timeval_subtract(&result, &tend, &timeArr[respID%MAX_HASH_SIZE]);
+ printf("elapsed time = %ld ms\n", result.tv_usec/1000);
+ // char tmp[8];
+ // snprintf(tmp, 8, "%ld \tms\n", result.tv_usec/1000);
+ // writeStringToLog(tmp);
+}
diff --git a/groundStation/src/vrpn_tracker.cpp b/groundStation/src/vrpn_tracker.cpp
index 49cbaed12e4599a22cecf95ad6351d51de9c6bf5..7f9b56a9909ac1512d6d2164d313214200645e22 100644
--- a/groundStation/src/vrpn_tracker.cpp
+++ b/groundStation/src/vrpn_tracker.cpp
@@ -1,5 +1,6 @@
#include <iostream>
#include <algorithm>
+#include <functional>
#include "vrpn_Tracker.h"
#include "quat.h"
@@ -9,28 +10,25 @@
namespace microcart
{
static void VRPN_CALLBACK vrpn_cb(void * param, const vrpn_TRACKERCB t);
+
+ TrackerData::TrackerData() :
+ x(0.0), y(0.0), z(0.0),
+ pitch(0.0), roll(0.0), yaw(0.0),
+ fps(0.0), timestamp()
+ {
+ }
- Tracker::Tracker(std::string server) : Tracker(server.c_str())
+ Tracker::Tracker(std::string server) :
+ remote(server.c_str()),
+ stop_flag(0),
+ trackerData()
{
}
Tracker::Tracker(const char * server) :
remote(server),
stop_flag(0),
- trackerData({
- .x = 0.0,
- .y = 0.0,
- .z = 0.0,
- .pitch = 0.0,
- .roll = 0.0,
- .yaw = 0.0,
- .fps = 0.0,
- .timestamp = {
- .tv_sec = 0,
- .tv_usec = 0
- }
- })
-
+ trackerData()
{
remote.register_change_handler(this, vrpn_cb);
@@ -92,10 +90,9 @@ namespace microcart
trackerData.fps = 1.0 / elapsed_time_usec;
auto td = trackerData;
- std::for_each(cb_vector.begin(), cb_vector.end(),
- [td](std::function<void(const TrackerData &)> &fn){
- fn(td);
- });
+ for(auto i = cb_vector.begin(); i != cb_vector.end(); ++i) {
+ (*i)(td);
+ }
}
void Tracker::addCallback(std::function<void(const TrackerData&)> cb)
@@ -115,6 +112,20 @@ struct ucart_vrpn_tracker {
microcart::Tracker * t;
};
+void cb_wrapper(void (*cb)(struct ucart_vrpn_TrackerData *),
+ const microcart::TrackerData &td)
+{
+ struct ucart_vrpn_TrackerData data;
+ data.x = td.x;
+ data.y = td.y;
+ data.z = td.z;
+ data.pitch = td.pitch;
+ data.roll = td.roll;
+ data.yaw = td.yaw;
+ data.fps = td.fps;
+ (*cb)(&data);
+}
+
extern "C"
{
struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
@@ -123,6 +134,7 @@ extern "C"
try {
auto inst = new struct ucart_vrpn_tracker;
inst->t = new microcart::Tracker(server);
+ return inst;
} catch(...) {
return NULL;
}
@@ -138,16 +150,8 @@ extern "C"
void (*cb)(struct ucart_vrpn_TrackerData *))
{
try {
- inst->t->addCallback([cb](const microcart::TrackerData & td) {
- struct ucart_vrpn_TrackerData data;
- data.x = td.x;
- data.y = td.y;
- data.z = td.z;
- data.pitch = td.pitch;
- data.roll = td.roll;
- data.yaw = td.yaw;
- (*cb)(&data);
- });
+ auto new_cb = bind(cb_wrapper, cb, std::placeholders::_1);
+ inst->t->addCallback(new_cb);
} catch(...) {
return -1;
}
diff --git a/groundStation/src/vrpn_tracker.hpp b/groundStation/src/vrpn_tracker.hpp
index ebd217551dab4fa6d26faf8ab1bd14a193aac317..d10ad6e79a0bd1a3db42d0c18b4b6b286146538a 100644
--- a/groundStation/src/vrpn_tracker.hpp
+++ b/groundStation/src/vrpn_tracker.hpp
@@ -54,6 +54,7 @@ extern "C"
namespace microcart
{
struct TrackerData {
+ public:
double x;
double y;
double z;
@@ -64,6 +65,8 @@ namespace microcart
double fps;
timeval timestamp;
+
+ TrackerData();
};
class Tracker {