Skip to content
Snippets Groups Projects
Commit b228070d authored by bbartels's avatar bbartels
Browse files

Merge branch 'master' of git.ece.iastate.edu:danc/MicroCART_17-18

parents 93267bd8 de83aac8
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
groundStation/.gitignore
+ 2
− 0
View file @ b228070d
......@@ -40,3 +40,5 @@ src/vrpn/pc_linux64/*
#Exacutables
BlueTooth
logs
client
BackEnd
groundStation/Makefile
+ 2
− 2
View file @ b228070d
......@@ -2,7 +2,7 @@
GCC=gcc
GXX=g++
CFLAGS= -Wall -Wpedantic -Wextra -Werror -std=c99 -g -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-but-set-variable
CXXFLAGS= -Wall -Wno-reorder -std=c++11 -g
CXXFLAGS= -Wall -Wpedantic -Wextra -Werror -Wno-reorder -Wno-unused-variable -std=c++0x -g
INCLUDES = $(foreach dir, $(INCDIR), -I$(dir))
# Directories
......@@ -11,7 +11,7 @@ INCDIR=inc src src/vrpn src/vrpn/quat src/vrpn/build
OBJDIR=obj
# Final exacutable name
EXE=BlueTooth
EXE=BackEnd
# File names
CSOURCES := $(foreach dir, $(SRCDIR), $(wildcard $(dir)/*.c ))
......
groundStation/README.md
+ 19
− 1
View file @ b228070d
# groundStation
\ No newline at end of file
# groundStation
## Make Process
First, if submodules were not recursevly added through git. Run this command if you have made any attempt to make vrpn manually.
run
'git submodule update --init --recursive'
Now that you have all of the files necissary.
cd into the groundstation folder.
cd groundStation
make vrpn
make
run the program with sudo privledges
sudo ./BackEnd
If you wish to change the way the backend communicates to the quad and vice versa, modify src/config.h .
groundStation/src/config.h
+ 9
− 0
View file @ b228070d
......@@ -7,4 +7,13 @@
#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"
#define QUAD_PORT_ENV "UCART_QUAD_PORT"
#define QUAD_PORT_DEFAULT 8080
#endif
groundStation/src/microcart_cli.c
+ 178
− 90
View file @ b228070d
......@@ -4,6 +4,7 @@
*/
#define _GNU_SOURCE
#define _BSD_SOURCE
//system includes
#include <err.h>
......@@ -13,11 +14,16 @@
#include <signal.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/select.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/rfcomm.h>
#include <pthread.h>
#include <assert.h>
#include <errno.h>
#include <string.h>
#include <sys/ioctl.h>
//user created includes
#include "communication.h"
......@@ -30,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);
......@@ -60,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];
// global variables
static volatile int keepRunning = 1;
......@@ -77,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
......@@ -93,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;
......@@ -153,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 Quad...");
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]))
......@@ -180,30 +182,26 @@ int main(int argc, char **argv)
perror("Error creating log file...");
exit(1);
}
// writeStringToLog(logHeader);
// writeStringToLog(logHeader);
// 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);
//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();
safe_fd_set(zyboSocket, &rfds_master, &max_fd);
// Tell the quad we are ready to send it vrpn data
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;
......@@ -212,11 +210,11 @@ 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))
if(wasDisconnected(fd)){
break;
}
if (fd == fileno(stdin)) {
// break;
......@@ -238,23 +236,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) {
perror("ERROR reading 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);
}
}
......@@ -269,25 +287,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,
......@@ -300,8 +310,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)
......@@ -319,19 +329,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));
......@@ -348,6 +359,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) {
......@@ -366,27 +380,69 @@ void printVrpnData(struct ucart_vrpn_TrackerData * td) {
int connectToZybo() {
int sock;
struct sockaddr_rc addr;
int status = 0;
// allocate a socket
sock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
/* Use bluetooth by default */
if (!getenv(QUAD_WIFI_ENV)) {
printf("Using BT Settings\n");
struct sockaddr_rc addr;
//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
int status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
// allocate a socket
sock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
//set the connection params ie. who to connect to
addr.rc_family = AF_BLUETOOTH;
addr.rc_channel = (uint8_t) QUAD_BT_CHANNEL;
str2ba( QUAD_BT_ADDR, &addr.rc_bdaddr );
printf("Attempting to connect to zybo. Please be patient...\n");
// blocking call to connect to socket sock ie. zybo board
status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
} else {
printf("Using WIFI settings\n");
struct sockaddr_in addr;
addr.sin_family = AF_INET;
/* Quick and Dirty */
if (getenv(QUAD_IP_ENV)) {
if (!inet_aton(getenv(QUAD_IP_ENV), &addr.sin_addr)) {
printf("Env var %s invalid IP %s\n",
QUAD_IP_ENV, getenv(QUAD_IP_ENV));
return -1;
}
} else {
if (!inet_aton(QUAD_IP_DEFAULT, &addr.sin_addr)) {
printf("Default IP %s is invalid\n",
QUAD_IP_DEFAULT);
return -1;
}
}
if (getenv(QUAD_PORT_ENV)) {
/* Quick 'n dirty, oh yeah! */
addr.sin_port = htons(atoi(getenv(QUAD_PORT_ENV)));
} else {
addr.sin_port = htons(QUAD_PORT_DEFAULT);
}
sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
if (sock < 0) {
perror("socket");
return -1;
}
printf("Connecting to Quad @ %s:%u\n", inet_ntoa(addr.sin_addr), ntohs(addr.sin_port));
status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
}
// connection failed
if(status < 0)
{
close(sock);
printf("Connection failed!...\n");
perror("connect");
return -1;
}
}
else
{
printf("connection successful!...\n");
......@@ -394,12 +450,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);
......@@ -424,13 +474,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 (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__);
}
......@@ -438,8 +488,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) {
......@@ -458,8 +519,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;
......@@ -469,8 +529,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;
......@@ -501,8 +560,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);
......@@ -537,11 +595,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);
......@@ -559,8 +617,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);
}
groundStation/src/vrpn_tracker.cpp
+ 33
− 29
View file @ b228070d
#include <iostream>
#include <algorithm>
#include <functional>
#include "vrpn_Tracker.h"
#include "quat.h"
......@@ -9,28 +10,25 @@
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) : Tracker(server.c_str())
Tracker::Tracker(std::string server) :
remote(server.c_str()),
stop_flag(0),
trackerData()
{
}
Tracker::Tracker(const char * server) :
remote(server),
stop_flag(0),
trackerData({
.x = 0.0,
.y = 0.0,
.z = 0.0,
.pitch = 0.0,
.roll = 0.0,
.yaw = 0.0,
.fps = 0.0,
.timestamp = {
.tv_sec = 0,
.tv_usec = 0
}
})
trackerData()
{
remote.register_change_handler(this, vrpn_cb);
......@@ -92,10 +90,9 @@ namespace microcart
trackerData.fps = 1.0 / elapsed_time_usec;
auto td = trackerData;
std::for_each(cb_vector.begin(), cb_vector.end(),
[td](std::function<void(const TrackerData &)> &fn){
fn(td);
});
for(auto i = cb_vector.begin(); i != cb_vector.end(); ++i) {
(*i)(td);
}
}
void Tracker::addCallback(std::function<void(const TrackerData&)> cb)
......@@ -115,6 +112,20 @@ struct ucart_vrpn_tracker {
microcart::Tracker * t;
};
void cb_wrapper(void (*cb)(struct ucart_vrpn_TrackerData *),
const microcart::TrackerData &td)
{
struct ucart_vrpn_TrackerData data;
data.x = td.x;
data.y = td.y;
data.z = td.z;
data.pitch = td.pitch;
data.roll = td.roll;
data.yaw = td.yaw;
data.fps = td.fps;
(*cb)(&data);
}
extern "C"
{
struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
......@@ -123,6 +134,7 @@ extern "C"
try {
auto inst = new struct ucart_vrpn_tracker;
inst->t = new microcart::Tracker(server);
return inst;
} catch(...) {
return NULL;
}
......@@ -138,16 +150,8 @@ extern "C"
void (*cb)(struct ucart_vrpn_TrackerData *))
{
try {
inst->t->addCallback([cb](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;
(*cb)(&data);
});
auto new_cb = bind(cb_wrapper, cb, std::placeholders::_1);
inst->t->addCallback(new_cb);
} catch(...) {
return -1;
}
......
groundStation/src/vrpn_tracker.hpp
+ 3
− 0
View file @ b228070d
......@@ -54,6 +54,7 @@ extern "C"
namespace microcart
{
struct TrackerData {
public:
double x;
double y;
double z;
......@@ -64,6 +65,8 @@ namespace microcart
double fps;
timeval timestamp;
TrackerData();
};
class Tracker {
......
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