Removed groundstation from quad branch

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;
-
-
- 	while ((c = getopt(argc, argv, "fc:r:")) != -1) {
-		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 (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 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},
- 		{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(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/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_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\ngetpitchd\n", 20);
-			break;
-		case PID_ROLL :
-			strncpy(line, "getrollp\ngetrolld\n", 18);
-			break;
-		case PID_YAW :
-			strncpy(line, "getyawp\ngetyawd\n", 17);
-			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
-#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"
-
-#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

vrpn @ 99c54dbe

-Subproject commit 99c54dbefe04897cc7c146101a126f62c0e65f97