Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
M
MicroCART
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Deploy
Releases
Model registry
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Distributed Autonomous Networked Control Lab
MicroCART
Commits
e5876302
Commit
e5876302
authored
8 years ago
by
burneykb
Browse files
Options
Downloads
Patches
Plain Diff
added functions to communication.c and adding functionality to microcart_cli.c
parent
08a8f51d
No related branches found
Branches containing commit
No related tags found
Tags containing commit
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
src/communication.c
+14
-1
14 additions, 1 deletion
src/communication.c
src/microcart_cli.c
+115
-173
115 additions, 173 deletions
src/microcart_cli.c
with
129 additions
and
174 deletions
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
;
}
This diff is collapsed.
Click to expand it.
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> "
);
u
pdatePrompt
=
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
))
{
u
nsigned
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
(
"
\n
leaving 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
(
std
err
,
"
\r\n
You used cmd '%s'
\n
"
,
registeredCommands
[
i
].
commandText
);
fprintf
(
std
out
,
"
\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
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment