From 003e10b683977b99ae717158cabd31083d36503c Mon Sep 17 00:00:00 2001
From: burneykb <burneykb@iastate.edu>
Date: Wed, 1 Feb 2017 18:48:23 -0600
Subject: [PATCH] changes to quad_recv
---
groundStation/src/backend/backend.c | 194 +++++++++-------------------
groundStation/src/backend/packet.c | 5 +
2 files changed, 67 insertions(+), 132 deletions(-)
diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index 9bf8aa57b..a0ea26cfb 100644
--- a/groundStation/src/backend/backend.c
+++ b/groundStation/src/backend/backend.c
@@ -34,6 +34,8 @@
#include "vrpn_tracker.hpp"
#include "type_def.h"
#include "logger.h"
+#include "packet.h"
+#include "respcontrol.h"
#include "config.h"
#define QUAD_BT_ADDR "00:06:66:64:61:D6"
@@ -272,80 +274,40 @@ int main(int argc, char **argv)
}
void sendStartPacket() {
- unsigned char packet[8] = {0};
- currMessageID++;
- metadata_t metadata =
- {
- (char) BEGIN_CHAR,
- BEGINUPDATE_ID,
- 1,
- 0
- };
+ uint8_t packet[64];
+ struct metadata m;
+ m.msg_type = BEGINUPDATE_ID;
+ m.data_len = 0;
+ m.msg_id = currMessageID++;
- packet[0] = metadata.begin_char; // BEGIN //PACKET_START_BYTE;
- packet[1] = metadata.msg_type & 0xff; // UPDATE //'U'; // U for vrpn camera update, C for command
- packet[2] = (metadata.msg_type >> 8) & 0xff; // BEGIN UPDATE
- 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)
-
- char checksum = 0;
- int i;
- for(i=0; i < metadata.data_len + 7; i++)
- checksum ^= packet[i];
-
- packet[metadata.data_len + 7] = checksum; //PACKET_END_BYTE;
- int status = writeQuad((char * ) packet, metadata.data_len + 8);
- if (status != 8)
- {
- perror("Error sending start packet...\n");
- keepRunning = 0;
- }else
- {
- printf("Start packet successfuly sent...\n");
- }
+ size_t psize;
+
+ if ((psize = EncodePacket(packet, 64, &m, NULL)) < 0) {
+ warnx("Big problems");
+ return;
+ }
+
+ writeQuad(packet, psize);
}
void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
- int pSize = 36;//sizeof(struct ucart_vrpn_TrackerData) + 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] = (currMessageID & 0x000000ff); // MSG ID(1)
- packet[4] = ((currMessageID >> 8) & 0x000000ff); // MSG ID(2)
- packet[5] = (28 & 0x000000ff); // DATALEN(1)
- packet[6] = ((28) >> 8 & 0x00000ff); // DATALEN(2)
- //memcpy(&packet[7], &info, sizeof(struct ucart_vrpn_TrackerData));
-
- memset(&packet[7], 0, 4);
- memcpy(&packet[11], &(info->y), 4);
- memcpy(&packet[15], &(info->x), 4);
- memcpy(&packet[19], &(info->z), 4);
- memcpy(&packet[23], &(info->roll), 4);
- memcpy(&packet[27], &(info->pitch), 4);
- memcpy(&packet[31], &(info->yaw), 4);
-
- char checksum = 0;
- int i;
- for(i=0; i < pSize - 1; i++)
- checksum ^= packet[i];
-
- packet[pSize - 1] = checksum; //PACKET_END_BYTE;
-
- n = writeQuad((char *) packet, pSize);
- if(n < 0) {
- perror("vrpnhandler: ERROR writing to socket");
- keepRunning = 0;
+ uint8_t packet[64];
+ struct metadata m;
+ uint8_t data[128];
+
+ if (EncodeUpdate(&m, data, 128, info)) {
+ warnx("Big problems");
+ return;
}
+ m.msg_id = currMessageID++;
- struct timeval tstart;
- gettimeofday(&tstart, NULL);
- timeArr[currMessageID%MAX_HASH_SIZE] = tstart;
+ size_t psize;
+ if ((psize = EncodePacket(packet, 64, &m, data)) < 0) {
+ warnx("Big problems");
+ return;
+ }
+
+ writeQuad(packet, psize);
}
void getVRPNPacket(struct ucart_vrpn_TrackerData *td) {
@@ -681,91 +643,59 @@ static void quad_recv() {
onto the clients, do so.
*/
- int validPacket;
- unsigned char data[256];
- metadata_t metadata;
static unsigned char respBuf[2048];
static size_t respBufLen;
- /**
- * Read the response from the control loop
- */
- int respLen = readQuad((char *) respBuf + respBufLen,
- CMD_MAX_LENGTH);
- if(respLen <= 0) {
+ struct metadata m;
+ uint8_t data[256];
+ size_t respLen;
+
+ respLen = readQuad((char *) respBuf + respBufLen,
+ CMD_MAX_LENGTH- respBufLen);
+ if (respLen <= 0) {
perror("ERROR reading from quad...\n");
return;
}
respBufLen += respLen;
- if (respBufLen < 8) {
- /* not long enough yet */
- printf("too short!!\n");
- return;
- }
-
- // Validate the message is correctly formatted
- validPacket = parse_packet((unsigned char *) respBuf, data, &metadata);
- if (validPacket == -1) {
- warnx("Doesn't have start byte.");
- /* nuke packet */
+ if (DecodePacket(&m, data, 256, respBuf, respBufLen) < 0) {
+ warnx("Packet format error");
respBufLen = 0;
return;
}
-
- /* Get datalen */
- size_t datalen = metadata.data_len;
- if (respBufLen < datalen + 8) {
- /* Packet not yet fully read */
- return;
- }
+ memmove(respBuf, respBuf + datalen + 8, respBufLen - (datalen + 8));
+ respBufLen -= PacketSize(&m);
- 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;
+ switch (m.msg_type) {
+ case RESPCONTROL_ID:
+ handleRespcontrol;
+ break;
+ case LOG_ID:
+ /* something like this */
+ log_write((char *) data, m.data_len);
+ break;
}
+}
- if(validPacket == -2) {
- fprintf(stderr, "Checksums did not match (Parse Packet)\n");
+static void handleRespcontrol(struct metadata *m, uint8_t * data)
+{
+ struct controller_message cm;
+ if (DecodeRespcontrol(&cm, m, data) < 0) {
+ warnx("Respcontrol error")
return;
}
- if (validPacket == 0) {
- /* At least enough data read to check checksum, and it was good!*/
- char * cmdText = MessageTypes[(int)metadata.msg_type].cmdText;
-
- /* TODO: Parse data correctly
- * - Switch on msg_type
- * - Parse data appropriately and format into a string
- * - Send append data string to cmdText and send to clients
- */
- 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[128];
- char buffer[1048];
- sprintf(buffer, "%s %lf\n", cmdText, value);
+ char * message = cmToString(RESPCONTROL_ID, cm.id, cm.value_id);
- for(int fd = 0; fd <= max_fd; ++fd) {
- if (get_client_index(fd) > -1) {
- write(fd, buffer, datalen + 8);
- }
+ size_t len = snprintf(buffer, 128, "%s %f\n", message, cm.value);
+
+ for(int fd = 0; fd <= max_fd; ++fd) {
+ if (get_client_index(fd) > -1) {
+ write(fd, buffer, len);
}
}
- memmove(respBuf, respBuf + datalen + 8, respBufLen - (datalen + 8));
- respBufLen -= datalen + 8;
}
static int wasDisconnected(int fd) {
diff --git a/groundStation/src/backend/packet.c b/groundStation/src/backend/packet.c
index 50432eab7..2830f5902 100644
--- a/groundStation/src/backend/packet.c
+++ b/groundStation/src/backend/packet.c
@@ -93,3 +93,8 @@ uint8_t PacketChecksum(const uint8_t * packet, size_t packet_size)
/* TODO implement */
return 42;
}
+
+size_t PacketSize(const struct metadata *m)
+{
+ return m.data_len + HDR_SIZE + CSUM_SIZE;
+}
\ No newline at end of file
--
GitLab