groundStation/src/cli/cli_getpid.c

+#include <stdio.h>
+#include <unistd.h>
+#include <getopt.h>
+
+#include "cli_getpid.h"
+
+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;
+	static int needHelp = 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,   	&getRollV, 	1},
+ 		{"pitchv",   no_argument,   &getPitchV, 1},
+ 		{"yawv",   no_argument,   	&getYawV, 	1},
+ 		{"height",   no_argument,   &getHeight,	1},
+ 		{"lat",   no_argument,   	&getLat, 	1},
+ 		{"long",   no_argument,   	&getLong, 	1},
+ 		{"help",	no_argument,	&needHelp,	1},
+ 		{0, 0, 0, 0}
+ 	};
+
+ 	while (1)
+	{
+		/* getopt_long stores the option index here. */
+		int option_index = 0;
+
+		c = getopt_long(argc, argv, "a", long_options, &option_index);
+
+		/* Detect the end of the options. */
+		if (c == -1)
+			break;
+
+		if (c == 'a') {
+			getAll = 1;
+		}
+	}
+
+	if (needHelp) {
+		printf("Getpid gets the p, i , and d constant values of any single controller\n");
+		printf("Usage Syntax : \n\t./Cli getpid controller [options...]\n");
+		printf("Symlink Usage Syntax : \n\t./getpid controller [options...]\n\n");
+		printf("Available 'controllers' include the following\n");
+		printf("\t[--pitch] : Pitch\n\t[--roll] : Roll\n\t[--yaw] : Yaw\n");
+		printf("\t[--pitchv] : Pitch Velocity\n\t[--rollv] : Roll Velocity\n\t[--yawv] : Yaw Velocity\n");
+		printf("\t[--height] : Z\n\t[--lat] : X\n\t[--long] : Y\n\n");
+		return 0;
+	}
+
+	if (argc < 2) {
+		printf("Incorrect Usage, run './cli getpid --help' for correct usage.\n");
+		return 1;
+	}
+	
+	int result;
+	if(getAll) {
+		for(int i = 0; i < PID_NUM_PIDS; ++i) {
+			pid_data.controller = i;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+	} else {
+		if(getPitch) {
+			pid_data.controller = PID_PITCH;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getRoll) {
+			pid_data.controller = PID_ROLL;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getYaw) {
+			pid_data.controller = PID_YAW;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getPitchV) {
+			pid_data.controller = PID_PITCH_RATE;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getRollV) {
+			pid_data.controller = PID_ROLL_RATE;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getYawV) {
+			pid_data.controller = PID_YAW_RATE;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getHeight) {
+			pid_data.controller = PID_HEIGHT;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getLat) {
+			pid_data.controller = PID_LAT;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+		if(getLong) {
+			pid_data.controller = PID_LONG;
+			if ((result = getValues(conn, &pid_data))) {
+				return result;
+			}
+		}
+
+	}
+
+	return 0;
+}
+
+int getValues(struct backend_conn * conn, struct frontend_pid_data * pid_data) {
+	if(frontend_getpid(conn, pid_data)) {
+		return 1;
+	}
+
+	switch(pid_data->controller) {
+		case PID_PITCH :
+			printf("Pitch Constants: P = %f\tI = %f\tD = %f\n",
+				pid_data->p, pid_data->i, pid_data->d);
+			break;
+		case PID_ROLL :
+			printf("Roll Constants: P = %f\tI = %f\tD = %f\n",
+				pid_data->p, pid_data->i, pid_data->d);
+			break;
+		case PID_YAW :
+			printf("Yaw Constants: P = %f\tI = %f\tD = %f\n",
+				pid_data->p, pid_data->i, pid_data->d);
+			break;
+		case PID_PITCH_RATE :
+			printf("Pitch Rate Constants: P = %f\tI = %f\tD = %f\n",
+				pid_data->p, pid_data->i, pid_data->d);
+			break;
+		case PID_ROLL_RATE :
+			printf("Roll Rate Constants: P = %f\tI = %f\tD = %f\n",
+				pid_data->p, pid_data->i, pid_data->d);
+			break;
+		case PID_YAW_RATE :
+			printf("Yaw Rate Constants: P = %f\tI = %f\tD = %f\n",
+				pid_data->p, pid_data->i, pid_data->d);
+			break;
+		case PID_HEIGHT :
+			printf("Height Constants: P = %f\tI = %f\tD = %f\n",
+				pid_data->p, pid_data->i, pid_data->d);
+			break;
+		case PID_LAT :
+			printf("Latitude Constants: P = %f\tI = %f\tD = %f\n",
+				pid_data->p, pid_data->i, pid_data->d);
+			break;
+		case PID_LONG :
+			printf("Longitude Constants: P = %f\tI = %f\tD = %f\n",
+				pid_data->p, pid_data->i, pid_data->d);
+			break;
+		default :
+			break;
+	}
+	return 0;
+}

groundStation/src/cli/cli_getpid.h

+#ifndef CLI_GETPID_H
+#define CLI_GETPID_H
+
+#include "frontend_getpid.h"
+
+int getValues(struct backend_conn *, struct frontend_pid_data *);
+int cli_getpid(struct backend_conn * conn, int argc, char ** argv);
+
+#endif
\ No newline at end of file

groundStation/src/cli/cli_monitor.c

+#define _GNU_SOURCE
+
+#include <stdio.h>
+#include <unistd.h>
+#include <getopt.h>
+#include <time.h>
+#include <unistd.h>
+#include <err.h>
+
+#include "cli_monitor.h"
+#include "frontend_tracker.h"
+
+int cli_monitor(struct backend_conn * conn,	int argc, char **argv) {
+	int c, result;
+	int countFlag = 0;
+	int count, rate = 10;
+	int forever = 0;
+	static int needHelp = 0;
+
+	static struct option long_options[] = {
+ 		/* These options don’t set a flag. We distinguish them by their indices. */
+ 		{"help",	no_argument,	&needHelp,	1},
+ 		{0, 0, 0, 0}
+ 	};
+
+ 	while (1)
+	{
+		/* getopt_long stores the option index here. */
+		int option_index = 0;
+		
+		// If you change this 		  VVV 		please also update the help message 
+		c = getopt_long(argc, argv, "fc:r:", long_options, &option_index);
+
+		if (c == -1)
+			break;
+
+ 		switch(c) {
+			case 'c' :
+				count = atoi(optarg);
+				countFlag = 1;
+				break;
+			case 'r' :
+				rate = atoi(optarg) + 1;
+				break;
+			case 'f' :
+				forever = 1;
+				break;
+			default :
+				break;
+		}
+	}
+	
+	if (needHelp) {
+		printf("Monitor provides real time information about the quad. Including positional data as well as controller constants\n\n");
+		printf("Usage Syntax : \n\t./Cli monitor [options...]\n");
+		printf("Symlink Usage Syntax : \n\t./monitor [options...]\n\n");
+		printf("Available options include the following\n");
+		printf("\t[-f] : Run monitor continuously until you kill the program. (ctrl-C)\n");
+		printf("\t[-c] 'count' : Sets the number of times the monitor will refresh\n");
+		printf("\t[-r] 'rate' : Sets the 'rate' at which the monitor will refresh per second\n");
+		return 0;
+	}
+
+	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;
+			}
+			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);
+	}
+
+	return result;
+}
+
+int monitor(struct backend_conn * conn) {
+	/* Get tracker data */
+	struct frontend_tracker_data td;
+	if (frontend_track(conn, &td)) {
+		errx(1, "Error reading tracker 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;
+}

groundStation/src/cli/cli_monitor.h

+#ifndef CLI_MONITOR_H
+#define CLI_MONITOR_H
+
+#include <time.h>
+
+#include "frontend_getpid.h"
+
+#define SECOND_IN_NANO 1000000000
+
+int cli_monitor(struct backend_conn * conn,	int argc, char **argv);
+
+// Return 0 on success and 1 otherwise
+int monitor(struct backend_conn * conn);
+#endif
\ No newline at end of file

groundStation/src/cli/cli_setpid.c

+#include <stdio.h>
+#include <unistd.h>
+#include <getopt.h>
+
+#include "cli_setpid.h"
+#include "frontend_setpid.h"
+
+int cli_setpid(struct backend_conn * conn,	int argc, char **argv) {
+	int c;
+	static int setRoll = 0, setPitch = 0, setYaw = 0, setAll = 0;
+	static int setRollV = 0, setPitchV = 0, setYawV = 0;
+	static int setHeight = 0, setLat = 0, setLong = 0;
+	struct frontend_pid_data pid_data;
+	static int mask;
+	static float pval = 0, ival = 0, dval = 0;
+	static int needHelp = 0;
+	static struct option long_options[] = {
+ 		/* These options don’t set a flag. We distinguish them by their indices. */
+ 		{"roll",	no_argument,	&setRoll,	1},
+ 		{"pitch",   no_argument,   	&setPitch, 	1},
+ 		{"yaw",   no_argument,   	&setYaw, 	1},
+ 		{"rollv",   no_argument,   	&setRollV, 	1},
+ 		{"pitchv",   no_argument,   &setPitchV, 	1},
+ 		{"yawv",   no_argument,   	&setYawV, 	1},
+ 		{"height",   no_argument,   &setHeight,	1},
+ 		{"lat",   no_argument,   	&setLat, 	1},
+ 		{"long",   no_argument,   	&setLong, 	1},
+ 		{"help",	no_argument,	&needHelp,	1},
+ 		{0, 0, 0, 0}
+ 	};
+
+ 	while (1)
+	{
+		/* getopt_long stores the option index here. */
+		int option_index = 0;
+
+		c = getopt_long(argc, argv, "p:i:d:", long_options, &option_index);
+
+		/* Detect the end of the options. */
+		if (c == -1)
+			break;
+
+		switch(c) {
+			case 'p' :
+				pid_data.p  = atof(optarg);
+				mask |= SET_P;
+				break;
+			case 'i' :
+				pid_data.i  = atof(optarg);
+				mask |= SET_I;
+				break;
+			case 'd' :
+				pid_data.d  = atof(optarg);
+				mask |= SET_D;
+				break;
+			default :
+				break;
+		}
+	}
+
+	if (needHelp) {
+		printf("Setpid sets the p, i , or d constant values of any single controller\n");
+		printf("Usage Syntax : \n\t./Cli setpid controller [options...]\n");
+		printf("Symlink Usage Syntax : \n\t./setpid controller [options...]\n\n");
+		printf("Available 'controllers' include the following\n");
+		printf("\t[--pitch] : Pitch\n\t[--roll] : Roll\n\t[--yaw] : Yaw\n");
+		printf("\t[--pitchv] : Pitch Velocity\n\t[--rollv] : Roll Velocity\n\t[--yawv] : Yaw Velocity\n");
+		printf("\t[--height] : Z\n\t[--lat] : X\n\t[--long] : Y\n\n");
+		printf("Available 'controller' options include the following\n");
+		printf("\t[-p] 'val' : Sets the p constant of the 'controller' to 'val'\n");
+		printf("\t[-i] 'val' : Sets the i constant of the 'controller' to 'val'\n");
+		printf("\t[-d] 'val' : Sets the d constant of the 'controller' to 'val'\n");
+		return 0;
+	}
+
+	if (argc < 2) {
+		printf("Incorrect Usage, run './cli setpid --help' for correct usage.\n");
+		return 1;
+	}
+
+	if (setRoll) {
+		pid_data.controller = PID_ROLL;
+	} else if (setYaw) {
+		pid_data.controller = PID_YAW;
+	} else if (setPitch) {
+		pid_data.controller = PID_PITCH;
+	} else if (setRollV) {
+		pid_data.controller = PID_ROLL_RATE;
+	} else if (setPitchV) {
+		pid_data.controller = PID_PITCH_RATE;
+	} else if (setYawV) {
+		pid_data.controller = PID_YAW_RATE;
+	} else if (setHeight) {
+		pid_data.controller = PID_HEIGHT;
+	} else if (setLong) {
+		pid_data.controller = PID_LAT;
+	} else if (setLat) {
+		pid_data.controller = PID_LONG;
+	}
+
+	frontend_setpid(conn, &pid_data, mask);
+	return 0;
+}

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/cli/cli_setsetpoint.c

+#include <stdio.h>
+#include <unistd.h>
+#include <getopt.h>
+
+#include "cli_setsetpoint.h"
+
+int cli_set(struct backend_conn * conn,	int argc, char **argv) {
+	int c;
+	static int setheight = 0, setlat = 0, setlong = 0;
+	struct frontend_setpoint_data setpoint_data;
+	static int needHelp = 0;
+	static int mask;
+	static struct option long_options[] = {
+ 		/* These options don’t set a flag. We distinguish them by their indices. */
+ 		{"help",	no_argument,	&needHelp,	1},
+ 		{"height",  required_argument,	0,  'h'},
+ 		{"long",	required_argument,	0,	't'},
+ 		{"lat",		required_argument, 	0,	'l'},
+ 		{0, 0, 0, 0}
+ 	};
+
+ 	while (1)
+	{
+		/* getopt_long stores the option index here. */
+		int option_index = 0;
+
+		c = getopt_long(argc, argv, "", long_options, &option_index);
+
+		/* Detect the end of the options. */
+		if (c == -1)
+			break;
+
+		switch(c) {
+			case 'h' :
+				setpoint_data.height  = atof(optarg);
+				mask |= SET_HEIGHT;
+				break;
+			case 't' :
+				setpoint_data.lat  = atof(optarg);
+				mask |= SET_LAT;
+				break;
+			case 'l' :
+				setpoint_data.longg  = atof(optarg);
+				mask |= SET_LONG;
+				break;
+			default :
+				break;
+		}
+
+	}
+
+	if (needHelp) {
+		printf("Setsetpoint sets the x, y , or z set point values for the quad.\n");
+		printf("Usage Syntax : \n\t./Cli setsetpoint [options...]\n");
+		printf("Symlink Usage Syntax : \n\t./setsetpoint [options...]\n\n");
+		printf("Available options include the following\n");
+		printf("\t[-x] 'val' : Sets the x setpoint value to 'val'\n");
+		printf("\t[-z] 'val' : Sets the y setpoint value to 'val'\n");
+		printf("\t[-z] 'val' : Sets the z setpoint value to 'val'\n");
+		return 0;
+	}
+
+	if (argc < 2) {
+		printf("Incorrect Usage, run './cli setpid --help' for correct usage.\n");
+		return 1;
+	}
+	
+	frontend_setsetpoint(conn, &setpoint_data, mask);
+	return 0;
+}

groundStation/src/cli/cli_setsetpoint.h

+#ifndef CLI_SETSETPOINT_H
+#define CLI_SETSETPOINT_H
+
+#include "frontend_setsetpoint.h"
+
+int cli_setsetpoint(struct backend_conn * conn, int argc, char ** argv);
+
+#endif /* CLI_SETSETPOINT_H */

groundStation/src/frontend/frontend_common.c

+#define _GNU_SOURCE
+
+#include "frontend_common.h"
+#include "config.h"
+
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+#include <unistd.h>
+#include <err.h>
+
+struct backend_conn {
+	FILE * socket;
+
+	size_t len;
+	char * buf;
+};
+
+struct backend_conn * ucart_backendConnect()
+{
+	int s;
+	struct sockaddr_un remote;
+	char str[100];
+
+	if ((s = socket(AF_UNIX, SOCK_STREAM, 0)) == -1) {
+		perror("socket");
+		exit(1);
+	}
+
+	struct backend_conn * conn = NULL;
+
+	printf("Trying to connect...\n");
+
+	remote.sun_family = AF_UNIX;
+	char * sock_env = getenv(SOCKET_ENV);
+	strcpy(remote.sun_path, sock_env ? sock_env : DEFAULT_SOCKET);
+	if (connect(s, (struct sockaddr *)&remote, sizeof(remote))  == -1) {
+		perror("connect");
+		goto fail_final;
+	}
+
+	conn = malloc(sizeof(struct backend_conn));
+	if (conn == NULL) {
+		perror("malloc");
+		goto fail_sock;
+	}
+
+	conn->len = 0;
+	conn->buf = NULL;
+	conn->socket = fdopen(s, "r+");
+	if (conn->socket == NULL) {
+		perror("fdopen");
+		goto fail_malloc_conn;
+	}
+	if (setvbuf(conn->socket, NULL, _IONBF, 0)) {
+		warn("setvbuf");
+	}
+
+	/* success */
+	goto fail_final;
+
+fail_malloc_conn:
+	free(conn);
+	conn = NULL;
+fail_sock:
+	close(s);
+fail_final:
+	return conn;
+}
+
+void ucart_backendDisconnect(struct backend_conn * conn)
+{
+	fclose(conn->socket);
+	if (conn->buf) {
+		free(conn->buf);
+	}
+	free(conn);
+}
+
+char * ucart_backendGetline(struct backend_conn *conn)
+{
+	getline(&conn->buf, &conn->len, conn->socket);
+	return conn->buf;
+}
+
+int ucart_backendWrite(struct backend_conn *conn, const char * line)
+{
+	return fputs(line, conn->socket);
+}

groundStation/src/frontend/frontend_common.h

+#ifndef FRONTEND_COMMON_H
+#define FRONTEND_COMMON_H
+#include <stdlib.h>
+
+struct backend_conn;
+
+/* Start connection to quad */
+struct backend_conn * ucart_backendConnect(void);
+
+/* Stop connection to quad */
+void ucart_backendDisconnect(struct backend_conn * conn);
+
+/* Get a line from the backend.
+ *
+ * The line will remain valid until the next call to ucart_backendGetline.
+ */
+char * ucart_backendGetline(struct backend_conn * conn);
+
+/* Write a line to the backend */
+int ucart_backendWrite(struct backend_conn * backend, const char * line);
+
+#endif

groundStation/src/frontend/frontend_getimu.h

+#ifndef FRONTEND_GETIMU_H
+#define FRONTEND_GETIMU_H
+
+#include "frontend_common.h"
+
+#endif
\ No newline at end of file

groundStation/src/frontend/frontend_getpid.c

+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include "frontend_getpid.h"
+#include "pid_common.h"
+
+/* Get a specified PID.
+ *
+ * Example:
+ *
+ * struct frontend_pid_data pid_data;
+ * pid_data.pid = PID_PITCH;
+ * if (frontend_getpid(conn, &pid_data)) {
+ * 		error
+ * } else {
+ * 		pid_data.p, pid_data.i, and pid_data.d are filled
+ * }
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_getpid(
+		struct backend_conn * conn, struct frontend_pid_data * pid_data) {
+	
+	char line[100] = "";
+	switch (pid_data->controller) {
+		case PID_PITCH :
+			strncpy(line, "getpitchp\ngetpitchi\ngetpitchd\n", 30);
+			break;
+		case PID_ROLL :
+			strncpy(line, "getrollp\ngetrolli\ngetrolld\n", 27);
+			break;
+		case PID_YAW :
+			strncpy(line, "getyawp\ngetyawi\ngetyawd\n", 24);
+			break;
+		case PID_PITCH_RATE :
+			strncpy(line, "getpitchratep\ngetpitchratei\ngetpitchrated\n", 33);
+			break;
+		case PID_ROLL_RATE :
+			strncpy(line, "getrollratep\ngetrollratei\ngetrollrated\n", 30);
+			break;
+		case PID_YAW_RATE :
+			strncpy(line, "getyawratep\ngetyawratei\ngetyawrated\n", 27);
+			break;
+		case PID_HEIGHT :
+			strncpy(line, "getheightp\ngetheighti\ngetheightd\n", 33);
+			break;
+		case PID_LAT :
+			strncpy(line, "getlatp\ngetlati\ngetlatd\n", 24);
+			break;
+		case PID_LONG :
+			strncpy(line, "getlongp\ngetlongi\ngetlongd\n", 27);
+			break;
+		default :
+			return 1;
+	}
+
+	int size;
+	if((size = ucart_backendWrite(conn, line)) < 0 ) {
+		return 1;
+	}
+
+	return 0;
+}	

groundStation/src/frontend/frontend_getpid.h

+#ifndef FRONTEND_GETPID_H
+#define FRONTEND_GETPID_H
+
+#include "frontend_common.h"
+#include "pid_common.h"
+
+/* Get a specified PID.
+ *
+ * Example:
+ *
+ * struct frontend_pid_data pid_data;
+ * pid_data.pid = PITCH;
+ * if (frontend_getpid(conn, &pid_data)) {
+ * 		error
+ * } else {
+ * 		pid_data.p, pid_data.i, and pid_data.d are filled
+ * }
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_getpid(
+		struct backend_conn * conn,
+		struct frontend_pid_data * pid_data);
+
+
+#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;
+		case PID_PITCH_RATE:
+			controller = "pitchrate";
+			break;
+		case PID_ROLL_RATE:
+			controller = "rollrate";
+			break;
+		case PID_YAW_RATE:
+			controller = "yawrate";
+			break;
+		case PID_HEIGHT:
+			controller = "height";
+			break;
+		case PID_LAT:
+			controller = "lat";
+			break;
+		case PID_LONG:
+			controller = "long";
+			break;
+		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
+
+#ifndef SET_ALL
+#define SET_ALL (SET_P | SET_I | SET_D)
+#endif /* SET_ALL */
+
+#endif /* FRONTEND_SETPID_H */
\ No newline at end of file

groundStation/src/frontend/frontend_setsetpoint.c

+#include <err.h>
+#include <stdio.h>
+
+#include "frontend_setsetpoint.h"
+#include "setpoint_common.h"
+#include "frontend_common.h"
+
+int frontend_setsetpoint(
+		struct backend_conn * conn,
+		struct frontend_setpoint_data * setpoint_data,
+		int mask)
+{
+	if (conn == NULL) {
+		return -1;
+	}
+
+	char buffer[2048];
+	/* Set the P, I, and D */
+	if (mask & SET_HEIGHT) {
+		if (snprintf(buffer, 2048,
+					"setheight %f\n",  
+					setpoint_data->height) >= 2048) {
+			errx(0, "Buffer to small to format!");
+		}
+		ucart_backendWrite(conn, buffer);
+	}
+	if (mask & SET_LAT) {
+		if (snprintf(buffer, 2048,
+					"setlat %f\n",
+					setpoint_data->lat) >= 2048) {
+			errx(0, "Buffer to small to format!");
+		}
+		ucart_backendWrite(conn, buffer);
+	}
+	if (mask & SET_LONG) {
+		if (snprintf(buffer, 2048,
+					"setlong %f\n",
+					setpoint_data->longg) >= 2048) {
+			errx(0, "Buffer to small to format!");
+		}
+		ucart_backendWrite(conn, buffer);
+	}
+
+	return 0;
+}

groundStation/src/frontend/frontend_setsetpoint.h

+#ifndef FRONTEND_SETSETPOINT_H
+#define FRONTEND_SETSETPOINT_H
+
+#include "setpoint_common.h"
+#include "frontend_common.h"
+
+int frontend_setsetpoint(
+		struct backend_conn * conn,
+		struct frontend_setpoint_data * setpoint_data,
+		int mask);
+
+#define SET_LAT 0x01
+#define SET_LONG 0x02
+#define SET_HEIGHT 0x04
+
+#ifndef SET_ALL
+#define SET_ALL (SET_LAT | SET_LONG | SET_HEIGHT)
+#endif /* SET_ALL */
+
+#endif /* FRONTEND_SETSETPOINT_H */
\ No newline at end of file

groundStation/src/frontend/frontend_tracker.c

+#include <err.h>
+#include <string.h>
+#include <stdio.h>
+
+#include "frontend_tracker.h"
+
+#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;
+}

groundStation/src/frontend/frontend_tracker.h

+#ifndef _FRONTEND_TRACKER_H
+#define _FRONTEND_TRACKER_H
+
+#include "frontend_common.h"
+
+/* Struct containing pos/att data */
+struct frontend_tracker_data {
+		double height;
+		double lateral;
+		double longitudinal;
+		double pitch;
+		double roll;
+		double yaw;
+};
+
+/* Get pos/att data from the tracking system
+ *
+ * conn: IN Connection to quad
+ * data: OUT Data is written to this struct
+ *
+ * Returns 0 on success, nonzero on error
+ *
+ */
+int frontend_track(struct backend_conn * conn, 
+		struct frontend_tracker_data *data);
+
+#endif

groundStation/src/frontend/pid_common.h

+#ifndef PID_COMMON_H
+#define PID_COMMON_H
+
+enum pid_controller {
+	PID_PITCH,
+	PID_ROLL,
+	PID_YAW,
+	PID_PITCH_RATE,
+	PID_ROLL_RATE,
+	PID_YAW_RATE,
+	PID_HEIGHT, /* Z */
+	PID_LAT, /* X */
+	PID_LONG, /* Y */
+	PID_NUM_PIDS
+};
+
+struct frontend_pid_data {
+	enum pid_controller controller;
+
+	float p;
+	float i;
+	float d;
+};
+
+
+#endif