diff --git a/groundStation/Makefile b/groundStation/Makefile
index 9123a2455fe5743716377865f398de713d0cef6e..4830eee98672171476ef58077e717f82f8915b2e 100644
--- a/groundStation/Makefile
+++ b/groundStation/Makefile
@@ -3,7 +3,7 @@
# Generic Variables
GCC=gcc
GXX=g++
-CFLAGS= -Wall -Wpedantic -Wextra -Werror -std=c99 -g -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-but-set-variable
+CFLAGS= -Wall -Wpedantic -Wextra -Werror -std=gnu11 -g -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-but-set-variable
CXXFLAGS= -Wall -Wpedantic -Wextra -Werror -Wno-reorder -Wno-unused-variable -std=c++0x -g
INCLUDES = $(foreach dir, $(INCDIR), -I$(dir))
INCDIR=inc src/vrpn src/vrpn/quat src/vrpn/build $(BESRCDIR) $(CLISRCDIR) $(FESRCDIR)
diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index 6cb684e0bb3a9e97f4802ba449fef244e236f44a..0803aa9830d5cebbb9dce91823456d04d96fc5ab 100644
--- a/groundStation/src/backend/backend.c
+++ b/groundStation/src/backend/backend.c
@@ -40,6 +40,9 @@
#define CMD_MAX_LENGTH 1024
#define MAX_HASH_SIZE 50
+/* Backend-internal command magics */
+#define TD_MAGIC "TRACKERDATA"
+
// function prototypes
void readAndPrint(void);
void sendVrpnPacket(struct ucart_vrpn_TrackerData *);
@@ -267,6 +270,7 @@ int main(int argc, char **argv)
}
}
} else if (get_client_index(fd) > -1) {
+ /* It is a socket to a frontend */
client_recv(fd);
}
}
@@ -625,7 +629,37 @@ static void client_recv(int fd) {
printf("Client(%d) : '%s'\n",fd, buffer);
unsigned char * packet;
if(formatCommand(buffer, &packet) == -1) {
- printf("Could not recognize command '%s'\n", buffer);
+ /* buffer was not a quad command, handling internally to
+ * backend instead of forwarding to quad
+ */
+ if (strncmp(buffer, TD_MAGIC, strlen(TD_MAGIC)) == 0) {
+ /* Request for tracker data */
+ struct ucart_vrpn_TrackerData td;
+ if (ucart_vrpn_tracker_getData(tracker, &td)) {
+ write(fd, TD_MAGIC " ERROR\n", strlen(TD_MAGIC " ERROR\n"));
+ } else {
+ /* more than sufficient buffer */
+ char buffer[2048];
+ /* Map VRPN XYZ to Height Lat Long (right now it's
+ * a guess). Format is Height Lat Long P R Y */
+ if (snprintf(buffer,
+ 2048,
+ TD_MAGIC " %lf %lf %lf %lf %lf %lf\n",
+ td.x,
+ td.y,
+ td.z,
+ td.pitch,
+ td.roll,
+ td.yaw) >= 2048) {
+
+ /* Output longer than buffer */
+ warnx("Increase format buffer size, output was too long!");
+ write(fd, TD_MAGIC " ERROR\n", strlen(TD_MAGIC " ERROR\n"));
+ }
+
+ }
+ }
+
} else {
if(clientAddPendResponses(fd, packet) == -1) {
warnx("Ran out of room! Consider increasing CLIENT_MAX_PENDING_RESPONSES!");
diff --git a/groundStation/src/cli/cli_monitor.c b/groundStation/src/cli/cli_monitor.c
index 924cee52e249de0e6d9f3d72791f65d98daebdea..aef07810b0d1a4ace7862c97e74666a572dcd64f 100644
--- a/groundStation/src/cli/cli_monitor.c
+++ b/groundStation/src/cli/cli_monitor.c
@@ -5,8 +5,10 @@
#include <getopt.h>
#include <time.h>
#include <unistd.h>
+#include <err.h>
#include "cli_monitor.h"
+#include "frontend_tracker.h"
static void timespec_diff(struct timespec *start, struct timespec *result);
@@ -66,14 +68,29 @@ int cli_monitor(struct backend_conn * conn, int argc, char **argv) {
}
int monitor(struct backend_conn * conn) {
- static char * line = "monitor\n";
- printf("monitoring\n");
- int size;
- if((size = ucart_backendWrite(conn, line)) < 0 ) {
- return 1;
+ /* Get tracker data */
+ struct frontend_tracker_data td;
+ if (frontend_track(conn, &td)) {
+ errx(1, "Error reading tracker data");
}
- //TODO : HANDLE RETURN DATA
+ /* TODO: Get PID constants and status */
+
+ /* Print stuff on screen */
+ /* Assuming a tab width of 8 columns */
+ printf("\033[2J");
+ printf("STATUS: NA\n");
+ printf("CTRLR :\tP\tR\tY\tP_V\tR_V\tY_V\tH\tLAT\tLON\n");
+ printf(" P :\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\n",
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+ printf(" I :\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\n",
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+ printf(" D :\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\n",
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+ printf("PosAtt:\tH\tLAT\tLON\tP\tR\tY\n");
+ printf(" :\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\t%7lf\n",
+ td.height, td.lateral, td.longitudinal,
+ td.pitch, td.roll, td.yaw);
return 0;
}
diff --git a/groundStation/src/frontend/frontend_tracker.c b/groundStation/src/frontend/frontend_tracker.c
index 31ae0636350f9f86b67dac9f94a042a133c4fc48..b93cc6ce01b16aa9e347abbc2f085ac8b22a61dd 100644
--- a/groundStation/src/frontend/frontend_tracker.c
+++ b/groundStation/src/frontend/frontend_tracker.c
@@ -1,4 +1,42 @@
+#include <err.h>
+#include <string.h>
+#include <stdio.h>
+
#include "frontend_tracker.h"
-int frontend_track(struct backend_conn * conn,
- struct frontend_tracker_data * data);
+#define MAGIC "TRACKERDATA"
+
+int frontend_track(
+ struct backend_conn * conn,
+ struct frontend_tracker_data * data)
+{
+ ucart_backendWrite(conn, MAGIC "\n");
+
+ char * line;
+ for (;;) {
+ line = ucart_backendGetline(conn);
+ if (line == NULL) {
+ warnx("Line not returned from backend");
+ return 1;
+ }
+
+ if (strncmp(line, MAGIC, strlen(MAGIC)) == 0) {
+ break;
+ }
+ }
+
+ if (strncmp(line, MAGIC " ERROR", strlen(MAGIC " ERROR")) == 0) {
+ warn("Backend returned an error");
+ return 1;
+ }
+
+ /* Line format: Height Lat Long Pitch Roll Yaw */
+ sscanf(line, MAGIC " %lf %lf %lf %lf %lf %lf ",
+ &data->height,
+ &data->lateral,
+ &data->longitudinal,
+ &data->pitch,
+ &data->roll,
+ &data->yaw);
+ return 0;
+}