Skip to content
Snippets Groups Projects
Commit 4e421832 authored by burneykb's avatar burneykb
Browse files

adding helpful messages to config.h

parent 5b193950
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
groundStation/src/config.h
+ 4
− 0
View file @ 4e421832
......@@ -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"
......
groundStation/src/microcart_cli.c
+ 129
− 80
View file @ 4e421832
......@@ -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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment