From ba89b94981b1021c3ddc4490d895aed7a83d734c Mon Sep 17 00:00:00 2001
From: Kris Burney <burneykb@iastate.edu>
Date: Mon, 5 Dec 2016 03:39:58 -0600
Subject: [PATCH] Fixed VRPN packet to 100%
---
groundStation/src/backend/backend.c | 26 +++++++++-----------------
1 file changed, 9 insertions(+), 17 deletions(-)
diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index 151360586..f521cf732 100644
--- a/groundStation/src/backend/backend.c
+++ b/groundStation/src/backend/backend.c
@@ -115,6 +115,7 @@ modular_structs_t structs;
// in the future.
static void cb(struct ucart_vrpn_TrackerData * td) {
static int count = 0;
+
if(!(count % 10)) {
sendVrpnPacket(td);
//updateLogFile(td);
@@ -324,12 +325,12 @@ void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
//memcpy(&packet[7], &info, sizeof(struct ucart_vrpn_TrackerData));
memset(&packet[7], 0, 4);
- memcpy(&packet[11], &info[4], 4);
- memcpy(&packet[15], &info[0], 4);
- memcpy(&packet[19], &info[8], 4);
- memcpy(&packet[23], &info[16], 4);
- memcpy(&packet[27], &info[12], 4);
- memcpy(&packet[31], &info[20], 4);
+ memcpy(&packet[11], &(info->y), 4);
+ memcpy(&packet[15], &(info->x), 4);
+ memcpy(&packet[19], &(info->z), 4);
+ memcpy(&packet[23], &(info->pitch), 4);
+ memcpy(&packet[27], &(info->roll), 4);
+ memcpy(&packet[31], &(info->yaw), 4);
char checksum = 0;
int i;
@@ -344,12 +345,6 @@ void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
keepRunning = 0;
}
- printf("vrpn packet : '");
- for(int i = 0; i < pSize; ++i) {
- printf(" %0x ");
- }
- printf("'\n");
-
struct timeval tstart;
gettimeofday(&tstart, NULL);
timeArr[currMessageID%MAX_HASH_SIZE] = tstart;
@@ -477,7 +472,6 @@ static ssize_t writeQuad(const char * buf, size_t count) {
if (pthread_mutex_lock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
}
- //setsockopt(zyboSocket, IPPROTO_TCP, TCP_QUICKACK, (int[]){1}, sizeof(int));
retval = write(zyboSocket, buf, count);
if (pthread_mutex_unlock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
@@ -492,9 +486,6 @@ static ssize_t readQuad(char * buf, size_t count) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
}
retval = read(zyboSocket, buf, count);
- //int result = setsockopt(zyboSocket, IPPROTO_TCP, TCP_QUICKACK, (int[]){1}, sizeof(int));
- //printf("result = %d\n", result);
-
if (pthread_mutex_unlock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
}
@@ -704,6 +695,7 @@ static void quad_recv() {
if (respBufLen < 8) {
/* not long enough yet */
+ printf("too short!!\n");
return;
}
@@ -733,7 +725,7 @@ static void quad_recv() {
/* At least enough data read to check checksum, and it was good!*/
char * cmdText = MessageTypes[(int)metadata.msg_type].subtypes[(int)metadata.msg_subtype].cmdText;
float value = getFloat((unsigned char *)respBuf, 7);
- printf("Quad : %s, %lf\n", cmdText, value);
+ //printf("Quad : %s, %lf\n", cmdText, value);
/*
Assuming the quad sends the correct info.. This hasn't been tested yet due to a lack of
quad software. We can check how to format by the cmdText and pass to every client.
--
GitLab