Newer
Older
/* Author: Kris Burney
*
* BlueTooth socket program for passing vrpn data to quad.
*/
//system includes
#include <stdio.h>
#include <stdlib.h>
#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>
#include "commands.h"
#include "vrpn_tracker.hpp"
#include "type_def.h"
#include "logger.h"
#define QUAD_BT_ADDR "00:06:66:64:61:D6"
#define QUAD_BT_CHANNEL 0x01
#define CMD_MAX_LENGTH 1024
// 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();
// 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 *);
pthread_mutex_t quadSocketMutex;
static ssize_t writeQuad(const char * buf, size_t count);
// global variables
static volatile int keepRunning = 1;
const char *TRACKER_IP = "UAV@192.168.0.120:3883";
struct ucart_vrpn_tracker * tracker = NULL;
const char *logHeader = "";//"#\n#\tDefault log header\n#\tEverything after '#'`s will be printed as is in the processed logs.\n#\n\0";
pthread_mutex_t quadResponseMutex, cliInputMutex ;
unsigned char *respBuf, *commandBuf;
int newQuadResponse = 0, newCliInput = 0;
// Structures to be used throughout
modular_structs_t structs = {};
// Callback to be ran whenever the tracker receives data.
// 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);
}**/
// pthread_t quadResponse, cliInput;
fd_set rfds;
int activity;
int max_fd = 0;
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 (pthread_mutex_unlock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
}
//tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
{
perror("Error creating log file...");
exit(1);
}
// 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);
printf("sending Start Packet...\n");
//sendStartPacket();
// this function will be called whenever tracker receives data
// ucart_vrpn_tracker_addCallback(tracker, cb);
// start the prompt
fprintf(stdout, "$microcart> ");
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 = writeQuad(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);
if (pthread_mutex_lock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
}
if (pthread_mutex_unlock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
}
closeLogFile();
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
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};
metadata_t metadata =
{
BEGIN_CHAR,
0x04,
0x01,
0x01,
0
};
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[5] = 0; // DATALEN(1)
packet[6] = 0; // DATALEN(2)
char checksum = 0;
int i;
for(i=0; i < metadata.data_len + 7; i++)
checksum ^= packet[i];
packet[metadata.data_len + 7] = checksum; //PACKET_END_BYTE;
int status = writeQuad((char * ) packet, metadata.data_len + 8);
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
if (status != 8)
{
perror("Error sending start packet...\n");
keepRunning = 0;
}else
{
printf("Start packet successfuly sent...\n");
}
}
void sendVrpnPacket(struct ucart_vrpn_TrackerData *info) {
int pSize = sizeof(info) + 8;
int n;
char packet[pSize];
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[5] = (sizeof(info) & 0x000000ff); // DATALEN(1)
packet[6] = ((sizeof(info) >> 8) & 0x00000ff); // DATALEN(2)
memcpy(&packet[7], &info, sizeof(info));
char checksum = 0;
int i;
for(i=0; i < pSize - 1; i++)
checksum ^= packet[i];
packet[pSize - 1] = checksum; //PACKET_END_BYTE;
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
if(n < 0) {
perror("vrpnhandler: ERROR writing to socket");
keepRunning = 0;
}
}
void getVRPNPacket(struct ucart_vrpn_TrackerData *td) {
int status;
if((status = ucart_vrpn_tracker_getData(tracker, td)) < 0)
{
perror("Error receiving VRPN data from tracker...");
keepRunning = 0;
}
}
void printVrpnData(struct ucart_vrpn_TrackerData * td) {
printf("FPS: %lf Pos (xyz): (%lf %lf %lf) Att (pry): (%lf %lf %lf)\n",
td->fps, td->x, td->y, td->z, td->pitch, td->roll, td->yaw);
}
int connectToZybo() {
int sock;
struct sockaddr_rc addr = { -1 };
// 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
int status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
// connection failed
if(status < 0)
{
close(sock);
printf("Connection failed!...\n");
return -1;
}
else
{
printf("connection successful!...\n");
return sock;
}
}
void performCommand(char *cmdName, char * command) {
for(int i = 0; i < NUM_COMMANDS; ++i)
{
if(startsWith(registeredCommands[i].commandText, command));
fprintf(stdout, "\r\n You used cmd '%s'\n",registeredCommands[i].commandText);
}
}
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;
static ssize_t writeQuad(const char * buf, size_t count) {
ssize_t retval;
if (pthread_mutex_lock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
}
retval = writeQuad(buf, count);
if (pthread_mutex_unlock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
}
return retval;
}