From 993c25bfcf8cc8d7b39bbe01910e59ad01553b14 Mon Sep 17 00:00:00 2001
From: Jake Drahos <j@kedrahos.com>
Date: Mon, 28 Nov 2016 12:30:44 -0600
Subject: [PATCH] Implemented frontend tracker and monitor
Monitor should totally work, as should backend/frontend trackerdata updating
---
groundStation/Makefile | 2 +-
groundStation/src/backend/backend.c | 36 +++++++++++++++-
groundStation/src/cli/cli_monitor.c | 29 ++++++++++---
groundStation/src/frontend/frontend_tracker.c | 42 ++++++++++++++++++-
4 files changed, 99 insertions(+), 10 deletions(-)
diff --git a/groundStation/Makefile b/groundStation/Makefile
index 9123a2455..4830eee98 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 6cb684e0b..0803aa983 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 924cee52e..aef07810b 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 31ae06363..b93cc6ce0 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;
+}
--
GitLab