From 2a85dc87770d613c6a969f43e92d027d20a9fa0a Mon Sep 17 00:00:00 2001
From: burneykb <burneykb@iastate.edu>
Date: Thu, 2 Feb 2017 03:18:48 -0600
Subject: [PATCH] vrpn sending works with joe's new command structure
---
groundStation/src/backend/backend.c | 33 ++++++++++++++++++++---------
groundStation/src/backend/bitwise.h | 2 +-
groundStation/src/backend/packet.c | 24 +++++++++++++--------
3 files changed, 39 insertions(+), 20 deletions(-)
diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index 006ef0a9a..9fc413c45 100644
--- a/groundStation/src/backend/backend.c
+++ b/groundStation/src/backend/backend.c
@@ -231,9 +231,11 @@ int main(int argc, char **argv)
quadlog_file = fopen(log_file, "a");
+
// Tell the quad we are ready to send it vrpn data
sendStartPacket();
+
if(!getenv(NOVRPN_ENV)){
// create vrpnTracker instance
tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
@@ -306,11 +308,12 @@ void sendStartPacket() {
ssize_t psize;
if ((psize = EncodePacket(packet, 64, &m, NULL)) < 0) {
- warnx("Big problems");
+ warnx("Big problems. sendStartPacket");
return;
}
writeQuad(packet, psize);
+ printf("Start Packet sent...\n");
}
void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
@@ -327,15 +330,15 @@ void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
u.roll = info->roll;
u.yaw = info->yaw;
- if (EncodeUpdate(&m, data, 128, &u)) {
- warnx("Big problems");
+ if (EncodeUpdate(&m, data, 128, &u) < 0) {
+ warnx("Big problems. sendVrpnPacket . EncodeUpdate");
return;
}
m.msg_id = currMessageID++;
ssize_t psize;
if ((psize = EncodePacket(packet, 64, &m, data)) < 0) {
- warnx("Big problems");
+ warnx("Big problems. sendVrpnPacket. EncodePacket");
return;
}
@@ -671,28 +674,38 @@ static void client_recv(int fd) {
}
static void quad_recv() {
- static unsigned char respBuf[2048];
+ static unsigned char respBuf[4096];
static size_t respBufLen;
struct metadata m;
- uint8_t data[1024];
+ uint8_t data[4096];
size_t respLen;
ssize_t datalen;
size_t packetlen;
respLen = readQuad((char *) respBuf + respBufLen,
- CMD_MAX_LENGTH- respBufLen);
+ CMD_MAX_LENGTH - respBufLen);
if (respLen <= 0) {
perror("ERROR reading from quad...\n");
return;
}
respBufLen += respLen;
-
- if ((datalen = DecodePacket(&m, data, 1024, respBuf, respBufLen)) < 0) {
- warnx("Packet format error");
+ datalen = DecodePacket(&m, data, 2048, respBuf, respBufLen);
+
+ if (datalen == -1) {
+ warnx("No start Byte!\n");
respBufLen = 0;
return;
}
+ if (datalen == -5) {
+ warnx("Chechsum mismatch!\n");
+ respBufLen = 0;
+ return;
+ }
+ if (datalen < 0){
+ /* Not enough data yet. We need to wait for more */
+ return;
+ }
packetlen = PacketSize(&m);
memmove(respBuf, respBuf + packetlen, respBufLen - packetlen);
diff --git a/groundStation/src/backend/bitwise.h b/groundStation/src/backend/bitwise.h
index 3d37af8e0..1622f9a8f 100644
--- a/groundStation/src/backend/bitwise.h
+++ b/groundStation/src/backend/bitwise.h
@@ -9,7 +9,7 @@
#define MSByte16(x) (((x) >> 8) & 0xff)
/* Build a 16-bit integer out of two bytes */
-#define BytesTo16(lsb, msb) (((lsb) & 0xff) | (((msb) << 8) & 0xff))
+#define BytesTo16(lsb, msb) (((lsb) & 0xff) | (((msb) & 0xff) << 8))
/* Same for 32bit */
#define IntByte1(x) ((x) & 0xff)
diff --git a/groundStation/src/backend/packet.c b/groundStation/src/backend/packet.c
index f29e946e5..81b4fff97 100644
--- a/groundStation/src/backend/packet.c
+++ b/groundStation/src/backend/packet.c
@@ -61,37 +61,43 @@ ssize_t DecodePacket(
const uint8_t * packet, /* Packet to decode */
size_t packet_size) /* Size of packet to decode */
{
+ uint8_t checkSum;
if (packet[BEGIN] != BEGIN_CHAR) {
return -1;
}
if (packet_size < ((uint8_t) HDR_SIZE + CSUM_SIZE)) {
- return -1;
+ return -2;
}
m->msg_type = BytesTo16(packet[MTYPE_L], packet[MTYPE_H]);
m->msg_id = BytesTo16(packet[ID_L], packet[ID_H]);
- m->data_len = BytesTo16(packet[DLEN_L], packet[DLEN_H]);
+ m->data_len = BytesTo16(packet[DLEN_L], packet[DLEN_H]);
if (packet_size < (HDR_SIZE + CSUM_SIZE + m->data_len)) {
- return -1;
+ return -3;
}
if (data_size < m->data_len) {
- return -1;
+ return -4;
}
- memcpy(data, &packet[HDR_SIZE], m->data_len);
+ checkSum = PacketChecksum(packet, packet_size);
+ if (checkSum != packet[HDR_SIZE + m->data_len]) {
+ return -5;
+ }
- /* Validate checksum */
- /* TODO */
+ memcpy(data, &packet[HDR_SIZE], m->data_len);
return m->data_len;
}
uint8_t PacketChecksum(const uint8_t * packet, size_t packet_size)
{
- /* TODO implement */
- return 42;
+ uint8_t checkSum = 0;
+ for(size_t i = 0; i < packet_size - CSUM_SIZE; i++){
+ checkSum ^= packet[i];
+ }
+ return checkSum;
}
size_t PacketSize(const struct metadata *m)
--
GitLab