From fb5bce1847c6676eb9b15c683d8e9e024875fde3 Mon Sep 17 00:00:00 2001
From: Jake Drahos <j@kedrahos.com>
Date: Sat, 3 Dec 2016 14:35:21 -0600
Subject: [PATCH] Removed groundstation from quad branch
---
groundStation/.gitignore | 44 -
groundStation/Makefile | 76 --
groundStation/README.md | 63 -
groundStation/src/backend/backend.c | 773 -----------
groundStation/src/backend/commands.c | 1150 -----------------
groundStation/src/backend/commands.h | 105 --
groundStation/src/backend/communication.c | 271 ----
groundStation/src/backend/communication.h | 19 -
groundStation/src/backend/config.h | 20 -
groundStation/src/backend/logger.c | 108 --
groundStation/src/backend/logger.h | 21 -
groundStation/src/backend/old_main.cold | 73 --
groundStation/src/backend/type_def.h | 361 ------
groundStation/src/backend/vrpn_tracker.cpp | 178 ---
groundStation/src/backend/vrpn_tracker.hpp | 100 --
groundStation/src/cli/cli.c | 53 -
groundStation/src/cli/cli.h | 32 -
groundStation/src/cli/cli_getimu.h | 9 -
groundStation/src/cli/cli_getpid.c | 105 --
groundStation/src/cli/cli_getpid.h | 9 -
groundStation/src/cli/cli_monitor.c | 102 --
groundStation/src/cli/cli_monitor.h | 14 -
groundStation/src/cli/cli_setpid.c | 81 --
groundStation/src/cli/cli_setpid.h | 8 -
groundStation/src/frontend/frontend_common.c | 93 --
groundStation/src/frontend/frontend_common.h | 22 -
groundStation/src/frontend/frontend_getpid.c | 46 -
groundStation/src/frontend/frontend_getpid.h | 26 -
groundStation/src/frontend/frontend_setpid.c | 82 --
groundStation/src/frontend/frontend_setpid.h | 17 -
groundStation/src/frontend/frontend_tracker.c | 42 -
groundStation/src/frontend/frontend_tracker.h | 27 -
groundStation/src/frontend/pid_common.h | 26 -
groundStation/src/vrpn | 1 -
34 files changed, 4157 deletions(-)
delete mode 100644 groundStation/.gitignore
delete mode 100644 groundStation/Makefile
delete mode 100644 groundStation/README.md
delete mode 100644 groundStation/src/backend/backend.c
delete mode 100644 groundStation/src/backend/commands.c
delete mode 100644 groundStation/src/backend/commands.h
delete mode 100644 groundStation/src/backend/communication.c
delete mode 100644 groundStation/src/backend/communication.h
delete mode 100644 groundStation/src/backend/config.h
delete mode 100644 groundStation/src/backend/logger.c
delete mode 100644 groundStation/src/backend/logger.h
delete mode 100644 groundStation/src/backend/old_main.cold
delete mode 100644 groundStation/src/backend/type_def.h
delete mode 100644 groundStation/src/backend/vrpn_tracker.cpp
delete mode 100644 groundStation/src/backend/vrpn_tracker.hpp
delete mode 100644 groundStation/src/cli/cli.c
delete mode 100644 groundStation/src/cli/cli.h
delete mode 100644 groundStation/src/cli/cli_getimu.h
delete mode 100644 groundStation/src/cli/cli_getpid.c
delete mode 100644 groundStation/src/cli/cli_getpid.h
delete mode 100644 groundStation/src/cli/cli_monitor.c
delete mode 100644 groundStation/src/cli/cli_monitor.h
delete mode 100644 groundStation/src/cli/cli_setpid.c
delete mode 100644 groundStation/src/cli/cli_setpid.h
delete mode 100644 groundStation/src/frontend/frontend_common.c
delete mode 100644 groundStation/src/frontend/frontend_common.h
delete mode 100644 groundStation/src/frontend/frontend_getpid.c
delete mode 100644 groundStation/src/frontend/frontend_getpid.h
delete mode 100644 groundStation/src/frontend/frontend_setpid.c
delete mode 100644 groundStation/src/frontend/frontend_setpid.h
delete mode 100644 groundStation/src/frontend/frontend_tracker.c
delete mode 100644 groundStation/src/frontend/frontend_tracker.h
delete mode 100644 groundStation/src/frontend/pid_common.h
delete mode 160000 groundStation/src/vrpn
diff --git a/groundStation/.gitignore b/groundStation/.gitignore
deleted file mode 100644
index 508a8f243..000000000
--- a/groundStation/.gitignore
+++ /dev/null
@@ -1,44 +0,0 @@
-# Object files
-*.o
-*.ko
-*.obj
-*.elf
-
-# Precompiled Headers
-*.gch
-*.pch
-
-# Libraries
-*.lib
-*.a
-*.la
-*.lo
-
-# Shared objects (inc. Windows DLLs)
-*.dll
-*.so
-*.so.*
-*.dylib
-
-# Executables
-*.exe
-*.out
-*.app
-*.i*86
-*.x86_64
-*.hex
-
-# Debug files
-*.dSYM/
-*.su
-
-
-# vrpn/build files
-src/vrpn/build*
-src/vrpn/pc_linux64/*
-
-#Exacutables
-logs
-BackEnd
-obj
-Cli
diff --git a/groundStation/Makefile b/groundStation/Makefile
deleted file mode 100644
index 50c78782e..000000000
--- a/groundStation/Makefile
+++ /dev/null
@@ -1,76 +0,0 @@
-# Declaration of variables
-
-# Generic Variables
-GCC=gcc
-GXX=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
-OBJDIR=obj
-
-# Backend Specific Variables
-BEBINARY=BackEnd
-BESRCDIR=src/backend
-BECSOURCES := $(wildcard $(BESRCDIR)/*.c )
-BECOBJECTS = $(BECSOURCES:$(BESRCDIR)/%.c=$(OBJDIR)/%.o)
-BECPPSOURCES := $(wildcard $(BESRCDIR)/*.cpp )
-BECPPOBJECTS = $(BECPPSOURCES:$(BESRCDIR)/%.cpp=$(OBJDIR)/%.o)
-
-# CLI Specific Variables
-CLIBINARY=Cli
-CLISRCDIR=src/cli
-CLISOURCES := $(wildcard $(CLISRCDIR)/*.c)
-CLIOBJECTS = $(CLISOURCES:$(CLISRCDIR)/%.c=$(OBJDIR)/%.o)
-
-# Frontend-common stuff
-FESRCDIR=src/frontend
-FECSOURCES := $(wildcard $(FESRCDIR)/*.c )
-FECOBJECTS = $(FECSOURCES:$(FESRCDIR)/%.c=$(OBJDIR)/%.o)
-
-
-OBJECTS= $(CLIOBJECTS) $(BECOBJECTS) $(BECPPOBJECTS) $(FECOBJECTS)
-
-# Default target
-all: logs objdir backend cli
-
-vrpn: vrpn/build
-
-cli: $(CLIOBJECTS) $(FECOBJECTS)
- $(GCC) $(CFLAGS) $^ -o $(CLIBINARY) $(INCLUDES) $(LIBS)
-
-$(CLIOBJECTS) : $(OBJDIR)/%.o : $(CLISRCDIR)/%.c
- $(GCC) $(CFLAGS) -c $^ -o $@ $(INCLUDES) $(LIBS)
-
-
-backend: $(BECPPOBJECTS) $(BECOBJECTS)
- $(GXX) $(CXXFLAGS) $^ -o $(BEBINARY) $(INCLUDES) $(LIBS)
-
-$(FECOBJECTS) : $(OBJDIR)/%.o : $(FESRCDIR)/%.c
- $(GCC) $(CFLAGS) -c $^ -o $@ $(INCLUDES) $(LIBS)
-
-$(BECOBJECTS) : $(OBJDIR)/%.o : $(BESRCDIR)/%.c
- $(GCC) $(CFLAGS) -c $^ -o $@ $(INCLUDES) $(LIBS)
-
-$(BECPPOBJECTS) : $(OBJDIR)/%.o : $(BESRCDIR)/%.cpp
- $(GXX) $(CXXFLAGS) -c $^ -o $@ $(INCLUDES) $(LIBS)
-
-vrpn/build:
- mkdir -p src/vrpn/build
- cd src/vrpn/build && cmake .. && make
-
-logs:
- mkdir -p logs
-
-objdir:
- mkdir -p obj
-
-clean_logs:
- rm -f logs/*
-
-clean:
- rm -rf $(OBJDIR)/ $(BEBINARY) $(CLIBINARY)
-
-debug:
- @echo $(OBJECTS)
diff --git a/groundStation/README.md b/groundStation/README.md
deleted file mode 100644
index a54b1f436..000000000
--- a/groundStation/README.md
+++ /dev/null
@@ -1,63 +0,0 @@
-# groundStation
-
-## Make Process
-
-First, if submodules were not recursevly added through git. Run this command if you have made any attempt to make vrpn manually.
-run
- 'git submodule update --init --recursive'
-
-Now that you have all of the files necissary.
-
-cd into the groundstation folder.
- cd groundStation
- make vrpn
- make
-
-run the program with sudo privledges
- sudo -E ./BackEnd
-
-If you wish to change the way the backend communicates to the quad and vice versa, look at src/config.h.
- This provides a list of environment variables which you can set and use for your computer or time of use.
- Because the backend must be ran with sudo privledges, you will need to preserve the env. vars. with sudo rights.
- Hence the "-E" flag.
-
-## 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`
-
diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
deleted file mode 100644
index 5c4dc9fa6..000000000
--- a/groundStation/src/backend/backend.c
+++ /dev/null
@@ -1,773 +0,0 @@
-/* Author: Kris Burney & Jake Drahos
- *
- * BlueTooth socket program for passing vrpn data to quad.
- */
-
-#define _GNU_SOURCE
-#define _BSD_SOURCE
-
-//system includes
-#include <err.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <signal.h>
-#include <sys/socket.h>
-#include <sys/un.h>
-#include <netinet/in.h>
-#include <arpa/inet.h>
-#include <sys/select.h>
-#include <bluetooth/bluetooth.h>
-#include <bluetooth/rfcomm.h>
-#include <pthread.h>
-#include <assert.h>
-#include <errno.h>
-#include <string.h>
-#include <sys/ioctl.h>
-#include <sys/types.h>
-#include <netinet/tcp.h>
-
-//user created includes
-#include "communication.h"
-#include "commands.h"
-#include "vrpn_tracker.hpp"
-#include "type_def.h"
-#include "logger.h"
-#include "config.h"
-
-#define QUAD_BT_ADDR "00:06:66:64:61:D6"
-#define QUAD_BT_CHANNEL 0x01
-#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 *);
-void sendStartPacket(void);
-void getVRPNPacket(struct ucart_vrpn_TrackerData *);
-void printVrpnData(struct ucart_vrpn_TrackerData *);
-int connectToZybo();
-int safe_fd_set(int , fd_set* , int* );
-int safe_fd_clr(int , fd_set* , int* );
-static void safe_close_fd(int fd, pthread_mutex_t *mutexLock);
-
-static void cb(struct ucart_vrpn_TrackerData *);
-static int new_client(int fd);
-/* Return index of client, or -1 */
-static ssize_t get_client_index(int fd);
-/* Returns pointer to client buffer, or -1 */
-static char * get_client_buffer(int fd);
-/* Return pointer to client pending responses, or -1*/
-static int * get_client_pend_responses(int fd);
-/* Return positive integer if successful, -1 otherwise */
-static int clientAddPendResponses(int fd, unsigned char *packet);
-/* Returns -1 on error */
-static int remove_client(int fd);
-/* Receive data from client */
-static void client_recv(int fd);
-/* Receive data from quad */
-static void quad_recv(const char * buf, size_t len);
-/* Checks to see if socket has disconnected. Returns 1 on disconnect, else returns 0 */
-static int wasDisconnected(int fd);
-
-/* Thread-safe wrappers */
-pthread_mutex_t quadSocketMutex;
-static ssize_t writeQuad(const char * buf, size_t count);
-static ssize_t readQuad(char * buf, size_t count);
-
-/* Functions for recording Latencies */
-void findTimeDiff(int respID);
-int timeval_subtract (struct timeval *result, struct timeval *x, struct timeval *y);
-//time stamp checking
-static unsigned int currMessageID = 0;
-struct timeval timeArr[MAX_HASH_SIZE];
-
-// global variables
-static volatile int keepRunning = 1;
-const char *TRACKER_IP = "UAV@192.168.0.120:3883";
-static int zyboSocket;
-static int backendSocket;
-struct ucart_vrpn_tracker * tracker = NULL;
-const char *logHeader = "";//"#\n#\tDefault log header\n#\tEverything after '#'`s will be printed as is in the processed logs.\n#\n\0";
-
-#define MAX_CLIENTS 32
-#define CLIENT_BUFFER_SIZE 1024
-#define CLIENT_MAX_PENDING_RESPONSES 5
-static char client_buffers[MAX_CLIENTS][CLIENT_BUFFER_SIZE];
-static int client_fds[MAX_CLIENTS];
-static int client_pending_responses[MAX_CLIENTS][CLIENT_MAX_PENDING_RESPONSES];
-fd_set rfds_master;
-int max_fd = 0;
-
-pthread_mutex_t quadResponseMutex, cliInputMutex ;
-char *respBuf, *commandBuf;
-int newQuadResponse = 0, newCliInput = 0;
-
-// Structures to be used throughout
-modular_structs_t structs;
-
-// Callback to be ran whenever the tracker receives data.
-// Currently doing much more than it should. It will be slimmed down
-// in the future.
-static void cb(struct ucart_vrpn_TrackerData * td) {
- static int count = 0;
-
- if(!(count % 10)) {
- sendVrpnPacket(td);
- //updateLogFile(td);
- }
- count++;
-}
-
-int main(int argc, char **argv)
-{
- int activity;
- FD_ZERO(&rfds_master);
-
- /*
- * Create backend listening socket
- */
- /* Determine socket path */
- char * backend_socket_path = DEFAULT_SOCKET;
- if (getenv(SOCKET_ENV)) {
- backend_socket_path = getenv(SOCKET_ENV);
- }
- /* Unlink if it exists */
- unlink(backend_socket_path);
-
- /* Create socket */
- backendSocket = socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK, 0);
- if (backendSocket < 0) {
- err(-1, "socket");
- }
-
- /* Create sockaddr and bind */
- struct sockaddr_un sa;
- sa.sun_family = AF_UNIX;
- strncpy(sa.sun_path, backend_socket_path, 107);
- sa.sun_path[107] = '\0';
- if (bind(backendSocket, (struct sockaddr *) &sa, sizeof(sa))) {
- err(-1, "bind");
- }
-
- /* Listen */
- if (listen(backendSocket, 16)) {
- err(-1, "listen");
- }
-
- /* Add to socket set */
- safe_fd_set(backendSocket, &rfds_master, &max_fd);
-
-
- /* Initialize client buffers */
- for (int i = 0; i < MAX_CLIENTS; i++) {
- client_fds[i] = -1;
- client_buffers[i][0] = '\n';
- for(int j = 0; j < CLIENT_MAX_PENDING_RESPONSES; j++) {
- client_pending_responses[i][j] = -1;
- }
- }
-
- if (pthread_mutex_lock(&quadSocketMutex)) {
- err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
- }
-
- if ((zyboSocket = connectToZybo()) < 0)
- {
- perror("Error connecting to Quad...");
- free(respBuf);
- free(commandBuf);
- exit(1);
- }
-
- if (pthread_mutex_unlock(&quadSocketMutex)) {
- err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
- }
-
- // open the log file
- if(createLogFile(argc, argv[1]))
- {
- perror("Error creating log file...");
- exit(1);
- }
- // writeStringToLog(logHeader);
-
- // watch for input from stdin (fd 0) to see when it has input
- safe_fd_set(fileno(stdin), &rfds_master, &max_fd);
-
- if (!getenv(NOQUAD_ENV)) {
- // watch for input from the zybo socket
- safe_fd_set(zyboSocket, &rfds_master, &max_fd);
-
- }
- // Tell the quad we are ready to send it vrpn data
- sendStartPacket();
-
- if(!getenv(NOVRPN_ENV)){
- // create vrpnTracker instance
- tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
- // this function will be called whenever tracker receives data
- ucart_vrpn_tracker_addCallback(tracker, cb);
- }
-
- struct timeval timeout = {
- .tv_sec = 1,
- .tv_usec = 0
- };
-
- respBuf = calloc(CMD_MAX_LENGTH, sizeof(unsigned char));
- sleep(3);
- while(keepRunning)
- {
- fd_set rfds;
- rfds = rfds_master;
- activity = select(max_fd+1, &rfds, NULL, NULL, NULL);
- if(activity == -1) {
- perror("select() ");
- } else if (activity) {
- for(int fd = 0; fd <= max_fd; ++fd) {
- if (FD_ISSET(fd, &rfds)) {
- if(wasDisconnected(fd)){
- break;
- }
-
- if (fd == fileno(stdin)) {
- /**
- * Ignore stdin from the backend
- */
- } else if (fd == zyboSocket) {
- printf("recieving from quad\n");
-
- /**
- * Read the response from the control loop
- */
- int available;
- ioctl(fd, FIONREAD, &available);
- if (available < 12) {
- continue;
- }
- int respLen = readQuad(respBuf, CMD_MAX_LENGTH);
- if(respLen <= 0) {
- perror("ERROR reading from quad...\n");
- }
-
- //int id = getInt((unsigned char *)respBuf, 7);
- //findTimeDiff(id);
-
- quad_recv(respBuf, respLen);
- // if(respLen == 11) {
- // int id = getInt((unsigned char *)respBuf, 7);
- // findTimeDiff(id);
- // printf("respLen = %d : id = %d'\n", respLen, id);
- // for(int i = 0; i <= respLen -1; ++i)
- // printf("%x ", (unsigned char)respBuf[i]);
- // printf("'\n");
- // }
- memset(respBuf, 0, respLen);
- } else if (fd == backendSocket) {
- int new_fd = 0;
- new_fd = accept(backendSocket, NULL, NULL);
- if (new_fd < 0) {
- warn("accept");
- } else {
- printf("Connection\n");
- if (new_client(new_fd)) {
- printf("Added client\n");
- safe_fd_set(new_fd, &rfds_master, &max_fd);
- }
- }
- } else if (get_client_index(fd) > -1) {
- /* It is a socket to a frontend */
- client_recv(fd);
- }
- }
- }
- } else {
- timeout.tv_sec = 1;
- timeout.tv_usec = 0;
- }
- }
-
- ucart_vrpn_tracker_freeInstance(tracker);
-
- safe_close_fd(zyboSocket, &quadSocketMutex);
-
- closeLogFile();
- return 0;
-}
-
-void sendStartPacket() {
- unsigned char packet[8] = {0};
- currMessageID++;
- metadata_t metadata =
- {
- (char) BEGIN_CHAR,
- 0x04,
- 0x01,
- 0x01,
- 0
- };
-
- packet[0] = metadata.begin_char; // BEGIN //PACKET_START_BYTE;
- packet[1] = metadata.msg_type; // UPDATE //'U'; // U for vrpn camera update, C for command
- packet[2] = metadata.msg_subtype; // BEGIN UPDATE
- packet[3] = (currMessageID & 0x000000ff); // MSG ID(1)
- packet[4] = ((currMessageID >> 8) & 0x000000ff); // MSG ID(2)
- packet[5] = 0; // DATALEN(1)
- packet[6] = 0; // DATALEN(2)
-
- char checksum = 0;
- int i;
- for(i=0; i < metadata.data_len + 7; i++)
- checksum ^= packet[i];
-
- packet[metadata.data_len + 7] = checksum; //PACKET_END_BYTE;
- int status = writeQuad((char * ) packet, metadata.data_len + 8);
- if (status != 8)
- {
- perror("Error sending start packet...\n");
- keepRunning = 0;
- }else
- {
- printf("Start packet successfuly sent...\n");
- }
-}
-
-void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
- int pSize = sizeof(struct ucart_vrpn_TrackerData) + 8;
- int n;
- unsigned char packet[pSize];
- currMessageID++;
- packet[0] = 0xBE; // BEGIN //PACKET_START_BYTE;
- packet[1] = 0x04; // UPDATE //'U'; // U for vrpn camera update, C for command
- packet[2] = 0x00; // N/A
- //TODO Figure out Packet ID with this new ucar_vrpn_TrackerData struct
- packet[3] = (currMessageID & 0x000000ff); // MSG ID(1)
- packet[4] = ((currMessageID >> 8) & 0x000000ff); // MSG ID(2)
- packet[5] = (sizeof(struct ucart_vrpn_TrackerData) & 0x000000ff); // DATALEN(1)
- packet[6] = ((sizeof(struct ucart_vrpn_TrackerData) >> 8) & 0x00000ff); // DATALEN(2)
- memcpy(&packet[7], &info, sizeof(struct ucart_vrpn_TrackerData));
-
- char checksum = 0;
- int i;
- for(i=0; i < pSize - 1; i++)
- checksum ^= packet[i];
-
- packet[pSize - 1] = checksum; //PACKET_END_BYTE;
-
- n = writeQuad((char *) packet, pSize);
- if(n < 0) {
- perror("vrpnhandler: ERROR writing to socket");
- keepRunning = 0;
- }
- struct timeval tstart;
- gettimeofday(&tstart, NULL);
- timeArr[currMessageID%MAX_HASH_SIZE] = tstart;
-}
-
-void getVRPNPacket(struct ucart_vrpn_TrackerData *td) {
- int status;
- if((status = ucart_vrpn_tracker_getData(tracker, td)) < 0)
- {
- perror("Error receiving VRPN data from tracker...");
- keepRunning = 0;
- }
-}
-
-void printVrpnData(struct ucart_vrpn_TrackerData * td) {
- printf("FPS: %lf Pos (xyz): (%lf %lf %lf) Att (pry): (%lf %lf %lf)\n",
- td->fps, td->x, td->y, td->z, td->pitch, td->roll, td->yaw);
-}
-
-int connectToZybo() {
- int sock;
- int status = 0;
-
- if (getenv(NOQUAD_ENV)) {
- return 0;
- }
-
- /* Use bluetooth by default */
- if (!getenv(QUAD_WIFI_ENV)) {
- printf("Using BT Settings\n");
- struct sockaddr_rc addr;
-
- // allocate a socket
- sock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
-
- //set the connection params ie. who to connect to
- addr.rc_family = AF_BLUETOOTH;
- addr.rc_channel = (uint8_t) QUAD_BT_CHANNEL;
- str2ba( QUAD_BT_ADDR, &addr.rc_bdaddr );
-
- printf("Attempting to connect to zybo. Please be patient...\n");
- // blocking call to connect to socket sock ie. zybo board
- status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
- } else {
- printf("Using WIFI settings\n");
- struct sockaddr_in addr;
-
- addr.sin_family = AF_INET;
-
- /* Quick and Dirty */
- if (getenv(QUAD_IP_ENV)) {
- if (!inet_aton(getenv(QUAD_IP_ENV), &addr.sin_addr)) {
- printf("Env var %s invalid IP %s\n",
- QUAD_IP_ENV, getenv(QUAD_IP_ENV));
- return -1;
- }
- } else {
- if (!inet_aton(QUAD_IP_DEFAULT, &addr.sin_addr)) {
- printf("Default IP %s is invalid\n",
- QUAD_IP_DEFAULT);
- return -1;
- }
- }
-
- if (getenv(QUAD_PORT_ENV)) {
- /* Quick 'n dirty, oh yeah! */
- addr.sin_port = htons(atoi(getenv(QUAD_PORT_ENV)));
- } else {
- addr.sin_port = htons(QUAD_PORT_DEFAULT);
- }
-
- sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
- if (sock < 0) {
- perror("socket");
- return -1;
- }
- printf("Connecting to Quad @ %s:%u\n", inet_ntoa(addr.sin_addr), ntohs(addr.sin_port));
-
- status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
- }
-
- // connection failed
- if(status < 0)
- {
- close(sock);
- perror("connect");
- return -1;
- }
- else
- {
- // int result = setsockopt(sock, IPPROTO_TCP, TCP_NODELAY, (int[]){1}, sizeof(int));
- // printf("result = %d\n", result);
- printf("connection successful!...\n");
- return sock;
- }
-}
-
-/* add a fd to fd_set, and update max_fd */
-int safe_fd_set(int fd, fd_set* fds, int* max_fd) {
- assert(max_fd != NULL);
-
- FD_SET(fd, fds);
- if (fd > *max_fd) {
- *max_fd = fd;
- }
- return 0;
-}
-
-/* clear fd from fds, update max fd if needed */
-int safe_fd_clr(int fd, fd_set* fds, int* max_fd) {
- assert(max_fd != NULL);
-
- FD_CLR(fd, fds);
- if (fd == *max_fd) {
- (*max_fd)--;
- }
- return 0;
-}
-
-static ssize_t writeQuad(const char * buf, size_t count) {
- ssize_t retval;
- if (getenv(NOQUAD_ENV)) {
- return count;
- }
- if (pthread_mutex_lock(&quadSocketMutex)) {
- err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
- }
- //setsockopt(zyboSocket, IPPROTO_TCP, TCP_QUICKACK, (int[]){1}, sizeof(int));
- retval = write(zyboSocket, buf, count);
- if (pthread_mutex_unlock(&quadSocketMutex)) {
- err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
- }
-
- return retval;
-}
-
-static ssize_t readQuad(char * buf, size_t count) {
- ssize_t retval;
- if (pthread_mutex_lock(&quadSocketMutex)) {
- err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
- }
- retval = read(zyboSocket, buf, count);
- //int result = setsockopt(zyboSocket, IPPROTO_TCP, TCP_QUICKACK, (int[]){1}, sizeof(int));
- //printf("result = %d\n", result);
-
- if (pthread_mutex_unlock(&quadSocketMutex)) {
- err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
- }
- return retval;
-}
-
-static int new_client(int fd) {
- ssize_t new_slot = -1;
- for (ssize_t i = 0; i < MAX_CLIENTS; i++) {
- if (client_fds[i] < 0) {
- new_slot = i;
- break;
- }
- }
- if (new_slot == -1) {
- warnx("Ran out of room! Consider increasing MAX_CLIENTS!");
- return 0;
- }
-
- client_fds[new_slot] = fd;
- client_buffers[new_slot][0] = '\0';
-
- return 1;
-}
-
-static ssize_t get_client_index(int fd) {
- for (ssize_t i = 0; i < MAX_CLIENTS; i++) {
- if (client_fds[i] == fd) {
- return i;
- }
- }
-
- return -1;
-}
-
-static char * get_client_buffer(int fd) {
- ssize_t slot = get_client_index(fd);
- if (slot == -1) {
- return NULL;
- } else {
- return client_buffers[slot];
- }
-}
-
-static int * get_client_pend_responses(int fd) {
- ssize_t slot = get_client_index(fd);
- if (slot == -1) {
- return NULL;
- } else {
- return client_pending_responses[slot];
- }
-}
-
-static int clientAddPendResponses(int fd, unsigned char *packet) {
- int *pendingResponses = get_client_pend_responses(fd);
- int packetID = (packet[4] << 8) | (packet[3]);
- for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) {
- if(pendingResponses[i] == -1) {
- pendingResponses[i] = packetID;
- return i;
- }
- }
- return -1;
-}
-
-static int remove_client(int fd) {
- ssize_t slot = get_client_index(fd);
- if(slot == -1)
- return -1;
- char *clientBuffer = get_client_buffer(fd);
- if(clientBuffer == NULL)
- return -1;
- clientBuffer[0] = '\0';
- int *pendingResponses = get_client_pend_responses(fd);
- if(pendingResponses == NULL)
- return -1;
- for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) {
- pendingResponses[i] = -1;
- }
- client_fds[slot] = -1;
- return 0;
-}
-
-static void safe_close_fd(int fd, pthread_mutex_t *mutexLock) {
- if (pthread_mutex_lock(mutexLock)) {
- err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
- }
- close(fd);
- if (pthread_mutex_unlock(mutexLock)) {
- err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
- }
-}
-
-static void client_recv(int fd) {
- char * buffer;
- ssize_t len_pre;
- buffer = get_client_buffer(fd);
- len_pre = strlen(buffer);
-
- char * cursor;
- cursor = buffer + len_pre;
-
- ssize_t r;
- r = read(fd, cursor, CLIENT_BUFFER_SIZE - len_pre - 1);
- if (r < 0) {
- warn("read (fd: %d)", fd);
- }
- buffer[len_pre + r] = '\0';
-
- int index = 0;
- /* Parse buffer and handle commands */
- while (1) {
- /* not using strtok because reasons */
- size_t len = strlen(buffer);
- ssize_t newline = -1;
- for (size_t i = 0; i < len; i++) {
- if (buffer[i] == '\n') {
- newline = i;
- break;
- }
- }
-
- /* No newline found. End parsing */
- if (newline == -1) {
- break;
- }
- buffer[newline] = '\0';
-
- printf("Client(%d) : '%s'\n",fd, buffer);
- unsigned char * packet;
- if(formatCommand(buffer, &packet) == -1) {
- /* 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!\n");
- } else {
- int datalen = (packet[6] << 8) | (packet[5]);
- printf("sending %lf '", getFloat(packet, 7));
- for(int i = 0; i < datalen + 8; ++i) {
- printf(" 0x%x", (signed) packet[i]);
- }
- printf("'\n");
- writeQuad((char *) packet, datalen +8);
- }
- }
-
- char * rest = &buffer[newline] + 1;
- size_t restLen = (strlen(rest) == 0) ? 1 : strlen(rest);
- /* Delete parsed data and move the rest to the left */
- memmove(buffer, rest, restLen +1);
- }
-}
-
-static void quad_recv(const char * buf, size_t len) {
- /* Check to see which command we are receiving. If it is one that needs to be passed on
- onto the clients, do so.
- */
-
- char packet[len];
- int validPacket;
- unsigned char *data;
- metadata_t metadata;
-
- memcpy(packet, buf, len);
-
- // Validate the message is correctly formatted
- validPacket = parse_packet((unsigned char *) packet, &data, &metadata);
- if(validPacket != 0) {
- warnx("Could not recognize packet from quad.\n");
- return;
- }
-
- int datalen = (packet[6] << 8) | (packet[5]);
- char * cmdText = MessageTypes[(int)metadata.msg_type].subtypes[(int)metadata.msg_subtype].cmdText;
- float value = getFloat((unsigned char *)packet, 7);
- printf("Quad : %s, %lf\n", cmdText, value);
- /*
- Assuming the quad sends the correct info.. This hasn't been tested yet due to a lack of
- quad software. We can check how to format by the cmdText and pass to every client.
- */
- char buffer[1048];
- sprintf(buffer, "%s %lf\n", cmdText, value);
-
- for(int fd = 0; fd <= max_fd; ++fd) {
- if (get_client_index(fd) > -1) {
- write(fd, buffer, datalen + 8);
- }
- }
-}
-
-static int wasDisconnected(int fd) {
- char buff;
- if(recv(fd, &buff, 1, MSG_PEEK | MSG_DONTWAIT) == 0)
- {
- remove_client(fd);
- safe_fd_clr(fd, &rfds_master, &max_fd);
- printf("fd %d has disconnect and was removed\n", fd);
- return 1;
- }
- return 0;
-}
-
-int timeval_subtract (struct timeval *result, struct timeval *x, struct timeval *y) {
- /* Perform the carry for the later subtraction by updating y. */
- if (x->tv_usec < y->tv_usec) {
- int nsec = (y->tv_usec - x->tv_usec) / 1000000 + 1;
- y->tv_usec -= 1000000 * nsec;
- y->tv_sec += nsec;
- }
- if (x->tv_usec - y->tv_usec > 1000000) {
- int nsec = (x->tv_usec - y->tv_usec) / 1000000;
- y->tv_usec += 1000000 * nsec;
- y->tv_sec -= nsec;
- }
- /* Compute the time remaining to wait.
- tv_usec is certainly positive. */
- result->tv_sec = x->tv_sec - y->tv_sec;
- result->tv_usec = x->tv_usec - y->tv_usec;
- /* Return 1 if result is negative. */
- return x->tv_sec < y->tv_sec;
-}
-
-void findTimeDiff(int respID) {
- struct timeval result, tend;
- gettimeofday(&tend, NULL);
- timeval_subtract(&result, &tend, &timeArr[respID%MAX_HASH_SIZE]);
- printf("elapsed time = %ld ms\n", result.tv_usec/1000);
- // char tmp[8];
- // snprintf(tmp, 8, "%ld \tms\n", result.tv_usec/1000);
- // writeStringToLog(tmp);
-}
diff --git a/groundStation/src/backend/commands.c b/groundStation/src/backend/commands.c
deleted file mode 100644
index a97197b81..000000000
--- a/groundStation/src/backend/commands.c
+++ /dev/null
@@ -1,1150 +0,0 @@
-#include "commands.h"
-
-// TAKE THESE OUT WHEN IMPLEMENTING ON THE QUAD SIDE
-float getFloat(unsigned char* str, int pos) {
- union {
- float f;
- int i;
- } x;
- x.i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
- return x.f;
-}
-
-int getInt(unsigned char* str, int pos) {
- int i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
- return i;
-}
-//------------------------------------------------
-
-struct MessageType MessageTypes[MAX_TYPE] =
-{
- // DEBUG
- {
- // Message Type ID
- 0x00,
-
- // Debug Subtypes
- {
- // NONE subtype
- {
- // ID
- 0x00,
- // Command text
- "debug",
- // Type of the command data
- stringType,
- // Function pointer
- &debug
- }
- }
- },
-
- // CALIBRATION
- {
- // Message Type ID
- 0x01,
-
- // Calibration Subtypes (PID coefficients)
- {
- // yaw p constant subtype
- {
- // ID
- 0x00,
- // Command text
- "setyawp",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // yaw i constant subtype
- {
- // ID
- 0x01,
- // Command text
- "setyawi",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // yaw d constant subtype
- {
- // ID
- 0x02,
- // Command text
- "setyawd",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // roll p constant subtype
- {
- // ID
- 0x03,
- // Command text
- "setrollp",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // roll i constant subtype
- {
- // ID
- 0x04,
- // Command text
- "setrolli",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // roll d constant subtype
- {
- // ID
- 0x05,
- // Command text
- "setrolld",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // pitch p constant subtype
- {
- // ID
- 0x06,
- // Command text
- "setpitchp",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // pitch i constant subtype
- {
- // ID
- 0x07,
- // Command text
- "setpitchi",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // pitch d constant subtype
- {
- // ID
- 0x08,
- // Command text
- "setpitchd",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // yawrate p constant subtype
- {
- // ID
- 0x09,
- // Command text
- "setyawratep",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // yawrate i constant subtype
- {
- // ID
- 0x0A,
- // Command text
- "setyawratei",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // yawrate d constant subtype
- {
- // ID
- 0x0B,
- // Command text
- "setyawrated",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // rollrate p constant subtype
- {
- // ID
- 0x0C,
- // Command text
- "setrollratep",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // rollrate i constant subtype
- {
- // ID
- 0x0D,
- // Command text
- "setrollratei",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // rollrate d constant subtype
- {
- // ID
- 0x0E,
- // Command text
- "setrollrated",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // pitchrate p constant subtype
- {
- // ID
- 0x0F,
- // Command text
- "setpitchratep",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // pitchrate i constant subtype
- {
- // ID
- 0x10,
- // Command text
- "setpitchratei",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // pitchrate d constant subtype
- {
- // ID
- 0x11,
- // Command text
- "setpitchrated",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // height p constant subtype
- {
- // ID
- 0x12,
- // Command text
- "setheightp",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // height i constant subtype
- {
- // ID
- 0x13,
- // Command text
- "setheighti",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // height d constant subtype
- {
- // ID
- 0x14,
- // Command text
- "setheightd",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // lat p constant subtype
- {
- // ID
- 0x15,
- // Command text
- "setlatp",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // lat i constant subtype
- {
- // ID
- 0x16,
- // Command text
- "setlati",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // lat d constant subtype
- {
- // ID
- 0x17,
- // Command text
- "setlatd",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // long p constant subtype
- {
- // ID
- 0x18,
- // Command text
- "setlongp",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // long i constant subtype
- {
- // ID
- 0x19,
- // Command text
- "setlongi",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- },
- // long d constant subtype
- {
- // ID
- 0x1A,
- // Command text
- "setlongd",
- // Type of the command data
- floatType,
- // Function pointer
- NULL
- }
- }
- },
-
- // REQUEST
- {
- // Message Type ID
- 0x02,
-
- // Request Subtypes
- {
- // accelerometer subtype
- {
- // ID
- 0x00,
- // Command text
- "getaccel",
- // Type of the command data
- floatType,
- // Function pointer
- &getaccel
- },
- // gyroscope subtype
- {
- // ID
- 0x01,
- // Command text
- "getgyro",
- // Type of the command data
- floatType,
- // Function pointer
- &getgyro
- },
- // pitch angle subtype
- {
- // ID
- 0x02,
- // Command text
- "getpitchangle",
- // Type of the command data
- floatType,
- // Function pointer
- &getpitchangle
- },
- // roll angle subtype
- {
- // ID
- 0x03,
- // Command text
- "getrollangle",
- // Type of the command data
- floatType,
- // Function pointer
- &getrollangle
- },
- // get yaw setpoint subtype
- {
- // ID
- 0x04,
- // Command text
- "getyaw",
- // Type of the command data
- floatType,
- // Function pointer
- &setyaw
- },
- // get yaw p constant subtype
- {
- // ID
- 0x05,
- // Command text
- "getyawp",
- // Type of the command data
- floatType,
- // Function pointer
- &getyawp
- },
- // get yaw d constant subtype
- {
- // ID
- 0x06,
- // Command text
- "getyawd",
- // Type of the command data
- floatType,
- // Function pointer
- &getyawd
- },
- // get roll setpoint subtype
- {
- // ID
- 0x07,
- // Command text
- "getroll",
- // Type of the command data
- floatType,
- // Function pointer
- &getroll
- },
- // get roll p constant subtype
- {
- // ID
- 0x08,
- // Command text
- "getrollp",
- // Type of the command data
- floatType,
- // Function pointer
- &getrollp
- },
- // getroll d constant subtype
- {
- // ID
- 0x09,
- // Command text
- "getrolld",
- // Type of the command data
- floatType,
- // Function pointer
- &getrolld
- },
- // get pitch setpoint subtype
- {
- // ID
- 0x0A,
- // Command text
- "getpitch",
- // Type of the command data
- floatType,
- // Function pointer
- &getpitch
- },
- // get pitch p constant subtype
- {
- // ID
- 0x0B,
- // Command text
- "getpitchp",
- // Type of the command data
- floatType,
- // Function pointer
- &getpitchp
- },
- // get pitch d constant subtype
- {
- // ID
- 0x0C,
- // Command text
- "getpitchd",
- // Type of the command data
- floatType,
- // Function pointer
- &getpitchd
- },
- // get throttle setpoint subtype
- {
- // ID
- 0x0D,
- // Command text
- "getthrottle",
- // Type of the command data
- floatType,
- // Function pointer
- &getthrottle
- },
- // get throttle p constant subtype
- {
- // ID
- 0x0E,
- // Command text
- "getthrottlep",
- // Type of the command data
- floatType,
- // Function pointer
- &getthrottlep
- },
- // get throttle i constant subtype
- {
- // ID
- 0x0F,
- // Command text
- "getthrottlei",
- // Type of the command data
- floatType,
- // Function pointer
- &getthrottlei
- },
- // get throttle d constant subtype
- {
- // ID
- 0x10,
- // Command text
- "getthrottled",
- // Type of the command data
- floatType,
- // Function pointer
- &getthrottled
- }
- }
- },
-
- // RESPONSE
- {
- // Message Type ID
- 0x03,
-
- // Response Subtypes
- {
- // accelerometer subtype
- {
- // ID
- 0x00,
- // Command text
- "respaccel",
- // Type of the command data
- floatType,
- // Function pointer
- &respaccel
- },
- // gyroscope subtype
- {
- // ID
- 0x01,
- // Command text
- "respgyro",
- // Type of the command data
- floatType,
- // Function pointer
- &respgyro
- },
- // pitch angle subtype
- {
- // ID
- 0x02,
- // Command text
- "resppitchangle",
- // Type of the command data
- floatType,
- // Function pointer
- &resppitchangle
- },
- // roll angle subtype
- {
- // ID
- 0x03,
- // Command text
- "resprollangle",
- // Type of the command data
- floatType,
- // Function pointer
- &resprollangle
- },
- // resp yaw setpoint subtype
- {
- // ID
- 0x04,
- // Command text
- "respyaw",
- // Type of the command data
- floatType,
- // Function pointer
- &setyaw
- },
- // resp yaw p constant subtype
- {
- // ID
- 0x05,
- // Command text
- "respyawp",
- // Type of the command data
- floatType,
- // Function pointer
- &respyawp
- },
- // resp yaw d constant subtype
- {
- // ID
- 0x06,
- // Command text
- "respyawd",
- // Type of the command data
- floatType,
- // Function pointer
- &respyawd
- },
- // resp roll setpoint subtype
- {
- // ID
- 0x07,
- // Command text
- "resproll",
- // Type of the command data
- floatType,
- // Function pointer
- &resproll
- },
- // resp roll p constant subtype
- {
- // ID
- 0x08,
- // Command text
- "resprollp",
- // Type of the command data
- floatType,
- // Function pointer
- &resprollp
- },
- // resproll d constant subtype
- {
- // ID
- 0x09,
- // Command text
- "resprolld",
- // Type of the command data
- floatType,
- // Function pointer
- &resprolld
- },
- // resp pitch setpoint subtype
- {
- // ID
- 0x0A,
- // Command text
- "resppitch",
- // Type of the command data
- floatType,
- // Function pointer
- &resppitch
- },
- // resp pitch p constant subtype
- {
- // ID
- 0x0B,
- // Command text
- "resppitchp",
- // Type of the command data
- floatType,
- // Function pointer
- &resppitchp
- },
- // resp pitch d constant subtype
- {
- // ID
- 0x0C,
- // Command text
- "resppitchd",
- // Type of the command data
- floatType,
- // Function pointer
- &resppitchd
- },
- // resp throttle setpoint subtype
- {
- // ID
- 0x0D,
- // Command text
- "respthrottle",
- // Type of the command data
- floatType,
- // Function pointer
- &respthrottle
- },
- // resp throttle p constant subtype
- {
- // ID
- 0x0E,
- // Command text
- "respthrottlep",
- // Type of the command data
- floatType,
- // Function pointer
- &respthrottlep
- },
- // resp throttle i constant subtype
- {
- // ID
- 0x0F,
- // Command text
- "respthrottlei",
- // Type of the command data
- floatType,
- // Function pointer
- &respthrottlei
- },
- // resp throttle d constant subtype
- {
- // ID
- 0x10,
- // Command text
- "respthrottled",
- // Type of the command data
- floatType,
- // Function pointer
- &respthrottled
- }
- }
- },
-
- // UPDATE
- {
- // Message Type ID
- 0x04,
-
- // Update Subtypes
- {
- // NONE subtype
- {
- // ID
- 0x00,
- // Command text
- "update",
- // Type of the command data
- stringType,
- // Function pointer
- &update
- }
- }
- },
-
- // LOG
- {
- // Message Type ID
- 0x05,
-
- // Log Subtypes
- {
- // NONE subtype
- {
- // ID
- 0x00,
- // Command text
- "log",
- // Type of the command data
- stringType,
- // Function pointer
- &logdata
- },
- // Response subtype
- {
- // ID
- 0x01,
- // Command text
- "response",
- // Type of the command data
- stringType,
- // Function pointer
- &response
- }
- }
- },
-
-};
-
-int debug(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- printf("function for debug\n");
- return 0;
-}
-
-int update(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- unsigned char update[28];
- memcpy(update, ((float *)packet), 28);
-
- int packetId = getInt(update, 0);
- float y_pos = getFloat(update, 4);
- float x_pos = getFloat(update, 8);
- float alt_pos = getFloat(update, 12);
- float roll = getFloat(update, 16);
- float pitch = getFloat(update, 20);
- float yaw = getFloat(update, 24);
-
- structs->log_struct.currentQuadPosition.packetId = packetId;
- structs->log_struct.currentQuadPosition.y_pos = y_pos;
- structs->log_struct.currentQuadPosition.x_pos = x_pos;
- structs->log_struct.currentQuadPosition.alt_pos = alt_pos;
- structs->log_struct.currentQuadPosition.roll = roll;
- structs->log_struct.currentQuadPosition.pitch = pitch;
- structs->log_struct.currentQuadPosition.yaw = yaw;
-
- printf("QUAD: VRPN Packet:");
- printf("Packet ID: %d\n", packetId);
- printf("Y Position: %f\n", y_pos);
- printf("X Position: %f\n", x_pos);
- printf("Altitude Position: %f\n", alt_pos);
- printf("Roll: %f\n", roll);
- printf("Pitch: %f\n", pitch);
- printf("Yaw: %f\n", yaw);
-
- printf("function for update\n");
- return 0;
-}
-
-// Why is this here?
-// This should be on the ground station side
-int logdata(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- printf("Logging: %s\n", packet);
- return 0;
-}
-
-int response(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- printf("This is the response: %s\n", packet);
-
- return 0;
-}
-
-// ------------------------------------------------------------------
-
-int setyaw(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- float value;
-
- memcpy(&value, ((float *)packet), dataLen);
-
- printf("%f\n", value);
-
- structs->setpoint_struct.desiredQuadPosition.yaw = value;
-
- printf("function for setyaw: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
-
- return 0;
-}
-
-int setyawp(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- float value;
-
- memcpy(&value, ((float *)packet), dataLen);
- structs->parameter_struct.yaw_angle_pid.Kp = value;
-
- printf("function for setyawp: %f\n", structs->parameter_struct.yaw_angle_pid.Kp);
-
- return 0;
-}
-
-int setyawd(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- float value;
-
- memcpy(&value, ((float *)packet), dataLen);
- structs->parameter_struct.yaw_angle_pid.Kd = value;
-
- printf("function for setyawd: %f\n", structs->parameter_struct.yaw_angle_pid.Kd);
-
- return 0;
-}
-
-int setroll(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- float value;
-
- memcpy(&value, ((float *)packet), dataLen);
- structs->setpoint_struct.desiredQuadPosition.roll = value;
-
- printf("function for setroll: %f\n", structs->setpoint_struct.desiredQuadPosition.roll);
-
- return 0;
-}
-
-int setrollp(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- float value;
-
- memcpy(&value, ((float *)packet), dataLen);
- structs->parameter_struct.local_y_pid.Kp = value;
-
- printf("function for setrollp: %f\n", structs->parameter_struct.local_y_pid.Kp);
-
- return 0;
-}
-
-int setrolld(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- float value;
-
- memcpy(&value, ((float *)packet), dataLen);
- structs->parameter_struct.local_y_pid.Kd = value;
-
- printf("function for setrolld: %f\n", structs->parameter_struct.local_y_pid.Kd);
-
- return 0;
-}
-
-int setpitch(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- float value;
-
- memcpy(&value, ((float *)packet), dataLen);
- structs->setpoint_struct.desiredQuadPosition.pitch = value;
-
- printf("function for setpitch: %f\n", structs->setpoint_struct.desiredQuadPosition.pitch);
-
- return 0;
-}
-
-int setpitchp(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- float value;
-
- memcpy(&value, ((float *)packet), dataLen);
- structs->parameter_struct.local_x_pid.Kp = value;
-
- printf("function for setpitchp: %f\n", structs->parameter_struct.local_x_pid.Kp);
-
- return 0;
-}
-
-int setpitchd(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- float value;
-
- memcpy(&value, ((float *)packet), dataLen);
- structs->parameter_struct.local_x_pid.Kd = value;
-
- printf("function for setpitchd: %f\n", structs->parameter_struct.local_x_pid.Kd);
-
- return 0;
-}
-
-// ------------------------------------------------------------
-// These should be renamed to altitude!
-int setthrottle(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- float value;
-
- memcpy(&value, ((float *)packet), dataLen);
- structs->setpoint_struct.desiredQuadPosition.alt_pos = value;
-
- printf("function for setthrottle: %f\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
-
- return 0;
-}
-
-int setthrottlep(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- float value;
-
- memcpy(&value, ((float *)packet), dataLen);
- structs->parameter_struct.alt_pid.Kp = value;
-
- printf("function for setthrottlep: %f\n", structs->parameter_struct.alt_pid.Kp);
-
- return 0;
-}
-
-int setthrottlei(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- float value;
-
- memcpy(&value, ((float *)packet), dataLen);
- structs->parameter_struct.alt_pid.Ki = value;
-
- printf("function for setthrottlei: %f\n", structs->parameter_struct.alt_pid.Ki);
-
- return 0;
-}
-
-int setthrottled(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- float value;
-
- memcpy(&value, ((float *)packet), dataLen);
- structs->parameter_struct.alt_pid.Kd = value;
-
- printf("function for setthrottled: %f\n", structs->parameter_struct.alt_pid.Kd);
-
- return 0;
-}
-int getyaw(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int getyawp(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int getyawd(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- return 0;
-}
-int getroll(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int getrollp(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int getrolld(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int getpitch(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int getpitchp(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int getpitchd(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int getthrottle(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int getthrottlep(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int getthrottlei(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int getthrottled(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-
-// These should be renamed to altitude!
-// ------------------------------------------------------------
-
-
-
-
-int getgyro(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- printf("function for getgyro\n");
- return 0;
-}
-
-int getpitchangle(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- printf("function for getpitchangle\n");
- return 0;
-}
-
-int getrollangle(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- printf("function for getrollangle\n");
- return 0;
-}
-
-
-int getaccel(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- printf("function for getaccel\n");
- return 0;
-}
-
-
-int respgyro(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- printf("function for respgyro\n");
- return 0;
-}
-
-int resppitchangle(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- printf("function for resppitchangle\n");
- return 0;
-}
-
-int resprollangle(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- printf("function for resprollangle\n");
- return 0;
-}
-
-
-int respaccel(unsigned char *packet, int dataLen, modular_structs_t *structs)
-{
- printf("function for respaccel\n");
- return 0;
-}
-
-int respyaw(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int respyawp(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int respyawd(unsigned char *packet, int dataLen, modular_structs_t *structs) {
- return 0;
-}
-int resproll(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int resprollp(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int resprolld(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int resppitch(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int resppitchp(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int resppitchd(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int respthrottle(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int respthrottlep(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int respthrottlei(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
-int respthrottled(unsigned char *packet, int dataLen, modular_structs_t *structs){
- return 0;
-}
diff --git a/groundStation/src/backend/commands.h b/groundStation/src/backend/commands.h
deleted file mode 100644
index b9c9956ea..000000000
--- a/groundStation/src/backend/commands.h
+++ /dev/null
@@ -1,105 +0,0 @@
-#ifndef _COMMANDS_H
-#define _COMMANDS_H
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include "type_def.h"
-
-
-// ----------------------
-// Helper stuff
-
-#define MAX_TYPE 6
-#define MAX_SUBTYPE 100
-
-enum Message{
- BEGIN_CHAR = 0xBE,
- END_CHAR = 0xED
-};
-
-// This should also have double to avoid confusion with float values.
-enum DataType
-{
- floatType,
- intType,
- stringType
-};
-
-// MESSAGE SUBTYPES
-struct MessageSubtype{
- char ID;
- char cmdText[100];
- char cmdDataType;
- int (*functionPtr)(unsigned char *command, int dataLen, modular_structs_t *structs);
-};
-
-// MESSAGE TYPES
-struct MessageType{
- char ID;
- struct MessageSubtype subtypes[MAX_SUBTYPE];
-};
-
-int debug(unsigned char *c, int dataLen, modular_structs_t *structs);
-int update(unsigned char *c, int dataLen, modular_structs_t *structs);
-int logdata(unsigned char *c, int dataLen, modular_structs_t *structs);
-int response(unsigned char *packet, int dataLen, modular_structs_t *structs);
-int setyaw(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setyawp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setyawd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setroll(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setrollp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setrolld(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setpitch(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setpitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setpitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setthrottle(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setthrottlep(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setthrottlei(unsigned char *c, int dataLen, modular_structs_t *structs);
-int setthrottled(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getyaw(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getyawp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getyawd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getroll(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getrollp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getrolld(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getpitch(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getpitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getpitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getthrottle(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getthrottlep(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getthrottlei(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getthrottled(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getaccel(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getgyro(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getpitchangle(unsigned char *c, int dataLen, modular_structs_t *structs);
-int getrollangle(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respaccel(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respgyro(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resppitchangle(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resprollangle(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respyaw(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respyawp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respyawd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resproll(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resprollp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resprolld(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resppitch(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resppitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resppitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respthrottle(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respthrottlep(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respthrottlei(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respthrottled(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respaccel(unsigned char *c, int dataLen, modular_structs_t *structs);
-int respgyro(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resppitchangle(unsigned char *c, int dataLen, modular_structs_t *structs);
-int resprollangle(unsigned char *c, int dataLen, modular_structs_t *structs);
-
-float getFloat(unsigned char* str, int pos);
-int getInt(unsigned char* str, int pos);
-
-// TODO add in string to be read from the command line when sending a subtype of message
-extern struct MessageType MessageTypes[MAX_TYPE];
-
-#endif
\ No newline at end of file
diff --git a/groundStation/src/backend/communication.c b/groundStation/src/backend/communication.c
deleted file mode 100644
index 2dacd03f9..000000000
--- a/groundStation/src/backend/communication.c
+++ /dev/null
@@ -1,271 +0,0 @@
-#include "communication.h"
-#include "commands.h"
-#include <string.h>
-#include <ctype.h>
-
-static int msgNum = 0;
-
-tokenList_t tokenize(char* cmd) {
- int maxTokens = 16;
- tokenList_t ret;
- ret.numTokens = 0;
- ret.tokens = malloc(sizeof(char*) * maxTokens);
- ret.tokens[0] = NULL;
-
- int i = 0;
- char* token = strtok(cmd, " ");
- while (token != NULL && i < maxTokens - 1) {
- ret.tokens[i++] = token;
- ret.tokens[i] = NULL;
- ret.numTokens++;
- token = strtok(NULL, " ");
- }
-
- return ret;
-}
-
-int checkFloat(char *floatString, float *value) {
- char *tmp;
- *value = strtod(floatString, &tmp);
- if(!(isspace(*tmp) || *tmp == 0)) {
- fprintf(stderr, "%s is not a valid floating-point number\n", floatString);
- return 0;
- }
- return 1;
-}
-
-int checkInt(char *intString, int *value) {
- char *tmp;
- long temp_long;
- temp_long = strtol(intString, &tmp, 0); // base 10 number inputted
- if(temp_long < INT_MIN || temp_long > INT_MAX || !(isspace(*tmp) || *tmp == 0)) {
- fprintf(stderr, "%s is not a valid integer number\n", intString);
- return 0;
- }
- printf("temp: %ld\n\n", temp_long);
- *value = (int) temp_long;
- return 1;
-}
-
-//--------------------------------
-// Ground Station
-//--------------------------------
-
-// Formatting commands from ground station CLI
-int formatCommand(char *command, unsigned char **formattedCommand) {
- //fprintf(stderr, "length = %li , received '%s'\n", strlen(command), command);
- char cmd[strlen(command)];
- strncpy(cmd, command, strlen(command));
- cmd[strlen(command)] = '\0';
-
- tokenList_t tokens = tokenize(cmd);
- float floatValue = 0.0;
- int intValue = 0;
- int valid;
- metadata_t metadata;
-
- // ----------------------------------------------
- if(tokens.numTokens > 0) {
- for(int type = 0; type < MAX_TYPE; type++)
- {
- for(int subtype = 0; subtype < MAX_SUBTYPE; subtype++)
- {
- if(strcmp(tokens.tokens[0], MessageTypes[type].subtypes[subtype].cmdText) == 0)
- {
- printf("Sending\n\ttype: %d, \n\tsubtype: %d\n\tcommand: %s\n", type, subtype, MessageTypes[type].subtypes[subtype].cmdText);
-
- // Make sure the second token is the right type
- switch (MessageTypes[type].subtypes[subtype].cmdDataType)
- {
- // Validate the float input
- case floatType:
- metadata.begin_char = (char) BEGIN_CHAR;
- metadata.msg_type = MessageTypes[type].ID;
- metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
-
- if(MessageTypes[type].ID == 0x01) {
- valid = checkFloat(tokens.tokens[1], &floatValue);
- if(!valid) {
- return -1;
- }
- metadata.data_len = sizeof(floatValue);
- } else {
- metadata.data_len = 0;
- }
-
- metadata.msg_id = msgNum++;
- formatPacket(&metadata, &floatValue, formattedCommand);
- break;
-
- // Validate the integer input
- case intType:
- metadata.begin_char = (char) BEGIN_CHAR;
- metadata.msg_type = MessageTypes[type].ID;
- metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
-
- if(MessageTypes[type].ID == 0x01) {
- valid = checkInt(tokens.tokens[1], &intValue);
- if(!valid) {
- return -1;
- }
- metadata.data_len = sizeof(intValue);
- } else {
- metadata.data_len = 0;
- }
-
- metadata.msg_id = msgNum++;
- formatPacket(&metadata, &intValue, formattedCommand);
-
- break;
-
- // Validate the string input (doesn't need to happen)
- case stringType:
- metadata.begin_char = (char) BEGIN_CHAR;
- metadata.msg_type = MessageTypes[type].ID;
- metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
- metadata.msg_id = msgNum++;
- metadata.data_len = strlen(tokens.tokens[1]);
-
- formatPacket(&metadata, &tokens.tokens[1], formattedCommand);
-
- break;
- default:
- return -1;
- }
- return 0;
- }
- }
- }
- }
-
- // Only gets here if the command does not exist
- return -1;
-}
-// QUAD & Ground Station
-// Format the log data from log_message
-//int formatData(unsigned char *log_msg, unsigned char *formattedCommand)
-int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand)
-{
- *formattedCommand = malloc(sizeof(unsigned char) * metadata->data_len + 8);
- //----------------------------------------------------------------------------------------------
- // index|| 0 | 1 | 2 | 3 & 4 | 5 & 6 | 7+ | end |
- //---------------------------------------------------------------------------------------------|
- // msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
- //-------------------------------------------------------------------------------------------- |
- // bytes|| 1 | 1 | 1 | 2 | 2 | var | 1 |
- //----------------------------------------------------------------------------------------------
-
- // Begin Char:
- (*formattedCommand)[0] = metadata->begin_char;
-
- // Msg type:
- (*formattedCommand)[1] = metadata->msg_type;
-
- // Msg subtype
- (*formattedCommand)[2] = metadata->msg_subtype;
-
- //Msg id (msgNum is 2 bytes)
- (*formattedCommand)[3] = (metadata->msg_id & 0x000000ff);
- (*formattedCommand)[4] = ((metadata->msg_id >> 8) & 0x000000ff);
-
- // Data length and data - bytes 5&6 for len, 7+ for data
- (*formattedCommand)[5] = metadata->data_len & 0x000000ff;
- (*formattedCommand)[6] = (metadata->data_len >> 8) & 0x000000ff;
-
- memcpy(&((*formattedCommand)[7]), data, metadata->data_len);
-
- // Checksum
- // receive data and calculate checksum
- int i;
- char data_checksum;
- for(i = 0; i < 7 + metadata->data_len; i++)
- {
- data_checksum ^= (*formattedCommand)[i];
- }
-
- (*formattedCommand)[7 + metadata->data_len] = data_checksum;
-
- return 0;
-}
-
-// returns the length of the data in bytes (datalen from packet) and fills data
-// and metadata with the packet information
-// use as follows:
-//
-// packet is the entire packet message (formatted)
-// data is an unallocated (char *) (pass it to this function as &data)
-// meta_data is a pointer to an instance of metadata_t
-//
-int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data)
-{
- //----------------------------------------------------------------------------------------------
- // index|| 0 | 1 | 2 | 3 & 4 | 5 & 6 | 7+ | end |
- //---------------------------------------------------------------------------------------------|
- // msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
- //-------------------------------------------------------------------------------------------- |
- // bytes|| 1 | 1 | 1 | 2 | 2 | var | 1 |
- //----------------------------------------------------------------------------------------------
-
- // first byte must be the begin char
- if(packet[0] != 0xBE)
- return -1;
-
- // receive metadata
- meta_data->begin_char = packet[0];
- meta_data->msg_type = packet[1];
- meta_data->msg_subtype = packet[2];
- meta_data->msg_id = (packet[4] << 8) | (packet[3]);
- meta_data->data_len = (packet[6] << 8) | (packet[5]);
- unsigned char packet_checksum = packet[7+meta_data->data_len];
- //fprintf(stderr, "datalen: %d\n", meta_data->data_len);
-
- int i;
-
- // receive data
- *data = malloc(meta_data->data_len);
- for(i = 0; i < meta_data->data_len; i++)
- {
- (*data)[i] = packet[7+i];
- }
-
- // calculate checksum
- unsigned char calculated_checksum = 0;
- for(i = 0; i < meta_data->data_len + 7; i++)
- {
- calculated_checksum ^= packet[i];
- }
-
- // compare checksum
- if(packet_checksum != calculated_checksum)
- fprintf(stderr, "Checksums did not match (Parse Packet): 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum);
-
- return 0;
-}
-
-// QUAD & Ground Station
-// Process the command received
-int processCommand(unsigned char *packet, modular_structs_t *structs) {
- int validPacket;
- unsigned char *data;
- metadata_t metadata;
-
- // Validate the message is correctly formatted
- validPacket = parse_packet(packet, &data, &metadata);
- if(validPacket != 0) {
- return -1;
- }
-
- if(metadata.data_len >= 0) {
- /* Null check*/
- if (MessageTypes[(unsigned char)metadata.msg_type].subtypes[
- (unsigned char) metadata.msg_subtype].functionPtr) {
- // Call the appropriate subtype function
- (* (MessageTypes[(unsigned char)metadata.msg_type].subtypes[(unsigned char)metadata.msg_subtype].functionPtr))(data, metadata.data_len, structs);
- }
-
- return 0;
- }
-
- // Only gets here if there is an error
- return -1;
-}
diff --git a/groundStation/src/backend/communication.h b/groundStation/src/backend/communication.h
deleted file mode 100644
index 746b9f5d4..000000000
--- a/groundStation/src/backend/communication.h
+++ /dev/null
@@ -1,19 +0,0 @@
-#ifndef _COMMUNICATION_H
-#define _COMMUNICATION_H
-
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-#include <limits.h>
-#include "type_def.h"
-
-tokenList_t tokenize(char* cmd);
-int checkFloat(char *floatString, float *value);
-int checkInt(char *intString, int *value);
-int formatCommand(char *command, unsigned char **formattedCommand);
-int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand);
-int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data);
-int processCommand(unsigned char *command, modular_structs_t *structs);
-int logData(unsigned char *log_msg, unsigned char *formattedCommand);
-
-#endif
\ No newline at end of file
diff --git a/groundStation/src/backend/config.h b/groundStation/src/backend/config.h
deleted file mode 100644
index bf88e6e2b..000000000
--- a/groundStation/src/backend/config.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef __CONFIG_H
-#define __CONFIG_H
-
-
-#define DEFAULT_SOCKET "/var/run/ucart.socket"
-#define SOCKET_ENV "UCART_SOCKET"
-#define NOQUAD_ENV "UCART_NO_QUAD"
-#define NOVRPN_ENV "UCART_NO_VRPN"
-
-
-// If you are planning on using any of these env vars and you have
-// exported them with normal user rights. You will need to run the
-// backend with sudo elevation and with the --preserve-env flag or -E
-#define QUAD_WIFI_ENV "UCART_USE_WIFI"
-#define QUAD_IP_ENV "UCART_QUAD_IP"
-#define QUAD_IP_DEFAULT "192.168.4.1"
-#define QUAD_PORT_ENV "UCART_QUAD_PORT"
-#define QUAD_PORT_DEFAULT 8080
-
-#endif
diff --git a/groundStation/src/backend/logger.c b/groundStation/src/backend/logger.c
deleted file mode 100644
index 028cfeac1..000000000
--- a/groundStation/src/backend/logger.c
+++ /dev/null
@@ -1,108 +0,0 @@
-/* Author: Kris Burney
- *
- * Logger file for holding functions pertaining to loging to a file.
- */
-#include "logger.h"
-#include <stdio.h>
-#include <err.h>
-#include <pthread.h>
-
-static FILE * quadlog_file = NULL;
-static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
-
-int createLogFile(int argc, char* argv)
-{
- if (quadlog_file != NULL) {
- return -1;
- }
-
- if (pthread_mutex_lock(&mutex)) {
- err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
- }
-
- char log_file[300];
- strcpy(log_file, "logs/");
- if(argc >= 2)
- {
- strncat(log_file, argv, 294);
- printf("Creating log file '%s'...\n",log_file);
- quadlog_file = fopen(log_file, "a");
- } else {
- time_t rawtime;
- char timestr [30];
- time(&rawtime);
- sprintf(timestr,"%s",ctime(&rawtime));
-
- // Lets convert space to _ in
- char *p = timestr;
- size_t i = 0;
- while(i < strlen(timestr))
- {
- if (*p == ' ')
- *p = '_';
- i++;
- p++;
- }
-
- // timestr has a weird char at the end of it.
- // we will not include it in our file name
- strncat(log_file, timestr, strlen(timestr) -1 );
- strcat(log_file, ".txt");
- printf("Creating log file '%s'...\n",log_file);
- quadlog_file = fopen(log_file, "a");
- }
- if (pthread_mutex_unlock(&mutex)) {
- err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
- }
- return 0;
-}
-
-int updateLogFile(const struct ucart_vrpn_TrackerData * td)
-{
- int retval;
-
- if (pthread_mutex_lock(&mutex)) {
- err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
- }
-
- retval = fprintf(quadlog_file,
- "FPS: %lf Pos (xyz): (%lf %lf %lf) Att (pry): (%lf %lf %lf)\n",
- td->fps, td->x, td->y, td->z, td->pitch, td->roll, td->yaw);
-
- if (pthread_mutex_unlock(&mutex)) {
- err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
- }
-
- return retval;
-}
-
-int writeStringToLog(const char * string)
-{
- int retval;
-
- if (pthread_mutex_lock(&mutex)) {
- err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
- }
-
- retval = fprintf(quadlog_file, "%s", string);
-
- if (pthread_mutex_unlock(&mutex)) {
- err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
- }
-
- return retval;
-}
-
-void closeLogFile(void)
-{
- if (pthread_mutex_lock(&mutex)) {
- err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
- }
-
- fclose(quadlog_file);
-
- if (pthread_mutex_unlock(&mutex)) {
- err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
- }
-
-}
diff --git a/groundStation/src/backend/logger.h b/groundStation/src/backend/logger.h
deleted file mode 100644
index edfb365ff..000000000
--- a/groundStation/src/backend/logger.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/* Author: Kris Burney
- *
- * Logger header file for holding info pertaining to loging to a file.
- */
-#ifndef _LOGGER_H
-#define _LOGGER_H
-
-#include <fcntl.h>
-#include <time.h>
-#include <stdio.h>
-#include <string.h>
-
-#include "vrpn_tracker.hpp"
-
-int createLogFile(int, char*);
-int writeStringToLog(const char*);
-int updateLogFile(const struct ucart_vrpn_TrackerData* );
-void closeLogFile();
-
-
-#endif
diff --git a/groundStation/src/backend/old_main.cold b/groundStation/src/backend/old_main.cold
deleted file mode 100644
index 5234bcc56..000000000
--- a/groundStation/src/backend/old_main.cold
+++ /dev/null
@@ -1,73 +0,0 @@
-#include <libcli.h>
-#include <stdlib.h>
-#include <sys/types.h>
-#include <sys/socket.h>
-#include <string.h>
-#include <strings.h>
-#include <signal.h>
-#include <unistd.h>
-#include <arpa/inet.h>
-
-
-#define CLITEST_PORT 8000
-
-int fn_help(struct cli_def *cli, const char *command, char *argv[], int argc);
-
-struct cli_def * cli;
-int on = 1;
-
-int main(int argc, char **argv)
-{
- int s, x;
- struct sockaddr_in addr;
-
- if ((s = socket(AF_INET, SOCK_STREAM, 0)) < 0)
- {
- perror("socket");
- return 1;
- }
- setsockopt(s, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on));
-
- memset(&addr, 0, sizeof(addr));
- addr.sin_family = AF_INET;
- addr.sin_addr.s_addr = htonl(INADDR_ANY);
- addr.sin_port = htons(CLITEST_PORT);
-
- if (bind(s, (struct sockaddr *) &addr, sizeof(addr)) < 0)
- {
- perror("bind");
- return 1;
- }
-
- if (listen(s, 50) < 0)
- {
- perror("listen");
- return 1;
- }
-
- printf("Listening on port %d\n", CLITEST_PORT);
-
- cli = cli_init();
- cli_set_banner(cli, "Welcome to my test cli");
-
- cli_register_command(cli, NULL, "helpme", fn_help, PRIVILEGE_UNPRIVILEGED, MODE_EXEC, "displays help info for a command");
-
-
-
-
- while((x = accept(s, NULL, NULL)))
- {
- cli_loop(cli, x);
- close(x);
- }
-
-
- cli_done(cli);
-}
-
-
-int fn_help(struct cli_def *cli, const char *command, char *argv[], int argc) {
- printf("you ran the help function!!\n");
- return 0;
-}
-
diff --git a/groundStation/src/backend/type_def.h b/groundStation/src/backend/type_def.h
deleted file mode 100644
index 55dca9e8f..000000000
--- a/groundStation/src/backend/type_def.h
+++ /dev/null
@@ -1,361 +0,0 @@
-/*
- * struct_def.h
- *
- * Created on: Mar 2, 2016
- * Author: ucart
- */
-
-#ifndef TYPE_DEF_H_
-#define TYPE_DEF_H_
-
-/**
- * @brief
- * The modes for autonomous and manual flight.
- *
- */
-enum flight_mode{
- AUTO_FLIGHT_MODE,
- MANUAL_FLIGHT_MODE
-};
-
-//----------------------------------------------------------------------------------------------
-// index|| 0 | 1 | 2 | 3 & 4 | 5 & 6 | 7+ | end |
-//---------------------------------------------------------------------------------------------|
-// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
-//-------------------------------------------------------------------------------------------- |
-// bytes|| 1 | 1 | 1 | 2 | 2 | var | 1 |
-//----------------------------------------------------------------------------------------------
-typedef struct {
- char begin_char;
- char msg_type;
- char msg_subtype;
- int msg_id;
- int data_len;
-} metadata_t;
-
-
-// String builder data type
-typedef struct stringBuilder_s {
- char* buf;
- int length;
- int capacity;
- int maxCapacity;
-
- // Methods
- int (*addStr)(struct stringBuilder_s*, char*);
- int (*addStrAt)(struct stringBuilder_s*, char*, int);
- int (*addChar)(struct stringBuilder_s*, char);
- int (*addCharAt)(struct stringBuilder_s*, char, int);
- int (*removeCharAt)(struct stringBuilder_s*, int);
- void (*clear)(struct stringBuilder_s*);
-} stringBuilder_t;
-
-typedef struct {
- char** tokens;
- int numTokens;
-} tokenList_t;
-
-typedef struct commands{
- int pitch, roll, yaw, throttle;
-}commands;
-
-typedef struct raw{
- int x,y,z;
-}raw;
-typedef struct PID_Consts{
- float P, I, D;
-}PID_Consts;
-
-//Camera system info
-typedef struct {
- int packetId;
-
- double y_pos;
- double x_pos;
- double alt_pos;
-
- double yaw;
- double roll;
- double pitch;
-} quadPosition_t;
-
-typedef struct {
- float yaw;
- float roll;
- float pitch;
- float throttle;
-} quadTrims_t;
-
-//Gyro, accelerometer, and magnetometer data structure
-//Used for reading an instance of the sensor data
-typedef struct {
-
- // GYRO
- //Xint16 raw_gyro_x, raw_gyro_y, raw_gyro_z;
-
- float gyro_xVel_p; // In degrees per second
- float gyro_yVel_q;
- float gyro_zVel_r;
-
- // ACCELEROMETER
- //Xint16 raw_accel_x, raw_accel_y, raw_accel_z;
-
- float accel_x; //In g
- float accel_y;
- float accel_z;
-
- float accel_roll;
- float accel_pitch;
-
-
- // MAG
- //Xint16 raw_mag_x, raw_mag_y, raw_mag_z;
-
- float heading; // In degrees
-
- float mag_x; //Magnetic north: ~50 uT
- float mag_y;
- float mag_z;
-
-
-
-}gam_t;
-
-typedef struct PID_t {
- double current_point; // Current value of the system
- double setpoint; // Desired value of the system
- float Kp; // Proportional constant
- float Ki; // Integral constant
- float Kd; // Derivative constant
- double prev_error; // Previous error
- double acc_error; // Accumulated error
- double pid_correction; // Correction factor computed by the PID
- float dt; // sample period
-} PID_t;
-
-typedef struct PID_values{
- float P; // The P component contribution to the correction output
- float I; // The I component contribution to the correction output
- float D; // The D component contribution to the correction output
- float error; // the error of this PID calculation
- float change_in_error; // error change from the previous calc. to this one
- float pid_correction; // the correction output (P + I + D)
-} PID_values;
-
-///////// MAIN MODULAR STRUCTS
-/**
- * @brief
- * Holds the data inputed by the user
- *
- */
-typedef struct user_input_t {
- int rc_commands[6]; // Commands from the RC transmitter
-
-
-// float cam_x_pos; // Current x position from the camera system
-// float cam_y_pos; // Current y position from the camera system
-// float cam_z_pos; // Current z position from the camera system
-// float cam_roll; // Current roll angle from the camera system
-// float cam_pitch; // Current pitch angle from the camera system
-// float cam_yaw; // Current yaw angle from the camera system
-
- float yaw_manual_setpoint;
- float roll_angle_manual_setpoint;
- float pitch_angle_manual_setpoint;
-
- int hasPacket;
- stringBuilder_t * sb;
-} user_input_t;
-
-/**
- * @brief
- * Holds the log data to be sent to the ground station. It may hold the
- * timestamp of when a sensor's data was obtained.
- *
- */
-typedef struct log_t {
- // Time
- float time_stamp;
- float time_slice;
-
- // Id
- int packetId;
-
- gam_t gam; // Raw and calculated gyro, accel, and mag values are all in gam_t
- float phi_dot, theta_dot, psi_dot; // gimbal equation values
-
- quadPosition_t currentQuadPosition;
-
- float roll_angle_filtered, pitch_angle_filtered;
- float lidar_altitude;
-
- float pid_P_component, pid_I_component, pid_D_component; // use these generically for any PID that you are testing
-
- // PID coefficients and errors
- PID_t local_x_PID, local_y_PID, altitude_PID;
- PID_t angle_yaw_PID, angle_roll_PID, angle_pitch_PID;
- PID_t ang_vel_yaw_PID, ang_vel_roll_PID, ang_vel_pitch_PID;
-
- PID_values local_x_PID_values, local_y_PID_values, altitude_PID_values;
- PID_values angle_yaw_PID_values, angle_roll_PID_values, angle_pitch_PID_values;
- PID_values ang_vel_yaw_PID_values, ang_vel_roll_PID_values, ang_vel_pitch_PID_values;
-
- // RC commands
- commands commands;
-
- //trimmed values
- quadTrims_t trims;
-
- int motors[4];
-
-} log_t;
-
-/**
- * @brief
- * Holds the raw data from the sensors and the timestamp if available
- *
- */
-typedef struct raw_sensor {
- int acc_x; // accelerometer x data
- int acc_x_t; // time of accelerometer x data
-
- int acc_y; // accelerometer y data
- int acc_y_t; // time of accelerometer y data
-
- int acc_z; // accelerometer z data
- int acc_z_t; // time of accelerometer z data
-
-
- int gyr_x; // gyroscope x data
- int gyr_x_t; // time of gyroscope x data
-
- int gyr_y; // gyroscope y data
- int gyr_y_t; // time of gyroscope y data
-
- int gyr_z; // gyroscope z data
- int gyr_z_t; // time of gyroscope z data
-
- int ldr_z; //lidar z data (altitude)
- int ldr_z_t; //time of lidar z data
-
- gam_t gam;
-
- // Structures to hold the current quad position & orientation
- quadPosition_t currentQuadPosition;
-
-} raw_sensor_t;
-
-/**
- * @brief
- * Holds the processed data from the sensors and the timestamp if available
- *
- */
-typedef struct sensor {
- int acc_x; // accelerometer x data
- int acc_x_t; // time of accelerometer x data
-
- int acc_y; // accelerometer y data
- int acc_y_t; // time of accelerometer y data
-
- int acc_z; // accelerometer z data
- int acc_z_t; // time of accelerometer z data
-
-
- int gyr_x; // gyroscope x data
- int gyr_x_t; // time of gyroscope x data
-
- int gyr_y; // gyroscope y data
- int gyr_y_t; // time of gyroscope y data
-
- int gyr_z; // gyroscope z data
- int gyr_z_t; // time of gyroscope z data
-
- int ldr_z; //lidar z data (altitude)
- int ldr_z_t; //time of lidar z data
-
- float pitch_angle_filtered;
- float roll_angle_filtered;
- float lidar_altitude;
-
- float phi_dot, theta_dot, psi_dot;
-
- // Structures to hold the current quad position & orientation
- quadPosition_t currentQuadPosition;
- quadTrims_t trims;
-
-} sensor_t;
-
-/**
- * @brief
- * Holds the setpoints to be used in the controller
- *
- */
-typedef struct setpoint_t {
- quadPosition_t desiredQuadPosition;
-} setpoint_t;
-
-/**
- * @brief
- * Holds the parameters that are specific to whatever type of
- * control algorithm is being used
- *
- */
-typedef struct parameter_t {
- PID_t roll_angle_pid, roll_ang_vel_pid;
- PID_t pitch_angle_pid, pitch_ang_vel_pid;
- PID_t yaw_ang_vel_pid;
- PID_t local_x_pid;
- PID_t local_y_pid;
- PID_t yaw_angle_pid;
- PID_t alt_pid;
-} parameter_t;
-
-/**
- * @brief
- * Holds user defined data for the controller
- *
- */
-typedef struct user_defined_t {
- int flight_mode;
- int engaging_auto;
-} user_defined_t;
-
-/**
- * @brief
- * Holds the raw actuator values before processing
- *
- */
-typedef struct raw_actuator_t {
-
- int controller_corrected_motor_commands[6];
-
-} raw_actuator_t;
-
-/**
- * @brief
- * Holds processed commands to go to the actuators
- *
- */
-typedef struct actuator_command_t {
- int pwms[4];
-} actuator_command_t;
-
-/**
- * @brief
- * Structures to be used throughout
- */
-typedef struct {
- user_input_t user_input_struct;
- log_t log_struct;
- raw_sensor_t raw_sensor_struct;
- sensor_t sensor_struct;
- setpoint_t setpoint_struct;
- parameter_t parameter_struct;
- user_defined_t user_defined_struct;
- raw_actuator_t raw_actuator_struct;
- actuator_command_t actuator_command_struct;
-}modular_structs_t;
-
-//////// END MAIN MODULAR STRUCTS
-
-#endif /* TYPE_DEF_H_ */
diff --git a/groundStation/src/backend/vrpn_tracker.cpp b/groundStation/src/backend/vrpn_tracker.cpp
deleted file mode 100644
index 7f9b56a99..000000000
--- a/groundStation/src/backend/vrpn_tracker.cpp
+++ /dev/null
@@ -1,178 +0,0 @@
-#include <iostream>
-#include <algorithm>
-#include <functional>
-
-#include "vrpn_Tracker.h"
-#include "quat.h"
-
-#include "vrpn_tracker.hpp"
-
-namespace microcart
-{
- static void VRPN_CALLBACK vrpn_cb(void * param, const vrpn_TRACKERCB t);
-
- TrackerData::TrackerData() :
- x(0.0), y(0.0), z(0.0),
- pitch(0.0), roll(0.0), yaw(0.0),
- fps(0.0), timestamp()
- {
- }
-
- Tracker::Tracker(std::string server) :
- remote(server.c_str()),
- stop_flag(0),
- trackerData()
- {
- }
-
- Tracker::Tracker(const char * server) :
- remote(server),
- stop_flag(0),
- trackerData()
- {
- remote.register_change_handler(this, vrpn_cb);
-
- stop_flag = 0;
- vrpn_thread = std::thread(&Tracker::vrpn_loop, this);
- }
-
- Tracker::~Tracker()
- {
- {
- std::lock_guard<std::mutex> guard(vrpn_mutex);
- stop_flag = 1;
- }
- vrpn_thread.join();
- }
-
- const struct TrackerData Tracker::getData(void)
- {
- std::lock_guard<std::mutex> guard(vrpn_mutex);
- return trackerData;
- }
-
- void Tracker::vrpn_loop(void)
- {
- while (1) {
- remote.mainloop();
- {
- std::lock_guard<std::mutex> guard(vrpn_mutex);
-
- if (stop_flag) {
- return;
- }
- }
- }
- }
-
- void Tracker::callback(const vrpn_TRACKERCB t)
- {
- std::lock_guard<std::mutex> guard(vrpn_mutex);
-
- q_vec_type euler;
- q_to_euler(euler, t.quat);
-
- trackerData.x = (double) t.pos[0];
- trackerData.y = (double) t.pos[1];
- trackerData.z = (double) t.pos[2];
-
- trackerData.roll = (double) euler[2];
- trackerData.pitch = (double) euler[1];
- trackerData.yaw = (double) euler[0];
-
- timeval elapsed_time;
- timersub(&t.msg_time, &trackerData.timestamp, &elapsed_time);
- trackerData.timestamp = t.msg_time;
-
- double elapsed_time_usec = (double) elapsed_time.tv_usec +
- ((1000000.0) * (double) elapsed_time.tv_sec);
-
- trackerData.fps = 1.0 / elapsed_time_usec;
-
- auto td = trackerData;
- for(auto i = cb_vector.begin(); i != cb_vector.end(); ++i) {
- (*i)(td);
- }
- }
-
- void Tracker::addCallback(std::function<void(const TrackerData&)> cb)
- {
- cb_vector.push_back(cb);
- }
-
- static void VRPN_CALLBACK vrpn_cb(void * param, const vrpn_TRACKERCB t)
- {
- Tracker * inst = (Tracker *)(param);
- inst->callback(t);
- }
-}
-
-/* C Interface stuff */
-struct ucart_vrpn_tracker {
- microcart::Tracker * t;
-};
-
-void cb_wrapper(void (*cb)(struct ucart_vrpn_TrackerData *),
- const microcart::TrackerData &td)
-{
- struct ucart_vrpn_TrackerData data;
- data.x = td.x;
- data.y = td.y;
- data.z = td.z;
- data.pitch = td.pitch;
- data.roll = td.roll;
- data.yaw = td.yaw;
- data.fps = td.fps;
- (*cb)(&data);
-}
-
-extern "C"
-{
- struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
- const char * server)
- {
- try {
- auto inst = new struct ucart_vrpn_tracker;
- inst->t = new microcart::Tracker(server);
- return inst;
- } catch(...) {
- return NULL;
- }
- }
-
- void ucart_vrpn_tracker_freeInstance(struct ucart_vrpn_tracker * inst)
- {
- delete inst->t;
- delete inst;
- }
-
- int ucart_vrpn_tracker_addCallback(struct ucart_vrpn_tracker * inst,
- void (*cb)(struct ucart_vrpn_TrackerData *))
- {
- try {
- auto new_cb = bind(cb_wrapper, cb, std::placeholders::_1);
- inst->t->addCallback(new_cb);
- } catch(...) {
- return -1;
- }
- return 0;
- }
-
- int ucart_vrpn_tracker_getData(struct ucart_vrpn_tracker *inst,
- struct ucart_vrpn_TrackerData *td)
- {
- try {
- auto data = inst->t->getData();
- td->x = data.x;
- td->y = data.y;
- td->z = data.z;
- td->pitch = data.pitch;
- td->roll = data.roll;
- td->yaw = data.yaw;
-
- return 0;
- } catch(...) {
- return -1;
- }
- }
-}
diff --git a/groundStation/src/backend/vrpn_tracker.hpp b/groundStation/src/backend/vrpn_tracker.hpp
deleted file mode 100644
index d10ad6e79..000000000
--- a/groundStation/src/backend/vrpn_tracker.hpp
+++ /dev/null
@@ -1,100 +0,0 @@
-/* Author: Jake Drahos
- *
- * VPRN tracker module header file.
- */
-
-#ifndef _MICROCART_VRPN_TRACKER_HPP
-#define _MICROCART_VRPN_TRACKER_HPP
-
-#ifdef __cplusplus
-#include <mutex>
-#include <thread>
-#include <functional>
-#include <vector>
-
-#include "vrpn_Tracker.h"
-#endif
-
-#include <sys/time.h>
-
-#ifdef __cplusplus
-extern "C"
-{
-#endif
- struct ucart_vrpn_tracker;
- struct ucart_vrpn_TrackerData {
- double x;
- double y;
- double z;
-
- double pitch;
- double roll;
- double yaw;
-
- double fps;
- struct timeval timestamp;
- };
-
- struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
- const char * server);
-
- void ucart_vrpn_tracker_freeInstance(struct ucart_vrpn_tracker * inst);
-
- int ucart_vrpn_tracker_addCallback(struct ucart_vrpn_tracker * inst,
- void (*cb)(struct ucart_vrpn_TrackerData *));
-
- int ucart_vrpn_tracker_getData(struct ucart_vrpn_tracker * inst,
- struct ucart_vrpn_TrackerData * td);
-
-#ifdef __cplusplus
-}
-#endif
-
-#ifdef __cplusplus
-namespace microcart
-{
- struct TrackerData {
- public:
- double x;
- double y;
- double z;
-
- double pitch;
- double roll;
- double yaw;
-
- double fps;
- timeval timestamp;
-
- TrackerData();
- };
-
- class Tracker {
- public:
- const struct TrackerData getData(void);
- void callback(const vrpn_TRACKERCB t);
-
- Tracker(std::string server);
- Tracker(const char * server);
- ~Tracker();
-
- void addCallback(std::function<void(const TrackerData &)> cb);
- private:
- int stop_flag;
- TrackerData trackerData;
- std::thread vrpn_thread;
- std::mutex vrpn_mutex;
-
- vrpn_Tracker_Remote remote;
-
- void vrpn_loop(void);
-
- std::vector<std::function<void(const TrackerData &)>> cb_vector;
- };
-
-
-}
-/* __cplusplus */
-#endif
-
-#endif
diff --git a/groundStation/src/cli/cli.c b/groundStation/src/cli/cli.c
deleted file mode 100644
index 58fe14b8c..000000000
--- a/groundStation/src/cli/cli.c
+++ /dev/null
@@ -1,53 +0,0 @@
-#include <stdio.h>
-#include <string.h>
-#include <err.h>
-#include <libgen.h>
-
-#include "cli.h"
-
-int main(int argc, char **argv)
-{
- int cmdID = -1;
- char * command;
- int i , useSymlink = 0;
- struct backend_conn *conn;
-
- command = basename(argv[0]);
- for(i = 0; i < MAX_COMMANDS; ++i) {
- if (strncmp(command, commandNames[i], strlen(commandNames[i])) == 0)
- {
- cmdID = i;
- useSymlink = 1;
- }
- }
-
- if(cmdID == -1) {
- command = argv[1];
- for(i = 0; i < MAX_COMMANDS; ++i) {
- if (strncmp(command, commandNames[i], strlen(commandNames[i])) == 0)
- {
- cmdID = i;
- }
- }
- }
-
- if(cmdID == -1){
- printf("Could not match input with a command. Please try again...\n");
- return -1;
- }
-
- printf("Parsed Command : %s\n", commandNames[cmdID]);
- conn = ucart_backendConnect();
- if(conn == NULL) {
- return -1;
- }
-
- if(useSymlink) {
- (*cli_functions[cmdID]) (conn, argc, &argv[0]);
- }else {
- (*cli_functions[cmdID]) (conn, argc-1, &argv[1]);
- }
-
- ucart_backendDisconnect(conn);
- return 0;
-}
diff --git a/groundStation/src/cli/cli.h b/groundStation/src/cli/cli.h
deleted file mode 100644
index f4c2fe3f4..000000000
--- a/groundStation/src/cli/cli.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef __CLI_H
-#define __CLI_H
-
-#include "cli_monitor.h"
-#include "cli_setpid.h"
-#include "cli_getpid.h"
-// #include "cli_getimu.h"
-enum CommandNameIds{
- CMD_MONITOR,
- CMD_GETPID,
- CMD_SETPID,
- CMD_GETIMU,
- MAX_COMMANDS
-};
-
-typedef int (*cli_function_ptr)(struct backend_conn *, int, char **);
-static cli_function_ptr cli_functions[] = {
- &cli_monitor,
- &cli_getpid,
- &cli_setpid
- //&cli_getimu
-};
-
-static char* commandNames[MAX_COMMANDS] = {
- "monitor",
- "getpid",
- "setpid",
- "getImu"
-};
-
-
-#endif
diff --git a/groundStation/src/cli/cli_getimu.h b/groundStation/src/cli/cli_getimu.h
deleted file mode 100644
index 3f69e4b15..000000000
--- a/groundStation/src/cli/cli_getimu.h
+++ /dev/null
@@ -1,9 +0,0 @@
-#ifndef CLI_GETIMU_H
-#define CLI_GETIMU_H
-#include "frontend_getimu.h"
-
-int cli_getimu(
- struct backend_conn * conn,
- struct frontend_pid_data * pid_data);
-
-#endif
\ No newline at end of file
diff --git a/groundStation/src/cli/cli_getpid.c b/groundStation/src/cli/cli_getpid.c
deleted file mode 100644
index bb887ca24..000000000
--- a/groundStation/src/cli/cli_getpid.c
+++ /dev/null
@@ -1,105 +0,0 @@
-#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;
- 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}
- };
-
- 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;
- }
- }
-
- int result;
- if(getAll) {
- pid_data.controller = PID_ROLL;
- if ((result = getValues(conn, &pid_data))) {
- return result;
- }
- pid_data.controller = PID_PITCH;
- if ((result = getValues(conn, &pid_data))) {
- return result;
- }
- pid_data.controller = PID_YAW;
- 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;
- }
- }
- }
-
- return 0;
-}
-
-int getValues(struct backend_conn * conn, struct frontend_pid_data * pid_data) {
- if(frontend_getpid(conn, pid_data)) {
- return 1;
- }
-
- return 0;
-
- 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;
- default :
- break;
- }
- return 0;
-}
diff --git a/groundStation/src/cli/cli_getpid.h b/groundStation/src/cli/cli_getpid.h
deleted file mode 100644
index f5c0b15e4..000000000
--- a/groundStation/src/cli/cli_getpid.h
+++ /dev/null
@@ -1,9 +0,0 @@
-#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
diff --git a/groundStation/src/cli/cli_monitor.c b/groundStation/src/cli/cli_monitor.c
deleted file mode 100644
index 4494e65b0..000000000
--- a/groundStation/src/cli/cli_monitor.c
+++ /dev/null
@@ -1,102 +0,0 @@
-#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;
-}
diff --git a/groundStation/src/cli/cli_monitor.h b/groundStation/src/cli/cli_monitor.h
deleted file mode 100644
index 308721bb8..000000000
--- a/groundStation/src/cli/cli_monitor.h
+++ /dev/null
@@ -1,14 +0,0 @@
-#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
diff --git a/groundStation/src/cli/cli_setpid.c b/groundStation/src/cli/cli_setpid.c
deleted file mode 100644
index 2fdc16fdb..000000000
--- a/groundStation/src/cli/cli_setpid.c
+++ /dev/null
@@ -1,81 +0,0 @@
-#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;
-}
diff --git a/groundStation/src/cli/cli_setpid.h b/groundStation/src/cli/cli_setpid.h
deleted file mode 100644
index 87fb0a4f4..000000000
--- a/groundStation/src/cli/cli_setpid.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#ifndef _CLI_SETPID_H
-#define _CLI_SETPID_H
-
-#include "frontend_setpid.h"
-
-int cli_setpid(struct backend_conn * conn, int argc, char ** argv);
-
-#endif
diff --git a/groundStation/src/frontend/frontend_common.c b/groundStation/src/frontend/frontend_common.c
deleted file mode 100644
index cbedbe42a..000000000
--- a/groundStation/src/frontend/frontend_common.c
+++ /dev/null
@@ -1,93 +0,0 @@
-#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);
-}
diff --git a/groundStation/src/frontend/frontend_common.h b/groundStation/src/frontend/frontend_common.h
deleted file mode 100644
index c8ea78771..000000000
--- a/groundStation/src/frontend/frontend_common.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#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
diff --git a/groundStation/src/frontend/frontend_getpid.c b/groundStation/src/frontend/frontend_getpid.c
deleted file mode 100644
index fbde309c3..000000000
--- a/groundStation/src/frontend/frontend_getpid.c
+++ /dev/null
@@ -1,46 +0,0 @@
-#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;
-}
diff --git a/groundStation/src/frontend/frontend_getpid.h b/groundStation/src/frontend/frontend_getpid.h
deleted file mode 100644
index 977ae97f3..000000000
--- a/groundStation/src/frontend/frontend_getpid.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#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
diff --git a/groundStation/src/frontend/frontend_setpid.c b/groundStation/src/frontend/frontend_setpid.c
deleted file mode 100644
index 58bf4109c..000000000
--- a/groundStation/src/frontend/frontend_setpid.c
+++ /dev/null
@@ -1,82 +0,0 @@
-#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;
-}
diff --git a/groundStation/src/frontend/frontend_setpid.h b/groundStation/src/frontend/frontend_setpid.h
deleted file mode 100644
index f1deb5d3d..000000000
--- a/groundStation/src/frontend/frontend_setpid.h
+++ /dev/null
@@ -1,17 +0,0 @@
-#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
diff --git a/groundStation/src/frontend/frontend_tracker.c b/groundStation/src/frontend/frontend_tracker.c
deleted file mode 100644
index 94597b63c..000000000
--- a/groundStation/src/frontend/frontend_tracker.c
+++ /dev/null
@@ -1,42 +0,0 @@
-#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;
-}
diff --git a/groundStation/src/frontend/frontend_tracker.h b/groundStation/src/frontend/frontend_tracker.h
deleted file mode 100644
index 5fa82487c..000000000
--- a/groundStation/src/frontend/frontend_tracker.h
+++ /dev/null
@@ -1,27 +0,0 @@
-#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
diff --git a/groundStation/src/frontend/pid_common.h b/groundStation/src/frontend/pid_common.h
deleted file mode 100644
index c40c4fb42..000000000
--- a/groundStation/src/frontend/pid_common.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#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
diff --git a/groundStation/src/vrpn b/groundStation/src/vrpn
deleted file mode 160000
index 99c54dbef..000000000
--- a/groundStation/src/vrpn
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 99c54dbefe04897cc7c146101a126f62c0e65f97
--
GitLab