diff --git a/documentation/how_to_demo.md b/documentation/how_to_demo.md
index 37e5015548119ecbe628fc38014197a4f5e5e237..8dcdb69e4648221698274ba6acacda374e852127 100644
--- a/documentation/how_to_demo.md
+++ b/documentation/how_to_demo.md
@@ -11,11 +11,11 @@ Follow this How-To to get the quadcopter up and running in Coover 3050.
1. To start up the camera system, log into the camera system computer (co3050-07) with the following username and password:
username: `camera` <br />
- password: `camera`
+ password: `Camera`
2. Once the OS is done loading, start up the program "Tracking Tools from the desktop".
3. From the startup window, choose "Open existing project"
-4. From `D:\TrackingToolsData\FromLaptop` choose "TrackingToolsProject 2017-04-24 3.28pm" in the "Optitrack_Configuration" folder
+4. From `D:\TrackingToolsData\FromLaptop` choose "TrackingToolsProject 2018-XX-XX X.XXpm", where the X's are the most recent project, in the "Optitrack_Configuration" folder
6. Then go to File -> Open and choose "Microcart.tra" in the same folder
- This should create a "UAV" under "Trackables" in the Project Explorer on the left side of the screen
7. Now you should be able to move the quadcopter trackable around in the tracking area, and see it update in real-time on the screen.
@@ -40,9 +40,19 @@ make vrpn
To make the actual ground station program, e.g. if there have been any changes, run `make` from the `groundStation` folder.
+## Setup Backend
+Navigate to the backend directory, from the ground station folder.
+```bash
+$ cd src/backend
+```
+
+Check that in config.h [`NUM_TRACKABLES`](../groundStation/src/backend/config.h#L128) is set correctly, usually 1.
+
+Check that in config.c the [`trackables`](../groundStation/src/backend/config.c#L16) array is setup correctly. By default the quad is first element in the array.
+
## Setup Transmitter
The RC transmitter is used to manually control the quad.
-1. Ensure the transmitter has the following state before turning it on:
+1. Ensure the transmitter has the following state before turning it on (Note these are marked on the controller):
- "Gear" is set to 0
- "Flap" is set to 1
- Throttle is set to the lowest position
diff --git a/groundStation/.gitignore b/groundStation/.gitignore
index 1506eb2ddc298701945fd3b082237458c7a83fd3..626d0161fb7b86803021a9a1bc0e6dd9b6733432 100644
--- a/groundStation/.gitignore
+++ b/groundStation/.gitignore
@@ -52,6 +52,8 @@ setsource
getsource
getparam
setparam
+gettrackable
+settrackable
#Qt
gui/MicroCART/Makefile
diff --git a/groundStation/Makefile b/groundStation/Makefile
index c59f815d284e45c10b1302a76c5edfe819133702..b9c4448111551e0ec67f036d52c0a9ce40a7bfba 100644
--- a/groundStation/Makefile
+++ b/groundStation/Makefile
@@ -23,7 +23,7 @@ CLIBINARY=Cli
CLISRCDIR=src/cli
CLISOURCES := $(wildcard $(CLISRCDIR)/*.c)
CLIOBJECTS = $(CLISOURCES:$(CLISRCDIR)/%.c=$(OBJDIR)/%.o)
-SYMLINKS= getnodes addnode getparam setparam getsource setsource getoutput
+SYMLINKS= getnodes addnode getparam setparam getsource setsource getoutput gettrackable settrackable
# Frontend-common stuff
FESRCDIR=src/frontend
diff --git a/groundStation/README.md b/groundStation/README.md
index 586465beddb0ec00447dfabd55c902d303da0f89..259c52199225376d2a16ad730ebb2b29a58a9ccb 100644
--- a/groundStation/README.md
+++ b/groundStation/README.md
@@ -27,7 +27,12 @@ If you wish to change the way the backend communicates to the quad and vice vers
This provides a list of environment variables which you can set and use for your computer or time of use.
## Using
-First, the backend daemon must be running. Run the backend with
+
+First, if you are using any adapters they must be started before the backend, so do so now.
+By default you will not be using adapters and you can continue if you do not intend to use
+this feature.
+
+Next, the backend daemon must be running. Run the backend with
```bash
./BackEnd
```
@@ -87,3 +92,13 @@ There are a couple of already written bash scripts in the `scripts/` folder.
- [Getting Started](./documentation/getting_started.md)
- [Packet Information](./documentation/packets.md)
+
+## Adapters
+
+Adapters are the MicroCART way of connecting to generic trackables that are not one of the MicroCART quads. There is one
+adapter created in house that allows the use of a Crazyflie through the Matt_test branch of [Custom_Crazyflie_Software](https://git.ece.iastate.edu/danc/Custom_CrazyFlie_Software/tree/Matt_test_branch) repo.
+This repository is part of the DANC lab and can be access similar to the MicroCART repositry. This branch makes their ground
+station allow connects in the same manner that our backend does. In order to use this adapter
+you must first start the lateral on quad ground station in the repository mentioned above, then run the crazyflie adapter.
+After this process is setup make sure that the backend config is set properly for this new trackable and it can be controlled
+as if it were one of our quads.
diff --git a/groundStation/adapters/.gitignore b/groundStation/adapters/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..df7921f534416aae37e6c5a13e993ea3145fa625
--- /dev/null
+++ b/groundStation/adapters/.gitignore
@@ -0,0 +1,5 @@
+test_connection
+test_adapter
+test_connection.o
+test_adapter.o
+*.socket
diff --git a/groundStation/adapters/README.md b/groundStation/adapters/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..86e7c5a5e8d1888fc878ee5ccbcd460c66c31c94
--- /dev/null
+++ b/groundStation/adapters/README.md
@@ -0,0 +1,62 @@
+# ADAPTERS
+Adapters are what allows for the current microcart groundstation to communicate with other vehicles. The adapters act as a translator by receiving packets from groundstation and then call the packets corrisponding callback. The developer can then modify callback functions to communicate.
+
+## CREATE NEW:
+There are 3 major components that are provided for creation of a new custom adapter.
+ - adapterlib - has functions to retrieve single groundstation packets from a socket
+ - callbacks - has predefined functions for every type of instruction supported by groundstation
+ - demoadapter - example code to set up a unix socket, read from it, and call the corresponding callback function
+
+These files rely upon groundstation header files and c files which are linked in the ./inc directory and should be included when compiling (for example see the crazyflie adapter makefile)
+
+To create your own adapter:
+
+ - You will need to modify the callbacks c file to interface with your vehicle.
+ - Modify demoadapter as needed to set up communications with your vehicle.
+ - Modify the trackables struct in groundstation.
+
+## WARNINGS:
+Running the executables provided use relative links that will be relative to your current working directory.
+
+## FUNCTIONS:
+This is a list of what groundstation and quad software functions are required.
+
+ - packet.c/h - DecodePacket()
+ - commands.h - metadata struct
+ - commands.c - MessageTypes, findCommand()
+ - callbacks.h - Typedef for command_cb
+
+###INCLUDES LOCATIONS:
+This is a list of all the required files to include to be able to use the function listed above. *(Note these paths are relative to this README)*
+
+ - ../../quad/inc/biquad_filter.h
+ - ../src/backend/bitwise.h
+ - ../../quad/inc/cb_default.h
+ - ../../quad/src/commands/commands.c
+ - ../../quad/inc/commands.h
+ - ../../quad/inc/communication.h
+ - ../../quad/inc/computation_graph.h
+ - ../../quad/inc/controllers.h
+ - ../../quad/inc/hw_iface.h
+ - ../../quad/inc/log_data.h
+ - ../src/backend/packet.c
+ - ../src/backend/packet.h
+ - ../../quad/inc/PID.h
+ - ../../quad/inc/timer.h
+ - ../../quad/inc/type_def.h
+ - ../../quad/src/quad_app/util.c
+ - ../../quad/src/quad_app/util.h
+
+See Crazyflie [Makefile](crazyflie/Makefile) for an example
+
+## TIPS:
+using ncat (nc) you can created sockets for testing your adapter
+```bash
+ nc -l -U <socketfilename> #creates a unix socket server for testing
+ nc -U <socketfilename> #creates a unix socket cient for testing
+```
+
+
+## ADAPTERS:
+- [Crazyflie](crazyflie/README.md)
+
diff --git a/groundStation/adapters/crazyflie/Makefile b/groundStation/adapters/crazyflie/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..60b42e7702cf306e935e375252a9f86d19278358
--- /dev/null
+++ b/groundStation/adapters/crazyflie/Makefile
@@ -0,0 +1,66 @@
+#compiler to use
+CC=gcc
+
+#directories that may contain src files
+SRCDIR = src inc .
+OBJDIR = . obj
+
+#symlink directories
+QUADINC = ../../../../quad/inc
+QUADSRC = ../../../../quad/src/
+BACKEND = ../../../src/backend
+INC = inc
+
+#cflags to use
+CFLAGS=-Iinc/ -std=c99
+
+#lists of all .h .o .c files used to compile
+H = $(foreach sdir,$(SRCDIR),$(wildcard $(sdir)/*.h))
+O = $(foreach sdir,$(OBJDIR),$(wildcard $(sdir)/*.o))
+C = $(foreach sdir,$(SRCDIR),$(wildcard $(sdir)/*.c))
+
+all: inc obj lib
+ mkdir bin/
+ $(CC) obj/*.o -o bin/cf_adapter
+
+
+obj: inc
+ $(CC) -c $(C) $(H) $(CFLAGS)
+ mkdir obj
+ mv *.o obj
+
+lib: obj inc
+ mkdir lib/
+ ar rc cf_adapterlib.a obj/*.o
+ mv *.a lib/
+
+inc:
+ mkdir $(INC)
+ ln -s $(QUADINC)/biquad_filter.h $(INC)
+ ln -s $(BACKEND)/bitwise.h $(INC)
+ ln -s $(QUADINC)/cb_default.h $(INC)
+ ln -s $(QUADSRC)/commands/commands.c $(INC)
+ ln -s $(QUADINC)/commands.h $(INC)
+ ln -s $(QUADINC)/communication.h $(INC)
+ ln -s $(QUADINC)/computation_graph.h $(INC)
+ ln -s $(QUADINC)/controllers.h $(INC)
+ ln -s $(QUADINC)/hw_iface.h $(INC)
+ ln -s $(QUADINC)/log_data.h $(INC)
+ ln -s $(BACKEND)/packet.c $(INC)
+ ln -s $(BACKEND)/packet.h $(INC)
+ ln -s $(QUADINC)/PID.h $(INC)
+ ln -s $(QUADINC)/timer.h $(INC)
+ ln -s $(QUADINC)/type_def.h $(INC)
+ ln -s $(QUADSRC)/quad_app/util.c $(INC)
+ ln -s $(QUADSRC)/quad_app/util.h $(INC)
+
+clean:
+ rm -rf obj/
+ rm -rf lib/
+ rm -rf bin/
+ rm -rf inc/
+ rm -rf *.o
+ rm -rf inc/*.gch
+ rm -rf src/*.gch
+
+
diff --git a/groundStation/adapters/crazyflie/README.md b/groundStation/adapters/crazyflie/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..0ebde8d84ce2c07377290330552a2df236b96ef2
--- /dev/null
+++ b/groundStation/adapters/crazyflie/README.md
@@ -0,0 +1,44 @@
+# Adapter Setup
+
+For the Crazyflie Adapter there is a modified CrazyFlie groundstation that must be used to allow communication.
+This is available in the DANC group in the Custom_CrazyFlie_Software repo in the [Matt_test_branch](https://git.ece.iastate.edu/danc/Custom_CrazyFlie_Software/tree/Matt_test_branch).
+Clone this repo and navigate into the groundstation-LateralOnQuad. Inside of the src
+directory open the quadcopterData.cpp file. There are two variables that you have to check:
+radios and crazyflie_info. For radios everything should be uncommented, and for crazyflie_info
+make sure that you have the crazyflie that you are intending to use uncommented. The fourth
+member of the struct is the channel number that specifies that crazyflie, and this should
+match the piece of tape connected to the crazyflie you are using. Then go to the inc
+directory in the quadcopterData.hpp and make sure that NUM_RADIOS and NUM_QUADS are set
+correctly, usually 1. Now from base directory in the groundstation run make and there
+should be a crazyflieGroundstation that can be run. When run, a socket will be created by
+the name of crazyflie_groundstation.socket in the same directory. The path to this socket
+needs to be specified in [cf_adapter.c](./src/cf_adapter.c#L12).
+
+# HOW TO BUILD:
+from the Microcart/groundstation/adapters/crazyflie/ directory
+```bash
+ make clean
+ make all
+```
+
+this will compile the adapter to the ./bin directory
+
+# TO RUN ADAPTER:
+
+First, turn on the crazyflie you want to use and run the Crazyflie groundstation metioned in Adapter Setup. This will connect to and setup the crazyflie and will prompt you to press
+enter. Upon doing so move onto the next steps to run the adapter.
+
+
+***NOTE to change where the sockets are located you must change the define statements in ./src/cf_adaper.c***
+
+***NOTE 2 Running the executables provided use relative links that will be relative to your current working directory.***
+
+- Ensure that the socket that the adapter is creating will be at the same location that the groundstation will look (see notes above).
+- compile the adapter (see HOW TO BUILD above)
+- from the Microcart/groundstation/adapters/crazyflie/ directory run the executable with the following command
+
+```bash
+ ./bin/cf_adapter
+```
+
+
diff --git a/groundStation/adapters/crazyflie/src/cf_adapter.c b/groundStation/adapters/crazyflie/src/cf_adapter.c
new file mode 100644
index 0000000000000000000000000000000000000000..c1b5908751b141dce112f6a5bd68e01ec002bdcc
--- /dev/null
+++ b/groundStation/adapters/crazyflie/src/cf_adapter.c
@@ -0,0 +1,266 @@
+#include "cf_adapter.h"
+
+//path to socket that groundstation will read/write
+#define SOCKPATH "../cf_adapter.socket"
+
+//socket that the crazyflie groundstation will read from, check the README for information
+//on how to setup this path.
+#define DEFAULT_ADAPTER_SOCKET "../../../../test/Custom_CrazyFlie_Software/groundstation-LateralOnQuad/crazyflie_groundstation.socket"
+#define SOCKET_ENV "ADAPTER_SOCKET"
+
+struct backend_conn * cfsocketconn = NULL;
+
+//socket that groundstation will read/write
+int adptsockfd;
+
+
+/**
+ * @brief writes to a socket
+ * @param conn socket to be written to
+ * @param line char array to write to socket
+ * @return 0 on success else error
+ */
+int adapterWrite(struct backend_conn *conn, const char * line) {
+ return fputs(line, conn->socket);
+}
+
+/**
+ * @brief creates a new connection to crazyflie groundstation
+ * @param conn is the struct that will contain the new connection
+ * @return 0 if successful else error
+ */
+int connectToCrazyflieGroundStation(struct backend_conn ** conn) {
+ int s;
+ struct sockaddr_un remote;
+ char str[100];
+
+ if ((s = socket(AF_UNIX, SOCK_STREAM, 0)) == -1) {
+ perror("failure to create socket in connectToCrazyflieGroundStation");
+ exit(1);
+ }
+
+ printf("Trying to connect...");
+
+ remote.sun_family = AF_UNIX;
+ char * sock_env = getenv(SOCKET_ENV);
+ strcpy(remote.sun_path, sock_env ? sock_env : DEFAULT_ADAPTER_SOCKET);
+ if (connect(s, (struct sockaddr *)&remote, sizeof(remote)) == -1) {
+ perror("failure to connect in connectToCrazyflieGroundStation");
+ return 1;
+ }
+
+ printf("path= %s \n\r", remote.sun_path);
+
+ (*conn) = malloc(sizeof(struct backend_conn));
+ if ((*conn) == NULL) {
+ perror("failure to malloc in connectToCrazyflieGroundStation");
+ close(s);
+ return 1;
+ }
+
+ (*conn)->len = 0;
+ (*conn)->buf = NULL;
+ (*conn)->socket = fdopen(s, "r+");
+ if ((*conn)->socket == NULL) {
+ perror("failure to fdopen in connectToCrazyflieGroundStation");
+ free(*conn);
+ conn = NULL;
+ close(s);
+ return 1;
+ }
+ if (setvbuf((*conn)->socket, NULL, _IONBF, 0)) {
+ warn("setvbuf");
+ }
+
+ printf("Success\n");
+
+ /* success */
+ return 0;
+}
+/**
+ * @brief handles termination signals and cleans up all socket files
+ * @param sigNum the signal number that was sent
+ */
+void signalHandler(int sigNum){
+ printf("\n%d signal recieved, cleaning up sockets\n",sigNum);
+ close(adptsockfd);
+ unlink(SOCKPATH);
+ printf("done...\n");
+}
+
+int main(int argc, char *args[]) {
+
+ //setup signal handlers to properly shutdown adapter
+ signal(SIGINT, signalHandler);
+ signal(SIGTERM, signalHandler);
+ signal(SIGQUIT, signalHandler);
+ signal(SIGHUP, signalHandler);
+
+ //create socket and bind
+ printf("Adapter started\n\r");
+ struct sockaddr_un adptsockinfo;
+ char adptsockpath[] = SOCKPATH;
+
+ adptsockfd = socket(AF_UNIX, SOCK_STREAM, 0);
+ if(adptsockfd < 0){
+ perror("failed to create groundstation socket\n");
+ }
+ else{
+ printf("created groundstation socket\n\r");
+ }
+
+ adptsockinfo.sun_family = AF_UNIX;
+ strcpy(adptsockinfo.sun_path, SOCKPATH);
+ if(bind(adptsockfd, (struct sockaddr *) &adptsockinfo, sizeof(adptsockinfo))){
+ perror("failed to bind to groundstation socket\n");
+ }
+ else{
+ printf("bound groundstation socket\n\r");
+ }
+
+ //wait for connection, connect, and set up receive buffer
+ if(listen(adptsockfd,16)==-1){
+ close(adptsockfd);
+ unlink(SOCKPATH);
+ perror("failed listen on groundstation socket\n");
+ exit(1);
+ }
+ else {
+ printf("listening on %s\n", adptsockpath);
+ }
+
+ struct sockaddr_un gssockinfo;
+ int len;
+ int gssockfd = accept(adptsockfd, (struct sockaddr *) &gssockinfo, &len);
+ if(gssockfd == -1){
+ close(adptsockfd);
+ unlink(SOCKPATH);
+ perror("failed to accept groundstation connection\n");
+ exit(1);
+ }
+ else {
+ printf("received a connection on groundstation socket\n\r");
+ }
+
+ char recvbuf[1000];
+ memset(recvbuf,0,1000);
+ int keepRunning = 1;
+
+ struct metadata metad; //metadata struct
+ uint8_t data[MAX_PACKET_SIZE]; //array used to store the data bytes of the packet
+ memset(data,0,sizeof(data));
+ ssize_t data_len = 0;
+ ssize_t packetsize = -1;
+ int cbreturn = 0;
+
+ if (connectToCrazyflieGroundStation(&cfsocketconn)) {
+ perror("Failed to connect to cf_groundstation");
+ close(adptsockfd);
+ unlink(SOCKPATH);
+ return 1;
+ }
+
+ printf("connected to CF groundstation\n");
+ while(keepRunning){
+
+ //grab a single packet blocking call
+ packetsize = getSinglePacket(gssockfd,recvbuf, sizeof(recvbuf));
+ if(packetsize <0){
+ perror("getSinglePacket returned with a negative value");
+ close(adptsockfd);
+ unlink(SOCKPATH);
+ return 1;
+ }
+ printf("\tpacktsize %d\n", packetsize);
+
+ //pass the single packet to the DecodePacket()
+ DecodePacket(&metad,data, MAX_PACKET_SIZE, recvbuf, packetsize);
+ printf("\tdata: %s psize %d msgtype %d\n", data, packetsize, metad.msg_type);
+
+
+ //call the appropriate callback
+ //example line 87 in MicroCART/quad/src/quad_app/communication.c
+ command_cb* func_ptr = MessageTypes[metad.msg_type].functionPtr;
+ if (metad.msg_type < MAX_TYPE_ID && func_ptr) {
+ cbreturn = (* func_ptr)(NULL,&metad,data, metad.data_len);
+ if(cbreturn <0){
+ perror("callback return in error");
+ }
+ }
+
+ //clear all the buffers and reset vars
+ memset(&metad, 0,sizeof(metad));
+ memset(data,0,sizeof(data));
+ memset(recvbuf,0,sizeof(recvbuf));
+ }
+
+
+}
+/** TODO move to adapterlib.c eventually
+ * @brief polling logic that will return with a full packet
+ * @param fd the socket to read from
+ * @param packet an array of max size MAX_PACKET_SIZE to be filled with single packet
+ * @param pSize the size of the packet array parameter, must be MAX_PACKET_SIZE
+ * @returns size of the packet being put into packet, -1 means error;
+ */
+size_t getSinglePacket(int fd, uint8_t * packet, int pSize){
+
+ //local vars
+ int8_t curLoc = 0;//current index in the packet
+ int8_t error = 0; //used to check error of recvs
+ int totalPacketSize;
+
+ //check if array is big enough
+ if(pSize < MAX_PACKET_SIZE){
+ printf("\t\tpsize = %d MPS = %d", pSize, MAX_PACKET_SIZE);
+ perror("getSinglePacket pSize var is not MAX_PACKET_SIZE");
+ return -1;
+ }
+
+ //clear packet
+ memset(packet,0,MAX_PACKET_SIZE);
+
+ //Get to data size bytes to find out total packet size
+ while(curLoc<6){
+ curLoc += recv(fd, packet,6,MSG_WAITALL);
+ }
+
+ //get the total packet size
+ totalPacketSize = packetSize(*(packet+4)<<8 | (*(packet +5)));
+ printf("\t\ttotal packet size %d\n",totalPacketSize);
+
+ //check that the total packet size isnt unreasonable TODO is this a valid check?
+ if(totalPacketSize > MAX_PACKET_SIZE){
+ perror("failed packet size sanity check when recving");
+ return -1;
+ }
+
+ //grab the rest of the single packet
+ printf("\t\tattempting to grab %d more bytes\n",totalPacketSize-curLoc);
+ printf("\t\tstart at packet[ %d ]\n",curLoc);
+ error = recv(fd, packet + curLoc, totalPacketSize-curLoc, MSG_WAITALL);// blocking recv
+ if(error == -1){
+ perror("error reading packet after data size");
+ return -1;
+ }
+
+ //debug print full received packet
+ printf("\t\tfull packet ");
+ for(int i=0; i <totalPacketSize; i++) {
+ printf("%2x ",*(packet+i));
+ }
+ printf("\n");
+
+
+ return totalPacketSize;
+}
+
+/**
+ * @brief used to calculate entire packet size based off of the variable data size
+ * @param dataSize size of data in bytes
+ * @return total packet size in bytes
+ */
+size_t packetSize(int dataSize){
+ return 8+dataSize;
+}
+
diff --git a/groundStation/adapters/crazyflie/src/cf_adapter.h b/groundStation/adapters/crazyflie/src/cf_adapter.h
new file mode 100644
index 0000000000000000000000000000000000000000..c775e4262d3d3878769da65921c0863d409cf61e
--- /dev/null
+++ b/groundStation/adapters/crazyflie/src/cf_adapter.h
@@ -0,0 +1,55 @@
+#ifndef _CF_ADAPTER_H
+#define _CF_ADAPTER_H
+#include <sys/socket.h>
+#include <sys/un.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <errno.h>
+#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 <pthread.h>
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <netinet/tcp.h>
+#include <fcntl.h>
+#include <sys/stat.h>
+#include <sys/time.h>
+#include <stdlib.h>
+
+#include "commands.h"
+#include "communication.h"
+#include "packet.h"
+#include "type_def.h"
+#include "cf_callbacks.h"
+
+struct backend_conn {
+ FILE * socket;
+ size_t len;
+ char * buf;
+};
+
+void signalHandler(int sigNum);
+
+int adapterWrite(struct backend_conn *conn, const char * line);
+
+int connectToCrazyflieBackend(struct backend_conn ** conn);
+
+int main(int argc, char *argv[]);
+
+size_t getSinglePacket(int fd, uint8_t * packet, int pSize);
+
+size_t packetSize(int dataSize);
+
+#endif /* _CF_ADAPTER_H */
+
diff --git a/groundStation/adapters/crazyflie/src/cf_callbacks.c b/groundStation/adapters/crazyflie/src/cf_callbacks.c
new file mode 100644
index 0000000000000000000000000000000000000000..40595595f712166cd9a70d584a11ad4cde662ce3
--- /dev/null
+++ b/groundStation/adapters/crazyflie/src/cf_callbacks.c
@@ -0,0 +1,269 @@
+//#include "adapterlib.h"
+#include "cf_callbacks.h"
+
+/* Misc. callbacks */
+//TODO there should be a better way to do this
+extern struct backend_conn * cfsocketconn;
+
+/**
+ * Currently does nothing.
+ */
+int cb_debug(struct modular_structs *structs, struct metadata *meta, unsigned char *data, unsigned short length){
+ return -1;
+}
+
+/**
+ * counts the number of packet logs.
+ */
+int cb_packetlog(struct modular_structs* structs, struct metadata *meta, u8 *data, u16 length) {
+ return -1;
+}
+
+/**
+ * Handles a get packet logs request and sends a response
+ * with the packet log data.
+ */
+int cb_getpacketlogs(struct modular_structs* structs, struct metadata *meta, u8 *data, u16 length) {
+ return -1;
+}
+
+/*
+ * Handles receiving new location updates.
+ */
+int cb_update(struct modular_structs *structs, struct metadata *meta, unsigned char *data, unsigned short length){
+ return -1;
+}
+
+/**
+ * This is called on the ground station to begin sending VRPN to the quad.
+ */
+int cb_beginupdate(struct modular_structs *structs, struct metadata *meta, unsigned char *data, unsigned short length) {
+ return -1;
+}
+
+/*
+ * -----------------------------------------------
+ * Callbacks for control network modification/info
+ * -----------------------------------------------
+ */
+
+// Struct for holding a node ID and either a parameter, output, or input index
+struct node_ids {
+ int16_t id;
+ int16_t sub_id;
+};
+
+/*
+ * Given a data array, returns a node_ids struct retrieved from the array.
+ * Assumes the given array is at least 4 bytes to hold the data.
+ */
+struct node_ids get_node_ids(u8 *data) {
+}
+
+/**
+ * Handles a command to set a node parameter on the quad.
+ *
+ * NOTE:
+ * Expects the uart buff to have data in the following format:
+ * |--------------------------------------------------------|
+ * | data index || 0 - 1 | 2 - 3 | 4 - 7 |
+ * |--------------------------------------------------------|
+ * | parameter || node ID | node parmID | param val |
+ * |--------------------------------------------------------|
+ * | bytes || 2 | 2 | 4 |
+ * |--------------------------------------------------------|
+ *
+ * returns >0 if successful
+ * returns -1 if param not permitted
+ * returns -2 other type of error
+ */
+int cb_setparam(struct modular_structs *structs, struct metadata *meta, unsigned char *data, unsigned short length)
+{
+ char cmd[100];
+ memset(cmd, 0, sizeof(cmd));
+ char dimension;
+ int error;
+
+ //TODO for debugging
+ printf("\t\tchecking parameter ");
+ for(int i = 0; i<8; i++){
+ printf("%2x ",*(data +i));
+ }
+ printf("\n");
+
+ //check if the connection is initiallized
+ if(cfsocketconn == NULL){
+ perror("\t\tcf groundstation connection not initialized\n");
+ return -2;
+ }
+
+
+ //if node is the x, y, or z
+ if(*(data) == 9){
+ dimension = 'x';
+ }
+ else if(*(data)== 10){
+ dimension = 'y';
+ }
+ else if(*(data)== 11){
+ dimension = 'z';
+ }
+ else{
+ //not supported node to set
+ return -1;
+ }
+
+ printf("\t\tabout to write to cf groundstation\n");
+ //build float value of the the param value
+ //send to cf groundstation over socket
+ //create command string
+ switch(dimension) {
+ case'x':
+ //sprintf(cmd, "1x%1.2f",build_float(data+4));
+ sprintf(cmd, "1t\n",build_float(data+4));
+ error = adapterWrite(cfsocketconn,cmd);
+ sprintf(cmd, "1k\n",build_float(data+4));
+ error = adapterWrite(cfsocketconn,cmd);
+ break;
+ case 'y':
+ sprintf(cmd, "1y%1.2f",build_float(data+4));
+ error = adapterWrite(cfsocketconn,cmd);
+ break;
+ case 'z':
+ sprintf(cmd, "1z%1.2f",build_float(data+4));
+ error = adapterWrite(cfsocketconn,cmd);
+ break;
+ default:
+ perror("cb_setparam reached default in switch statement");
+ return(-2);
+ }
+
+ return error;
+}
+
+/**
+ * Handles a command to get a controller parameter from the quad.
+ *
+ * NOTE:
+ * Expects the uart buff to have data in the following format:
+ * |-------------------------------------------|
+ * | data index || 0 - 1 | 2 - 3 |
+ * |-------------------------------------------|
+ * | parameter || node ID | node parmID |
+ * |-------------------------------------------|
+ * | bytes || 2 | 2 |
+ * |-------------------------------------------|
+ *
+ * Sends a response of type RESPPARAM_ID.
+ * The response will have a message ID equal to the one originally received.
+ * The data of the response will be in the following format:
+ * |--------------------------------------------------------|
+ * | data index || 0 - 1 | 2 - 3 | 4 - 7 |
+ * |--------------------------------------------------------|
+ * | parameter || node ID | node parmID | param val |
+ * |--------------------------------------------------------|
+ * | bytes || 2 | 2 | 4 |
+ * |--------------------------------------------------------|
+ */
+int cb_getparam(struct modular_structs *structs, struct metadata *meta, unsigned char *data, unsigned short length)
+{
+ return -1;
+}
+
+/**
+ * Handles a command to set a node's input source
+ *
+ * NOTE:
+ * Expects the uart buff to have data in the following format:
+ * |---------------------------------------------------------------------------|
+ * | data index || 0 - 1 | 2 - 3 | 4 - 5 | 6 - 7 |
+ * |---------------------------------------------------------------------------|
+ * | parameter || dest node ID | dest input ID | src node ID | src output ID |
+ * |---------------------------------------------------------------------------|
+ * | bytes || 2 | 2 | 2 | 2 |
+ * |---------------------------------------------------------------------------|
+ *
+ * Does not send anything in response.
+ */
+int cb_setsource(struct modular_structs *structs, struct metadata *meta, unsigned char *data, unsigned short length) {
+ return -1;
+}
+
+/**
+ * Handles a command to get the source of a node's input
+ *
+ * NOTE:
+ * Expects the uart buff to have data in the following format:
+ * |---------------------------------------------|
+ * | data index || 0 - 1 | 2 - 3 |
+ * |---------------------------------------------|
+ * | parameter || node ID | node input ID |
+ * |---------------------------------------------|
+ * | bytes || 2 | 2 |
+ * |---------------------------------------------|
+ *
+ * Sends a response of type RESPSOURCE_ID.
+ * The response will have a message ID equal to the one originally received.
+ * The data of the response will be in the following format:
+ * |---------------------------------------------------------------------------|
+ * | data index || 0 - 1 | 2 - 3 | 4 - 5 | 6 - 7 |
+ * |---------------------------------------------------------------------------|
+ * | parameter || dest node ID | dest input ID | src node ID | src output ID |
+ * |---------------------------------------------------------------------------|
+ * | bytes || 2 | 2 | 2 | 2 |
+ * |---------------------------------------------------------------------------|
+ */
+int cb_getsource(struct modular_structs *structs, struct metadata *meta, unsigned char *data, unsigned short length) {
+ return -1;
+}
+
+
+/**
+ * Handles a command to get a node output value from the quad.
+ * Packet structure is the same as getparam
+ */
+int cb_getoutput(struct modular_structs *structs, struct metadata *meta, unsigned char *data, unsigned short length)
+{
+ return -1;
+}
+
+/*
+ * Handles a request for the list of nodes in the graph
+ * For N total nodes, returns data in the following format:
+ * The node IDs and type IDs are consecutive shorts
+ * The node names are null-separated
+ * |---------------------------------------------------------------|
+ * | data index || 0 - 2*N-1 | 2*N - 4*N-1 | 4*N - (< 4096) |
+ * |---------------------------------------------------------------|
+ * | parameter || Node IDs | Node type IDs | Node names |
+ * |---------------------------------------------------------------|
+ * | bytes || 2*N | 2*N | < 4096 |
+ * |---------------------------------------------------------------|
+ */
+int cb_getnodes(struct modular_structs *structs, struct metadata *meta, unsigned char *data, unsigned short length) {
+ return -1;
+}
+
+/*
+ * Handles adding a new node with a particular type and name
+ * Expects the uart buff to have data in the following format:
+ * |---------------------------------------------|
+ * | data index || 0 - 1 | 2 - ? |
+ * |---------------------------------------------|
+ * | parameter || type ID | New node name |
+ * |---------------------------------------------|
+ * | bytes || 2 | ? |
+ * |---------------------------------------------|
+ *
+ * Returns the new node ID in the following format:
+ * |-----------------------------|
+ * | data index || 0 - 1 |
+ * |-----------------------------|
+ * | parameter || node ID |
+ * |------------------------------
+ * | bytes || 2 |
+ * |-----------------------------|
+ */
+int cb_addnode(struct modular_structs *structs, struct metadata *meta, unsigned char *data, unsigned short length) {
+ return -1;
+}
diff --git a/groundStation/adapters/crazyflie/src/cf_callbacks.h b/groundStation/adapters/crazyflie/src/cf_callbacks.h
new file mode 100644
index 0000000000000000000000000000000000000000..077dbdad71807bfa5e76276cad6b19afdb04884e
--- /dev/null
+++ b/groundStation/adapters/crazyflie/src/cf_callbacks.h
@@ -0,0 +1,8 @@
+#ifndef CF_CALLBACKS_H
+#define CF_CALLBACKS_H
+
+#include <stdlib.h>
+#include "util.h"
+#include "cf_adapter.h"
+
+#endif
diff --git a/groundStation/adapters/src/adapterlib.c b/groundStation/adapters/src/adapterlib.c
new file mode 100644
index 0000000000000000000000000000000000000000..3992bd0b38e9eb8f2682d47eea840a918113af12
--- /dev/null
+++ b/groundStation/adapters/src/adapterlib.c
@@ -0,0 +1,69 @@
+#include "adapterlib.h"
+
+/**
+ * @brief polling logic that will return with a full packet
+ * @param fd the socket to read from
+ * @param packet an array of max size MAX_PACKET_SIZE to be filled with single packet
+ * @param pSize the size of the packet array parameter, must be MAX_PACKET_SIZE
+ * @returns size of the packet being put into packet, -1 means error;
+ */
+size_t getSinglePacket(int fd, uint8_t * packet, int pSize){
+
+ //local vars
+ int8_t curLoc = 0;//current index in the packet
+ int8_t error = 0; //used to check error of recvs
+ int totalPacketSize = packetSize(*(packet+4)<<8 | (*(packet +5)));
+
+ //check if array is big enough
+ if(pSize < MAX_PACKET_SIZE){
+ printf("\t\tpsize = %d MPS = %d", pSize, MAX_PACKET_SIZE);
+ perror("getSinglePacket pSize var is not MAX_PACKET_SIZE");
+ return -1;
+ }
+
+ //clear packet
+ memset(packet,0,MAX_PACKET_SIZE);
+
+ //Get to data size bytes to find out total packet size
+ while(curLoc<6){
+ curLoc += recv(fd, packet,6,MSG_WAITALL);
+ }
+
+ //get the total packet size
+ totalPacketSize = packetSize(*(packet+4)<<8 | (*(packet +5)));
+ printf("\t\ttotal packet size %d\n",totalPacketSize);
+
+ //check that the total packet size isnt unreasonable TODO is this a valid check?
+ if(totalPacketSize > MAX_PACKET_SIZE){
+ perror("failed packet size sanity check when recving");
+ return -1;
+ }
+
+ //grab the rest of the single packet
+ printf("\t\tattempting to grab %d more bytes\n",totalPacketSize-curLoc);
+ printf("\t\tstart at packet[ %d ]\n",curLoc);
+ error = recv(fd, packet + curLoc, totalPacketSize-curLoc, MSG_WAITALL);// blocking recv
+ if(error == -1){
+ perror("error reading packet after data size");
+ return -1;
+ }
+
+ //debug print full received packet
+ printf("\t\tfull packet ");
+ for(int i=0; i <totalPacketSize; i++) {
+ printf("%2x ",*(packet+i));
+ }
+ printf("\n");
+
+
+ return totalPacketSize;
+}
+
+/**
+ * @brief used to calculate entire packet size ased off of the variable data size
+ * @param dataSize size of data in bytes
+ * @return total packet size in bytes
+ */
+size_t packetSize(int dataSize){
+ return 8+dataSize;
+}
diff --git a/groundStation/adapters/src/adapterlib.h b/groundStation/adapters/src/adapterlib.h
new file mode 100644
index 0000000000000000000000000000000000000000..80ccb7ca355cc0dd55c39077da2764d082752218
--- /dev/null
+++ b/groundStation/adapters/src/adapterlib.h
@@ -0,0 +1,27 @@
+#ifndef _ADAPTERLIB_H
+#define _ADAPTERLIB_H
+
+#include "packet.h" //for parsing and encoding
+#include "commands.h" //for messagetypeid
+#include <stdint.h>
+
+/**
+ * @brief polling logic that will return with a full packet from the groundstation backend
+ * @param fd the socket to read from
+ * @param packet an array of max size MAX_PACKET_SIZE to be filled with single packet
+ * @param pSize the size of the packet array parameter, must be MAX_PACKET_SIZE
+ * @returns size of the packet being put into packet, -1 means error;
+ */
+size_t getSinglePacket(int fd, uint8_t * packet, int pSize);
+
+/**
+ * @brief used to calculate entire packet size ased off of the variable data size
+ * @param dataSize size of data in bytes
+ * @return total packet size in bytes
+ */
+size_t packetSize(int dataSize);
+
+
+#endif /* _ADAPTERLIB_H */
+
+
diff --git a/groundStation/adapters/src/demo_adapter.c b/groundStation/adapters/src/demo_adapter.c
new file mode 100644
index 0000000000000000000000000000000000000000..6b4e6756b5c30d0144e81d6f404b77ed54fadd6d
--- /dev/null
+++ b/groundStation/adapters/src/demo_adapter.c
@@ -0,0 +1,112 @@
+#include "adapterlib.h"
+
+//TODO should be moved to a config file or define
+//TODO how do we handle multiple adapters of same type?
+//TODO create configfile and add functionality to read from config.
+#define SOCKPATH "adapter.socket"
+
+int main(int argc, char *args[]) {
+
+ //create socket and bind
+ printf("Adapter started\n\r");
+ int adptsockfd;
+ struct sockaddr_un adptsockinfo;
+ char adptsockpath[] = SOCKPATH;
+
+ adptsockfd = socket(AF_UNIX, SOCK_STREAM, 0);
+ if(adptsockfd < 0){
+ perror("failed to create socket\n");
+ //TODO check if file already exists and delete it?
+ }
+ else{
+ printf("created socket\n\r");
+ }
+
+ adptsockinfo.sun_family = AF_UNIX;
+ strcpy(adptsockinfo.sun_path, SOCKPATH);
+ if(bind(adptsockfd, (struct sockaddr *) &adptsockinfo, sizeof(adptsockinfo))){
+ perror("failed to bind\n");
+ }
+ else{
+ printf("bound socket\n\r");
+ }
+
+ //wait for connection, connect, and set up receive buffer
+ if(listen(adptsockfd,16)==-1){
+ close(adptsockfd);
+ unlink(SOCKPATH);
+ perror("failed listen\n");
+ exit(1);
+ }
+ else {
+ printf("listening on %s\n", adptsockpath);
+ }
+
+ struct sockaddr_un gssockinfo;
+ int len;
+ int gssockfd = accept(adptsockfd, (struct sockaddr *) &gssockinfo, &len);
+ if(gssockfd == -1){
+ close(adptsockfd);
+ unlink(SOCKPATH);
+ perror("failed to accept groundstation connection\n");
+ exit(1);
+ }
+ else {
+ printf("received a connection\n\r");
+ }
+
+ char recvbuf[1000];
+ memset(recvbuf,0,1000);
+ int keepRunning = 1;
+
+ /* needed files
+ * MicroCART/groundStation/src/backend/packet.c - DecodePacket()
+ * MicroCART/quad/inc/commands.h - metadata struct
+ * MicroCART/quad/src/commands/commands.c - MessageTypes, findCommand()
+ * - callbacks.h. Typedef for command_cb
+ * - cb_default.h. Implementation of cb_default.
+ * - callbacks.c file. Contains implemented callbacks.
+ */
+
+ struct metadata metad; //metadata struct
+ uint8_t data[MAX_PACKET_SIZE]; //array used to store the data bytes of the packet
+ memset(data,0,sizeof(data));
+ ssize_t data_len = 0;
+ ssize_t packetsize = -1;
+ int cbreturn = 0;
+
+ while(keepRunning){
+ printf("in main loop\n");
+ //grab a single packet blocking call
+ packetsize = getSinglePacket(gssockfd,recvbuf, sizeof(recvbuf));
+ printf("\tpacktsize %d\n", packetsize);
+
+
+ //pass the single packet to the DecodePacket()
+ DecodePacket(&metad,data, MAX_PACKET_SIZE, recvbuf, packetsize);
+ printf("\tdata: %s psize %d msgtype %d\n", data, packetsize, metad.msg_type);
+
+
+ //call the appropriate callback example line 87 in MicroCART/quad/src/quad_app/communication.c
+ command_cb* func_ptr = MessageTypes[metad.msg_type].functionPtr;
+ if (metad.msg_type < MAX_TYPE_ID && func_ptr) {
+ cbreturn = (* func_ptr)(NULL,&metad,data, metad.data_len);
+ if(cbreturn <0){
+ perror("callback return in error");
+ }
+ }
+
+ //clear all the buffers and reset vars
+ memset(&metad, 0,sizeof(metad));
+ memset(data,0,sizeof(data));
+ memset(recvbuf,0,sizeof(recvbuf));
+
+
+
+ }
+
+ close(adptsockfd);
+ unlink(SOCKPATH);
+
+}
+
diff --git a/groundStation/adapters/src/demo_adapter.h b/groundStation/adapters/src/demo_adapter.h
new file mode 100644
index 0000000000000000000000000000000000000000..8bcbc876c5ae460ab9d92d7128cf9bbb10a209fd
--- /dev/null
+++ b/groundStation/adapters/src/demo_adapter.h
@@ -0,0 +1,42 @@
+#ifndef _DEMO_ADAPTER_H
+#define _DEMO_ADAPTER_H
+//TODO clean up these includes
+#include <sys/socket.h>
+#include <sys/un.h>
+#include <unistd.h>
+#include <stdio.h>
+#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 <pthread.h>
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <netinet/tcp.h>
+#include <fcntl.h>
+#include <sys/stat.h>
+#include <sys/time.h>
+#include <stdlib.h>
+
+#include "commands.h"
+#include "communication.h"
+#include "packet.h"
+#include "type_def.h"
+#include "callbacks.h"
+#include "adapterlib.h"
+
+int main(int argc, char *argv[]);
+
+
+
+#endif /* _DEMO_ADAPTER_H */
+
diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index a32de916f06618d31ead92445686d05bfd0f01ea..e8500ca51201514d5170426cbe638c6b24fe2475 100644
--- a/groundStation/src/backend/backend.c
+++ b/groundStation/src/backend/backend.c
@@ -1,8 +1,10 @@
-/* Author: Kris Burney & Jake Drahos
+/* Original Author: Kris Burney & Jake Drahos
+ * Last Edits: Matthew Kelly & Dane Larson
*
* Interface between Quad, front end, Virtual and VRPN tracker.
- * Main uses a standard socket select loop to receive data from any source described above.
- * Based on the data received from sockets it calls receive functions based on the socket received from.
+ * Main uses a standard socket select loop to receive data from any
+ * source described above. Based on the data received from sockets it
+ * calls receive functions based on the socket received from.
*/
#define _GNU_SOURCE
@@ -44,11 +46,10 @@
#include "output.h"
#include "nodes.h"
#include "bitwise.h"
+#include "backend_adapter.h"
+#include "backend_utils.h"
-#define QUAD_BT_ADDR "00:06:66:64:61:D6"
-#define QUAD_BT_CHANNEL 0x01
#define CMD_MAX_LENGTH 4096
-#define MAX_HASH_SIZE 50
/* Backend-internal command magics */
#define TD_MAGIC "TRACKERDATA"
@@ -58,7 +59,7 @@ void readAndPrint(void);
void sendVrpnPacket(struct ucart_vrpn_TrackerData *);
void getVRPNPacket(struct ucart_vrpn_TrackerData *);
void printVrpnData(struct ucart_vrpn_TrackerData *);
-int connectToZybo();
+int connectToZybo(int index);
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);
@@ -80,35 +81,25 @@ static int remove_client(int fd);
/* Receive data from client */
static void client_recv(int fd);
/* Receive data from quad */
-static void quad_recv();
+static void quad_recv(int index);
/* Checks to see if socket has disconnected. Returns 1 on disconnect, else returns 0 */
static int wasDisconnected(int fd);
/* handle controller responses from quad to frontend */
static void handleResponse(struct metadata *m, uint8_t * data);
-/* Create new dynamic logfile name */
-char * create_log_name(char * buffer, size_t max);
/* Thread-safe wrappers */
-pthread_mutex_t quadSocketMutex;
+static ssize_t readQuad(char * buf, size_t count, int index);
static ssize_t writeQuad(const uint8_t * buf, size_t count);
-static ssize_t readQuad(char * buf, size_t count);
+static ssize_t writeQuadIndex(const uint8_t * buf, size_t count, int index);
static int local_comm_channel;
-static int zybo_fifo_rx;
-static int zybo_fifo_tx;
-/* 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"; // If using 3050 lab computer to run vrpn
-//const char *TRACKER_IP = "UAV@192.168.0.194:3883"; // If using Eric's old laptop to run vrpn
-static int zyboSocket;
+static char trackable[254] = "UAV";
static int backendSocket;
+//TODO - need to remove this
struct ucart_vrpn_tracker * tracker = NULL;
#define MAX_CLIENTS 32
@@ -121,341 +112,421 @@ fd_set rfds_master;
int max_fd = 0;
static FILE * quadlog_file;
-static int quadlog_file_open;
+static int quadlog_file_open = 0;
static char user_specified_log_name[256] = "";
-pthread_mutex_t quadResponseMutex, cliInputMutex ;
-unsigned char *commandBuf;
-int newQuadResponse = 0, newCliInput = 0;
-
static void sig_handler(int s) {
- printf("Caught SIGPIPE from quad fifo..\n");
+ printf("Caught SIGPIPE from quad fifo..\n");
+}
+
+/**
+ * Handler function for exit signals. This allows the backend to properly close.
+ */
+static void sig_exit_handler(int s) {
+ printf("Exiting with condition %d\n", s);
+ keepRunning = 0;
}
// Callback to be ran whenever the tracker receives data.
-// Currently doing much more than it should. It will be slimmed down
-// in the future.
+// 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;
- sendVrpnPacket(td);
- count++;
+ static int count = 0;
+ sendVrpnPacket(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 */
- mode_t old_umask = umask(0111);
- backendSocket = socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK, 0);
- if (backendSocket < 0) {
- err(-1, "socket");
- }
-
- printf("backendSocket = %d\n", backendSocket);
-
- /* 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");
- }
- umask(old_umask);
-
- /* 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(commandBuf);
- exit(1);
- }
-
- if (!local_comm_channel) {
- printf("zyboSocket = %d\n", zyboSocket);
- } else {
- /* if we are using fifo's we don't want the quad to be able to shut us down. */
- signal(SIGPIPE, sig_handler);
- printf("zybo_fifo_rx = %d\nzybo_fifo_tx = %d\n", zybo_fifo_rx, zybo_fifo_tx);
- }
-
- if (pthread_mutex_unlock(&quadSocketMutex)) {
- err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
- }
-
- // 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);
- }
-
- if(argc >= 2)
- {
- strncat(user_specified_log_name, argv[1], strlen(argv[1]));
- }
-
- if(!getenv(NOVRPN_ENV)){
- printf("Creating VRPN tracker...\n");
- // 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);
- } else {
- printf("Ignoring VRPN information...\n");
- }
-
- struct timeval timeout = {
- .tv_sec = 1,
- .tv_usec = 0
- };
-
- 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) {
- quad_recv();
- } 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);
-
- if (!local_comm_channel) {
- safe_close_fd(zyboSocket, &quadSocketMutex);
- } else {
- safe_close_fd(zybo_fifo_rx, &quadSocketMutex);
- safe_close_fd(zybo_fifo_tx, &quadSocketMutex);
- }
-
- fclose(quadlog_file);
-
- return 0;
+ int activity;
+ int i,j;
+
+ //Setup exit signals
+ signal(SIGINT, sig_exit_handler);
+ signal(SIGABRT, sig_exit_handler);
+ signal(SIGQUIT, sig_exit_handler);
+ signal(SIGTERM, sig_exit_handler);
+
+ FD_ZERO(&rfds_master);
+
+ /* 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 */
+ mode_t old_umask = umask(0111);
+ backendSocket = socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK, 0);
+ if (backendSocket < 0) {
+ err(-1, "socket");
+ }
+
+ printf("backendSocket = %d\n", backendSocket);
+
+ /* 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");
+ }
+ umask(old_umask);
+
+ /* Listen */
+ if (listen(backendSocket, 16)) {
+ err(-1, "listen");
+ }
+
+ /* Add to socket set */
+ safe_fd_set(backendSocket, &rfds_master, &max_fd);
+
+ /* Initialize client buffers */
+ for (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;
+ }
+ }
+
+ //Socket and communication setup
+ for (i = 0; i < NUM_TRACKABLES; i++) {
+ //Lock the trackable communication mutex
+ if (pthread_mutex_lock(&(trackables[i].socket_mutex))) {
+ err(-2, "pthread_mutex_lock (%s:%d):", __FILE__, __LINE__);
+ }
+ //Check for trackable type
+ if (trackables[i].isAdapter == 0) {
+ if ((trackables[i].socket = connectToZybo(i)) < 0) {
+ perror("Error connecting to Quad...");
+ break;
+ }
+ if (!local_comm_channel) {
+ //if (trackables[i].isLocal == 0) {
+ printf("zyboSocket = %d\n", trackables[i].socket);
+ } else {
+ signal(SIGPIPE, sig_handler);
+ printf("zybo_fifo_rx = %d\nzybo_fifo_tx = %d\n", trackables[i].fifo_rx, trackables[i].fifo_tx);
+ }
+ } else {
+ printf("Starting client socket: %s\n", trackables[i].name);
+ if (adapterConnect(&(trackables[i])) != 0) {
+ perror("Error connecting to Adapter...");
+ break;
+ }
+ }
+ //Unlock the trackable communication mutex
+ if (pthread_mutex_unlock(&(trackables[i].socket_mutex))) {
+ err(-2, "pthread_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+ }
+ }
+ if (i != NUM_TRACKABLES) {
+ for (j = 0; j < (i-1); j++) {
+ if (trackables[j].isAdapter == 1) {
+ adapterDisconnect(&(trackables[j]));
+ }
+ //else if (trackables[j].isLocal == 1) {
+ else if (local_comm_channel) {
+ safe_close_fd(trackables[j].fifo_rx, &trackables[j].socket_mutex);
+ safe_close_fd(trackables[j].fifo_tx, &trackables[j].socket_mutex);
+ }
+ else {
+ if (!getenv(NOQUAD_ENV)) {
+ safe_close_fd(trackables[j].socket, &trackables[j].socket_mutex);
+ }
+ }
+ }
+ exit(1);
+ }
+
+ // watch for input from stdin (fd 0) to see when it has input
+ safe_fd_set(fileno(stdin), &rfds_master, &max_fd);
+
+ //Setup safe fd set for all trackables
+ for (i = 0; i < NUM_TRACKABLES; i++) {
+ if (trackables[i].isAdapter == 0) {
+ if (!getenv(NOQUAD_ENV)) {
+ // watch for input from the zybo socket
+ safe_fd_set(trackables[i].socket, &rfds_master, &max_fd);
+ }
+ } else {
+ // watch for input from the adapter sockets
+ safe_fd_set(trackables[i].socket, &rfds_master, &max_fd);
+ }
+ }
+
+ //Check for a user specified log name
+ if(argc >= 2) {
+ strncat(user_specified_log_name, argv[1], strlen(argv[1]));
+ }
+
+ //Create VRPN trackers if using environment variable not set
+ if(!getenv(NOVRPN_ENV)){
+ printf("Creating VRPN trackers...\n");
+ for (i = 0; i < NUM_TRACKABLES; i++) {
+ //create vrpnTracker instance
+ trackables[i].tracker = ucart_vrpn_tracker_createInstance(trackables[i].server_name);
+ // this function will be called whenever tracker receives data
+ ucart_vrpn_tracker_addCallback(trackables[i].tracker, cb);
+ }
+ //TODO - remove tracker instance
+ tracker = trackables[0].tracker;
+ printf("Attempting to Start VRPN thread... ");
+ if (ucart_vrpn_start()) {
+ warnx("Failed\n");
+ }
+ else {
+ printf("Success\n");
+ }
+ } else {
+ printf("Ignoring VRPN information...\n");
+ }
+
+ struct timeval timeout = {
+ .tv_sec = 1,
+ .tv_usec = 0
+ };
+
+ //Main backend loop
+ 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 == 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 {
+ for (i = 0; i < NUM_TRACKABLES; i++) {
+ if (fd == trackables[i].socket) {
+ quad_recv(i);
+ }
+ }
+ }
+ }
+ }
+ } else {
+ timeout.tv_sec = 1;
+ timeout.tv_usec = 0;
+ }
+ }
+
+ // Free VRPN trackers and stop VRPN Tracker Thread
+ if(!getenv(NOVRPN_ENV)){
+ if (ucart_vrpn_stop()) {
+ warnx("VRPN thread stop failed...\n");
+ } else {
+ printf("VRPN thread stopped\n");
+ }
+ for (i = 0; i < NUM_TRACKABLES; i++) {
+ ucart_vrpn_tracker_freeInstance(trackables[i].tracker);
+ }
+ }
+
+ //Disconnecting from trackables
+ printf("Disconnecting from Trackables\n");
+ for (i = 0; i < NUM_TRACKABLES; i++) {
+ if (trackables[i].isAdapter == 1) {
+ adapterDisconnect(&(trackables[i]));
+ }
+ //else if (trackables[i].isLocal == 1) {
+ else if (local_comm_channel) {
+ safe_close_fd(trackables[i].fifo_rx, &trackables[i].socket_mutex);
+ safe_close_fd(trackables[i].fifo_tx, &trackables[i].socket_mutex);
+ }
+ else {
+ if (!getenv(NOQUAD_ENV)) {
+ safe_close_fd(trackables[i].socket, &trackables[i].socket_mutex);
+ }
+ }
+ }
+
+ //Close log file if one was open
+ if (quadlog_file_open) {
+ fclose(quadlog_file);
+ }
+
+ return 0;
}
void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
- uint8_t packet[64];
- struct metadata m;
- uint8_t data[128];
-
- struct position_update u;
- u.id = currMessageID;
- u.x = info->x;
- u.y = info->y;
- u.z = info->z;
- u.pitch = info->pitch;
- u.roll = info->roll;
- u.yaw = info->yaw;
-
- if (EncodeUpdate(&m, data, 128, &u) < 0) {
- warnx("Big problems. sendVrpnPacket . EncodeUpdate");
- return;
- }
- m.msg_id = currMessageID++;
-
- ssize_t psize;
- if ((psize = EncodePacket(packet, 64, &m, data)) < 0) {
- warnx("Big problems. sendVrpnPacket. EncodePacket");
- return;
- }
-
- writeQuad(packet, psize);
+ uint8_t packet[64];
+ struct metadata m;
+ uint8_t data[128];
+
+ struct position_update u;
+ u.id = currMessageID;
+ u.x = info->x;
+ u.y = info->y;
+ u.z = info->z;
+ u.pitch = info->pitch;
+ u.roll = info->roll;
+ u.yaw = info->yaw;
+
+ if (EncodeUpdate(&m, data, 128, &u) < 0) {
+ warnx("Big problems. sendVrpnPacket . EncodeUpdate");
+ return;
+ }
+ m.msg_id = currMessageID++;
+
+ ssize_t psize;
+ if ((psize = EncodePacket(packet, 64, &m, data)) < 0) {
+ warnx("Big problems. sendVrpnPacket. EncodePacket");
+ return;
+ }
+
+ for (int i = 0; i < NUM_TRACKABLES; i++) {
+ if (strcmp(trackables[i].server_name, info->server_name) == 0) {
+ writeQuadIndex(packet, psize, i);
+ break;
+ }
+ }
}
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;
- }
+ int status;
+ //for (int i = 0; i < NUM_TRACKABLES; i++) {
+ // if (strcmp(trackable, trackables[i].name) == 0) {
+ // break;
+ // }
+ //}
+ //if ((status = ucart_vrpn_tracker_getData(trackables[i].tracker, td)) < 0) {
+ // TODO - remove tracker instance
+ if((status = ucart_vrpn_tracker_getData(tracker, td)) < 0) {
+ perror("Error receiving VRPN data from tracker...");
+ keepRunning = 0;
+ }
}
+/* void getVRPNPacket(struct ucart_vrpn_TrackerData *td, int index) {
+ int status;
+ if ((status = ucart_vrpn_tracker_getData(trackables[i].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);
+ 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 = -1;
-
- if (getenv(NOQUAD_ENV)) {
- return 0;
- }
-
- /* Use wifi by default */
- if (getenv(QUAD_COMM_ENV)) {
- /* Determine if we are using bluetooth or local */
- if(strcmp(getenv(QUAD_COMM_ENV), "local") == 0) {
- printf("Using Local Fifo Settings\n");
- local_comm_channel = 1;
- char * fifo_rx = QUAD_LOCAL_DEFAULT_TX;
- char * fifo_tx = QUAD_LOCAL_DEFAULT_RX;
-
- if (getenv(QUAD_LOCAL_RX)) {
- fifo_tx = getenv(QUAD_LOCAL_RX);
- }
- if (getenv(QUAD_LOCAL_TX)) {
- fifo_rx = getenv(QUAD_LOCAL_TX);
- }
-
- zybo_fifo_tx = open(fifo_tx, O_WRONLY | O_NONBLOCK);
- if (zybo_fifo_tx < 0) {
- warnx("Opening zybo_fifo_tx...");
- return -1;
- }
- /* Must use O_RDWR so that there is at least one writer on the fifo
- and each subsequent call to select() won't return EOF
- */
- zybo_fifo_rx = open(fifo_rx, O_RDWR | O_NONBLOCK);
- if (zybo_fifo_rx < 0) {
- warnx("Opening zybo_fifo_rx...");
- return -1;
- }
- status = 0;
- sock = zybo_fifo_rx;
- } else if (strcmp(getenv(QUAD_COMM_ENV), "bluetooth") == 0) {
- 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));
-
- int flag = 1;
- int result = setsockopt(sock, /* socket affected */
- IPPROTO_TCP, /* set option at TCP level */
- TCP_NODELAY, /* name of option */
- (char *) &flag, /* the cast is historical cruft */
- sizeof(int)); /* length of option value */
- }
-
- // connection failed
- if(status < 0)
- {
- close(sock);
- perror("connect");
- return -1;
- }
- else
- {
- printf("connection successful!...\n");
- return sock;
- }
+int connectToZybo(int index) {
+ int sock;
+ int status = -1;
+
+ if (getenv(NOQUAD_ENV)) {
+ return 0;
+ }
+
+ // Check for local comm channel or bluetooth
+ //if (trackables[index].isLocal == 1) {
+ if (getenv(QUAD_COMM_ENV) && strcmp(getenv(QUAD_COMM_ENV), "local") == 0) {
+ printf("Using Local Fifo Settings\n");
+ local_comm_channel = 1;
+ char * fifo_rx = trackables[index].fifo_tx_path;
+ char * fifo_tx = trackables[index].fifo_rx_path;
+
+ trackables[index].fifo_tx = open(fifo_tx, O_WRONLY | O_NONBLOCK);
+ if (trackables[index].fifo_tx < 0) {
+ warnx("Open zybo_fifo_tx...");
+ return -1;
+ }
+
+ trackables[index].fifo_rx = open(fifo_rx, O_RDWR | O_NONBLOCK);
+ if (trackables[index].fifo_rx < 0) {
+ warnx("Opening zybo_fifo_rx...");
+ return -1;
+ }
+ status = 0;
+ sock = trackables[index].fifo_rx;
+ }
+ else if (trackables[index].isBluetooth == 1) {
+ 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 connect to
+ addr.rc_family = AF_BLUETOOTH;
+ addr.rc_channel = trackables[index].bt_channel;
+ str2ba( trackables[index].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 (!inet_aton(trackables[index].quad_ip, &addr.sin_addr)) {
+ printf("Default IP %s is invalid\n",
+ trackables[index].quad_ip);
+ return -1;
+ }
+
+ /* Quick 'n dirty, oh yeah! */
+ addr.sin_port = htons(trackables[index].quad_port);
+
+ 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));
+
+ int flag = 1;
+ int result = setsockopt(sock, /* socket affected */
+ IPPROTO_TCP, /* set option at TCP level */
+ TCP_NODELAY, /* name of option */
+ (char *) &flag, /* the cast is historical cruft */
+ sizeof(int)); /* length of option value */
+ }
+
+ // connection failed
+ if(status < 0) {
+ close(sock);
+ perror("connect");
+ return -1;
+ } else {
+ printf("connection successful!...\n");
+ return sock;
+ }
}
/* add a fd to fd_set, and update max_fd */
@@ -481,527 +552,530 @@ int safe_fd_clr(int fd, fd_set* fds, int* max_fd) {
}
static ssize_t writeQuad(const uint8_t * 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__);
- }
-
- if (local_comm_channel) {
- retval = write(zybo_fifo_tx, buf, count);
- } else {
- retval = write(zyboSocket, buf, count);
- }
-
- if (pthread_mutex_unlock(&quadSocketMutex)) {
- err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
- }
-
- return retval;
+ ssize_t retval;
+ int index = 0;
+ for (int i = 0; i < NUM_TRACKABLES; i++) {
+ if (strcmp(trackables[i].name, trackable) == 0) {
+ index = i;
+ //printf("Index: %d\tName: %s\n", index, trackables[index].name);
+ //printf("packetToQuad = '");
+ //for(int i = 0; i < (int)count; ++i) {
+ // printf(" %.2x ", buf[i]);
+ //}
+ //printf("'\n");
+ return writeQuadIndex(buf, count, index);
+ //break;
+ }
+ }
+ //FIXME - Should never reach here
+ return -1;
+}
+
+static ssize_t writeQuadIndex(const uint8_t * buf, size_t count, int index) {
+ ssize_t retval;
+
+ //printf("packetToQuad = '");
+ //for(int i = 0; i < (int)count; ++i) {
+ // printf(" %.2x ", buf[i]);
+ //}
+ //printf("'\n");
+
+ if (trackables[index].isAdapter == 0 && getenv(NOQUAD_ENV)) {
+ return count;
+ }
+
+ if (pthread_mutex_lock(&trackables[index].socket_mutex)) {
+ err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+ }
+
+ //if (trackables[index].isLocal == 1) {
+ if (trackables[index].isAdapter == 0 && local_comm_channel) {
+ retval = write(trackables[index].fifo_tx, buf, count);
+ } else {
+ if (trackables[index].isAdapter == 0) {
+ retval = write(trackables[index].socket, buf, count);
+ } else {
+ retval = adapterWrite(trackables[index].conn, (const char *) buf, count);
+ }
+ }
+
+ if (pthread_mutex_unlock(&trackables[index].socket_mutex)) {
+ err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+ }
+
+ return retval;
}
-static ssize_t readQuad(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__);
- }
-
- if (local_comm_channel) {
- retval = read(zybo_fifo_rx, buf, count);
- } else {
- retval = read(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, int index) {
+ ssize_t retval;
+ if (trackables[index].isAdapter == 0 && getenv(NOQUAD_ENV)) {
+ return count;
+ }
+ if (pthread_mutex_lock(&trackables[index].socket_mutex)) {
+ err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+ }
+
+ retval = read(trackables[index].socket, buf, count);
+
+ if (pthread_mutex_unlock(&trackables[index].socket_mutex)) {
+ 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;
+ 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;
- }
- }
+ for (ssize_t i = 0; i < MAX_CLIENTS; i++) {
+ if (client_fds[i] == fd) {
+ return i;
+ }
+ }
- return -1;
+ 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];
- }
+ 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];
- }
+ ssize_t slot = get_client_index(fd);
+ if (slot == -1) {
+ return NULL;
+ } else {
+ return client_pending_responses[slot];
+ }
}
static int clientAddPendResponses(int fd, uint16_t packet_id) {
- int *pendingResponses = get_client_pend_responses(fd);
- for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) {
- if(pendingResponses[i] == -1) {
- pendingResponses[i] = packet_id;
- return i;
- }
- }
- return -1;
+ int *pendingResponses = get_client_pend_responses(fd);
+ for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) {
+ if(pendingResponses[i] == -1) {
+ pendingResponses[i] = packet_id;
+ return i;
+ }
+ }
+ return -1;
}
static int clientRemovePendResponses(int fd, uint16_t packet_id) {
- int *pendingResponses = get_client_pend_responses(fd);
- for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) {
- if(pendingResponses[i] == packet_id) {
- pendingResponses[i] = -1;
- return i;
- }
- }
- return -1;
+ int *pendingResponses = get_client_pend_responses(fd);
+ for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) {
+ if(pendingResponses[i] == packet_id) {
+ pendingResponses[i] = -1;
+ 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;
+ 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__);
- }
+ 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;
- char cmdString[64];
- char * cursor;
- ssize_t r;
- int index = 0;
+ char * buffer;
+ ssize_t len_pre;
+ char cmdString[64];
+ char * cursor;
+ ssize_t r;
+ int index = 0;
buffer = get_client_buffer(fd);
- len_pre = strlen(buffer);
- cursor = buffer + len_pre;
-
- r = read(fd, cursor, CLIENT_BUFFER_SIZE - len_pre - 1);
- if (r < 0) {
- warn("read (fd: %d)", fd);
- }
- buffer[len_pre + r] = '\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);
-
- char * first_word;
- char * tmp = strdup(buffer);
- // printf("tmpbuff = '%s'\n", tmp);
- first_word = strtok(tmp, " ");
- // printf("first word = '%s'\n", first_word);
-
- ssize_t msg_type, i;
- for (i = 0; i < MAX_TYPE_ID; ++i) {
- if ((msg_type = findCommand(first_word)) != -1)
- break;
- }
-
- free(tmp);
-
- if (msg_type == -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 {
- uint8_t packet[64];
- struct metadata m;
- uint8_t *data = malloc(sizeof(*data) * 128);
- ssize_t result;
- ssize_t psize;
-
- printf(" found a msg_type of %ld\n", msg_type);
-
- switch (msg_type) {
- case SETPARAM_ID:
- result = EncodeSetParam(&m, data, 128, buffer);
- break;
- case GETPARAM_ID:
- result = EncodeGetParam(&m, data, 128, buffer);
- break;
- case SETSOURCE_ID:
- result = EncodeSetSource(&m, data, 128, buffer);
- break;
- case GETSOURCE_ID:
- result = EncodeGetSource(&m, data, 128, buffer);
- break;
- case GETOUTPUT_ID:
- result = EncodeGetOutput(&m, data, 128, buffer);
- break;
- case GETNODES_ID:
- result = EncodeGetNodes(&m, data, 128, buffer);
- break;
- case ADDNODE_ID:
- result = EncodeAddNode(&m, data, 128, buffer);
- break;
- default:
- result = -1;
- break;
- }
-
- if (result < 0) {
- warnx("Big problems. client_recv. EncodeMetaData");
- free(data);
- return;
- }
-
- m.msg_id = currMessageID++;
-
- if ((psize = EncodePacket(packet, 64, &m, data)) < 0) {
- warnx("Big problems. client_recv. EncodePacket");
- free(data);
- return;
- }
-
- /* Only add the client to the pending responses if it was a getparam command */
- if (m.msg_type == GETPARAM_ID || m.msg_type == GETOUTPUT_ID ||
- m.msg_type == GETSOURCE_ID || m.msg_type == GETNODES_ID || m.msg_type == ADDNODE_ID) {
- if (clientAddPendResponses(fd, BytesTo16(packet[ID_L], packet[ID_H])) == -1) {
- warnx("Ran out of room! Consider increasing CLIENT_MAX_PENDING_RESPONSES!\n");
- }
- }
-
- // printf("packetToQuad = '");
- // for(int i = 0; i < (int)psize; ++i) {
- // printf(" %.2x ", packet[i]);
- // }
- // printf("'\n");
-
- int retval = writeQuad(packet, psize);
- // printf("sent %d bytes\n", retval);
-
- free(data);
- }
-
- 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);
- }
+ len_pre = strlen(buffer);
+ cursor = buffer + len_pre;
+
+ r = read(fd, cursor, CLIENT_BUFFER_SIZE - len_pre - 1);
+ if (r < 0) {
+ warn("read (fd: %d)", fd);
+ }
+ buffer[len_pre + r] = '\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);
+
+ char * first_word;
+ char * tmp = strdup(buffer);
+ // printf("tmpbuff = '%s'\n", tmp);
+ first_word = strtok(tmp, " ");
+ // printf("first word = '%s'\n", first_word);
+
+ ssize_t msg_type, i;
+ for (i = 0; i < MAX_TYPE_ID; ++i) {
+ if ((msg_type = findCommand(first_word)) != -1)
+ break;
+ }
+
+ free(tmp);
+
+ if (msg_type == -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;
+ // TODO - check tracker of a specific trackable, also implement a request
+ // that can get the vrpn of a specific trackable
+ 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 (strncmp(buffer, "gettrackable", strlen("gettrackable")) == 0) {
+ char buffer[256];
+ if (snprintf(buffer,
+ 256,
+ "gettrackable %s\n",
+ trackable) >= 256) {
+ warnx("Increase format buffer size, output was too long!");
+ write(fd, "gettrackable ERROR\n", strlen("gettrackable ERROR\n"));
+ }
+ write(fd, buffer, strlen(buffer));
+ }
+ else if (strncmp(buffer, "settrackable", strlen("settrackable")) == 0) {
+ char check[256];
+ sscanf(buffer,
+ "settrackable %s",
+ check);
+ if (strcmp(check,"(null)") != 0) {
+ for (i = 0; i < NUM_TRACKABLES; i++) {
+ if (strcmp(trackables[i].name, check) == 0) {
+ sscanf(buffer,
+ "settrackable %s",
+ trackable);
+ break;
+ }
+ }
+ }
+ }
+
+ } else {
+ uint8_t packet[64];
+ struct metadata m;
+ uint8_t *data = malloc(sizeof(*data) * 128);
+ ssize_t result;
+ ssize_t psize;
+
+ printf(" found a msg_type of %ld\n", msg_type);
+
+ switch (msg_type) {
+ case SETPARAM_ID:
+ result = EncodeSetParam(&m, data, 128, buffer);
+ break;
+ case GETPARAM_ID:
+ result = EncodeGetParam(&m, data, 128, buffer);
+ break;
+ case SETSOURCE_ID:
+ result = EncodeSetSource(&m, data, 128, buffer);
+ break;
+ case GETSOURCE_ID:
+ result = EncodeGetSource(&m, data, 128, buffer);
+ break;
+ case GETOUTPUT_ID:
+ result = EncodeGetOutput(&m, data, 128, buffer);
+ break;
+ case GETNODES_ID:
+ result = EncodeGetNodes(&m, data, 128, buffer);
+ break;
+ case ADDNODE_ID:
+ result = EncodeAddNode(&m, data, 128, buffer);
+ break;
+ default:
+ result = -1;
+ break;
+ }
+
+ if (result < 0) {
+ warnx("Big problems. client_recv. EncodeMetaData");
+ free(data);
+ return;
+ }
+
+ m.msg_id = currMessageID++;
+
+ if ((psize = EncodePacket(packet, 64, &m, data)) < 0) {
+ warnx("Big problems. client_recv. EncodePacket");
+ free(data);
+ return;
+ }
+
+ /* Only add the client to the pending responses if it was a getparam command */
+ if (m.msg_type == GETPARAM_ID || m.msg_type == GETOUTPUT_ID ||
+ m.msg_type == GETSOURCE_ID || m.msg_type == GETNODES_ID || m.msg_type == ADDNODE_ID) {
+ if (clientAddPendResponses(fd, BytesTo16(packet[ID_L], packet[ID_H])) == -1) {
+ warnx("Ran out of room! Consider increasing CLIENT_MAX_PENDING_RESPONSES!\n");
+ }
+ }
+
+
+ int retval = writeQuad(packet, psize);
+ // printf("sent %d bytes\n", retval);
+
+ free(data);
+ }
+
+ 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() {
- static unsigned char respBuf[CMD_MAX_LENGTH];
- static size_t respBufLen;
- static int receiving_logs;
-
- struct metadata m;
- uint8_t data[CMD_MAX_LENGTH];
- size_t respLen;
- ssize_t datalen;
- size_t packetlen;
-
- respLen = readQuad((char *) respBuf + respBufLen,
- CMD_MAX_LENGTH - respBufLen);
- if (respLen <= 0) {
- perror("ERROR reading from quad...\n");
- respBufLen = 0;
- return;
- }
- respBufLen += respLen;
-
- // printf("packetFromQuad = '");
- // for(int i = 0; i < (int)respBufLen; ++i) {
- // printf(" %.2x ", respBuf[i]);
- // }
- // printf("'\n");
-
-
- while(respBufLen) {
- datalen = DecodePacket(&m, data, CMD_MAX_LENGTH, respBuf, respBufLen);
- if (datalen == -1) {
- warnx("No start Byte");
- for (size_t i = 0; i < respBufLen; ++i) {
- if (respBuf[i] == BEGIN_CHAR) {
- memmove(respBuf, respBuf + i, respBufLen - i);
- respBufLen -=i;
- return;
- }
-
- }
- respBufLen = 0;
- return;
- }
- if (datalen == -5) {
- warnx("Chechsum mismatch");
- for (size_t i = 0; i < respBufLen; ++i) {
- if (respBuf[i] == BEGIN_CHAR) {
- memmove(respBuf, respBuf + i, respBufLen - i);
- respBufLen -=i;
- return;
- }
- }
- respBufLen = 0;
- return;
- }
- if (datalen < 0){
- /* Not enough data yet. We need to wait for more */
- // printf("not enough\n");
- return;
- }
-
- packetlen = PacketSize(&m);
- memmove(respBuf, respBuf + packetlen, respBufLen - packetlen);
- respBufLen -= packetlen;
-
- char * debug_string;
- switch (m.msg_type) {
- case DEBUG_ID:
- /* in case of debug. Quad send null terminated string in data */
- debug_string = strndup((char *)data, m.data_len);
- printf(" (Quad) : %s\n", debug_string);
- free(debug_string);
- break;
- case LOG_ID:
- if (!quadlog_file_open) {
- char log_file[256];
- create_log_name(log_file, 256);
- printf("New log file created: '%s'\n", log_file);
- quadlog_file = fopen(log_file, "w");
- quadlog_file_open = 1;
- }
- if (!receiving_logs) {
- printf("(Quad) : Log found\n");
- receiving_logs = 1;
- } else {
- printf(".");
- fflush(0);
- }
- fwrite((char *) data, sizeof(char), m.data_len, quadlog_file);
- break;
- case RESPPARAM_ID:
- case RESPSOURCE_ID:
- case RESPOUTPUT_ID:
- case RESPNODES_ID:
- case RESPADDNODE_ID:
- handleResponse(&m, data);
- break;
- case LOG_END_ID:
- if (quadlog_file_open) {
- fclose(quadlog_file);
- quadlog_file_open = 0;
- }
- printf("\n(Quad) : Log End found\n");
- receiving_logs = 0;
- break;
- default:
- printf("(Backend): message type %d ignored from quad\n", m.msg_type);
- break;
- }
- }
+static void quad_recv(int index) {
+ static unsigned char respBuf[CMD_MAX_LENGTH];
+ static size_t respBufLen;
+ static int receiving_logs;
+
+ struct metadata m;
+ uint8_t data[CMD_MAX_LENGTH];
+ size_t respLen;
+ ssize_t datalen;
+ size_t packetlen;
+
+ respLen = readQuad((char *) respBuf + respBufLen,
+ CMD_MAX_LENGTH - respBufLen, index);
+ if (respLen <= 0) {
+ perror("ERROR reading from quad...\n");
+ respBufLen = 0;
+ return;
+ }
+ respBufLen += respLen;
+
+ // printf("packetFromQuad = '");
+ // for(int i = 0; i < (int)respBufLen; ++i) {
+ // printf(" %.2x ", respBuf[i]);
+ // }
+ // printf("'\n");
+
+ while(respBufLen) {
+ datalen = DecodePacket(&m, data, CMD_MAX_LENGTH, respBuf, respBufLen);
+ if (datalen == -1) {
+ warnx("No start Byte");
+ for (size_t i = 0; i < respBufLen; ++i) {
+ if (respBuf[i] == BEGIN_CHAR) {
+ memmove(respBuf, respBuf + i, respBufLen - i);
+ respBufLen -=i;
+ return;
+ }
+
+ }
+ respBufLen = 0;
+ return;
+ }
+ if (datalen == -5) {
+ warnx("Chechsum mismatch");
+ for (size_t i = 0; i < respBufLen; ++i) {
+ if (respBuf[i] == BEGIN_CHAR) {
+ memmove(respBuf, respBuf + i, respBufLen - i);
+ respBufLen -=i;
+ return;
+ }
+ }
+ respBufLen = 0;
+ return;
+ }
+ if (datalen < 0){
+ /* Not enough data yet. We need to wait for more */
+ return;
+ }
+
+ packetlen = PacketSize(&m);
+ memmove(respBuf, respBuf + packetlen, respBufLen - packetlen);
+ respBufLen -= packetlen;
+
+ char * debug_string;
+ switch (m.msg_type) {
+ case DEBUG_ID:
+ /* in case of debug. Quad send null terminated string in data */
+ debug_string = strndup((char *)data, m.data_len);
+ printf(" (Quad) : %s\n", debug_string);
+ free(debug_string);
+ break;
+ case LOG_ID:
+ if (!quadlog_file_open) {
+ char log_file[256];
+ create_log_name(log_file, 256, user_specified_log_name);
+ printf("New log file created: '%s'\n", log_file);
+ quadlog_file = fopen(log_file, "w");
+ quadlog_file_open = 1;
+ }
+ if (!receiving_logs) {
+ printf("(Quad) : Log found\n");
+ receiving_logs = 1;
+ } else {
+ printf(".");
+ fflush(0);
+ }
+ fwrite((char *) data, sizeof(char), m.data_len, quadlog_file);
+ break;
+ case RESPPARAM_ID:
+ case RESPSOURCE_ID:
+ case RESPOUTPUT_ID:
+ case RESPNODES_ID:
+ case RESPADDNODE_ID:
+ handleResponse(&m, data);
+ break;
+ case LOG_END_ID:
+ if (quadlog_file_open) {
+ fclose(quadlog_file);
+ quadlog_file_open = 0;
+ }
+ printf("\n(Quad) : Log End found\n");
+ receiving_logs = 0;
+ break;
+ default:
+ printf("(Backend): message type %d ignored from quad\n", m.msg_type);
+ break;
+ }
+ }
}
static void handleResponse(struct metadata *m, uint8_t * data)
{
- ssize_t result = 0;
- char *buffer = malloc(sizeof(*buffer) * 128);
- if (!buffer) {
- warnx("failed immediatly");
- return;
- }
-
- switch (m->msg_type) {
- case RESPPARAM_ID:
- result = DecodeResponseParam(buffer, 128, m, data);
- break;
- case RESPSOURCE_ID:
- result = DecodeResponseSource(buffer, 128, m, data);
- break;
- case RESPOUTPUT_ID:
- result = DecodeResponseOutput(buffer, 128, m, data);
- break;
- case RESPNODES_ID:
- result = DecodeResponseGetNodes(&buffer, 128, m, data);
- break;
- case RESPADDNODE_ID:
- result = DecodeResponseAddNode(buffer, 128, m, data);
- break;
- default:
- result = -2;
- break;
- }
-
- if (result == -2) {
- warnx("DecodeResponse error");
- free(buffer);
- return;
- } else if (result < 0) {
- warnx("DecodeResponse error");
- return;
- }
-
- // printf("msg to client = '%s'\n", buffer);
-
- for(int fd = 0; fd <= max_fd; ++fd) {
- if (get_client_index(fd) > -1) {
- clientRemovePendResponses(fd, m->msg_id);
- write(fd, buffer, result);
- }
- }
-
- free(buffer);
-}
-
-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;
-}
+ ssize_t result = 0;
+ char *buffer = malloc(sizeof(*buffer) * 128);
+ if (!buffer) {
+ warnx("failed immediatly");
+ return;
+ }
-void findTimeDiff(int respID) {
- struct timeval result, tend;
- gettimeofday(&tend, NULL);
- timeval_subtract(&result, &tend, &timeArr[respID%MAX_HASH_SIZE]);
- printf("(BackEnd): Elapsed time = %ld ms\n", result.tv_usec/1000);
-}
+ switch (m->msg_type) {
+ case RESPPARAM_ID:
+ result = DecodeResponseParam(buffer, 128, m, data);
+ break;
+ case RESPSOURCE_ID:
+ result = DecodeResponseSource(buffer, 128, m, data);
+ break;
+ case RESPOUTPUT_ID:
+ result = DecodeResponseOutput(buffer, 128, m, data);
+ break;
+ case RESPNODES_ID:
+ result = DecodeResponseGetNodes(&buffer, 128, m, data);
+ break;
+ case RESPADDNODE_ID:
+ result = DecodeResponseAddNode(buffer, 128, m, data);
+ break;
+ default:
+ result = -2;
+ break;
+ }
-char * create_log_name(char * buffer, size_t max) {
- static const char * prefix = "logs";
- static size_t num_logs = 0;
- static const char * format_string = "%F_%-l:%M";
+ if (result == -2) {
+ warnx("DecodeResponse error");
+ free(buffer);
+ return;
+ } else if (result < 0) {
+ warnx("DecodeResponse error");
+ return;
+ }
- time_t curr_time;
- char c_time_string[256];
- struct tm * tmp;
+ // printf("msg to client = '%s'\n", buffer);
- curr_time = time(NULL);
- tmp = localtime(&curr_time);
- strftime(c_time_string, 256, format_string, tmp);
+ for(int fd = 0; fd <= max_fd; ++fd) {
+ if (get_client_index(fd) > -1) {
+ clientRemovePendResponses(fd, m->msg_id);
+ write(fd, buffer, result);
+ }
+ }
- if(strcmp(user_specified_log_name, "") == 0) {
- sprintf(buffer, "%s/%s_%lu.txt", prefix, c_time_string, num_logs++);
- } else {
- sprintf(buffer, "%s/%s_%lu.txt", prefix, user_specified_log_name, num_logs++);
- }
+ free(buffer);
+}
- return buffer;
+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;
}
diff --git a/groundStation/src/backend/backend_adapter.c b/groundStation/src/backend/backend_adapter.c
new file mode 100644
index 0000000000000000000000000000000000000000..11de7f81b836ef8b7122b6dc4610b1ccdc013745
--- /dev/null
+++ b/groundStation/src/backend/backend_adapter.c
@@ -0,0 +1,99 @@
+/**
+ * Contains methods to connect to, disconnect from, and write to an adapter
+ * socket.
+ */
+#include <string.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+#include <stdint.h>
+#include <unistd.h>
+#include <err.h>
+
+#include "backend_adapter.h"
+
+/**
+ * Writes a packet onto the adapter socket.
+ *
+ * conn - Adapter connection
+ * line - data to write
+ * count - number of bytes
+ */
+int adapterWrite(struct adapter_conn *conn, const char * line, size_t count) {
+ //Print out packet being sent to the adapter.
+ //printf("packetToAdapter = '");
+ //for(int i = 0; i < count; ++i) {
+ // printf(" %.2x ", ((const uint8_t *) line)[i]);
+ //}
+ //printf("'\n");
+ return fwrite((const uint8_t *) line, sizeof(uint8_t), count, conn->socket);
+}
+
+/**
+ * Opens a connection to an adapter based upon the data in the trackables
+ * struct.
+ *
+ * trackable: adapter represented by trackable stuct to be connected to.
+ */
+int adapterConnect(trackable_t * trackable) {
+ int s;
+ struct sockaddr_un remote;
+ char str[100];
+
+ if ((s = socket(AF_UNIX, SOCK_STREAM, 0)) == -1) {
+ perror("socket");
+ exit(1);
+ }
+
+ struct adapter_conn ** conn = &(trackable->conn);
+
+ printf("Trying to connect...");
+
+ remote.sun_family = AF_UNIX;
+ strcpy(remote.sun_path, trackable->socket_path);
+
+ if (connect(s, (struct sockaddr *)&remote, sizeof(remote)) == -1) {
+ perror("connect");
+ return 1;
+ }
+
+ *conn = malloc(sizeof(struct adapter_conn));
+ if (*conn == NULL) {
+ perror("malloc");
+ close(s);
+ printf("Failure\n");
+ return 2;
+ }
+
+ (*conn)->len = 0;
+ (*conn)->buf = NULL;
+ (*conn)->socket = fdopen(s, "r+");
+ if ((*conn)->socket == NULL) {
+ perror("fdopen");
+ free(*conn);
+ *conn = NULL;
+ close(s);
+ printf("Failure\n");
+ return 3;
+ }
+ if (setvbuf((*conn)->socket, NULL, _IONBF, 0)) {
+ warn("setvbuf");
+ }
+
+ printf("Success\n");
+
+ return 0;
+}
+
+/**
+ * Disconnect from an adapter.
+ *
+ * trackable: adapter represented by trackable stuct to be connected to.
+ */
+int adapterDisconnect(trackable_t * trackable) {
+ fclose(trackable->conn->socket);
+ if (trackable->conn->buf) {
+ free(trackable->conn->buf);
+ }
+ free(trackable->conn);
+ return 0;
+}
diff --git a/groundStation/src/backend/backend_adapter.h b/groundStation/src/backend/backend_adapter.h
new file mode 100644
index 0000000000000000000000000000000000000000..9f9588e1d4e3536af2702791f49192c4e74cda49
--- /dev/null
+++ b/groundStation/src/backend/backend_adapter.h
@@ -0,0 +1,35 @@
+/**
+ * Contains methods to connect to, disconnect from, and write to an adapter
+ * socket.
+ */
+#ifndef __BACKEND_ADAPTER_H
+#define __BACKEND_ADAPTER_H
+
+#include <stdlib.h>
+#include "config.h"
+
+/**
+ * Opens a connection to an adapter based upon the data in the trackables
+ * struct.
+ *
+ * trackable: adapter represented by trackable struct to be connected to.
+ */
+int adapterConnect(trackable_t * trackable);
+
+/**
+ * Writes a packet to the adapter socket.
+ *
+ * conn - Adapter connection
+ * line - data to write
+ * count - number of bytes
+ */
+int adapterWrite(struct adapter_conn * conn, const char * line, size_t count);
+
+/**
+ * Disconnect from an adapter.
+ *
+ * trackable: adapter represented by trackable struct to be connected to.
+ */
+int adapterDisconnect(trackable_t * trackable);
+
+#endif /* __BACKEND_ADAPTER_H */
diff --git a/groundStation/src/backend/backend_utils.c b/groundStation/src/backend/backend_utils.c
new file mode 100644
index 0000000000000000000000000000000000000000..7080a956d9923021f4976368bbf15998359a6eda
--- /dev/null
+++ b/groundStation/src/backend/backend_utils.c
@@ -0,0 +1,58 @@
+#include <stdio.h>
+#include <time.h>
+#include <string.h>
+
+#include "backend_utils.h"
+
+#define MAX_HASH_SIZE 50
+
+static struct timeval timeArr[MAX_HASH_SIZE];
+
+static 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 remaing 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("(BackEnd): Elapsed time = %ld ms\n", result.tv_usec/1000);
+}
+
+char * create_log_name(char * buffer, size_t max, char * user_specified_log_name) {
+ static const char * prefix = "logs";
+ static size_t num_logs = 0;
+ static const char * format_string = "%F_%-l:%M";
+
+ time_t curr_time;
+ char c_time_string[256];
+ struct tm * tmp;
+
+ curr_time = time(NULL);
+ tmp = localtime(&curr_time);
+ strftime(c_time_string, 256, format_string, tmp);
+
+ if(strcmp(user_specified_log_name, "") == 0) {
+ sprintf(buffer, "%s/%s_%lu.txt", prefix, c_time_string, num_logs++);
+ } else {
+ sprintf(buffer, "%s/%s_%lu.txt", prefix, user_specified_log_name, num_logs++);
+ }
+
+ return buffer;
+}
diff --git a/groundStation/src/backend/backend_utils.h b/groundStation/src/backend/backend_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..cabdaa554a3be2c9737c91f7903876e93f37fae1
--- /dev/null
+++ b/groundStation/src/backend/backend_utils.h
@@ -0,0 +1,30 @@
+#ifndef __BACKEND_UTILS_H
+#define __BACKEND_UTILS_H
+
+#include <sys/time.h>
+
+#define MAX_HASH_SIZE 50
+
+/**
+ * Function for recording latencies.
+ */
+void findTimeDiff(int respID);
+//int timeval_subtract(struct timeval *result, struct timeval *x, struct timeval *y);
+
+/**
+ * Time stamp checking.
+ */
+//struct timeval timeArr[MAX_HASH_SIZE];
+
+/**
+ * Creates a path for a log file inside the logs directory within the groundStation directory.
+ * If the user inputs a specified name the name of the file will be as follows:
+ * <name>_#.txt
+ * Otherwise the format is shown below:
+ * <year>-<month>-<day>_<time>_#.txt
+ * In both cases the # will start and 0 and increment for each log file saved during the life
+ * of the backend process.
+ */
+char * create_log_name(char * buffer, size_t max, char * user_specified_log_name);
+
+#endif /*__BACKEND_UTILS_H */
diff --git a/groundStation/src/backend/config.c b/groundStation/src/backend/config.c
new file mode 100644
index 0000000000000000000000000000000000000000..a717cbc4030880150b28104e131410b0df23bf6c
--- /dev/null
+++ b/groundStation/src/backend/config.c
@@ -0,0 +1,66 @@
+#include "config.h"
+
+//DEFAULTS
+//#define QUAD_LOCAL_DEFAULT_RX "../quad/bin/virt-quad-fifos/uart-rx"
+//#define QUAD_LOCAL_DEFAULT_TX "../quad/bin/virt-quad-fifos/uart-tx"
+#define QUAD_IP_DEFAULT "192.168.1.1"
+#define QUAD_PORT_DEFAULT 8080
+#define QUAD_BT_ADDR_DEFAULT "00:06:66:64:61:D6"
+#define QUAD_BT_CHANNEL_DEFAULT 0x01
+#define QUAD_SERVER_NAME_DEFAULT "UAV@192.168.0.120:3883"
+
+/**
+ * Trackables array defines all trackables that will be used in flight.
+ * The quad should always be the first trackable in the array.
+ */
+trackable_t trackables[] = {
+ //Original Quadcopter
+ [0] = {
+ "UAV", //Name
+ 0, //Set to 0, the backend will modify on start
+ //Wifi Variables
+ QUAD_IP_DEFAULT,
+ QUAD_PORT_DEFAULT,
+ //Adapter Variables
+ 0,
+ NULL,
+ NULL,
+ NULL,
+ //Local Communication Variables
+ 0, //Set to 1 if Intending to use local Fifo
+ 0, //Will be set by Backend
+ 0, //Will be set by Backend
+ QUAD_LOCAL_DEFAULT_TX,
+ QUAD_LOCAL_DEFAULT_RX,
+ //Bluetooth Variables
+ 0, //Set to 1 if using bluetooth
+ QUAD_BT_CHANNEL_DEFAULT,
+ QUAD_BT_ADDR_DEFAULT,
+ //VRPN Variables
+ QUAD_SERVER_NAME_DEFAULT,
+ NULL, //Created by backend
+ PTHREAD_MUTEX_INITIALIZER
+ },
+ //Crazyflie 45
+ [1] = {
+ "cf1",
+ 0,
+ NULL,
+ 0,
+ 1,
+ "./adapters/test_adapter",
+ "./adapters/adapter.socket",
+ NULL,
+ 0,
+ 0,
+ 0,
+ NULL,
+ NULL,
+ 0,
+ 0,
+ NULL,
+ "Crazyflie22@192.168.0.120:3883",
+ NULL,
+ PTHREAD_MUTEX_INITIALIZER
+ },
+};
diff --git a/groundStation/src/backend/config.h b/groundStation/src/backend/config.h
index 4236b1328142c9583dee075e92380f499412995e..3bdc2209b4b2f411155e93ad409ddf2a8646918e 100644
--- a/groundStation/src/backend/config.h
+++ b/groundStation/src/backend/config.h
@@ -1,27 +1,136 @@
+/**
+ * Config.h defines how to setup the backend when run. User may use many of
+ * the defined variables to change default communication schemes. 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 --preserve-env or -E flag.
+ *
+ * Example environment variable usage:
+ *
+ * export UCART_SOCKET=./backend.socket
+ */
#ifndef __CONFIG_H
#define __CONFIG_H
+#include <stdio.h>
+#include <pthread.h>
+#include <stdint.h>
+/**
+ * Socket used to communicate with the backend.
+ */
#define DEFAULT_SOCKET "./ucart.socket"
+
+/**
+ * Defines the name of an environmnet variable that the user may use to
+ * set a different path for the backend socket.
+ * ex. export UCART_SOCKET=./backend.socket
+ */
#define SOCKET_ENV "UCART_SOCKET"
+
+/**
+ * Defines the name of environment variables used to config the backend.
+ * The value that these variables are set to does not matter, if they exist
+ * in the system the backend will adjust accordingly.
+ */
#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
+/**
+ * Defines the name of environment variable used to config the backend
+ * comm channel.
+ */
#define QUAD_COMM_ENV "UCART_COMM_CHANNEL"
+/**
+ * Default local communication configuration variables. Use is same as
+ * socket above.
+ */
#define QUAD_LOCAL_RX "UCART_LOCAL_RX"
#define QUAD_LOCAL_TX "UCART_LOCAL_TX"
#define QUAD_LOCAL_DEFAULT_RX "../quad/bin/virt-quad-fifos/uart-rx"
#define QUAD_LOCAL_DEFAULT_TX "../quad/bin/virt-quad-fifos/uart-tx"
+/**
+ * Adapter connection struct for socket communication. Same as struct
+ * used by CLI and GUI to communicate with the backend.
+ */
+struct adapter_conn {
+ FILE *socket;
+ size_t len;
+ char *buf;
+};
+
+/**
+ * Struct that defines a trackable object.
+ *
+ * name - name of the trackable object, used by the get and set trackable commands
+ * socket - socket that the trackable is connected to, set to 0, will be adjusted by the backend
+ * quad_ip - IP of the MicroCART Quadcopter, set when using wifi
+ * Default - 191.168.1.1
+ * quad_port - Port of the the MicroCART Quadcopter, set when using wifi
+ * Default - 8080
+ * isAdapter - 0 if using Quad or other communication, 1 if using Adapter
+ * adapter_path - path to the adapter, set to NULL if not using an adapter
+ * socket_path - path to the socket for an adapter, set to NULL if not
+ * using an adapter
+ * conn - pointer to struct used by an adapter to communicate between generic trackables, set to NULL
+ * isLocal - 1 if using local Fifo (virtual quad), 0 otherwise
+ * fifo_tx - Tx Fifo, set to 0, will be set by backend
+ * fifo_rx - Rx Fifo, set to 0, will be set by backend
+ * fifo_tx_path - Path to tx fifo, set to NULL if not using Local
+ * Default - ../quad/bin/virt-quad-fifos/uart-tx
+ * fifo_rx_path - Path to rx fifo, set to NULL if not using Local
+ * Default - ../quad/bin/virt-quad-fifos/uart-rx
+ * isBluetooth - 1 if using bluetooth communication to quad, 0 otherwise
+ * bt_channel - bluetooth channel for the quad, 0 if not using bluetooth
+ * Default - 0x01
+ * bt_addr - bluetooth address for the quad, NULL if not using bluetooth
+ * Default - 00:06:66:65:61:D6
+ * server_name - Server name for the VRPN system <NAME>:<IP>:<PORT>
+ * Default IP: 192.168.1.1
+ * Default Port: 8080
+ * tracker - pointer to vrpn tracker for a trackable, set to NULL
+ * socket_mutex - mutex used when communicating with a trackable, set to PTHREAD_MUTEX_INITIALIZER
+ */
+typedef struct {
+ char * name;
+ int socket;
+ //Wifi Variables
+ char * quad_ip;
+ uint16_t quad_port;
+ //Adapter Variables
+ int isAdapter;
+ char * adapter_path;
+ char * socket_path;
+ struct adapter_conn * conn;
+ //Local Communication Variables
+ int isLocal;
+ int fifo_tx;
+ int fifo_rx;
+ char * fifo_tx_path;
+ char * fifo_rx_path;
+ //Bluetooth Variables
+ int isBluetooth;
+ uint8_t bt_channel;
+ char * bt_addr;
+ //VRPN Variables
+ char * server_name;
+ struct ucart_vrpn_tracker * tracker;
+ //Mutex for trackable
+ pthread_mutex_t socket_mutex;
+} trackable_t;
+
+/**
+ * Defines the number of flying objects controlled through the backend.
+ * Needs to be at least one to accomodate for Quad.
+ */
+#define NUM_TRACKABLES 1
-#define QUAD_IP_ENV "UCART_QUAD_IP"
-#define QUAD_IP_DEFAULT "192.168.1.1"
-#define QUAD_PORT_ENV "UCART_QUAD_PORT"
-#define QUAD_PORT_DEFAULT 8080
+/**
+ * Trackables array defines all trackables that will be used in flight.
+ * The quad should always be the first trackable in the array.
+ */
+extern trackable_t trackables[];
#endif /* __CONFIG_H */
diff --git a/groundStation/src/backend/vrpn_tracker.cpp b/groundStation/src/backend/vrpn_tracker.cpp
index 7f9b56a9909ac1512d6d2164d313214200645e22..12f32897abede7117fbdd27309552ebd6c3adae7 100644
--- a/groundStation/src/backend/vrpn_tracker.cpp
+++ b/groundStation/src/backend/vrpn_tracker.cpp
@@ -1,178 +1,225 @@
#include <iostream>
#include <algorithm>
#include <functional>
+#include <cstring>
#include "vrpn_Tracker.h"
#include "quat.h"
-
#include "vrpn_tracker.hpp"
+static microcart::Tracker_handler handler;
+
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);
- }
+ static void VRPN_CALLBACK vrpn_cb(void * param, const vrpn_TRACKERCB t);
+
+ Tracker_handler::Tracker_handler() :
+ stop_flag(0),
+ num_trackers(0)
+ {
+ stop_flag = 0;
+ num_trackers = 0;
+ }
+
+ Tracker_handler::~Tracker_handler() {}
+
+ void Tracker_handler::vrpn_start() {
+ stop_flag = 0;
+ vrpn_thread = std::thread(&Tracker_handler::vrpn_loop, this);
+ }
+
+ void Tracker_handler::vrpn_stop() {
+ stop_flag = 1;
+ vrpn_thread.join();
+ }
+
+ TrackerData::TrackerData(std::string server) :
+ x(0.0), y(0.0), z(0.0),
+ pitch(0.0), roll(0.0), yaw(0.0),
+ fps(0.0), timestamp()
+ {
+ strncpy(server_name, server.c_str(), 256);
+ }
+
+ TrackerData::TrackerData(const char * server) :
+ x(0.0), y(0.0), z(0.0),
+ pitch(0.0), roll(0.0), yaw(0.0),
+ fps(0.0), timestamp()
+ {
+ strncpy(server_name, server, 256);
+ }
+
+ Tracker::Tracker(std::string server) :
+ remote(server.c_str()),
+ trackerData(server)
+ {
+ }
+
+ Tracker::Tracker(const char * server) :
+ remote(server),
+ trackerData(server)
+ {
+ remote.register_change_handler(this, vrpn_cb);
+ }
+
+ Tracker::~Tracker()
+ {
+ //TODO - may not be needed
+ {
+ std::lock_guard<std::mutex> guard(vrpn_mutex);
+ }
+ }
+
+ const struct TrackerData Tracker::getData(void)
+ {
+ std::lock_guard<std::mutex> guard(vrpn_mutex);
+ return trackerData;
+ }
+
+ void Tracker_handler::vrpn_loop(void)
+ {
+ while (1) {
+ for (int i = 0; i < NUM_TRACKABLES; i++) {
+ trackers[i]->remote.mainloop();
+ {
+ std::lock_guard<std::mutex> guard(trackers[i]->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;
+ microcart::Tracker * t;
};
void cb_wrapper(void (*cb)(struct ucart_vrpn_TrackerData *),
- const microcart::TrackerData &td)
+ 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);
+ 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;
+ strncpy(data.server_name, td.server_name, 256);
+ (*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;
- }
- }
+ 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);
+ handler.trackers[handler.num_trackers] = inst->t;
+ handler.num_trackers++;
+ 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;
+ strncpy(td->server_name, data.server_name, 256);
+
+ return 0;
+ } catch(...) {
+ return -1;
+ }
+ }
+
+ int ucart_vrpn_start() {
+ if (handler.num_trackers > 0) {
+ handler.vrpn_start();
+ return 0;
+ }
+ return 1;
+ }
+
+ int ucart_vrpn_stop() {
+ if (handler.stop_flag != 1) {
+ handler.vrpn_stop();
+ return 0;
+ }
+ return 1;
+ }
}
diff --git a/groundStation/src/backend/vrpn_tracker.hpp b/groundStation/src/backend/vrpn_tracker.hpp
index 6775ce13320f60acfcd67f5f755fe7d1e3def8ae..47fb8370563d20b08470fae59e498a5cefa4aa49 100644
--- a/groundStation/src/backend/vrpn_tracker.hpp
+++ b/groundStation/src/backend/vrpn_tracker.hpp
@@ -13,6 +13,8 @@
#include <vector>
#include "vrpn_Tracker.h"
+#include "config.h"
+
#endif
#include <sys/time.h>
@@ -21,31 +23,34 @@
extern "C"
{
#endif
- struct ucart_vrpn_tracker;
- struct ucart_vrpn_TrackerData {
- float x;
- float y;
- float z;
+ struct ucart_vrpn_tracker;
+ struct ucart_vrpn_TrackerData {
+ float x;
+ float y;
+ float z;
- float pitch;
- float roll;
- float yaw;
+ float pitch;
+ float roll;
+ float yaw;
- double fps;
- struct timeval timestamp;
- };
+ double fps;
+ struct timeval timestamp;
+ char server_name[256];
+ };
- struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
- const char * server);
+ struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
+ const char * server);
- void ucart_vrpn_tracker_freeInstance(struct ucart_vrpn_tracker * inst);
+ 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_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);
+ int ucart_vrpn_tracker_getData(struct ucart_vrpn_tracker * inst,
+ struct ucart_vrpn_TrackerData * td);
+ int ucart_vrpn_start();
+ int ucart_vrpn_stop();
#ifdef __cplusplus
}
#endif
@@ -53,46 +58,57 @@ extern "C"
#ifdef __cplusplus
namespace microcart
{
- struct TrackerData {
- public:
- float x;
- float y;
- float z;
-
- float pitch;
- float roll;
- float 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;
- };
-
+ struct TrackerData {
+ public:
+ float x;
+ float y;
+ float z;
+
+ float pitch;
+ float roll;
+ float yaw;
+
+ double fps;
+ timeval timestamp;
+ char server_name[256];
+
+ TrackerData(const char * server);
+ TrackerData(std::string server);
+ };
+
+ class Tracker {
+ public:
+ const struct TrackerData getData(void);
+ void callback(const vrpn_TRACKERCB t);
+
+ Tracker(std::string server);
+ Tracker(const char * server);
+ ~Tracker();
+
+ vrpn_Tracker_Remote remote;
+ std::mutex vrpn_mutex;
+
+ void addCallback(std::function<void(const TrackerData &)> cb);
+ private:
+ TrackerData trackerData;
+ std::vector<std::function<void(const TrackerData &)>> cb_vector;
+ };
+
+
+ class Tracker_handler {
+ public:
+ Tracker_handler();
+ ~Tracker_handler();
+ void vrpn_start();
+ void vrpn_stop();
+ int num_trackers;
+ int stop_flag;
+ Tracker * trackers[NUM_TRACKABLES];
+ private:
+ std::thread vrpn_thread;
+ void vrpn_loop(void);
+ };
}
/* __cplusplus */
#endif
diff --git a/groundStation/src/cli/cli.h b/groundStation/src/cli/cli.h
index 8a14201024f41c39a921beea10f19e36428047af..f91e40fbce9ef8184592b1aa9cfd04d8d415b165 100644
--- a/groundStation/src/cli/cli.h
+++ b/groundStation/src/cli/cli.h
@@ -6,6 +6,7 @@
#include "cli_param.h"
#include "cli_output.h"
#include "cli_nodes.h"
+#include "cli_trackable.h"
struct backend_conn;
@@ -17,6 +18,8 @@ enum CommandNameIds{
CMD_GETOUTPUT,
CMD_GETNODES,
CMD_ADDNODE,
+ CMD_GETTRACKABLE,
+ CMD_SETTRACKABLE,
MAX_COMMANDS
};
@@ -28,7 +31,9 @@ static cli_function_ptr cli_functions[] = {
&cli_setparam,
&cli_getoutput,
&cli_getnodes,
- &cli_addnode
+ &cli_addnode,
+ &cli_gettrackable,
+ &cli_settrackable
};
static char* commandNames[MAX_COMMANDS] = {
@@ -38,7 +43,9 @@ static char* commandNames[MAX_COMMANDS] = {
"setparam",
"getoutput",
"getnodes",
- "addnode"
+ "addnode",
+ "gettrackable",
+ "settrackable"
};
/**
@@ -67,7 +74,7 @@ int isNumber(char *number);
* find the correct values by using the optional strings passed in from argv
* @param conn backend connection struct
* @param argv argv from the cmd line
- * @param convert_data saves the necissary converted data in this struct for later use.
+ * @param convert_data saves the necessary converted data in this struct for later use.
* @return returns an error integer
*/
int convert_to_id(struct backend_conn * conn, char **argv, struct convert_data * convert_data, int conversion_requirement);
@@ -79,4 +86,4 @@ int convert_to_id(struct backend_conn * conn, char **argv, struct convert_data *
*/
int help_check(int argc, char ** argv);
-#endif /* _CLI_H */
\ No newline at end of file
+#endif /* _CLI_H */
diff --git a/groundStation/src/cli/cli_trackable.c b/groundStation/src/cli/cli_trackable.c
new file mode 100644
index 0000000000000000000000000000000000000000..29e2bb0cf5813180be13ccc4cefc857190cc545a
--- /dev/null
+++ b/groundStation/src/cli/cli_trackable.c
@@ -0,0 +1,73 @@
+#include <stdio.h>
+#include <unistd.h>
+#include <inttypes.h>
+#include <string.h>
+
+#include "cli.h"
+#include "cli_trackable.h"
+#include "frontend_trackable.h"
+
+/**
+ * Prints the current trackable set in the backend.
+ */
+int cli_gettrackable(struct backend_conn * conn, int argc, char ** argv) {
+ char name[64];
+ int needHelp = 0;
+
+ if ((needHelp = help_check(argc, argv))) {
+ printf("gettrackable gets the name of the trackable being communicated with\n");
+ printf("Usage Syntax : \n\t./Cli gettrackable\n");
+ printf("Symlink Usage Syntax : \n\t./gettrackable\n\n");
+ return 0;
+ }
+
+ if (argc < 1) {
+ printf("Incorrect Usage, run './cli gettrackable --help' for correct usage.\n");
+ return 1;
+ }
+
+ if (frontend_gettrackable(conn, name)) {
+ return 1;
+ }
+
+ printf("Trackable - %s\n", name);
+ return 0;
+}
+
+/**
+ * Sets a new trackable in the backend. The input trackable must be the name of a
+ * trackable within the trackables array set in config.c
+ */
+int cli_settrackable(struct backend_conn * conn, int argc, char ** argv) {
+ int needHelp = 0;
+
+ if ((needHelp = help_check(argc, argv))) {
+ printf("settrackable sets the trackable being communicated with in the backend\n");
+ printf("Usage Syntax : \n\t./Cli settrackable trackable\n");
+ printf("Symlink Usage Syntax : \n\t./settrackable trackable\n\n");
+ return 0;
+ }
+
+ if (argc < 2) {
+ printf("Incorrect Usage, run './Cli settrackable --help' for correct usage.\n");
+ return 1;
+ }
+
+ int i = 0;
+
+ for (i = 0; i < argc; i++) {
+ if (strstr(argv[i], "settrackable") != NULL) {
+ break;
+ }
+ }
+
+ if (i >= argc) {
+ printf("Incorrect Usage, run '.cli settrackable --help' for correct usage.\n");
+ return 1;
+ }
+
+ if (frontend_settrackable(conn, argv[++i])) {
+ return 1;
+ }
+ return 0;
+}
diff --git a/groundStation/src/cli/cli_trackable.h b/groundStation/src/cli/cli_trackable.h
new file mode 100644
index 0000000000000000000000000000000000000000..be322e91044cdbd372ce61e5ce053c41a1c178fb
--- /dev/null
+++ b/groundStation/src/cli/cli_trackable.h
@@ -0,0 +1,19 @@
+/**
+ * CLI functions for the gettrackable and settrackable commands.
+ */
+
+#ifndef _CLI_TRACKABLE_H
+#define _CLI_TRACKABLE_H
+
+/**
+ * Prints the current trackable set in the backend.
+ */
+int cli_gettrackable(struct backend_conn * conn, int argc, char ** argv);
+
+/**
+ * Sets a new trackable in the backend. The input trackable must be the name of a
+ * trackable within the trackables array set in config.c
+ */
+int cli_settrackable(struct backend_conn * conn, int argc, char ** argv);
+
+#endif /* _CLI_TRACKABLE_H */
diff --git a/groundStation/src/frontend/frontend_trackable.c b/groundStation/src/frontend/frontend_trackable.c
new file mode 100644
index 0000000000000000000000000000000000000000..43a2c7b74a8dfc7cb7d0c71598e0e2ceef90e1c1
--- /dev/null
+++ b/groundStation/src/frontend/frontend_trackable.c
@@ -0,0 +1,70 @@
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <err.h>
+#include <inttypes.h>
+
+#include "frontend_trackable.h"
+
+
+/* Get the trackable name that is being communicated with.
+ *
+ * conn: backend connection.
+ * trackable: on success this will contain the name of the trackable.
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_gettrackable(
+ struct backend_conn * conn,
+ char * trackable) {
+
+ char msg[64] = "";
+ int written;
+
+ snprintf(msg, 64, "gettrackable\n");
+
+ if((written = ucart_backendWrite(conn, msg)) < 0) {
+ return 1;
+ }
+
+ size_t pendingResponses = 1;
+ char * response;
+
+ while (pendingResponses) {
+ response = ucart_backendGetline(conn);
+ if (response == NULL) {
+ warnx("Line not returned from backend");
+ return 1;
+ }
+
+ if (strncmp(response, "gettrackable", 12) == 0) {
+ sscanf(response, "gettrackable %s",
+ trackable);
+ pendingResponses--;
+ }
+ }
+
+ return 0;
+}
+
+/* Set the trackable that is being communicated with.
+ *
+ * conn: backend connection.
+ * trackable: name of the trackable (names in the ground_station)
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_settrackable(
+ struct backend_conn * conn,
+ const char * trackable) {
+ char msg[64] = "";
+ int written;
+
+ snprintf(msg, 64, "settrackable %s\n", trackable);
+
+ if((written = ucart_backendWrite(conn, msg)) < 0) {
+ return 1;
+ }
+
+ return 0;
+}
diff --git a/groundStation/src/frontend/frontend_trackable.h b/groundStation/src/frontend/frontend_trackable.h
new file mode 100644
index 0000000000000000000000000000000000000000..f63973fea3f72d703d4152304383a89867760028
--- /dev/null
+++ b/groundStation/src/frontend/frontend_trackable.h
@@ -0,0 +1,35 @@
+#ifndef _FRONTEND_TRACKABLE_H
+#define _FRONTEND_TRACKABLE_H
+
+#include "frontend_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Get the trackable name that is being communicated with.
+ *
+ * conn: backend connection.
+ * trackable: on success this will contain the name of the trackable.
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_gettrackable(
+ struct backend_conn * conn,
+ char * trackable);
+
+/* Set the trackable that is being communicated with.
+ *
+ * conn: backend connection.
+ * trackable: name of the trackable (names in the ground_station)
+ *
+ * Returns 0 on success, 1 on error
+ */
+int frontend_settrackable(
+ struct backend_conn * conn,
+ const char * trackable);
+
+#ifdef __cplusplus
+}
+#endif
+#endif /* _FRONTEND_TRACKABLE_H */