From 25b3bf9de77d42707066045869767f2f158a506b Mon Sep 17 00:00:00 2001
From: burneykb <burneykb@iastate.edu>
Date: Fri, 27 Jan 2017 02:00:39 -0600
Subject: [PATCH] Working groundstation and partially working auto. data dump
---
groundStation/src/backend/backend.c | 32 ++++++++++++++++-------
groundStation/src/backend/communication.c | 3 +--
2 files changed, 24 insertions(+), 11 deletions(-)
diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index 041a91edf..7efcbc855 100644
--- a/groundStation/src/backend/backend.c
+++ b/groundStation/src/backend/backend.c
@@ -195,7 +195,6 @@ int main(int argc, char **argv)
perror("Error creating log file...");
exit(1);
}
- // writeStringToLog(logHeader);
// watch for input from stdin (fd 0) to see when it has input
safe_fd_set(fileno(stdin), &rfds_master, &max_fd);
@@ -328,8 +327,8 @@ void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
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[23], &(info->roll), 4);
+ memcpy(&packet[27], &(info->pitch), 4);
memcpy(&packet[31], &(info->yaw), 4);
char checksum = 0;
@@ -482,6 +481,9 @@ static ssize_t writeQuad(const char * buf, size_t count) {
static ssize_t readQuad(char * buf, size_t count) {
ssize_t retval;
+ if (getenv(NOQUAD_ENV)) {
+ return count;
+ }
if (pthread_mutex_lock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
}
@@ -709,17 +711,23 @@ static void quad_recv() {
respBufLen = 0;
return;
}
-
+
/* Get datalen */
size_t datalen = metadata.data_len;
+ if (respBufLen < datalen + 8) {
+ /* Packet not yet fully read */
+ return;
+ }
+
if (datalen > CMD_MAX_LENGTH - 8) {
/* Very invalid packet. Nuke that shit */
+ warnx("data len is over the maximum packet length. Try reducing the data in the packet sent to the groundstation.\n");
respBufLen = 0;
return;
}
-
- if (respBufLen < datalen + 8) {
- /* Packet not yet fully read */
+
+ if(validPacket == -2) {
+ fprintf(stderr, "Checksums did not match (Parse Packet)\n");
return;
}
@@ -728,10 +736,18 @@ static void quad_recv() {
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);
+
/*
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.
*/
+ if(strncmp(cmdText, "log", strlen(cmdText)) == 0) {
+ char log_text[datalen+1];
+ strncpy(log_text, (char *) data, datalen);
+ log_text[datalen] = '\0';
+ printf("log='%s'\n", log_text);
+ }
+
char buffer[1048];
sprintf(buffer, "%s %lf\n", cmdText, value);
@@ -740,8 +756,6 @@ static void quad_recv() {
write(fd, buffer, datalen + 8);
}
}
- } else {
- warnx("Checksum mismatch!");
}
memmove(respBuf, respBuf + datalen + 8, respBufLen - (datalen + 8));
respBufLen -= datalen + 8;
diff --git a/groundStation/src/backend/communication.c b/groundStation/src/backend/communication.c
index c00878aa8..ca4e82632 100644
--- a/groundStation/src/backend/communication.c
+++ b/groundStation/src/backend/communication.c
@@ -230,14 +230,13 @@ int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * met
// calculate checksum
unsigned char calculated_checksum = 0;
- for(i = 0; i < meta_data->data_len + 7; i++)
+ for(i = 0; i < meta_data->data_len + 7 - 1; i++)
{
calculated_checksum ^= packet[i];
}
// compare checksum
if(packet_checksum != calculated_checksum) {
- fprintf(stderr, "Checksums did not match (Parse Packet): 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum);
return -2;
}
--
GitLab