From 08a39b428873907418922ce67452df423dbf714f Mon Sep 17 00:00:00 2001
From: burneykb <burneykb@iastate.edu>
Date: Wed, 1 Feb 2017 22:56:20 -0600
Subject: [PATCH] Adding cmToString function
---
groundStation/src/backend/backend.c | 51 ++++++++++---
groundStation/src/backend/cmHandler.c | 98 ++++++++++++++++++++++++-
groundStation/src/backend/cmHandler.h | 3 +-
groundStation/src/backend/commands.c | 3 +-
groundStation/src/backend/respcontrol.c | 1 -
5 files changed, 139 insertions(+), 17 deletions(-)
diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index 3a686e8c1..a3dbaacc0 100644
--- a/groundStation/src/backend/backend.c
+++ b/groundStation/src/backend/backend.c
@@ -627,16 +627,14 @@ static void client_recv(int fd) {
}
static void quad_recv() {
- /* Check to see which command we are receiving. If it is one that needs to be passed on
- onto the clients, do so.
- */
-
static unsigned char respBuf[2048];
static size_t respBufLen;
struct metadata m;
- uint8_t data[256];
+ uint8_t data[1024];
size_t respLen;
+ size_t datalen;
+ size_t packetlen;
respLen = readQuad((char *) respBuf + respBufLen,
CMD_MAX_LENGTH- respBufLen);
@@ -646,22 +644,51 @@ static void quad_recv() {
}
respBufLen += respLen;
- if (DecodePacket(&m, data, 256, respBuf, respBufLen) < 0) {
+ if ((datalen = DecodePacket(&m, data, 1024, respBuf, respBufLen)) < 0) {
warnx("Packet format error");
respBufLen = 0;
return;
}
- memmove(respBuf, respBuf + datalen + 8, respBufLen - (datalen + 8));
- respBufLen -= PacketSize(&m);
+ packetlen = PacketSize(&m);
+ memmove(respBuf, respBuf + packetlen, respBufLen - packetlen);
+ respBufLen -= packetlen;
+
+/*
+ DEBUG_ID, // 00
+ PACKETLOG_ID, // 01
+ GETPACKETLOGS_ID, // 02
+ UPDATE_ID, // 03
+ BEGINUPDATE_ID, // 04
+ LOG_ID, // 05
+ RESPONSE_ID, // 06
+ SETCONTROL_ID, // 07 - Setting controller values. Example: PID constants
+ GETCONTROL_ID, // 08 - Getting controller values. Example: PID constants
+ RESPCONTROL_ID, // 09 - Responding with controller values. Example: PID constants
+ MAX_TYPE_ID
+*/
switch (m.msg_type) {
- case RESPCONTROL_ID:
- handleRespcontrol;
+ 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 */
log_write((char *) data, m.data_len);
break;
+ case RESPONSE_ID:
+ case SETCONTROL_ID:
+ case GETCONTROL_ID:
+ printf("(Backend): Command '%s' ignored\n", MessageTypes[m.msg_type].cmdText);
+ break;
+ case RESPCONTROL_ID:
+ handleRespcontrol(&m, data);
+ break;
+ default:
+ printf("(Backend): message type %d unrecognized\n", m.msg_type);
}
}
@@ -669,13 +696,13 @@ static void handleRespcontrol(struct metadata *m, uint8_t * data)
{
struct controller_message cm;
if (DecodeRespcontrol(&cm, m, data) < 0) {
- warnx("Respcontrol error")
+ warnx("DecodeRespcontrol error")
return;
}
char buffer[128];
- char * message = cmToString(RESPCONTROL_ID, cm.id, cm.value_id);
+ char * message = cmToString(RESPCONTROL_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 22bf01656..b69f2f876 100644
--- a/groundStation/src/backend/cmHandler.c
+++ b/groundStation/src/backend/cmHandler.c
@@ -1 +1,97 @@
-#include "cmHandler.h"
\ No newline at end of file
+#include "cmHandler.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 ,
+ GET_LONG_RATE,
+ MAX_COMMANDS
+};*/
+
+const char * respContStrings[MAX_COMMANDS] {
+ "getrollp",
+ "getrolli",
+ "getrolld",
+ "getroll",
+ "getpitchp",
+ "getpitchi",
+ "getpitchd",
+ "getpitch",
+ "getyawp",
+ "getyawi",
+ "getyawd",
+ "getyaw",
+ "getrollratep",
+ "getrollratei",
+ "getrollrated",
+ "getrollrate",
+ "getpitchratep",
+ "getpitchratei",
+ "getpitchrated",
+ "getpitchrate",
+ "getyawratep",
+ "getyawratei",
+ "getyawrated",
+ "getyawrate",
+ "getlatp",
+ "getlati",
+ "getlatd",
+ "getlat",
+ "getlongp",
+ "getlongi",
+ "getlongd",
+ "getlong",
+ "getheightp",
+ "getheighti",
+ "getheightd",
+ "getheight",
+}
+
+const char * cmToString(int msgType, const struct controller_message *cm)
+{
+ switch (msgType) {
+ case DEBUG_ID :
+ case PACKETLOG_ID :
+ case GETPACKETLOGS_ID :
+ case UPDATE_ID :
+ case BEGINUPDATE_ID :
+ case LOG_ID:
+ case RESPONSE_ID:
+ case SETCONTROL_ID:
+ case GETCONTROL_ID:
+ return NULL;
+ break;
+ case RESPCONTROL_ID:
+ return respContStrings[cm->id + cm->value_id];
+ break;
+ default:
+ return NULL;
+ break;
+ }
+}
diff --git a/groundStation/src/backend/cmHandler.h b/groundStation/src/backend/cmHandler.h
index a977c5bed..136d15459 100644
--- a/groundStation/src/backend/cmHandler.h
+++ b/groundStation/src/backend/cmHandler.h
@@ -1,7 +1,8 @@
#ifndef _CMHANDLER_H
#define _CMHANDLER_H
+#include "controller.h"
-
+const char * cmToString(int msgType, const struct controller_message *cm);
#endif /* _CMHANDLER_H */
\ No newline at end of file
diff --git a/groundStation/src/backend/commands.c b/groundStation/src/backend/commands.c
index 989eead44..ab89e699a 100644
--- a/groundStation/src/backend/commands.c
+++ b/groundStation/src/backend/commands.c
@@ -167,7 +167,6 @@ struct MessageType MessageTypes[MAX_TYPE_ID] =
};
-
int findCommand(char * str)
{
for (int i = 0; i < MAX_TYPE_ID; i++) {
@@ -176,4 +175,4 @@ int findCommand(char * str)
}
}
return -1;
-}
+}
\ No newline at end of file
diff --git a/groundStation/src/backend/respcontrol.c b/groundStation/src/backend/respcontrol.c
index 78c3f290c..34a2c7979 100644
--- a/groundStation/src/backend/respcontrol.c
+++ b/groundStation/src/backend/respcontrol.c
@@ -55,7 +55,6 @@ int DecodeRespcontrol(
return -1;
}
-
cm->id = data[CTRL_ID];
cm->value_id = data[CTRLVAL_ID];
cm->value = BytesToFloat(data[VAL_1], data[VAL_2],
--
GitLab