diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index 5fd676888c0d04afc5ae57ac8676e5d61fe74215..7d6af37b94f233d03603dc1b28a46ad677e3faed 100644
--- a/groundStation/src/backend/backend.c
+++ b/groundStation/src/backend/backend.c
@@ -34,10 +34,12 @@
#include "vrpn_tracker.hpp"
#include "type_def.h"
#include "packet.h"
-#include "response.h"
+#include "responseparam.h"
#include "update.h"
#include "config.h"
#include "cmHandler.h"
+#include "getparam.h"
+#include "setparam.h"
#define QUAD_BT_ADDR "00:06:66:64:61:D6"
#define QUAD_BT_CHANNEL 0x01
@@ -77,7 +79,7 @@ static void quad_recv();
/* Checks to see if socket has disconnected. Returns 1 on disconnect, else returns 0 */
static int wasDisconnected(int fd);
/* handle controller responses from quad to frontend */
-static void handleResponse(struct metadata *m, uint8_t * data);
+static void handleResponseParam(struct metadata *m, uint8_t * data);
/* Thread-safe wrappers */
@@ -428,8 +430,6 @@ int connectToZybo() {
}
else
{
- // int result = setsockopt(sock, IPPROTO_TCP, TCP_NODELAY, (int[]){1}, sizeof(int));
- // printf("result = %d\n", result);
printf("connection successful!...\n");
return sock;
}
@@ -578,20 +578,22 @@ static void safe_close_fd(int fd, pthread_mutex_t *mutexLock) {
static void client_recv(int fd) {
char * buffer;
ssize_t len_pre;
- buffer = get_client_buffer(fd);
- len_pre = strlen(buffer);
-
+ char cmdString[64];
char * cursor;
+ ssize_t r;
+ int index = 0;
+
+
+ buffer = get_client_buffer(fd);
+ len_pre = strlen(buffer);
cursor = buffer + len_pre;
- ssize_t r;
r = read(fd, cursor, CLIENT_BUFFER_SIZE - len_pre - 1);
if (r < 0) {
warn("read (fd: %d)", fd);
}
buffer[len_pre + r] = '\0';
- int index = 0;
/* Parse buffer and handle commands */
while (1) {
/* not using strtok because reasons */
@@ -611,9 +613,9 @@ static void client_recv(int fd) {
buffer[newline] = '\0';
printf("Client(%d) : '%s'\n",fd, buffer);
- unsigned char packet[1024];
- ssize_t packetSize;
- if((packetSize = formatCommand(buffer, packet)) == -1) {
+
+ struct controller_message cm;
+ if (stringToCm(buffer, &cm) == NULL) {
/* buffer was not a quad command, handling internally to
* backend instead of forwarding to quad
*/
@@ -648,21 +650,44 @@ static void client_recv(int fd) {
}
}
} else {
- if (packet[1] == 0x02) {
+ uint8_t packet[64];
+ struct metadata m;
+ uint8_t data[128];
+ ssize_t result;
+
+
+ if (strncmp(buffer, "set", 3) == 0) {
+ result = EncodeSetparam(&m, data, 128, &cm);
+ } else if (strncmp(buffer, "get", 3) == 0) {
+ result = EncodeGetparam(&m, data, 128, &cm);
+ } else {
+ warnx("not implemented yet");
+ }
+
+ if (result < 0) {
+ warnx("Big problems. client_recv. EncodeMetaData");
+ return;
+ }
+
+ ssize_t psize;
+ if ((psize = EncodePacket(packet, 64, &m, data)) < 0) {
+ warnx("Big problems. client_recv. EncodePacket");
+ return;
+ }
+
+ m.msg_id = currMessageID++;
+
+ /* Only add the client to the pending responses if it was a getparam command */
+ if (m.msg_type == GETPARAM_ID) {
if (clientAddPendResponses(fd, packet) == -1) {
warnx("Ran out of room! Consider increasing CLIENT_MAX_PENDING_RESPONSES!\n");
}
}
+
+ writeQuad(packet, psize);
- /* Don't know about this */
- int datalen = (packet[6] << 8) | (packet[5]);
- printf("sending %lf '", getFloat(packet, 7));
- for(int i = 0; i < datalen + 8; ++i) {
- printf(" 0x%.2x", (signed) packet[i]);
- }
- printf("'\n");
- writeQuad(packet, packetSize);
}
+
char * rest = &buffer[newline] + 1;
size_t restLen = (strlen(rest) == 0) ? 1 : strlen(rest);
@@ -675,6 +700,7 @@ static void quad_recv() {
static unsigned char respBuf[4096];
static size_t respBufLen;
+ int unfinished = 1;
struct metadata m;
uint8_t data[4096];
size_t respLen;
@@ -688,63 +714,80 @@ static void quad_recv() {
return;
}
respBufLen += respLen;
- 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);
- respBufLen -= packetlen;
+ // printf("packet = '");
+ // for(int i = 0; i < (int)respBufLen; ++i) {
+ // printf(" %.2x ", respBuf[i]);
+ // }
+ // printf("'\n");
- switch (m.msg_type) {
- case DEBUG_ID :
- case PACKETLOG_ID :
- case GETPACKETLOGS_ID :
- case UPDATE_ID :
- case BEGINUPDATE_ID :
- printf("(Backend): Command '%s' ignored\n", MessageTypes[m.msg_type].cmdText);
- break;
- case LOG_ID:
- /* something like this */
- fwrite((char *) data, sizeof(char), m.data_len, quadlog_file);
- fflush(quadlog_file);
- break;
- case SETPARAM_ID:
- case GETPARAM_ID:
- printf("(Backend): Command '%s' ignored\n", MessageTypes[m.msg_type].cmdText);
- break;
- case RESPONSE_ID:
- handleResponse(&m, data);
- break;
- default:
- printf("(Backend): message type %d unrecognized\n", m.msg_type);
+ while(unfinished){
+ 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);
+ respBufLen -= packetlen;
+
+ switch (m.msg_type) {
+ case DEBUG_ID :
+ case PACKETLOG_ID :
+ case GETPACKETLOGS_ID :
+ case UPDATE_ID :
+ case BEGINUPDATE_ID :
+ printf("(Backend): Command '%s' ignored\n", MessageTypes[m.msg_type].cmdText);
+ break;
+ case LOG_ID:
+ /* something like this */
+ fwrite((char *) data, sizeof(char), m.data_len, quadlog_file);
+ fflush(quadlog_file);
+ break;
+ case RESPONSE_ID:
+ printf("(Backend): response id found\n");
+ break;
+ case SETPARAM_ID:
+ case GETPARAM_ID:
+ printf("(Backend): Command '%s' ignored\n", MessageTypes[m.msg_type].cmdText);
+ break;
+ case RESPPARAM_ID:
+ printf("(Backend): response param id found\n");
+ handleResponseParam(&m, data);
+ break;
+ default:
+ printf("(Backend): message type %d unrecognized\n", m.msg_type);
+ }
+ if (respBufLen == 0) {
+ unfinished = 0;
+ }
}
+
}
-static void handleResponse(struct metadata *m, uint8_t * data)
+static void handleResponseParam(struct metadata *m, uint8_t * data)
{
struct controller_message cm;
- if (DecodeResponse(&cm, m, data) < 0) {
+ if (DecodeResponseParam(&cm, m, data) < 0) {
warnx("DecodeResponse error");
return;
}
char buffer[128];
- const char * message = cmToString(RESPONSE_ID, &cm);
+ const char * message = cmToString(RESPPARAM_ID, &cm);
size_t len = snprintf(buffer, 128, "%s %f\n", message, cm.value);
diff --git a/groundStation/src/backend/cmHandler.c b/groundStation/src/backend/cmHandler.c
index adb5fa165a8590a10c21f75acf9f5751f474dcb3..36946112118494137a2e31866255c6a16268ff39 100644
--- a/groundStation/src/backend/cmHandler.c
+++ b/groundStation/src/backend/cmHandler.c
@@ -1,40 +1,8 @@
-#include "cmHandler.h"
+#include <stdlib.h>
-/*
-enum respContIndices {
- GET_PITCH_P ,
- GET_PITCH_I ,
- GET_PITCH_D ,
- GET_PITCH ,
- GET_PITCH_RATE,
- GET_ROLL_P ,
- GET_ROLL_I ,
- GET_ROLL_D ,
- GET_ROLL ,
- GET_ROLL_RATE,
- GET_YAW_P ,
- GET_YAW_I ,
- GET_YAW_D ,
- GET_YAW ,
- GET_YAW_RATE,
- GET_HEIGHT_P,
- GET_HEIGHT_I,
- GET_HEIGHT_D,
- GET_HEIGHT ,
- GET_HEIGHT_RATE,
- GET_LAT_P ,
- GET_LAT_I ,
- GET_LAT_D ,
- GET_LAT ,
- GET_LONG_RATE,
- GET_LONG_P ,
- GET_LONG_I ,
- GET_LONG_D ,
- GET_LONG ,
- MAX_COMMANDS
-};
+#include "cmHandler.h"
-const char * respContStrings[MAX_COMMANDS] = {
+const char * respParamStrings[MAX_PARAM_COMMANDS] = {
"getrollp",
"getrolli",
"getrolld",
@@ -72,10 +40,50 @@ const char * respContStrings[MAX_COMMANDS] = {
"getheightd",
"getheight"
};
-*/
+
+const char * setParamStrings[MAX_PARAM_COMMANDS] = {
+ "setrollp",
+ "setrolli",
+ "setrolld",
+ "setroll",
+ "setpitchp",
+ "setpitchi",
+ "setpitchd",
+ "setpitch",
+ "setyawp",
+ "setyawi",
+ "setyawd",
+ "setyaw",
+ "setrollratep",
+ "setrollratei",
+ "setrollrated",
+ "setrollrate",
+ "setpitchratep",
+ "setpitchratei",
+ "setpitchrated",
+ "setpitchrate",
+ "setyawratep",
+ "setyawratei",
+ "setyawrated",
+ "setyawrate",
+ "setlatp",
+ "setlati",
+ "setlatd",
+ "setlat",
+ "setlongp",
+ "setlongi",
+ "setlongd",
+ "setlong",
+ "setheightp",
+ "setheighti",
+ "setheightd",
+ "setheight"
+};
+
const char * cmToString(int msgType, const struct controller_message *cm)
{
+ size_t index = (cm->id * MAX_CONTROL_PARAM_ID) + cm->value_id;
switch (msgType) {
case DEBUG_ID :
case PACKETLOG_ID :
@@ -84,15 +92,74 @@ const char * cmToString(int msgType, const struct controller_message *cm)
case BEGINUPDATE_ID :
case LOG_ID:
case SETPARAM_ID:
+ return setParamStrings[index];
case GETPARAM_ID:
return NULL;
break;
case RESPONSE_ID:
- return "FOOBAR";
-// return respContStrings[cm->id + cm->value_id];
- break;
+ return NULL;
+ case RESPPARAM_ID:
+ return respParamStrings[index];
default:
return NULL;
break;
}
}
+
+struct controller_message * stringToCm(const char * string, struct controller_message *cm)
+{
+ char st[128];
+ strncpy(st, string, strlen(string));
+ size_t i;
+ int result;
+ float cmdValue;
+ char * cmdString = strtok(st, " ");
+ cm->value = strtof(&string[strlen(cmdString)],NULL);
+ int index = -1;
+
+ for(i = 0; i < MAX_PARAM_COMMANDS; ++i) {
+ if((result = strncmp(cmdString, respParamStrings[i], strlen(respParamStrings[i]))) == 0) {
+ index = i;
+ break;
+ } else if ((result = strncmp(cmdString, setParamStrings[i], strlen(setParamStrings[i]))) == 0) {
+ index = i;
+ break;
+ }
+ }
+
+ /* Error, nothing found */
+ if (index == -1) {
+ return NULL;
+ } else {
+ cm->value_id = i % MAX_CONTROL_PARAM_ID;
+ if (i <= PARAM_ROLL) {
+
+ cm->id = ROLL_ID;
+ } else if (i <= PARAM_PITCH) {
+
+ cm->id = PITCH_ID;
+ } else if (i <= PARAM_YAW) {
+
+ cm->id = YAW_ID;
+ } else if (i <= PARAM_ROLL_RATE) {
+
+ cm->id = ROLL_RATE_ID;
+ } else if (i <= PARAM_PITCH_RATE) {
+
+ cm->id = PITCH_RATE_ID;
+ } else if (i <= PARAM_YAW_RATE) {
+
+ cm->id = YAW_RATE_ID;
+ } else if (i <= PARAM_LOCAL_X) {
+
+ cm->id = LOCAL_X_ID;
+ } else if (i <= PARAM_LOCAL_Y) {
+
+ cm->id = LOCAL_Y_ID;
+ } else if (i <= PARAM_ALT) {
+
+ cm->id = ALT_ID;
+ }
+ }
+ return cm;
+}
\ No newline at end of file
diff --git a/groundStation/src/backend/cmHandler.h b/groundStation/src/backend/cmHandler.h
index 136d154590c5b7416f254a5abb644ebf32cb51d2..ee51ca658206d9d7b0874c546d43f557ab0cdd7a 100644
--- a/groundStation/src/backend/cmHandler.h
+++ b/groundStation/src/backend/cmHandler.h
@@ -3,6 +3,52 @@
#include "controller.h"
+enum paramIndices {
+ PARAM_ROLL_P ,
+ PARAM_ROLL_I ,
+ PARAM_ROLL_D ,
+ PARAM_ROLL ,
+ PARAM_PITCH_P ,
+ PARAM_PITCH_I ,
+ PARAM_PITCH_D ,
+ PARAM_PITCH ,
+ PARAM_YAW_P ,
+ PARAM_YAW_I ,
+ PARAM_YAW_D ,
+ PARAM_YAW ,
+ PARAM_ROLL_RATE_P,
+ PARAM_ROLL_RATE_I,
+ PARAM_ROLL_RATE_D,
+ PARAM_ROLL_RATE,
+ PARAM_PITCH_RATE_P,
+ PARAM_PITCH_RATE_I,
+ PARAM_PITCH_RATE_D,
+ PARAM_PITCH_RATE,
+ PARAM_YAW_RATE_P,
+ PARAM_YAW_RATE_I,
+ PARAM_YAW_RATE_D,
+ PARAM_YAW_RATE,
+ PARAM_LOCAL_X_P ,
+ PARAM_LOCAL_X_I ,
+ PARAM_LOCAL_X_D ,
+ PARAM_LOCAL_X ,
+ PARAM_LOCAL_Y_P ,
+ PARAM_LOCAL_Y_I ,
+ PARAM_LOCAL_Y_D ,
+ PARAM_LOCAL_Y ,
+ PARAM_ALT_P,
+ PARAM_ALT_I,
+ PARAM_ALT_D,
+ PARAM_ALT ,
+ MAX_PARAM_COMMANDS
+};
+
+extern const char * respParamStrings[MAX_PARAM_COMMANDS];
+
+extern const char * setParamStrings[MAX_PARAM_COMMANDS];
+
const char * cmToString(int msgType, const struct controller_message *cm);
+struct controller_message * stringToCm(const char * string, struct controller_message *cm);
+
#endif /* _CMHANDLER_H */
\ No newline at end of file
diff --git a/groundStation/src/backend/communication.c b/groundStation/src/backend/communication.c
index 2a6a920e482ebf18d855656598d75fff2c5f7ebd..a6f14f29c36501907bee69b479b6541c2fe1fdf3 100644
--- a/groundStation/src/backend/communication.c
+++ b/groundStation/src/backend/communication.c
@@ -10,7 +10,7 @@ static int msgNum = 0;
// Formatting commands from ground station CLI
int formatCommand(char *command, unsigned char *formattedCommand) {
- //fprintf(stderr, "length = %li , received '%s'\n", strlen(command), command);
+ // fprintf(stderr, "length = %li , received '%s'\n", strlen(command), command);
char cmd[512];
strncpy(cmd, command, 512);
diff --git a/groundStation/src/backend/packet.c b/groundStation/src/backend/packet.c
index 81b4fff972bd17e5564913832ef7c95e77600a22..d0aafbf04424b89b15509cd5aec6314b73238f57 100644
--- a/groundStation/src/backend/packet.c
+++ b/groundStation/src/backend/packet.c
@@ -40,11 +40,10 @@ ssize_t EncodePacket(
packet[ID_H] = MSByte16(m->msg_id);
packet[DLEN_L] = LSByte16(m->data_len);
packet[DLEN_H] = MSByte16(m->data_len);
-
memcpy(&packet[HDR_SIZE], data, m->data_len);
packet[HDR_SIZE + m->data_len] = PacketChecksum(
- packet, HDR_SIZE + m->data_len);
+ packet, HDR_SIZE + m->data_len + CSUM_SIZE);
return m->data_len + HDR_SIZE + CSUM_SIZE;
}
@@ -82,7 +81,7 @@ ssize_t DecodePacket(
return -4;
}
- checkSum = PacketChecksum(packet, packet_size);
+ checkSum = PacketChecksum(packet, HDR_SIZE + m->data_len + CSUM_SIZE);
if (checkSum != packet[HDR_SIZE + m->data_len]) {
return -5;
}
diff --git a/groundStation/src/backend/response.c b/groundStation/src/backend/responseparam.c
similarity index 90%
rename from groundStation/src/backend/response.c
rename to groundStation/src/backend/responseparam.c
index c85f5e4017b9f2844bc8545c104e4a28dfc95c92..f6a8d44b268493b0501759327334ed58e5cb0685 100644
--- a/groundStation/src/backend/response.c
+++ b/groundStation/src/backend/responseparam.c
@@ -1,4 +1,4 @@
-#include "response.h"
+#include "responseparam.h"
#include "commands.h"
#include "bitwise.h"
@@ -17,13 +17,13 @@ enum ResponseData {
/* Creates data and metadata for a respcontrol packet
* Returns data size.
*/
-ssize_t EncodeResponse(
+ssize_t EncodeResponseParam(
struct metadata * m, /* data_len and msg_type will be populated*/
uint8_t * data, /* Output buffer */
size_t data_size, /* Max buffer size */
const struct controller_message * cm) /* Message to encode */
{
- m->msg_type = RESPONSE_ID;
+ m->msg_type = RESPPARAM_ID;
m->data_len = RESP_DATA_SIZE;
if (data_size < RESP_DATA_SIZE) {
@@ -43,7 +43,7 @@ ssize_t EncodeResponse(
/* Decode a metadata and data to populate a controller.
* Returns 0 on success, -1 on failure.
*/
-int DecodeResponse(
+int DecodeResponseParam(
struct controller_message * cm, /* Decoded controller message */
const struct metadata * m, /* Metadata to aid in decoding */
const uint8_t * data) /* Data to decode */
@@ -51,7 +51,7 @@ int DecodeResponse(
if (m->data_len < RESP_DATA_SIZE) {
return -1;
}
- if (m->msg_type != RESPONSE_ID) {
+ if (m->msg_type != RESPPARAM_ID) {
return -1;
}
diff --git a/groundStation/src/backend/response.h b/groundStation/src/backend/responseparam.h
similarity index 84%
rename from groundStation/src/backend/response.h
rename to groundStation/src/backend/responseparam.h
index 565458a8f11e9b5b592049f8bd65b2de523c7287..5e3224477b46d8384e6f10b30f07e5571d089cfa 100644
--- a/groundStation/src/backend/response.h
+++ b/groundStation/src/backend/responseparam.h
@@ -1,5 +1,5 @@
-#ifndef _response_h
-#define _response_h
+#ifndef _responseparam_h
+#define _responseparam_h
#include "packet.h"
#include "controller.h"
@@ -10,7 +10,7 @@
/* Creates data and metadata for a respcontrol packet.
* Returns data size.
*/
-ssize_t EncodeResponse(
+ssize_t EncodeResponseParam(
struct metadata *m, /* Out */
uint8_t *data, /* Out */
size_t data_size, /* Data buffer max size */
@@ -19,7 +19,7 @@ ssize_t EncodeResponse(
/* Decode a metadata and data to populate a message
* Returns 0 on success, -1 on failure
*/
-int DecodeResponse(
+int DecodeResponseParam(
struct controller_message *cm, /* Out */
const struct metadata *m, /* In */
const uint8_t * data); /* In */
diff --git a/groundStation/src/frontend/frontend_getpid.c b/groundStation/src/frontend/frontend_getpid.c
index 08827a528fdaec30b36910e5d91438100c12e4de..56188a2427199b1f96dce02245b367b715e635c2 100644
--- a/groundStation/src/frontend/frontend_getpid.c
+++ b/groundStation/src/frontend/frontend_getpid.c
@@ -62,22 +62,30 @@ int frontend_getpid(
}
char * response;
- for (;;) {
+ char tmpresponse[64];
+ char * cmdString;
+ size_t pendingResponses = 3;
+ float value;
+ while (pendingResponses) {
response = ucart_backendGetline(conn);
if (response == NULL) {
warnx("Line not returned from backend");
return 1;
}
- printf("received : %s\n", response);
- // if (strncmp(response, MAGIC, strlen(MAGIC)) == 0) {
- // break;
- // }
+ strncpy(tmpresponse, response, 64);
+ if (strncmp(tmpresponse, "get", 3) == 0) {
+ cmdString = strtok(tmpresponse, " ");
+ value = strtof(&response[strlen(cmdString)],NULL);
+
+ if(cmdString[strlen(cmdString)-1] == 'p') {
+ pid_data->p = value;
+ } else if(cmdString[strlen(cmdString)-1] == 'i') {
+ pid_data->i = value;
+ } else if(cmdString[strlen(cmdString)-1] == 'd') {
+ pid_data->d = value;
+ }
+ pendingResponses--;
+ }
}
-
- // if (strncmp(response, MAGIC " ERROR", strlen(MAGIC " ERROR")) == 0) {
- // warnx("Backend returned an error: %s", strstr(response, "ERROR"));
- // return 1;
- // }
-
return 0;
}