diff --git a/groundStation/src/config.h b/groundStation/src/config.h
index 916058810e1834cb57a3e7af0cad2cd745e3c922..74b0bc671b11b02c99f5dddc16e7d7f8d48678f4 100644
--- a/groundStation/src/config.h
+++ b/groundStation/src/config.h
@@ -6,6 +6,10 @@
#define SOCKET_ENV "UCART_SOCKET"
#define NOQUAD_ENV "UCART_NO_QUAD"
+
+// If you are planning on using any of these env vars and you have
+// exported them with normal user rights. You will need to run the
+// backend with sudo elevation and with the --preserve-env flag or -E
#define QUAD_WIFI_ENV "UCART_USE_WIFI"
#define QUAD_IP_ENV "UCART_QUAD_IP"
#define QUAD_IP_DEFAULT "192.168.4.1"
diff --git a/groundStation/src/microcart_cli.c b/groundStation/src/microcart_cli.c
index 9ee3712b861bbedfa9982304d6261611253547d2..3fa609c223640fb3d8378441352a8cf4899a01e8 100644
--- a/groundStation/src/microcart_cli.c
+++ b/groundStation/src/microcart_cli.c
@@ -21,6 +21,9 @@
#include <bluetooth/rfcomm.h>
#include <pthread.h>
#include <assert.h>
+#include <errno.h>
+#include <string.h>
+#include <sys/ioctl.h>
//user created includes
#include "communication.h"
@@ -33,16 +36,15 @@
#define QUAD_BT_ADDR "00:06:66:64:61:D6"
#define QUAD_BT_CHANNEL 0x01
#define CMD_MAX_LENGTH 1024
+#define MAX_HASH_SIZE 15
// function prototypes
-void killHandler(int);
void readAndPrint(void);
void sendVrpnPacket(struct ucart_vrpn_TrackerData *);
void sendStartPacket(void);
void getVRPNPacket(struct ucart_vrpn_TrackerData *);
void printVrpnData(struct ucart_vrpn_TrackerData *);
int connectToZybo();
-int startsWith(const char *, const char *);
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);
@@ -63,6 +65,14 @@ static int wasDisconnected(int fd);
/* Thread-safe wrappers */
pthread_mutex_t quadSocketMutex;
static ssize_t writeQuad(const char * buf, size_t count);
+static ssize_t readQuad(char * buf, size_t count);
+
+/* Functions for recording Latencies */
+void findTimeDiff(int respID);
+int timeval_subtract (struct timeval *result, struct timeval *x, struct timeval *y);
+//time stamp checking
+static unsigned int currMessageID = 0;
+struct timeval timeArr[MAX_HASH_SIZE] = {0};
// global variables
static volatile int keepRunning = 1;
@@ -80,7 +90,7 @@ fd_set rfds_master;
int max_fd = 0;
pthread_mutex_t quadResponseMutex, cliInputMutex ;
-unsigned char *respBuf, *commandBuf;
+char *respBuf, *commandBuf;
int newQuadResponse = 0, newCliInput = 0;
// Structures to be used throughout
@@ -96,20 +106,11 @@ static void cb(struct ucart_vrpn_TrackerData * td)
if(!(count % 10)) {
sendVrpnPacket(td);
- updateLogFile(td);
+ //updateLogFile(td);
}
-
count++;
- // This will print the vrpn data to the terminal if necissary.
- // Commented out because the callback will cover quad log data
- // at the end of flight.
- /**if(!(count % 100)) {
- printVrpnData(td);
- printf("[Info] Received %d tracker updates.\n", count);
- }**/
}
-
int main(int argc, char **argv)
{
int activity;
@@ -156,26 +157,24 @@ int main(int argc, char **argv)
client_buffers[i][0] = '\n';
}
- //signal(SIGINT, killHandler);
-
if (pthread_mutex_lock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
}
- // if ((zyboSocket = connectToZybo()) < 0)
- // {
- // perror("Error connecting to Zybo...");
- // free(respBuf);
- // free(commandBuf);
- // exit(1);
- // }
+ if ((zyboSocket = connectToZybo()) < 0)
+ {
+ perror("Error connecting to Zybo...");
+ free(respBuf);
+ free(commandBuf);
+ exit(1);
+ }
if (pthread_mutex_unlock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
}
// create vrpnTracker instance
- //tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
+ tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
// open the log file
if(createLogFile(argc, argv[1]))
@@ -188,25 +187,23 @@ int main(int argc, char **argv)
// watch for input from stdin (fd 0) to see when it has input
safe_fd_set(fileno(stdin), &rfds_master, &max_fd);
// watch for input from the zybo socket
- //safe_fd_set(zyboSocket, &rfds_master, &max_fd);
+ safe_fd_set(zyboSocket, &rfds_master, &max_fd);
//printf("zyboSocket = %d, max_fd = %d\n", zyboSocket, max_fd);
//tell the quad we are ready to send it vrpn data
- printf("sending Start Packet...\n");
- //sendStartPacket();
-
+ sendStartPacket();
// this function will be called whenever tracker receives data
- // ucart_vrpn_tracker_addCallback(tracker, cb);
-
- // start the prompt
- // fprintf(stdout, "$microcart> ");
+ ucart_vrpn_tracker_addCallback(tracker, cb);
struct timeval timeout = {
.tv_sec = 1,
.tv_usec = 0
};
+
+ respBuf = calloc(CMD_MAX_LENGTH, sizeof(unsigned char));
+ sleep(3);
while(keepRunning)
{
fd_set rfds;
@@ -215,7 +212,6 @@ int main(int argc, char **argv)
if(activity == -1) {
perror("select() ");
} else if (activity) {
- fprintf(stderr, "Select activity = %d\n", activity);
for(int fd = 0; fd <= max_fd; ++fd) {
if (FD_ISSET(fd, &rfds)) {
if(wasDisconnected(fd))
@@ -241,23 +237,43 @@ int main(int argc, char **argv)
// // Write the command to the control_loop socket
// // int n = writeQuad(packet, ((packet[6] << 8) | packet[5]) + 8);
// // if(n < 0) {
- // // fprintf(stdout, "CLI: ERROR writing to socket\n");
+ // // printf("CLI: ERROR writing to socket\n");
// // }
// }
- // fprintf(stdout, "$microcart> ");
+ // printf("$microcart> ");
// memset(userCommand, 0, cmdLen);
} else if (fd == zyboSocket) {
-
+ // Read the response from the control loop
+ int available;
+ ioctl(fd, FIONREAD, &available);
+ if (available < 12)
+ continue;
+ int respLen = readQuad(respBuf, 12);
+ if(respLen <= 0) {
+ printf("CLI: ERROR reading from socket %d: %s\n", respLen, strerror(errno));
+ }
+ //printf("recognized info from quad\n");
+ int id = getInt((unsigned char *)respBuf, 7);
+ findTimeDiff(id);
+ // if(respLen == 11) {
+ // int id = getInt((unsigned char *)respBuf, 7);
+ // findTimeDiff(id);
+ // printf("respLen = %d : id = %d'\n", respLen, id);
+ // for(int i = 0; i <= respLen -1; ++i)
+ // printf("%x ", (unsigned char)respBuf[i]);
+ // printf("'\n");
+ // }
+ memset(respBuf, 0, respLen);
} else if (fd == backendSocket) {
int new_fd = 0;
new_fd = accept(backendSocket, NULL, NULL);
if (new_fd < 0) {
warn("accept");
} else {
- fprintf(stderr, "Connection\n");
+ printf("Connection\n");
if (new_client(new_fd)) {
- fprintf(stderr, "Added client\n");
+ printf("Added client\n");
safe_fd_set(new_fd, &rfds_master, &max_fd);
}
}
@@ -272,25 +288,17 @@ int main(int argc, char **argv)
}
}
- // ucart_vrpn_tracker_freeInstance(tracker);
+ ucart_vrpn_tracker_freeInstance(tracker);
safe_close_fd(zyboSocket, &quadSocketMutex);
-
-
closeLogFile();
return 0;
}
-// signal handler to exit while loop of main function
-void killHandler(int dummy) {
- keepRunning = 0;
- printf("\nleaving Bluetooth module\n");
-}
-
void sendStartPacket() {
unsigned char packet[8] = {0};
-
+ currMessageID++;
metadata_t metadata =
{
(char) BEGIN_CHAR,
@@ -303,8 +311,8 @@ void sendStartPacket() {
packet[0] = metadata.begin_char; // BEGIN //PACKET_START_BYTE;
packet[1] = metadata.msg_type; // UPDATE //'U'; // U for vrpn camera update, C for command
packet[2] = metadata.msg_subtype; // BEGIN UPDATE
- packet[3] = 1; // MSG ID(1)
- packet[4] = 0; // MSG ID(2)
+ packet[3] = (currMessageID & 0x000000ff); // MSG ID(1)
+ packet[4] = ((currMessageID >> 8) & 0x000000ff); // MSG ID(2)
packet[5] = 0; // DATALEN(1)
packet[6] = 0; // DATALEN(2)
@@ -322,19 +330,20 @@ void sendStartPacket() {
}else
{
printf("Start packet successfuly sent...\n");
- }
+ }
}
void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
int pSize = sizeof(info) + 8;
int n;
unsigned char packet[pSize];
+ currMessageID++;
packet[0] = 0xBE; // BEGIN //PACKET_START_BYTE;
packet[1] = 0x04; // UPDATE //'U'; // U for vrpn camera update, C for command
packet[2] = 0x00; // N/A
//TODO Figure out Packet ID with this new ucar_vrpn_TrackerData struct
- packet[3] = (0x00 & 0x000000ff); // MSG ID(1)
- packet[4] = ((0x00 >> 8) & 0x000000ff); // MSG ID(2)
+ packet[3] = (currMessageID & 0x000000ff); // MSG ID(1)
+ packet[4] = ((currMessageID >> 8) & 0x000000ff); // MSG ID(2)
packet[5] = (sizeof(info) & 0x000000ff); // DATALEN(1)
packet[6] = ((sizeof(info) >> 8) & 0x00000ff); // DATALEN(2)
memcpy(&packet[7], &info, sizeof(info));
@@ -351,6 +360,9 @@ void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
perror("vrpnhandler: ERROR writing to socket");
keepRunning = 0;
}
+ struct timeval tstart;
+ gettimeofday(&tstart, NULL);
+ timeArr[currMessageID%MAX_HASH_SIZE] = tstart;
}
void getVRPNPacket(struct ucart_vrpn_TrackerData *td) {
@@ -369,10 +381,11 @@ void printVrpnData(struct ucart_vrpn_TrackerData * td) {
int connectToZybo() {
int sock;
- int status;
+ int status = 0;
/* Use bluetooth by default */
- if (getenv(QUAD_WIFI_ENV) == NULL) {
+ if (getenv(QUAD_WIFI_ENV) == NULL && 0) {
+ printf("Using BT Settings\n")
struct sockaddr_rc addr;
// allocate a socket
@@ -387,6 +400,7 @@ int connectToZybo() {
// 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;
@@ -394,13 +408,13 @@ int connectToZybo() {
/* Quick and Dirty */
if (getenv(QUAD_IP_ENV)) {
if (!inet_aton(getenv(QUAD_IP_ENV), &addr.sin_addr)) {
- fprintf(stderr, "Env var %s invalid IP %s\n",
+ 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)) {
- fprintf(stderr, "Default IP %s is invalid\n",
+ printf("Default IP %s is invalid\n",
QUAD_IP_DEFAULT);
return -1;
}
@@ -408,16 +422,19 @@ int connectToZybo() {
if (getenv(QUAD_PORT_ENV)) {
/* Quick 'n dirty, oh yeah! */
- addr.sin_family = atoi(getenv(QUAD_PORT_ENV));
+ addr.sin_port = htons(atoi(getenv(QUAD_PORT_ENV)));
} else {
- addr.sin_family = QUAD_PORT_DEFAULT;
+ printf("using default port\n");
+ addr.sin_port = htons(QUAD_PORT_DEFAULT);
}
- sock = socket(AF_INET, SOCK_STREAM, 0);
+ sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
if (sock < 0) {
perror("socket");
return -1;
}
+
+ status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
}
// connection failed
@@ -426,7 +443,7 @@ int connectToZybo() {
close(sock);
perror("connect");
return -1;
- }
+ }
else
{
printf("connection successful!...\n");
@@ -434,12 +451,6 @@ int connectToZybo() {
}
}
-int startsWith(const char *pre, const char *str) {
- size_t lenpre = strlen(pre),
- lenstr = strlen(str);
- return lenstr < lenpre ? 0 : (strncmp(pre, str, lenpre) == 0);
-}
-
/* add a fd to fd_set, and update max_fd */
int safe_fd_set(int fd, fd_set* fds, int* max_fd) {
assert(max_fd != NULL);
@@ -464,13 +475,13 @@ int safe_fd_clr(int fd, fd_set* fds, int* max_fd) {
static ssize_t writeQuad(const char * buf, size_t count) {
ssize_t retval;
- if (1) {//getenv(NOQUAD_ENV)) {
+ if (0) {//getenv(NOQUAD_ENV)) {
return count;
}
if (pthread_mutex_lock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
}
- retval = writeQuad(buf, count);
+ retval = write(zyboSocket, buf, count);
if (pthread_mutex_unlock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
}
@@ -478,8 +489,19 @@ static ssize_t writeQuad(const char * buf, size_t count) {
return retval;
}
-static int new_client(int fd)
-{
+static ssize_t readQuad(char * buf, size_t count) {
+ ssize_t retval;
+ if (pthread_mutex_lock(&quadSocketMutex)) {
+ err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
+ }
+ retval = read(zyboSocket, buf, count);
+ if (pthread_mutex_unlock(&quadSocketMutex)) {
+ err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
+ }
+ return retval;
+}
+
+static int new_client(int fd) {
ssize_t new_slot = -1;
for (ssize_t i = 0; i < MAX_CLIENTS; i++) {
if (client_fds[i] < 0) {
@@ -498,8 +520,7 @@ static int new_client(int fd)
return 1;
}
-static ssize_t get_client_index(int fd)
-{
+static ssize_t get_client_index(int fd) {
for (ssize_t i = 0; i < MAX_CLIENTS; i++) {
if (client_fds[i] == fd) {
return i;
@@ -509,8 +530,7 @@ static ssize_t get_client_index(int fd)
return -1;
}
-static char * get_client_buffer(int fd)
-{
+static char * get_client_buffer(int fd) {
ssize_t slot = get_client_index(fd);
if (slot == -1) {
return NULL;
@@ -541,8 +561,7 @@ static void safe_close_fd(int fd, pthread_mutex_t *mutexLock) {
}
}
-static void client_recv(int fd)
-{
+static void client_recv(int fd) {
char * buffer;
ssize_t len_pre;
buffer = get_client_buffer(fd);
@@ -577,11 +596,11 @@ static void client_recv(int fd)
buffer[newline] = '\0';
unsigned char * packet;
- // fprintf(stderr, "newline =%li, Client sent: '%s'\n", newline, buffer);
+ // printf("newline =%li, Client sent: '%s'\n", newline, buffer);
if(formatCommand(buffer, &packet) != -1) {
- fprintf(stdout, "Backend sees as: %f\n", getFloat(packet, 7));
+ printf("Backend sees as: %f\n", getFloat(packet, 7));
} else {
- fprintf(stderr, "Could not recognize command '%s'\n", buffer);
+ printf("Could not recognize command '%s'\n", buffer);
}
writeQuad((char *) packet, len);
//free(packet);
@@ -599,8 +618,38 @@ static int wasDisconnected(int fd) {
{
remove_client(fd);
safe_fd_clr(fd, &rfds_master, &max_fd);
- fprintf(stderr, "fd %d has disconnect and was removed\n", fd);
+ printf("fd %d has disconnect and was removed\n", fd);
return 1;
}
return 0;
}
+
+int timeval_subtract (struct timeval *result, struct timeval *x, struct timeval *y) {
+ /* Perform the carry for the later subtraction by updating y. */
+ if (x->tv_usec < y->tv_usec) {
+ int nsec = (y->tv_usec - x->tv_usec) / 1000000 + 1;
+ y->tv_usec -= 1000000 * nsec;
+ y->tv_sec += nsec;
+ }
+ if (x->tv_usec - y->tv_usec > 1000000) {
+ int nsec = (x->tv_usec - y->tv_usec) / 1000000;
+ y->tv_usec += 1000000 * nsec;
+ y->tv_sec -= nsec;
+ }
+ /* Compute the time remaining to wait.
+ tv_usec is certainly positive. */
+ result->tv_sec = x->tv_sec - y->tv_sec;
+ result->tv_usec = x->tv_usec - y->tv_usec;
+ /* Return 1 if result is negative. */
+ return x->tv_sec < y->tv_sec;
+}
+
+void findTimeDiff(int respID) {
+ struct timeval result, tend;
+ gettimeofday(&tend, NULL);
+ timeval_subtract(&result, &tend, &timeArr[respID%MAX_HASH_SIZE]);
+ printf("elapsed time = %ld ms\n", result.tv_usec/1000);
+ // char tmp[8];
+ // snprintf(tmp, 8, "%ld \tms\n", result.tv_usec/1000);
+ // writeStringToLog(tmp);
+}
\ No newline at end of file