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

added functions to communication.c and adding functionality to microcart_cli.c

parent 08a8f51d
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/communication.c
+ 14
− 1
View file @ e5876302
......@@ -67,7 +67,6 @@ int formatCommand(unsigned char *command, unsigned char **formattedCommand) {
{
if(strcmp(tokens.tokens[0], registeredCommands[cmdIndex].commandText) == 0)
{
switch (registeredCommands[cmdIndex].dataType)
{
// Validate the float input
......@@ -253,3 +252,17 @@ int processCommand(unsigned char *packet, unsigned int cmdIndex) {
// Only gets here if there is an error
return -1;
}
float getFloat(unsigned char* str, int pos) {
union {
float f;
int i;
} x;
x.i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
return x.f;
}
int getInt(unsigned char* str, int pos) {
int i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
return i;
}
src/microcart_cli.c
+ 115
− 173
View file @ e5876302
......@@ -8,9 +8,11 @@
#include <unistd.h>
#include <signal.h>
#include <sys/socket.h>
#include <sys/select.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/rfcomm.h>
#include <pthread.h>
#include <assert.h>
//user created includes
#include "communication.h"
......@@ -31,13 +33,15 @@ void sendStartPacket(void);
void getVRPNPacket(struct ucart_vrpn_TrackerData *);
void printVrpnData(struct ucart_vrpn_TrackerData *);
int connectToZybo();
void *handleQuadResponse();
void *handleCliInput();
int atomic_check(int*, pthread_mutex_t*);
void performCommand(char *cmdName, char * command);
int startsWith(const char *pre, const char *str);
// void *handleQuadResponse();
// void *handleCliInput();
// int atomic_check(int*, pthread_mutex_t*);
void performCommand(char *, char *);
int startsWith(const char *, const char *);
int safe_fd_set(int , fd_set* , int* );
int safe_fd_clr(int , fd_set* , int* );
//static void cb(struct ucart_vrpn_TrackerData *);
static void cb(struct ucart_vrpn_TrackerData *);
// global variables
static volatile int keepRunning = 1;
......@@ -58,184 +62,119 @@ modular_structs_t structs = {};
// Currently doing much more than it should. It will be slimmed down
// in the future.
// static void cb(struct ucart_vrpn_TrackerData * td)
// {
// static int count = 0;
// if(!(count % 10)) {
// sendVrpnPacket(td);
// updateLogFile(td);
// }
// count++;
// // 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);
// }**/
// }
void *handleQuadResponse() {
unsigned char buffer[255];
while(keepRunning) {
// Clear the buffer and read the message from the socket (from the server)
memset(buffer, 0, 255);
// If there was an error reading from the socket, throw an error
if(read(zyboSocket, buffer, 255) <= 0) {
fprintf(stderr, "CLI QUAD: ERROR reading from quad.\n");
continue;
}
pthread_mutex_lock(&quadResponseMutex);
newQuadResponse = 1;
memcpy(respBuf, buffer, 255);
pthread_mutex_unlock(&quadResponseMutex);
static void cb(struct ucart_vrpn_TrackerData * td)
{
static int count = 0;
//parse_packet(buffer, &respBuf, &metadata);
// fprintf(stderr, "CLI QUAD: %s\n", buffer);
if(!(count % 10)) {
sendVrpnPacket(td);
updateLogFile(td);
}
pthread_exit(NULL);
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);
}**/
}
void *handleCliInput() {
sleep(1);
while(keepRunning)
{
char userCommand[CMD_MAX_LENGTH] = {};
fprintf(stderr, "$microcart> ");
while(fgets(userCommand, sizeof(userCommand), stdin) != NULL && keepRunning) {
// if the user simply hit enter then let them try again
if((userCommand[0] == '\n') || (userCommand[0] == '\r'))
{
fprintf(stderr, "$microcart> ");
memset(userCommand, 0, CMD_MAX_LENGTH);
continue;
}
if((userCommand[strlen(userCommand) - 1] == '\n') || (userCommand[strlen(userCommand) - 1] == '\r'))
userCommand[strlen(userCommand) - 1] = '\0';
pthread_mutex_lock(&cliInputMutex);
newCliInput = 1;
memcpy(commandBuf, &userCommand, CMD_MAX_LENGTH);
pthread_mutex_unlock(&cliInputMutex);
fprintf(stderr, "$microcart> ");
memset(userCommand, 0, CMD_MAX_LENGTH);
}
}
pthread_exit(NULL);
}
int main(int argc, char **argv)
{
pthread_t quadResponse, cliInput;
respBuf = malloc(1024);
commandBuf = malloc(CMD_MAX_LENGTH);
// pthread_t quadResponse, cliInput;
fd_set rfds;
int activity;
int max_fd = 0;
signal(SIGINT, killHandler);
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);
// }
// create vrpnTracker instance
// tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
// Retrieve VRPN data from the control loop
fprintf(stderr, "CLI: Starting quad receiving thread...\n");
pthread_create(&quadResponse, NULL, handleQuadResponse, NULL);
fprintf(stderr, "CLI: Thread quad receiving started.\n");
// Retrieve user command input
fprintf(stderr, "CLI: Starting cli input thread...\n");
pthread_create(&cliInput, NULL, handleCliInput, NULL);
fprintf(stderr, "CLI: Thread Cli input started.\n");
//tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
// open the log file
if( (status = createLogFile(argc, argv[1])) < 0)
{
perror("Error creating log file...");
free(respBuf);
free(commandBuf);
exit(1);
}
writeStringToLog(logHeader);
// writeStringToLog(logHeader);
// clear the fd set
FD_ZERO(&rfds);
// watch for input from stdin (fd 0) to see when it has input
safe_fd_set(fileno(stdin), &rfds, &max_fd);
// watch for input from the zybo socket
//safe_fd_set(zyboSocket, &rfds, &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);
int updatePrompt = 0;
// start the prompt
fprintf(stdout, "$microcart> ");
while(keepRunning)
{
char tmpRespBuf[1024];
unsigned char tmpCommandBuf[CMD_MAX_LENGTH];
if(updatePrompt)
{
fprintf(stderr, "$microcart> ");
updatePrompt = 0;
}
//check for user input via cli
if(atomic_check(&newCliInput, &cliInputMutex))
{
pthread_mutex_lock(&cliInputMutex);
newCliInput = !newCliInput;
memcpy(tmpCommandBuf, commandBuf, CMD_MAX_LENGTH);
pthread_mutex_unlock(&cliInputMutex);
// I can use printf becuase the command was gathered using fgets.
//fprintf(stderr, "\rINPUT FOUND via CLI: '%s'\n", tmpCommandBuf);
updatePrompt = !updatePrompt;
unsigned char *packet;
formatCommand(tmpCommandBuf, &packet);
}
//check for update/response from quad
if(atomic_check(&newQuadResponse, &quadResponseMutex))
{
pthread_mutex_lock(&quadResponseMutex);
newQuadResponse = !newQuadResponse;
memcpy(tmpRespBuf, respBuf, 1024);
pthread_mutex_unlock(&quadResponseMutex);
char buf[1025];
memcpy(buf, tmpRespBuf, 1024);
buf[1025] = '\0';
//fprintf(stderr, "\rINPUT FOUND via QUAD: '%s'\n", buf);
writeStringToLog(buf);
updatePrompt = !updatePrompt;
activity = select(max_fd+1, &rfds, NULL, NULL, NULL);
if(activity == -1) {
perror("select() ");
} else if (activity) {
for(unsigned int fd = 0; fd <= max_fd; ++fd) {
if (FD_ISSET(fd, &rfds)) {
if (fd == fileno(stdin)) {
unsigned char userCommand[CMD_MAX_LENGTH] = {};
read(fileno(stdin), (char *)userCommand, sizeof(userCommand));
unsigned int cmdLen = strlen((char*) userCommand);
// if the user simply hit enter then let them try again
if((userCommand[0] != '\n') && (userCommand[0] != '\r'))
{
// remove newline and return line chars
if((userCommand[cmdLen - 1] == '\n') || (userCommand[cmdLen - 1] == '\r'))
userCommand[cmdLen - 1] = '\0';
unsigned char *packet;
formatCommand(userCommand, &packet);
printf("received input from cli: '%s'\n", userCommand);
fprintf(stdout, "CLI sees as: %f\n", getFloat(packet, 7));
// Write the command to the control_loop socket
// int n = write(zyboSocket, packet, ((packet[6] << 8) | packet[5]) + 8);
// if(n < 0) {
// fprintf(stdout, "CLI: ERROR writing to socket\n");
// }
}
fprintf(stdout, "$microcart> ");
memset(userCommand, 0, cmdLen);
} else if (fd == zyboSocket) {
}
}
}
}
}
//ucart_vrpn_tracker_freeInstance(tracker);
//free(vrpnData);
free(respBuf);
free(commandBuf);
pthread_mutex_destroy(&cliInputMutex);
pthread_mutex_destroy(&quadResponseMutex);
// ucart_vrpn_tracker_freeInstance(tracker);
close(zyboSocket);
close(quadlog_file);
return 0;
......@@ -247,18 +186,6 @@ void killHandler(int dummy) {
printf("\nleaving Bluetooth module\n");
}
void readAndPrint() {
// read data from the server
// this is a blocking call.
//TODO: Implement a non blocking version of this.
bytes_read = read(zyboSocket, respBuf, sizeof(respBuf) -1);
if( bytes_read > 0)
{
respBuf[bytes_read] = '\0';
printf("%s", respBuf);
}
}
void sendStartPacket() {
unsigned char packet[8] = {0};
......@@ -368,18 +295,11 @@ int connectToZybo() {
}
}
int atomic_check(int* atomicFlag, pthread_mutex_t* mutex) {
pthread_mutex_lock(mutex);
int result = *atomicFlag;
pthread_mutex_unlock(mutex);
return result;
}
void performCommand(char *cmdName, char * command) {
for(int i = 0; i < NUM_COMMANDS; ++i)
{
if(startsWith(registeredCommands[i].commandText, command));
fprintf(stderr, "\r\n You used cmd '%s'\n",registeredCommands[i].commandText);
fprintf(stdout, "\r\n You used cmd '%s'\n",registeredCommands[i].commandText);
}
}
......@@ -387,4 +307,26 @@ 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);
FD_SET(fd, fds);
if (fd > *max_fd) {
*max_fd = fd;
}
return 0;
}
/* clear fd from fds, update max fd if needed */
int safe_fd_clr(int fd, fd_set* fds, int* max_fd) {
assert(max_fd != NULL);
FD_CLR(fd, fds);
if (fd == *max_fd) {
(*max_fd)--;
}
return 0;
}
\ 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