Merge branch 'master' of git.ece.iastate.edu:danc/MicroCART_17-18

groundStation/Makefile

@@ -3,8 +3,8 @@
 # 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
-CXXFLAGS= -Wall -Wpedantic -Wextra -Werror -Wno-reorder -Wno-unused-variable -std=c++0x -g
+CFLAGS= -Wall -pedantic -Wextra -Werror -std=gnu99 -g -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-but-set-variable
+CXXFLAGS= -Wall -pedantic -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)
 LIBS= -lpthread -lbluetooth -lvrpn -lquat -Lsrc/vrpn/build -Lsrc/vrpn/build/quat 

groundStation/README.md

@@ -23,3 +23,41 @@ If you wish to change the way the backend communicates to the quad and vice vers
 
 ## Modifying
 See MODIFYING for the software architecture/organization and how to add new functionality.
+
+## Using
+First, the backend daemon must be running. Run the backend with ./BackEnd. Note
+the environment variables in config.h, especially the backend socket path. The backend
+requires root for bluetooth, but can run as a normal user with TCP, as long as the
+socket path is writable by the user.
+
+Once the backend is running, various CLI tools can be used to view the state of the
+quad and send commands. Each of these tools is given as the first argument
+to the CLI program, for example, to monitor the quad, use `cli monitor`. Note that
+the backend socket environment variable must be set to the same value as it
+was for the backend. The CLI program also supports busybox-style symbolic links.
+For example, if there is a symlink named `monitor` that points to the `cli` binary,
+running the `monitor` program will effectively run `cli monitor`.
+
+The names of the binaries is subject to change.
+
+### Example
+In one terminal or screen, run the backend:
+
+`UCART_SOCKET=./ucart.socket ./BackEnd`
+
+This will activate the quad and the backend, and the backend will be available for
+connections from the frontend tools. One useful tool is the monitor. In another
+terminal window, run the monitor forever:
+
+`UCART_SOCKET=./ucart.socket ./cli monitor -f`
+
+This will begin a periodic monitoring that updates 10 times per second.
+
+Finally, in a third window, export the socket path:
+
+`export UCART_SOCKET=./ucart.socket`
+
+and then run any other tools to modify the quad, for example modifying PID constants:
+
+`./cli setpid --pitch -p 1.000`
+

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,40 @@ 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 (tracker == NULL) {
+					char * dummy = TD_MAGIC " 1.0 2.0 3.0 4.0 5.0 6.0\n";
+					write(fd, dummy, strlen(dummy));
+				}else 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.z,
+							td.y,
+							td.x,
+							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"));
+					}
+					write(fd, buffer, strlen(buffer));
+				}
+			}
+
 		} else {
 			if(clientAddPendResponses(fd, packet) == -1) {
 				warnx("Ran out of room! Consider increasing CLIENT_MAX_PENDING_RESPONSES!");

groundStation/src/cli/cli.h

@@ -2,29 +2,29 @@
 #define __CLI_H
 
 #include "cli_monitor.h"
-// #include "cli_setpid.h"
+#include "cli_setpid.h"
 #include "cli_getpid.h"
 // #include "cli_getimu.h"
 enum CommandNameIds{
 	CMD_MONITOR,
-	CMD_SETPID,
 	CMD_GETPID,
+	CMD_SETPID,
 	CMD_GETIMU,
 	MAX_COMMANDS
 };
 
 typedef int (*cli_function_ptr)(struct backend_conn *, int, char **);
-static cli_function_ptr cli_functions[2] = {
+static cli_function_ptr cli_functions[] = {
 	&cli_monitor,
-	&cli_getpid
-	// &cli_setpid,
-	// &cli_getimu
+	&cli_getpid,
+	&cli_setpid
+	//&cli_getimu
 };
 
 static char* commandNames[MAX_COMMANDS] = {
 	"monitor",
-	"getPid",
-	"setPid",
+	"getpid",
+	"setpid",
 	"getImu"
 };
 

groundStation/src/cli/cli_getpid.c

@@ -7,12 +7,20 @@
 int cli_getpid(struct backend_conn * conn,	int argc, char **argv) {
 	int c;
 	static int getRoll = 0, getPitch = 0, getYaw = 0, getAll = 0;
+	static int getRollV = 0, getPitchV = 0, getYawV = 0;
+	static int getHeight = 0, getLat = 0, getLong = 0;
 	struct frontend_pid_data pid_data;
 	static struct option long_options[] = {
  		/* These options don’t set a flag. We distinguish them by their indices. */
  		{"roll",	no_argument,	&getRoll,	1},
  		{"pitch",   no_argument,   	&getPitch, 	1},
  		{"yaw",   no_argument,   	&getYaw, 	1},
+ 		{"rollv",   no_argument,   	&getYawV, 	1},
+ 		{"pitchv",   no_argument,   	&getYawV, 	1},
+ 		{"yawv",   no_argument,   	&getYawV, 	1},
+ 		{"height",   no_argument,   	&getHeight,	1},
+ 		{"lat",   no_argument,   	&getLat, 	1},
+ 		{"long",   no_argument,   	&getLong, 	1},
  		{0, 0, 0, 0}
  	};
 

groundStation/src/cli/cli_monitor.c

@@ -5,49 +5,62 @@
 #include <getopt.h>
 #include <time.h>
 #include <unistd.h>
+#include <err.h>
 
 #include "cli_monitor.h"
-
-static void timespec_diff(struct timespec *start,  struct timespec *result);
+#include "frontend_tracker.h"
 
 int cli_monitor(struct backend_conn * conn,	int argc, char **argv) {
 	int c, result;
-	int timeFlag = 0;
+	int countFlag = 0;
+
+	int count, rate = 10;
+	int forever = 0;
 
-	static struct timespec startTime;
-	static struct timespec elapsedTime;
-	static struct timespec lastChecked;
-	static int nsecs, rate = 10;
 
- 	while ((c = getopt(argc, argv, "t:r:")) != -1) {
+ 	while ((c = getopt(argc, argv, "fc:r:")) != -1) {
 		switch(c) {
-			case 't' :
-				nsecs = atoi(optarg);
-				timeFlag = 1;
+			case 'c' :
+				count = atoi(optarg);
+				countFlag = 1;
 				break;
 			case 'r' :
 				rate = atoi(optarg) + 1;
 				break;
+			case 'f' :
+				forever = 1;
+				break;
 			default :
 				break;
 		}
 	}
 
-	if(timeFlag) {
-		clock_gettime(CLOCK_REALTIME, &startTime);
-		clock_gettime(CLOCK_REALTIME, &lastChecked);
-		while((result = monitor(conn)) == 0) {
-			timespec_diff(&startTime, &elapsedTime);
-			if(elapsedTime.tv_sec > nsecs) {
-				break;
+	if (forever) {
+		for (;;) {
+			struct timespec req;
+			if (rate == 1) {
+				req.tv_sec = 1;
+				req.tv_nsec = 0;
+			} else { 
+				req.tv_sec = 0;
+				req.tv_nsec = 1000000000 / rate;
 			}
-			while(1) {
-				timespec_diff(&lastChecked, &elapsedTime);
-				if(elapsedTime.tv_nsec >= (long) ((SECOND_IN_NANO * 2) / rate)) {
-					clock_gettime(CLOCK_REALTIME, &lastChecked);
-					break;
-				}
+			nanosleep(&req, NULL);
+			monitor(conn);
+		}
+	} else if (countFlag) {
+		for (int i = 0; i < count; i++) {
+			result = monitor(conn);
+
+			struct timespec req;
+			if (rate == 1) {
+				req.tv_sec = 1;
+				req.tv_nsec = 0;
+			} else { 
+				req.tv_sec = 0;
+				req.tv_nsec = 1000000000 / rate;
 			}
+			nanosleep(&req, NULL);
 		}
 	} else {
 		return monitor(conn);
@@ -57,29 +70,33 @@ 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 */
+	/* It might be a good idea to only read the pid constants
+	 * every few seconds, so count iterations and only do it if 
+	 * this is every (rate * 2 or 3) pass through the loop
+	 */
+
+	/* 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%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
+			0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+	printf("  I   :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
+			0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+	printf("  D   :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\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%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
+			td.height, td.lateral, td.longitudinal,
+			td.pitch, td.roll, td.yaw);
 
 	return 0;
 }
-
-static void timespec_diff(struct timespec *start, struct timespec *result)
-{
-	struct timespec stop;
-	clock_gettime(CLOCK_REALTIME, &stop);
-    if ((stop.tv_nsec - start->tv_nsec) < 0) {
-        result->tv_sec = stop.tv_sec - start->tv_sec - 1;
-        result->tv_nsec = stop.tv_nsec - start->tv_nsec + 1000000000;
-    } else {
-        result->tv_sec = stop.tv_sec - start->tv_sec;
-        result->tv_nsec = stop.tv_nsec - start->tv_nsec;
-    }
-
-    return;
-}
\ No newline at end of file

groundStation/src/cli/cli_setpid.h

+#ifndef _CLI_SETPID_H
+#define _CLI_SETPID_H
+
+#include "frontend_setpid.h"
+
+int cli_setpid(struct backend_conn * conn, int argc, char ** argv);
+
+#endif

groundStation/src/frontend/frontend_setpid.c

+#include <err.h>
+#include <stdio.h>
+
+#include "frontend_setpid.h"
+#include "pid_common.h"
+#include "frontend_common.h"
+
+int frontend_setpid(
+		struct backend_conn * conn,
+		struct frontend_pid_data * pid_data,
+		int mask)
+{
+	if (conn == NULL) {
+		return -1;
+	}
+
+	char * controller;
+	switch (pid_data->controller) {
+		case PID_PITCH:
+			controller = "pitch";
+			break;
+		case PID_ROLL:
+			controller = "roll";
+			break;
+		case PID_YAW:
+			controller = "yaw";
+			break;
+			/* What is the "throttle" pid constant? */
+		default:
+			warnx("Unsupported PID constant");
+			return -1;
+	}
+
+	char buffer[2048];
+	/* Set the P, I, and D */
+	if (mask & SET_P) {
+		if (snprintf(buffer, 2048,
+					"set%sp %f\n", 
+					controller, 
+					pid_data->p) >= 2048) {
+			errx(0, "Buffer to small to format!");
+		}
+		ucart_backendWrite(conn, buffer);
+	}
+	if (mask & SET_I) {
+		if (snprintf(buffer, 2048,
+					"set%si %f\n", 
+					controller, 
+					pid_data->i) >= 2048) {
+			errx(0, "Buffer to small to format!");
+		}
+		ucart_backendWrite(conn, buffer);
+	}
+	if (mask & SET_D) {
+		if (snprintf(buffer, 2048,
+					"set%sd %f\n", 
+					controller, 
+					pid_data->d) >= 2048) {
+			errx(0, "Buffer to small to format!");
+		}
+		ucart_backendWrite(conn, buffer);
+	}
+
+	return 0;
+}

groundStation/src/frontend/frontend_setpid.h

+#ifndef _FRONTEND_SETPID_H
+#define _FRONTEND_SETPID_H
+
+#include "pid_common.h"
+#include "frontend_common.h"
+
+int frontend_setpid(
+		struct backend_conn * conn,
+		struct frontend_pid_data * pid_data,
+		int mask);
+
+#define SET_P 0x01
+#define SET_I 0x02
+#define SET_D 0x04
+#define SET_ALL (SET_P | SET_I | SET_D)
+
+#endif

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) {
+		warnx("Backend returned an error: %s", strstr(line, "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;
+}