Implemented frontend tracker and monitor

Monitor should totally work, as should backend/frontend trackerdata updating

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)

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!");

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;
 }

groundStation/src/frontend/frontend_tracker.c

+#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;
+}