Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • danc/MicroCART
  • snawerdt/MicroCART_17-18
  • bbartels/MicroCART_17-18
  • jonahu/MicroCART
4 results
Show changes
Showing
with 2789 additions and 114 deletions
/* Author: Kris Burney & Jake Drahos
*
* BlueTooth socket program for passing vrpn data to quad.
*/
#define _GNU_SOURCE
#define _BSD_SOURCE
//system includes
#include <err.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/select.h>
#include <sys/stat.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>
#include <sys/types.h>
#include <netinet/tcp.h>
//user created includes
#include "communication.h"
#include "commands.h"
#include "vrpn_tracker.hpp"
#include "type_def.h"
#include "logger.h"
#include "config.h"
#define QUAD_BT_ADDR "00:06:66:64:61:D6"
#define QUAD_BT_CHANNEL 0x01
#define CMD_MAX_LENGTH 1024
#define MAX_HASH_SIZE 50
/* Backend-internal command magics */
#define TD_MAGIC "TRACKERDATA"
// function prototypes
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 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);
static void cb(struct ucart_vrpn_TrackerData *);
static int new_client(int fd);
/* Return index of client, or -1 */
static ssize_t get_client_index(int fd);
/* Returns pointer to client buffer, or -1 */
static char * get_client_buffer(int fd);
/* Return pointer to client pending responses, or -1*/
static int * get_client_pend_responses(int fd);
/* Return positive integer if successful, -1 otherwise */
static int clientAddPendResponses(int fd, unsigned char *packet);
/* Returns -1 on error */
static int remove_client(int fd);
/* Receive data from client */
static void client_recv(int fd);
/* Receive data from quad */
static void quad_recv();
/* Checks to see if socket has disconnected. Returns 1 on disconnect, else returns 0 */
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;
const char *TRACKER_IP = "UAV@192.168.0.120:3883";
static int zyboSocket;
static int backendSocket;
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";
#define MAX_CLIENTS 32
#define CLIENT_BUFFER_SIZE 1024
#define CLIENT_MAX_PENDING_RESPONSES 5
static char client_buffers[MAX_CLIENTS][CLIENT_BUFFER_SIZE];
static int client_fds[MAX_CLIENTS];
static int client_pending_responses[MAX_CLIENTS][CLIENT_MAX_PENDING_RESPONSES];
fd_set rfds_master;
int max_fd = 0;
pthread_mutex_t quadResponseMutex, cliInputMutex ;
unsigned char *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++;
}
int main(int argc, char **argv)
{
int activity;
FD_ZERO(&rfds_master);
/*
* Create backend listening socket
*/
/* Determine socket path */
char * backend_socket_path = DEFAULT_SOCKET;
if (getenv(SOCKET_ENV)) {
backend_socket_path = getenv(SOCKET_ENV);
}
/* Unlink if it exists */
unlink(backend_socket_path);
/* Create socket */
mode_t old_umask = umask(0111);
backendSocket = socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK, 0);
if (backendSocket < 0) {
err(-1, "socket");
}
/* Create sockaddr and bind */
struct sockaddr_un sa;
sa.sun_family = AF_UNIX;
strncpy(sa.sun_path, backend_socket_path, 107);
sa.sun_path[107] = '\0';
if (bind(backendSocket, (struct sockaddr *) &sa, sizeof(sa))) {
err(-1, "bind");
}
umask(old_umask);
/* Listen */
if (listen(backendSocket, 16)) {
err(-1, "listen");
}
/* Add to socket set */
safe_fd_set(backendSocket, &rfds_master, &max_fd);
/* Initialize client buffers */
for (int i = 0; i < MAX_CLIENTS; i++) {
client_fds[i] = -1;
client_buffers[i][0] = '\n';
for(int j = 0; j < CLIENT_MAX_PENDING_RESPONSES; j++) {
client_pending_responses[i][j] = -1;
}
}
if (pthread_mutex_lock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
}
if ((zyboSocket = connectToZybo()) < 0)
{
perror("Error connecting to Quad...");
free(commandBuf);
exit(1);
}
if (pthread_mutex_unlock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
}
// open the log file
if(createLogFile(argc, argv[1]))
{
perror("Error creating log file...");
exit(1);
}
// writeStringToLog(logHeader);
// watch for input from stdin (fd 0) to see when it has input
safe_fd_set(fileno(stdin), &rfds_master, &max_fd);
if (!getenv(NOQUAD_ENV)) {
// watch for input from the zybo socket
safe_fd_set(zyboSocket, &rfds_master, &max_fd);
}
// Tell the quad we are ready to send it vrpn data
sendStartPacket();
if(!getenv(NOVRPN_ENV)){
// create vrpnTracker instance
tracker = ucart_vrpn_tracker_createInstance(TRACKER_IP);
// this function will be called whenever tracker receives data
ucart_vrpn_tracker_addCallback(tracker, cb);
}
struct timeval timeout = {
.tv_sec = 1,
.tv_usec = 0
};
while(keepRunning)
{
fd_set rfds;
rfds = rfds_master;
activity = select(max_fd+1, &rfds, NULL, NULL, NULL);
if(activity == -1) {
perror("select() ");
} else if (activity) {
for(int fd = 0; fd <= max_fd; ++fd) {
if (FD_ISSET(fd, &rfds)) {
if(wasDisconnected(fd)){
break;
}
if (fd == fileno(stdin)) {
/**
* Ignore stdin from the backend
*/
} else if (fd == zyboSocket) {
printf("recieving from quad\n");
quad_recv();
} else if (fd == backendSocket) {
int new_fd = 0;
new_fd = accept(backendSocket, NULL, NULL);
if (new_fd < 0) {
warn("accept");
} else {
printf("Connection\n");
if (new_client(new_fd)) {
printf("Added client\n");
safe_fd_set(new_fd, &rfds_master, &max_fd);
}
}
} else if (get_client_index(fd) > -1) {
/* It is a socket to a frontend */
client_recv(fd);
}
}
}
} else {
timeout.tv_sec = 1;
timeout.tv_usec = 0;
}
}
ucart_vrpn_tracker_freeInstance(tracker);
safe_close_fd(zyboSocket, &quadSocketMutex);
closeLogFile();
return 0;
}
void sendStartPacket() {
unsigned char packet[8] = {0};
currMessageID++;
metadata_t metadata =
{
(char) 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] = (currMessageID & 0x000000ff); // MSG ID(1)
packet[4] = ((currMessageID >> 8) & 0x000000ff); // 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);
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 = 36;//sizeof(struct ucart_vrpn_TrackerData) + 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] = (currMessageID & 0x000000ff); // MSG ID(1)
packet[4] = ((currMessageID >> 8) & 0x000000ff); // MSG ID(2)
packet[5] = (28 & 0x000000ff); // DATALEN(1)
packet[6] = ((28) >> 8 & 0x00000ff); // DATALEN(2)
//memcpy(&packet[7], &info, sizeof(struct ucart_vrpn_TrackerData));
memset(&packet[7], 0, 4);
memcpy(&packet[11], &(info->y), 4);
memcpy(&packet[15], &(info->x), 4);
memcpy(&packet[19], &(info->z), 4);
memcpy(&packet[23], &(info->pitch), 4);
memcpy(&packet[27], &(info->roll), 4);
memcpy(&packet[31], &(info->yaw), 4);
char checksum = 0;
int i;
for(i=0; i < pSize - 1; i++)
checksum ^= packet[i];
packet[pSize - 1] = checksum; //PACKET_END_BYTE;
n = writeQuad((char *) packet, pSize);
if(n < 0) {
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) {
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;
int status = 0;
if (getenv(NOQUAD_ENV)) {
return 0;
}
/* Use bluetooth by default */
if (!getenv(QUAD_WIFI_ENV)) {
printf("Using BT Settings\n");
struct sockaddr_rc 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);
perror("connect");
return -1;
}
else
{
// int result = setsockopt(sock, IPPROTO_TCP, TCP_NODELAY, (int[]){1}, sizeof(int));
// printf("result = %d\n", result);
printf("connection successful!...\n");
return sock;
}
}
/* 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 (getenv(NOQUAD_ENV)) {
return count;
}
if (pthread_mutex_lock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
}
retval = write(zyboSocket, buf, count);
if (pthread_mutex_unlock(&quadSocketMutex)) {
err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
}
return retval;
}
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) {
new_slot = i;
break;
}
}
if (new_slot == -1) {
warnx("Ran out of room! Consider increasing MAX_CLIENTS!");
return 0;
}
client_fds[new_slot] = fd;
client_buffers[new_slot][0] = '\0';
return 1;
}
static ssize_t get_client_index(int fd) {
for (ssize_t i = 0; i < MAX_CLIENTS; i++) {
if (client_fds[i] == fd) {
return i;
}
}
return -1;
}
static char * get_client_buffer(int fd) {
ssize_t slot = get_client_index(fd);
if (slot == -1) {
return NULL;
} else {
return client_buffers[slot];
}
}
static int * get_client_pend_responses(int fd) {
ssize_t slot = get_client_index(fd);
if (slot == -1) {
return NULL;
} else {
return client_pending_responses[slot];
}
}
static int clientAddPendResponses(int fd, unsigned char *packet) {
int *pendingResponses = get_client_pend_responses(fd);
int packetID = (packet[4] << 8) | (packet[3]);
for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) {
if(pendingResponses[i] == -1) {
pendingResponses[i] = packetID;
return i;
}
}
return -1;
}
static int remove_client(int fd) {
ssize_t slot = get_client_index(fd);
if(slot == -1)
return -1;
char *clientBuffer = get_client_buffer(fd);
if(clientBuffer == NULL)
return -1;
clientBuffer[0] = '\0';
int *pendingResponses = get_client_pend_responses(fd);
if(pendingResponses == NULL)
return -1;
for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) {
pendingResponses[i] = -1;
}
client_fds[slot] = -1;
return 0;
}
static void safe_close_fd(int fd, pthread_mutex_t *mutexLock) {
if (pthread_mutex_lock(mutexLock)) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
}
close(fd);
if (pthread_mutex_unlock(mutexLock)) {
err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
}
}
static void client_recv(int fd) {
char * buffer;
ssize_t len_pre;
buffer = get_client_buffer(fd);
len_pre = strlen(buffer);
char * cursor;
cursor = buffer + len_pre;
ssize_t r;
r = read(fd, cursor, CLIENT_BUFFER_SIZE - len_pre - 1);
if (r < 0) {
warn("read (fd: %d)", fd);
}
buffer[len_pre + r] = '\0';
int index = 0;
/* Parse buffer and handle commands */
while (1) {
/* not using strtok because reasons */
size_t len = strlen(buffer);
ssize_t newline = -1;
for (size_t i = 0; i < len; i++) {
if (buffer[i] == '\n') {
newline = i;
break;
}
}
/* No newline found. End parsing */
if (newline == -1) {
break;
}
buffer[newline] = '\0';
printf("Client(%d) : '%s'\n",fd, buffer);
unsigned char * packet;
if(formatCommand(buffer, &packet) == -1) {
/* buffer was not a quad command, handling internally to
* backend instead of forwarding to quad
*/
if (strncmp(buffer, TD_MAGIC, strlen(TD_MAGIC)) == 0) {
/* Request for tracker data */
struct ucart_vrpn_TrackerData td;
if (tracker == NULL) {
char * dummy = TD_MAGIC " 1.0 2.0 3.0 4.0 5.0 6.0\n";
write(fd, dummy, strlen(dummy));
}else if (ucart_vrpn_tracker_getData(tracker, &td)) {
write(fd, TD_MAGIC " ERROR\n", strlen(TD_MAGIC " ERROR\n"));
} else {
/* more than sufficient buffer */
char buffer[2048];
/* Map VRPN XYZ to Height Lat Long (right now it's
* a guess). Format is Height Lat Long P R Y */
if (snprintf(buffer,
2048,
TD_MAGIC " %lf %lf %lf %lf %lf %lf\n",
td.z,
td.y,
td.x,
td.pitch,
td.roll,
td.yaw) >= 2048) {
/* Output longer than buffer */
warnx("Increase format buffer size, output was too long!");
write(fd, TD_MAGIC " ERROR\n", strlen(TD_MAGIC " ERROR\n"));
}
write(fd, buffer, strlen(buffer));
}
}
} else {
if(clientAddPendResponses(fd, packet) == -1) {
warnx("Ran out of room! Consider increasing CLIENT_MAX_PENDING_RESPONSES!\n");
} else {
int datalen = (packet[6] << 8) | (packet[5]);
printf("sending %lf '", getFloat(packet, 7));
for(int i = 0; i < datalen + 8; ++i) {
printf(" 0x%x", (signed) packet[i]);
}
printf("'\n");
writeQuad((char *) packet, datalen +8);
}
}
char * rest = &buffer[newline] + 1;
size_t restLen = (strlen(rest) == 0) ? 1 : strlen(rest);
/* Delete parsed data and move the rest to the left */
memmove(buffer, rest, restLen +1);
}
}
static void quad_recv() {
/* Check to see which command we are receiving. If it is one that needs to be passed on
onto the clients, do so.
*/
int validPacket;
unsigned char *data;
metadata_t metadata;
static unsigned char respBuf[2048];
static size_t respBufLen;
/**
* Read the response from the control loop
*/
int respLen = readQuad((char *) respBuf + respBufLen,
CMD_MAX_LENGTH);
if(respLen <= 0) {
perror("ERROR reading from quad...\n");
return;
}
respBufLen += respLen;
if (respBufLen < 8) {
/* not long enough yet */
printf("too short!!\n");
return;
}
// Validate the message is correctly formatted
validPacket = parse_packet((unsigned char *) respBuf, &data, &metadata);
if (validPacket == -1) {
warnx("Doesn't have start byte.");
/* nuke packet */
respBufLen = 0;
return;
}
/* Get datalen */
size_t datalen = metadata.data_len;
if (datalen > CMD_MAX_LENGTH - 8) {
/* Very invalid packet. Nuke that shit */
respBufLen = 0;
return;
}
if (respBufLen < datalen + 8) {
/* Packet not yet fully read */
return;
}
if (validPacket == 0) {
/* At least enough data read to check checksum, and it was good!*/
char * cmdText = MessageTypes[(int)metadata.msg_type].subtypes[(int)metadata.msg_subtype].cmdText;
float value = getFloat((unsigned char *)respBuf, 7);
//printf("Quad : %s, %lf\n", cmdText, value);
/*
Assuming the quad sends the correct info.. This hasn't been tested yet due to a lack of
quad software. We can check how to format by the cmdText and pass to every client.
*/
char buffer[1048];
sprintf(buffer, "%s %lf\n", cmdText, value);
for(int fd = 0; fd <= max_fd; ++fd) {
if (get_client_index(fd) > -1) {
write(fd, buffer, datalen + 8);
}
}
} else {
warnx("Checksum mismatch!");
}
memmove(respBuf, respBuf + datalen + 8, respBufLen - (datalen + 8));
respBufLen -= datalen + 8;
}
static int wasDisconnected(int fd) {
char buff;
if(recv(fd, &buff, 1, MSG_PEEK | MSG_DONTWAIT) == 0)
{
remove_client(fd);
safe_fd_clr(fd, &rfds_master, &max_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("(BackEnd): Elapsed time = %ld ms\n", result.tv_usec/1000);
// printf("print to log\n");
// char tmp[8];
// snprintf(tmp, 8, "%ld \tms\n", result.tv_usec/1000);
// writeStringToLog(tmp);
}
#include "commands.h"
// TAKE THESE OUT WHEN IMPLEMENTING ON THE QUAD SIDE
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;
}
//------------------------------------------------
struct MessageType MessageTypes[MAX_TYPE] =
{
// DEBUG
{
// Message Type ID
0x00,
// Debug Subtypes
{
// NONE subtype
{
// ID
0x00,
// Command text
"debug",
// Type of the command data
stringType,
// Function pointer
&debug
}
}
},
// CALIBRATION
{
// Message Type ID
0x01,
// Calibration Subtypes (PID coefficients)
{
// yaw p constant subtype
{
// ID
0x00,
// Command text
"setyawp",
// Type of the command data
floatType,
// Function pointer
NULL
},
// yaw i constant subtype
{
// ID
0x01,
// Command text
"setyawi",
// Type of the command data
floatType,
// Function pointer
NULL
},
// yaw d constant subtype
{
// ID
0x02,
// Command text
"setyawd",
// Type of the command data
floatType,
// Function pointer
NULL
},
// roll p constant subtype
{
// ID
0x03,
// Command text
"setrollp",
// Type of the command data
floatType,
// Function pointer
NULL
},
// roll i constant subtype
{
// ID
0x04,
// Command text
"setrolli",
// Type of the command data
floatType,
// Function pointer
NULL
},
// roll d constant subtype
{
// ID
0x05,
// Command text
"setrolld",
// Type of the command data
floatType,
// Function pointer
NULL
},
// pitch p constant subtype
{
// ID
0x06,
// Command text
"setpitchp",
// Type of the command data
floatType,
// Function pointer
NULL
},
// pitch i constant subtype
{
// ID
0x07,
// Command text
"setpitchi",
// Type of the command data
floatType,
// Function pointer
NULL
},
// pitch d constant subtype
{
// ID
0x08,
// Command text
"setpitchd",
// Type of the command data
floatType,
// Function pointer
NULL
},
// yawrate p constant subtype
{
// ID
0x09,
// Command text
"setyawratep",
// Type of the command data
floatType,
// Function pointer
NULL
},
// yawrate i constant subtype
{
// ID
0x0A,
// Command text
"setyawratei",
// Type of the command data
floatType,
// Function pointer
NULL
},
// yawrate d constant subtype
{
// ID
0x0B,
// Command text
"setyawrated",
// Type of the command data
floatType,
// Function pointer
NULL
},
// rollrate p constant subtype
{
// ID
0x0C,
// Command text
"setrollratep",
// Type of the command data
floatType,
// Function pointer
NULL
},
// rollrate i constant subtype
{
// ID
0x0D,
// Command text
"setrollratei",
// Type of the command data
floatType,
// Function pointer
NULL
},
// rollrate d constant subtype
{
// ID
0x0E,
// Command text
"setrollrated",
// Type of the command data
floatType,
// Function pointer
NULL
},
// pitchrate p constant subtype
{
// ID
0x0F,
// Command text
"setpitchratep",
// Type of the command data
floatType,
// Function pointer
NULL
},
// pitchrate i constant subtype
{
// ID
0x10,
// Command text
"setpitchratei",
// Type of the command data
floatType,
// Function pointer
NULL
},
// pitchrate d constant subtype
{
// ID
0x11,
// Command text
"setpitchrated",
// Type of the command data
floatType,
// Function pointer
NULL
},
// height p constant subtype
{
// ID
0x12,
// Command text
"setheightp",
// Type of the command data
floatType,
// Function pointer
NULL
},
// height i constant subtype
{
// ID
0x13,
// Command text
"setheighti",
// Type of the command data
floatType,
// Function pointer
NULL
},
// height d constant subtype
{
// ID
0x14,
// Command text
"setheightd",
// Type of the command data
floatType,
// Function pointer
NULL
},
// lat p constant subtype
{
// ID
0x15,
// Command text
"setlatp",
// Type of the command data
floatType,
// Function pointer
NULL
},
// lat i constant subtype
{
// ID
0x16,
// Command text
"setlati",
// Type of the command data
floatType,
// Function pointer
NULL
},
// lat d constant subtype
{
// ID
0x17,
// Command text
"setlatd",
// Type of the command data
floatType,
// Function pointer
NULL
},
// long p constant subtype
{
// ID
0x18,
// Command text
"setlongp",
// Type of the command data
floatType,
// Function pointer
NULL
},
// long i constant subtype
{
// ID
0x19,
// Command text
"setlongi",
// Type of the command data
floatType,
// Function pointer
NULL
},
// long d constant subtype
{
// ID
0x1A,
// Command text
"setlongd",
// Type of the command data
floatType,
// Function pointer
NULL
}
}
},
// REQUEST
{
// Message Type ID
0x02,
// Request Subtypes
{
// accelerometer subtype
{
// ID
0x00,
// Command text
"getaccel",
// Type of the command data
floatType,
// Function pointer
&getaccel
},
// gyroscope subtype
{
// ID
0x01,
// Command text
"getgyro",
// Type of the command data
floatType,
// Function pointer
&getgyro
},
// pitch angle subtype
{
// ID
0x02,
// Command text
"getpitchangle",
// Type of the command data
floatType,
// Function pointer
&getpitchangle
},
// roll angle subtype
{
// ID
0x03,
// Command text
"getrollangle",
// Type of the command data
floatType,
// Function pointer
&getrollangle
},
// get yaw setpoint subtype
{
// ID
0x04,
// Command text
"getyaw",
// Type of the command data
floatType,
// Function pointer
&setyaw
},
// get yaw p constant subtype
{
// ID
0x05,
// Command text
"getyawp",
// Type of the command data
floatType,
// Function pointer
&getyawp
},
// get yaw d constant subtype
{
// ID
0x06,
// Command text
"getyawd",
// Type of the command data
floatType,
// Function pointer
&getyawd
},
// get roll setpoint subtype
{
// ID
0x07,
// Command text
"getroll",
// Type of the command data
floatType,
// Function pointer
&getroll
},
// get roll p constant subtype
{
// ID
0x08,
// Command text
"getrollp",
// Type of the command data
floatType,
// Function pointer
&getrollp
},
// getroll d constant subtype
{
// ID
0x09,
// Command text
"getrolld",
// Type of the command data
floatType,
// Function pointer
&getrolld
},
// get pitch setpoint subtype
{
// ID
0x0A,
// Command text
"getpitch",
// Type of the command data
floatType,
// Function pointer
&getpitch
},
// get pitch p constant subtype
{
// ID
0x0B,
// Command text
"getpitchp",
// Type of the command data
floatType,
// Function pointer
&getpitchp
},
// get pitch d constant subtype
{
// ID
0x0C,
// Command text
"getpitchd",
// Type of the command data
floatType,
// Function pointer
&getpitchd
},
// get throttle setpoint subtype
{
// ID
0x0D,
// Command text
"getthrottle",
// Type of the command data
floatType,
// Function pointer
&getthrottle
},
// get throttle p constant subtype
{
// ID
0x0E,
// Command text
"getthrottlep",
// Type of the command data
floatType,
// Function pointer
&getthrottlep
},
// get throttle i constant subtype
{
// ID
0x0F,
// Command text
"getthrottlei",
// Type of the command data
floatType,
// Function pointer
&getthrottlei
},
// get throttle d constant subtype
{
// ID
0x10,
// Command text
"getthrottled",
// Type of the command data
floatType,
// Function pointer
&getthrottled
}
}
},
// RESPONSE
{
// Message Type ID
0x03,
// Response Subtypes
{
// accelerometer subtype
{
// ID
0x00,
// Command text
"respaccel",
// Type of the command data
floatType,
// Function pointer
&respaccel
},
// gyroscope subtype
{
// ID
0x01,
// Command text
"respgyro",
// Type of the command data
floatType,
// Function pointer
&respgyro
},
// pitch angle subtype
{
// ID
0x02,
// Command text
"resppitchangle",
// Type of the command data
floatType,
// Function pointer
&resppitchangle
},
// roll angle subtype
{
// ID
0x03,
// Command text
"resprollangle",
// Type of the command data
floatType,
// Function pointer
&resprollangle
},
// resp yaw setpoint subtype
{
// ID
0x04,
// Command text
"respyaw",
// Type of the command data
floatType,
// Function pointer
&setyaw
},
// resp yaw p constant subtype
{
// ID
0x05,
// Command text
"respyawp",
// Type of the command data
floatType,
// Function pointer
&respyawp
},
// resp yaw d constant subtype
{
// ID
0x06,
// Command text
"respyawd",
// Type of the command data
floatType,
// Function pointer
&respyawd
},
// resp roll setpoint subtype
{
// ID
0x07,
// Command text
"resproll",
// Type of the command data
floatType,
// Function pointer
&resproll
},
// resp roll p constant subtype
{
// ID
0x08,
// Command text
"resprollp",
// Type of the command data
floatType,
// Function pointer
&resprollp
},
// resproll d constant subtype
{
// ID
0x09,
// Command text
"resprolld",
// Type of the command data
floatType,
// Function pointer
&resprolld
},
// resp pitch setpoint subtype
{
// ID
0x0A,
// Command text
"resppitch",
// Type of the command data
floatType,
// Function pointer
&resppitch
},
// resp pitch p constant subtype
{
// ID
0x0B,
// Command text
"resppitchp",
// Type of the command data
floatType,
// Function pointer
&resppitchp
},
// resp pitch d constant subtype
{
// ID
0x0C,
// Command text
"resppitchd",
// Type of the command data
floatType,
// Function pointer
&resppitchd
},
// resp throttle setpoint subtype
{
// ID
0x0D,
// Command text
"respthrottle",
// Type of the command data
floatType,
// Function pointer
&respthrottle
},
// resp throttle p constant subtype
{
// ID
0x0E,
// Command text
"respthrottlep",
// Type of the command data
floatType,
// Function pointer
&respthrottlep
},
// resp throttle i constant subtype
{
// ID
0x0F,
// Command text
"respthrottlei",
// Type of the command data
floatType,
// Function pointer
&respthrottlei
},
// resp throttle d constant subtype
{
// ID
0x10,
// Command text
"respthrottled",
// Type of the command data
floatType,
// Function pointer
&respthrottled
}
}
},
// UPDATE
{
// Message Type ID
0x04,
// Update Subtypes
{
// NONE subtype
{
// ID
0x00,
// Command text
"update",
// Type of the command data
stringType,
// Function pointer
&update
}
}
},
// LOG
{
// Message Type ID
0x05,
// Log Subtypes
{
// NONE subtype
{
// ID
0x00,
// Command text
"log",
// Type of the command data
stringType,
// Function pointer
&logdata
},
// Response subtype
{
// ID
0x01,
// Command text
"response",
// Type of the command data
stringType,
// Function pointer
&response
}
}
},
};
int debug(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for debug\n");
return 0;
}
int update(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
unsigned char update[28];
memcpy(update, ((float *)packet), 28);
int packetId = getInt(update, 0);
float y_pos = getFloat(update, 4);
float x_pos = getFloat(update, 8);
float alt_pos = getFloat(update, 12);
float roll = getFloat(update, 16);
float pitch = getFloat(update, 20);
float yaw = getFloat(update, 24);
structs->log_struct.currentQuadPosition.packetId = packetId;
structs->log_struct.currentQuadPosition.y_pos = y_pos;
structs->log_struct.currentQuadPosition.x_pos = x_pos;
structs->log_struct.currentQuadPosition.alt_pos = alt_pos;
structs->log_struct.currentQuadPosition.roll = roll;
structs->log_struct.currentQuadPosition.pitch = pitch;
structs->log_struct.currentQuadPosition.yaw = yaw;
printf("QUAD: VRPN Packet:");
printf("Packet ID: %d\n", packetId);
printf("Y Position: %f\n", y_pos);
printf("X Position: %f\n", x_pos);
printf("Altitude Position: %f\n", alt_pos);
printf("Roll: %f\n", roll);
printf("Pitch: %f\n", pitch);
printf("Yaw: %f\n", yaw);
printf("function for update\n");
return 0;
}
// Why is this here?
// This should be on the ground station side
int logdata(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("Logging: %s\n", packet);
return 0;
}
int response(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("This is the response: %s\n", packet);
return 0;
}
// ------------------------------------------------------------------
int setyaw(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
printf("%f\n", value);
structs->setpoint_struct.desiredQuadPosition.yaw = value;
printf("function for setyaw: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
return 0;
}
int setyawp(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.yaw_angle_pid.Kp = value;
printf("function for setyawp: %f\n", structs->parameter_struct.yaw_angle_pid.Kp);
return 0;
}
int setyawd(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.yaw_angle_pid.Kd = value;
printf("function for setyawd: %f\n", structs->parameter_struct.yaw_angle_pid.Kd);
return 0;
}
int setroll(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->setpoint_struct.desiredQuadPosition.roll = value;
printf("function for setroll: %f\n", structs->setpoint_struct.desiredQuadPosition.roll);
return 0;
}
int setrollp(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.local_y_pid.Kp = value;
printf("function for setrollp: %f\n", structs->parameter_struct.local_y_pid.Kp);
return 0;
}
int setrolld(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.local_y_pid.Kd = value;
printf("function for setrolld: %f\n", structs->parameter_struct.local_y_pid.Kd);
return 0;
}
int setpitch(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->setpoint_struct.desiredQuadPosition.pitch = value;
printf("function for setpitch: %f\n", structs->setpoint_struct.desiredQuadPosition.pitch);
return 0;
}
int setpitchp(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.local_x_pid.Kp = value;
printf("function for setpitchp: %f\n", structs->parameter_struct.local_x_pid.Kp);
return 0;
}
int setpitchd(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.local_x_pid.Kd = value;
printf("function for setpitchd: %f\n", structs->parameter_struct.local_x_pid.Kd);
return 0;
}
// ------------------------------------------------------------
// These should be renamed to altitude!
int setthrottle(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->setpoint_struct.desiredQuadPosition.alt_pos = value;
printf("function for setthrottle: %f\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
return 0;
}
int setthrottlep(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.alt_pid.Kp = value;
printf("function for setthrottlep: %f\n", structs->parameter_struct.alt_pid.Kp);
return 0;
}
int setthrottlei(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.alt_pid.Ki = value;
printf("function for setthrottlei: %f\n", structs->parameter_struct.alt_pid.Ki);
return 0;
}
int setthrottled(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
float value;
memcpy(&value, ((float *)packet), dataLen);
structs->parameter_struct.alt_pid.Kd = value;
printf("function for setthrottled: %f\n", structs->parameter_struct.alt_pid.Kd);
return 0;
}
int getyaw(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int getyawp(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int getyawd(unsigned char *packet, int dataLen, modular_structs_t *structs) {
return 0;
}
int getroll(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int getrollp(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int getrolld(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int getpitch(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int getpitchp(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int getpitchd(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int getthrottle(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int getthrottlep(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int getthrottlei(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int getthrottled(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
// These should be renamed to altitude!
// ------------------------------------------------------------
int getgyro(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for getgyro\n");
return 0;
}
int getpitchangle(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for getpitchangle\n");
return 0;
}
int getrollangle(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for getrollangle\n");
return 0;
}
int getaccel(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for getaccel\n");
return 0;
}
int respgyro(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for respgyro\n");
return 0;
}
int resppitchangle(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for resppitchangle\n");
return 0;
}
int resprollangle(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for resprollangle\n");
return 0;
}
int respaccel(unsigned char *packet, int dataLen, modular_structs_t *structs)
{
printf("function for respaccel\n");
return 0;
}
int respyaw(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int respyawp(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int respyawd(unsigned char *packet, int dataLen, modular_structs_t *structs) {
return 0;
}
int resproll(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int resprollp(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int resprolld(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int resppitch(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int resppitchp(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int resppitchd(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int respthrottle(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int respthrottlep(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int respthrottlei(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
int respthrottled(unsigned char *packet, int dataLen, modular_structs_t *structs){
return 0;
}
#ifndef _COMMANDS_H
#define _COMMANDS_H
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "type_def.h"
// ----------------------
// Helper stuff
#define MAX_TYPE 6
#define MAX_SUBTYPE 100
enum Message{
BEGIN_CHAR = 0xBE,
END_CHAR = 0xED
};
// This should also have double to avoid confusion with float values.
enum DataType
{
floatType,
intType,
stringType
};
// MESSAGE SUBTYPES
struct MessageSubtype{
char ID;
char cmdText[100];
char cmdDataType;
int (*functionPtr)(unsigned char *command, int dataLen, modular_structs_t *structs);
};
// MESSAGE TYPES
struct MessageType{
char ID;
struct MessageSubtype subtypes[MAX_SUBTYPE];
};
int debug(unsigned char *c, int dataLen, modular_structs_t *structs);
int update(unsigned char *c, int dataLen, modular_structs_t *structs);
int logdata(unsigned char *c, int dataLen, modular_structs_t *structs);
int response(unsigned char *packet, int dataLen, modular_structs_t *structs);
int setyaw(unsigned char *c, int dataLen, modular_structs_t *structs);
int setyawp(unsigned char *c, int dataLen, modular_structs_t *structs);
int setyawd(unsigned char *c, int dataLen, modular_structs_t *structs);
int setroll(unsigned char *c, int dataLen, modular_structs_t *structs);
int setrollp(unsigned char *c, int dataLen, modular_structs_t *structs);
int setrolld(unsigned char *c, int dataLen, modular_structs_t *structs);
int setpitch(unsigned char *c, int dataLen, modular_structs_t *structs);
int setpitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
int setpitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
int setthrottle(unsigned char *c, int dataLen, modular_structs_t *structs);
int setthrottlep(unsigned char *c, int dataLen, modular_structs_t *structs);
int setthrottlei(unsigned char *c, int dataLen, modular_structs_t *structs);
int setthrottled(unsigned char *c, int dataLen, modular_structs_t *structs);
int getyaw(unsigned char *c, int dataLen, modular_structs_t *structs);
int getyawp(unsigned char *c, int dataLen, modular_structs_t *structs);
int getyawd(unsigned char *c, int dataLen, modular_structs_t *structs);
int getroll(unsigned char *c, int dataLen, modular_structs_t *structs);
int getrollp(unsigned char *c, int dataLen, modular_structs_t *structs);
int getrolld(unsigned char *c, int dataLen, modular_structs_t *structs);
int getpitch(unsigned char *c, int dataLen, modular_structs_t *structs);
int getpitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
int getpitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
int getthrottle(unsigned char *c, int dataLen, modular_structs_t *structs);
int getthrottlep(unsigned char *c, int dataLen, modular_structs_t *structs);
int getthrottlei(unsigned char *c, int dataLen, modular_structs_t *structs);
int getthrottled(unsigned char *c, int dataLen, modular_structs_t *structs);
int getaccel(unsigned char *c, int dataLen, modular_structs_t *structs);
int getgyro(unsigned char *c, int dataLen, modular_structs_t *structs);
int getpitchangle(unsigned char *c, int dataLen, modular_structs_t *structs);
int getrollangle(unsigned char *c, int dataLen, modular_structs_t *structs);
int respaccel(unsigned char *c, int dataLen, modular_structs_t *structs);
int respgyro(unsigned char *c, int dataLen, modular_structs_t *structs);
int resppitchangle(unsigned char *c, int dataLen, modular_structs_t *structs);
int resprollangle(unsigned char *c, int dataLen, modular_structs_t *structs);
int respyaw(unsigned char *c, int dataLen, modular_structs_t *structs);
int respyawp(unsigned char *c, int dataLen, modular_structs_t *structs);
int respyawd(unsigned char *c, int dataLen, modular_structs_t *structs);
int resproll(unsigned char *c, int dataLen, modular_structs_t *structs);
int resprollp(unsigned char *c, int dataLen, modular_structs_t *structs);
int resprolld(unsigned char *c, int dataLen, modular_structs_t *structs);
int resppitch(unsigned char *c, int dataLen, modular_structs_t *structs);
int resppitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
int resppitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
int respthrottle(unsigned char *c, int dataLen, modular_structs_t *structs);
int respthrottlep(unsigned char *c, int dataLen, modular_structs_t *structs);
int respthrottlei(unsigned char *c, int dataLen, modular_structs_t *structs);
int respthrottled(unsigned char *c, int dataLen, modular_structs_t *structs);
int respaccel(unsigned char *c, int dataLen, modular_structs_t *structs);
int respgyro(unsigned char *c, int dataLen, modular_structs_t *structs);
int resppitchangle(unsigned char *c, int dataLen, modular_structs_t *structs);
int resprollangle(unsigned char *c, int dataLen, modular_structs_t *structs);
float getFloat(unsigned char* str, int pos);
int getInt(unsigned char* str, int pos);
// TODO add in string to be read from the command line when sending a subtype of message
extern struct MessageType MessageTypes[MAX_TYPE];
#endif
\ No newline at end of file
...@@ -52,84 +52,95 @@ int checkInt(char *intString, int *value) { ...@@ -52,84 +52,95 @@ int checkInt(char *intString, int *value) {
//-------------------------------- //--------------------------------
// Formatting commands from ground station CLI // Formatting commands from ground station CLI
int formatCommand(unsigned char *command, unsigned char **formattedCommand) { int formatCommand(char *command, unsigned char **formattedCommand) {
//command[strlen((char *)command) - 1] = 0; //fprintf(stderr, "length = %li , received '%s'\n", strlen(command), command);
char cmd[strlen(command)];
tokenList_t tokens = tokenize((char *)command); strncpy(cmd, command, strlen(command));
cmd[strlen(command)] = '\0';
tokenList_t tokens = tokenize(cmd);
float floatValue = 0.0; float floatValue = 0.0;
int intValue = 0; int intValue = 0;
int valid; int valid;
metadata_t metadata = {}; metadata_t metadata;
// ---------------------------------------------- // ----------------------------------------------
if(tokens.numTokens > 1) { if(tokens.numTokens > 0) {
for(int cmdIndex = 0; cmdIndex < NUM_COMMANDS; ++cmdIndex) for(int type = 0; type < MAX_TYPE; type++)
{ {
if(strcmp(tokens.tokens[0], registeredCommands[cmdIndex].commandText) == 0) for(int subtype = 0; subtype < MAX_SUBTYPE; subtype++)
{ {
if(strcmp(tokens.tokens[0], MessageTypes[type].subtypes[subtype].cmdText) == 0)
switch (registeredCommands[cmdIndex].dataType)
{ {
// Validate the float input printf("Sending\n\ttype: %d, \n\tsubtype: %d\n\tcommand: %s\n", type, subtype, MessageTypes[type].subtypes[subtype].cmdText);
case floatType:
valid = checkFloat(tokens.tokens[1], &floatValue);
if(!valid) {
return -1;
}
printf("%f, %s\n", floatValue, tokens.tokens[1]);
metadata.begin_char = BEGIN_CHAR;
metadata.msg_type = registeredCommands[cmdIndex].ID;
metadata.msg_subtype = registeredCommands[cmdIndex].subID;
metadata.msg_id = msgNum++;
metadata.data_len = sizeof(floatValue);
formatPacket(&metadata, &floatValue, formattedCommand);
break;
// Validate the integer input // Make sure the second token is the right type
case intType: switch (MessageTypes[type].subtypes[subtype].cmdDataType)
valid = checkInt(tokens.tokens[1], &intValue); {
if(!valid) { // Validate the float input
return -1; case floatType:
} metadata.begin_char = (char) BEGIN_CHAR;
metadata.msg_type = MessageTypes[type].ID;
metadata.begin_char = BEGIN_CHAR; metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
metadata.msg_type = registeredCommands[cmdIndex].ID;
metadata.msg_subtype = registeredCommands[cmdIndex].subID; if(MessageTypes[type].ID == 0x01) {
metadata.msg_id = msgNum++; valid = checkFloat(tokens.tokens[1], &floatValue);
metadata.data_len = sizeof(intValue); if(!valid) {
return -1;
formatPacket(&metadata, &intValue, formattedCommand); }
metadata.data_len = sizeof(floatValue);
break; } else {
metadata.data_len = 0;
// Validate the string input (doesn't need to happen) }
case stringType:
metadata.begin_char = BEGIN_CHAR; metadata.msg_id = msgNum++;
metadata.msg_type = registeredCommands[cmdIndex].ID; formatPacket(&metadata, &floatValue, formattedCommand);
metadata.msg_subtype = registeredCommands[cmdIndex].subID; break;
metadata.msg_id = msgNum++;
metadata.data_len = strlen(tokens.tokens[1]);
formatPacket(&metadata, &tokens.tokens[1], formattedCommand); // Validate the integer input
case intType:
metadata.begin_char = (char) BEGIN_CHAR;
metadata.msg_type = MessageTypes[type].ID;
metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
if(MessageTypes[type].ID == 0x01) {
valid = checkInt(tokens.tokens[1], &intValue);
if(!valid) {
return -1;
}
metadata.data_len = sizeof(intValue);
} else {
metadata.data_len = 0;
}
metadata.msg_id = msgNum++;
formatPacket(&metadata, &intValue, formattedCommand);
break;
break; // Validate the string input (doesn't need to happen)
default: case stringType:
return -1; metadata.begin_char = (char) BEGIN_CHAR;
metadata.msg_type = MessageTypes[type].ID;
metadata.msg_subtype = MessageTypes[type].subtypes[subtype].ID;
metadata.msg_id = msgNum++;
metadata.data_len = strlen(tokens.tokens[1]);
formatPacket(&metadata, &tokens.tokens[1], formattedCommand);
break;
default:
return -1;
}
return 0;
} }
return 0;
} }
} }
} }
// Only gets here if the command does not exist // Only gets here if the command does not exist
return -1; return -1;
} }
// QUAD & Ground Station // QUAD & Ground Station
// Format the log data from log_message // Format the log data from log_message
//int formatData(unsigned char *log_msg, unsigned char *formattedCommand) //int formatData(unsigned char *log_msg, unsigned char *formattedCommand)
...@@ -154,7 +165,8 @@ int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedComm ...@@ -154,7 +165,8 @@ int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedComm
(*formattedCommand)[2] = metadata->msg_subtype; (*formattedCommand)[2] = metadata->msg_subtype;
//Msg id (msgNum is 2 bytes) //Msg id (msgNum is 2 bytes)
(*formattedCommand)[3] = metadata->msg_id; (*formattedCommand)[3] = (metadata->msg_id & 0x000000ff);
(*formattedCommand)[4] = ((metadata->msg_id >> 8) & 0x000000ff);
// Data length and data - bytes 5&6 for len, 7+ for data // Data length and data - bytes 5&6 for len, 7+ for data
(*formattedCommand)[5] = metadata->data_len & 0x000000ff; (*formattedCommand)[5] = metadata->data_len & 0x000000ff;
...@@ -224,15 +236,17 @@ int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * met ...@@ -224,15 +236,17 @@ int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * met
} }
// compare checksum // compare checksum
if(packet_checksum != calculated_checksum) if(packet_checksum != calculated_checksum) {
fprintf(stderr, "Checksums did not match (Parse Packet): 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum); fprintf(stderr, "Checksums did not match (Parse Packet): 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum);
return -2;
}
return 0; return 0;
} }
// QUAD & Ground Station // QUAD & Ground Station
// Process the command received // Process the command received
int processCommand(unsigned char *packet, unsigned int cmdIndex) { int processCommand(unsigned char *packet, modular_structs_t *structs) {
int validPacket; int validPacket;
unsigned char *data; unsigned char *data;
metadata_t metadata; metadata_t metadata;
...@@ -244,9 +258,13 @@ int processCommand(unsigned char *packet, unsigned int cmdIndex) { ...@@ -244,9 +258,13 @@ int processCommand(unsigned char *packet, unsigned int cmdIndex) {
} }
if(metadata.data_len >= 0) { if(metadata.data_len >= 0) {
// Call the appropriate subtype function /* Null check*/
(* (registeredCommands[cmdIndex].functionPtr))(data, metadata.data_len); if (MessageTypes[(unsigned char)metadata.msg_type].subtypes[
(unsigned char) metadata.msg_subtype].functionPtr) {
// Call the appropriate subtype function
(* (MessageTypes[(unsigned char)metadata.msg_type].subtypes[(unsigned char)metadata.msg_subtype].functionPtr))(data, metadata.data_len, structs);
}
return 0; return 0;
} }
......
...@@ -10,12 +10,10 @@ ...@@ -10,12 +10,10 @@
tokenList_t tokenize(char* cmd); tokenList_t tokenize(char* cmd);
int checkFloat(char *floatString, float *value); int checkFloat(char *floatString, float *value);
int checkInt(char *intString, int *value); int checkInt(char *intString, int *value);
int formatCommand(unsigned char *command, unsigned char **formattedCommand); int formatCommand(char *command, unsigned char **formattedCommand);
int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand); int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand);
int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data); int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data);
int processCommand(unsigned char *command, unsigned int cmdIndex); int processCommand(unsigned char *command, modular_structs_t *structs);
int logData(unsigned char *log_msg, unsigned char *formattedCommand); int logData(unsigned char *log_msg, unsigned char *formattedCommand);
float getFloat(unsigned char* str, int pos);
int getInt(unsigned char* str, int pos);
#endif #endif
\ No newline at end of file
#ifndef __CONFIG_H
#define __CONFIG_H
#define DEFAULT_SOCKET "/var/run/ucart.socket"
#define SOCKET_ENV "UCART_SOCKET"
#define NOQUAD_ENV "UCART_NO_QUAD"
#define NOVRPN_ENV "UCART_NO_VRPN"
// 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
...@@ -4,21 +4,30 @@ ...@@ -4,21 +4,30 @@
*/ */
#include "logger.h" #include "logger.h"
#include <stdio.h> #include <stdio.h>
#include <err.h>
#include <pthread.h>
int quadlog_file; static FILE * quadlog_file = NULL;
static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
int createLogFile(int argc, char* argv) int createLogFile(int argc, char* argv)
{ {
char log_file[300] = {'l', 'o', 'g','s', '/'}; if (quadlog_file != NULL) {
return -1;
}
if (pthread_mutex_lock(&mutex)) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
}
char log_file[300];
strcpy(log_file, "logs/");
if(argc >= 2) if(argc >= 2)
{ {
strcat(log_file, argv); strncat(log_file, argv, 294);
printf("Creating log file '%s'...\n",log_file); printf("Creating log file '%s'...\n",log_file);
quadlog_file = open(log_file, O_WRONLY | O_CREAT, 0666); quadlog_file = fopen(log_file, "a");
return quadlog_file; } else {
}
else
{
time_t rawtime; time_t rawtime;
char timestr [30]; char timestr [30];
time(&rawtime); time(&rawtime);
...@@ -26,7 +35,7 @@ int createLogFile(int argc, char* argv) ...@@ -26,7 +35,7 @@ int createLogFile(int argc, char* argv)
// Lets convert space to _ in // Lets convert space to _ in
char *p = timestr; char *p = timestr;
int i = 0; size_t i = 0;
while(i < strlen(timestr)) while(i < strlen(timestr))
{ {
if (*p == ' ') if (*p == ' ')
...@@ -40,18 +49,60 @@ int createLogFile(int argc, char* argv) ...@@ -40,18 +49,60 @@ int createLogFile(int argc, char* argv)
strncat(log_file, timestr, strlen(timestr) -1 ); strncat(log_file, timestr, strlen(timestr) -1 );
strcat(log_file, ".txt"); strcat(log_file, ".txt");
printf("Creating log file '%s'...\n",log_file); printf("Creating log file '%s'...\n",log_file);
quadlog_file = open(log_file, O_WRONLY | O_CREAT, 0666); quadlog_file = fopen(log_file, "a");
return quadlog_file; }
if (pthread_mutex_unlock(&mutex)) {
err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
} }
return 0;
} }
int updateLogFile(const struct ucart_vrpn_TrackerData * td) int updateLogFile(const struct ucart_vrpn_TrackerData * td)
{ {
return dprintf(quadlog_file, "FPS: %lf Pos (xyz): (%lf %lf %lf) Att (pry): (%lf %lf %lf)\n", int retval;
if (pthread_mutex_lock(&mutex)) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
}
retval = fprintf(quadlog_file,
"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); td->fps, td->x, td->y, td->z, td->pitch, td->roll, td->yaw);
if (pthread_mutex_unlock(&mutex)) {
err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
}
return retval;
} }
int writeStringToLog(const char * string) int writeStringToLog(const char * string)
{ {
return dprintf(quadlog_file, "%s", string); int retval;
if (pthread_mutex_lock(&mutex)) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
}
retval = fprintf(quadlog_file, "%s", string);
if (pthread_mutex_unlock(&mutex)) {
err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
}
return retval;
}
void closeLogFile(void)
{
if (pthread_mutex_lock(&mutex)) {
err(-2, "pthrtead_mutex_lock (%s:%d):", __FILE__, __LINE__);
}
fclose(quadlog_file);
if (pthread_mutex_unlock(&mutex)) {
err(-2, "pthrtead_mutex_unlock (%s:%d):", __FILE__, __LINE__);
}
} }
...@@ -15,6 +15,7 @@ ...@@ -15,6 +15,7 @@
int createLogFile(int, char*); int createLogFile(int, char*);
int writeStringToLog(const char*); int writeStringToLog(const char*);
int updateLogFile(const struct ucart_vrpn_TrackerData* ); int updateLogFile(const struct ucart_vrpn_TrackerData* );
void closeLogFile();
#endif #endif
\ No newline at end of file
File moved
#include <iostream> #include <iostream>
#include <algorithm> #include <algorithm>
#include <functional>
#include "vrpn_Tracker.h" #include "vrpn_Tracker.h"
#include "quat.h" #include "quat.h"
...@@ -9,28 +10,25 @@ ...@@ -9,28 +10,25 @@
namespace microcart namespace microcart
{ {
static void VRPN_CALLBACK vrpn_cb(void * param, const vrpn_TRACKERCB t); 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) : Tracker::Tracker(const char * server) :
remote(server), remote(server),
stop_flag(0), stop_flag(0),
trackerData({ 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
}
})
{ {
remote.register_change_handler(this, vrpn_cb); remote.register_change_handler(this, vrpn_cb);
...@@ -92,10 +90,9 @@ namespace microcart ...@@ -92,10 +90,9 @@ namespace microcart
trackerData.fps = 1.0 / elapsed_time_usec; trackerData.fps = 1.0 / elapsed_time_usec;
auto td = trackerData; auto td = trackerData;
std::for_each(cb_vector.begin(), cb_vector.end(), for(auto i = cb_vector.begin(); i != cb_vector.end(); ++i) {
[td](std::function<void(const TrackerData &)> &fn){ (*i)(td);
fn(td); }
});
} }
void Tracker::addCallback(std::function<void(const TrackerData&)> cb) void Tracker::addCallback(std::function<void(const TrackerData&)> cb)
...@@ -115,6 +112,20 @@ struct ucart_vrpn_tracker { ...@@ -115,6 +112,20 @@ struct ucart_vrpn_tracker {
microcart::Tracker * t; 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" extern "C"
{ {
struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance( struct ucart_vrpn_tracker * ucart_vrpn_tracker_createInstance(
...@@ -123,6 +134,7 @@ extern "C" ...@@ -123,6 +134,7 @@ extern "C"
try { try {
auto inst = new struct ucart_vrpn_tracker; auto inst = new struct ucart_vrpn_tracker;
inst->t = new microcart::Tracker(server); inst->t = new microcart::Tracker(server);
return inst;
} catch(...) { } catch(...) {
return NULL; return NULL;
} }
...@@ -138,16 +150,8 @@ extern "C" ...@@ -138,16 +150,8 @@ extern "C"
void (*cb)(struct ucart_vrpn_TrackerData *)) void (*cb)(struct ucart_vrpn_TrackerData *))
{ {
try { try {
inst->t->addCallback([cb](const microcart::TrackerData & td) { auto new_cb = bind(cb_wrapper, cb, std::placeholders::_1);
struct ucart_vrpn_TrackerData data; inst->t->addCallback(new_cb);
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);
});
} catch(...) { } catch(...) {
return -1; return -1;
} }
......
...@@ -23,13 +23,13 @@ extern "C" ...@@ -23,13 +23,13 @@ extern "C"
#endif #endif
struct ucart_vrpn_tracker; struct ucart_vrpn_tracker;
struct ucart_vrpn_TrackerData { struct ucart_vrpn_TrackerData {
double x; float x;
double y; float y;
double z; float z;
double pitch; float pitch;
double roll; float roll;
double yaw; float yaw;
double fps; double fps;
struct timeval timestamp; struct timeval timestamp;
...@@ -54,16 +54,19 @@ extern "C" ...@@ -54,16 +54,19 @@ extern "C"
namespace microcart namespace microcart
{ {
struct TrackerData { struct TrackerData {
double x; public:
double y; float x;
double z; float y;
float z;
double pitch; float pitch;
double roll; float roll;
double yaw; float yaw;
double fps; double fps;
timeval timestamp; timeval timestamp;
TrackerData();
}; };
class Tracker { class Tracker {
......
# CLI
## Usage
The Cli has been designed to function similarly to command line programs. Run the program using ./Cli
To run a specific command in the
#include <stdio.h>
#include <string.h>
#include <err.h>
#include <libgen.h>
#include <getopt.h>
#include "cli.h"
int main(int argc, char **argv)
{
int cmdID = -1;
char * command;
int c;
int i , useSymlink = 0;
struct backend_conn *conn;
int needCliHelp = 0, needCommandHelp = 0;
// Determine if the cli was called using a symlink
command = basename(argv[0]);
for(i = 0; i < MAX_COMMANDS; ++i) {
if (strncmp(command, commandNames[i], strlen(commandNames[i])) == 0)
{
cmdID = i;
useSymlink = 1;
}
}
// Verify the user has entered enough information to continue
if (argc < 2 && !useSymlink) {
printf("Incorrect usage :\n");
printf("Usage Syntax: \n\t./Cli command [options...]\n\n");
printf("For a list of available 'command' names\n\t./Cli --help\n\n");
printf("For a list of available options for a 'command'\n\t./Cli command --help\n");
return -1;
}
if (!useSymlink) {
// Determine if the user called for help on the cli
needCliHelp = (strncmp("--help", argv[1], strlen(argv[1])) == 0);
}
// If the user runs './Cli help' , provide the user with a list of commands available.
if (needCliHelp) {
printf("Usage Syntax: \n\t./Cli command [options...]\n\n");
printf("Available 'command' names include the following\n");
for(int i = 0; i < MAX_COMMANDS; ++i) {
printf("\t '%s'\n",commandNames[i]);
}
return 0;
}
// recognize which cli command the user has entered
if (cmdID == -1) {
command = argv[1];
for(i = 0; i < MAX_COMMANDS; ++i) {
if (strncmp(command, commandNames[i], strlen(commandNames[i])) == 0)
{
cmdID = i;
}
}
}
if (cmdID == -1){
printf("Could not match '%s' with a command. Please try again...\n", command);
printf("For help running the program, run ./Cli --help\n");
return -1;
}
// Determine if the user called for help on the command
if (!useSymlink && argc > 2) {
if (strncmp("--help", argv[2], strlen(argv[2])) == 0) {
needCommandHelp = 1;
}
} else if (useSymlink && argc > 1) {
if (strncmp("--help", argv[1], strlen(argv[1])) == 0) {
needCommandHelp = 1;
}
}
/**
* I the user has asked for help, and we have already found
* the command that they are trying to use. Then we need
* to be able to provide the help info wihtout the
* requirement of the backend
*
* It is important to note that this will only work if the
* command the user has asked for handles this help case
*
*/
// Only require the backend if we will need to use it.
if (!needCommandHelp) {
// Create the connection to the backend
conn = ucart_backendConnect();
if (conn == NULL) {
return -1;
}
}
// Call the appropriate function
if (useSymlink) {
(*cli_functions[cmdID]) (conn, argc, &argv[0]);
}else {
(*cli_functions[cmdID]) (conn, argc-1, &argv[1]);
}
// Disconnect from the backend
if (!needCommandHelp) {
ucart_backendDisconnect(conn);
}
return 0;
}
\ No newline at end of file
#ifndef __CLI_H
#define __CLI_H
#include "cli_monitor.h"
#include "cli_setpid.h"
#include "cli_getpid.h"
#include "cli_getimu.h"
enum CommandNameIds{
CMD_MONITOR,
CMD_GETPID,
CMD_SETPID,
CMD_GETIMU,
MAX_COMMANDS
};
typedef int (*cli_function_ptr)(struct backend_conn *, int, char **);
static cli_function_ptr cli_functions[] = {
&cli_monitor,
&cli_getpid,
&cli_setpid,
&cli_getimu
};
static char* commandNames[MAX_COMMANDS] = {
"monitor",
"getpid",
"setpid",
"getimu"
};
#endif
#include <stdio.h>
#include "cli_getimu.h"
int cli_getimu(struct backend_conn * conn, int argc, char ** argv) {
printf("This functionality has not been added yet\n");
return 0;
}
#ifndef CLI_GETIMU_H
#define CLI_GETIMU_H
#include "frontend_getimu.h"
int cli_getimu(struct backend_conn * conn, int argc, char ** argv);
#endif
\ No newline at end of file
#include <stdio.h>
#include <unistd.h>
#include <getopt.h>
#include "cli_getpid.h"
int cli_getpid(struct backend_conn * conn, int argc, char **argv) {
int c;
static int getRoll = 0, getPitch = 0, getYaw = 0, getAll = 0;
static int getRollV = 0, getPitchV = 0, getYawV = 0;
static int getHeight = 0, getLat = 0, getLong = 0;
static int needHelp = 0;
struct frontend_pid_data pid_data;
static struct option long_options[] = {
/* These options don’t set a flag. We distinguish them by their indices. */
{"roll", no_argument, &getRoll, 1},
{"pitch", no_argument, &getPitch, 1},
{"yaw", no_argument, &getYaw, 1},
{"rollv", no_argument, &getRollV, 1},
{"pitchv", no_argument, &getPitchV, 1},
{"yawv", no_argument, &getYawV, 1},
{"height", no_argument, &getHeight, 1},
{"lat", no_argument, &getLat, 1},
{"long", no_argument, &getLong, 1},
{"help", no_argument, &needHelp, 1},
{0, 0, 0, 0}
};
while (1)
{
/* getopt_long stores the option index here. */
int option_index = 0;
c = getopt_long(argc, argv, "a", long_options, &option_index);
/* Detect the end of the options. */
if (c == -1)
break;
if (c == 'a') {
getAll = 1;
}
}
if (needHelp) {
printf("Getpid gets the p, i , and d constant values of any single controller\n");
printf("Usage Syntax : \n\t./Cli getpid controller [options...]\n");
printf("Symlink Usage Syntax : \n\t./getpid controller [options...]\n\n");
printf("Available 'controllers' include the following\n");
printf("\t[--pitch] : Pitch\n\t[--roll] : Roll\n\t[--yaw] : Yaw\n");
printf("\t[--pitchv] : Pitch Velocity\n\t[--rollv] : Roll Velocity\n\t[--yawv] : Yaw Velocity\n");
printf("\t[--height] : Z\n\t[--lat] : X\n\t[--long] : Y\n\n");
return 0;
}
if (argc < 2) {
printf("Incorrect Usage, run './cli getpid --help' for correct usage.\n");
return 1;
}
int result;
if(getAll) {
for(int i = 0; i < PID_NUM_PIDS; ++i) {
pid_data.controller = i;
if ((result = getValues(conn, &pid_data))) {
return result;
}
}
} else {
if(getPitch) {
pid_data.controller = PID_PITCH;
if ((result = getValues(conn, &pid_data))) {
return result;
}
}
if(getRoll) {
pid_data.controller = PID_ROLL;
if ((result = getValues(conn, &pid_data))) {
return result;
}
}
if(getYaw) {
pid_data.controller = PID_YAW;
if ((result = getValues(conn, &pid_data))) {
return result;
}
}
if(getPitchV) {
pid_data.controller = PID_PITCH_RATE;
if ((result = getValues(conn, &pid_data))) {
return result;
}
}
if(getRollV) {
pid_data.controller = PID_ROLL_RATE;
if ((result = getValues(conn, &pid_data))) {
return result;
}
}
if(getYawV) {
pid_data.controller = PID_YAW_RATE;
if ((result = getValues(conn, &pid_data))) {
return result;
}
}
if(getHeight) {
pid_data.controller = PID_HEIGHT;
if ((result = getValues(conn, &pid_data))) {
return result;
}
}
if(getLat) {
pid_data.controller = PID_LAT;
if ((result = getValues(conn, &pid_data))) {
return result;
}
}
if(getLong) {
pid_data.controller = PID_LONG;
if ((result = getValues(conn, &pid_data))) {
return result;
}
}
}
return 0;
}
int getValues(struct backend_conn * conn, struct frontend_pid_data * pid_data) {
if(frontend_getpid(conn, pid_data)) {
return 1;
}
switch(pid_data->controller) {
case PID_PITCH :
printf("Pitch Constants: P = %f\tI = %f\tD = %f\n",
pid_data->p, pid_data->i, pid_data->d);
break;
case PID_ROLL :
printf("Roll Constants: P = %f\tI = %f\tD = %f\n",
pid_data->p, pid_data->i, pid_data->d);
break;
case PID_YAW :
printf("Yaw Constants: P = %f\tI = %f\tD = %f\n",
pid_data->p, pid_data->i, pid_data->d);
break;
case PID_PITCH_RATE :
printf("Pitch Rate Constants: P = %f\tI = %f\tD = %f\n",
pid_data->p, pid_data->i, pid_data->d);
break;
case PID_ROLL_RATE :
printf("Roll Rate Constants: P = %f\tI = %f\tD = %f\n",
pid_data->p, pid_data->i, pid_data->d);
break;
case PID_YAW_RATE :
printf("Yaw Rate Constants: P = %f\tI = %f\tD = %f\n",
pid_data->p, pid_data->i, pid_data->d);
break;
case PID_HEIGHT :
printf("Height Constants: P = %f\tI = %f\tD = %f\n",
pid_data->p, pid_data->i, pid_data->d);
break;
case PID_LAT :
printf("Latitude Constants: P = %f\tI = %f\tD = %f\n",
pid_data->p, pid_data->i, pid_data->d);
break;
case PID_LONG :
printf("Longitude Constants: P = %f\tI = %f\tD = %f\n",
pid_data->p, pid_data->i, pid_data->d);
break;
default :
break;
}
return 0;
}
#ifndef CLI_GETPID_H
#define CLI_GETPID_H
#include "frontend_getpid.h"
int getValues(struct backend_conn *, struct frontend_pid_data *);
int cli_getpid(struct backend_conn * conn, int argc, char ** argv);
#endif
\ No newline at end of file
#define _GNU_SOURCE
#include <stdio.h>
#include <unistd.h>
#include <getopt.h>
#include <time.h>
#include <unistd.h>
#include <err.h>
#include "cli_monitor.h"
#include "frontend_tracker.h"
int cli_monitor(struct backend_conn * conn, int argc, char **argv) {
int c, result;
int countFlag = 0;
int count, rate = 10;
int forever = 0;
static int needHelp = 0;
static struct option long_options[] = {
/* These options don’t set a flag. We distinguish them by their indices. */
{"help", no_argument, &needHelp, 1},
{0, 0, 0, 0}
};
while (1)
{
/* getopt_long stores the option index here. */
int option_index = 0;
// If you change this VVV please also update the help message
c = getopt_long(argc, argv, "fc:r:", long_options, &option_index);
if (c == -1)
break;
switch(c) {
case 'c' :
count = atoi(optarg);
countFlag = 1;
break;
case 'r' :
rate = atoi(optarg) + 1;
break;
case 'f' :
forever = 1;
break;
default :
break;
}
}
if (needHelp) {
printf("Monitor provides real time information about the quad. Including positional data as well as controller constants\n\n");
printf("Usage Syntax : \n\t./Cli monitor [options...]\n");
printf("Symlink Usage Syntax : \n\t./monitor [options...]\n\n");
printf("Available options include the following\n");
printf("\t[-f] : Run monitor continuously until you kill the program. (ctrl-C)\n");
printf("\t[-c] 'count' : Sets the number of times the monitor will refresh\n");
printf("\t[-r] 'rate' : Sets the 'rate' at which the monitor will refresh per second\n");
return 0;
}
if (forever) {
for (;;) {
struct timespec req;
if (rate == 1) {
req.tv_sec = 1;
req.tv_nsec = 0;
} else {
req.tv_sec = 0;
req.tv_nsec = 1000000000 / rate;
}
nanosleep(&req, NULL);
monitor(conn);
}
} else if (countFlag) {
for (int i = 0; i < count; i++) {
result = monitor(conn);
struct timespec req;
if (rate == 1) {
req.tv_sec = 1;
req.tv_nsec = 0;
} else {
req.tv_sec = 0;
req.tv_nsec = 1000000000 / rate;
}
nanosleep(&req, NULL);
}
} else {
return monitor(conn);
}
return result;
}
int monitor(struct backend_conn * conn) {
/* Get tracker data */
struct frontend_tracker_data td;
if (frontend_track(conn, &td)) {
errx(1, "Error reading tracker data");
}
/* TODO: Get PID constants and status */
/* It might be a good idea to only read the pid constants
* every few seconds, so count iterations and only do it if
* this is every (rate * 2 or 3) pass through the loop
*/
/* Print stuff on screen */
/* Assuming a tab width of 8 columns */
printf("\033[2J");
printf("STATUS: NA\n");
printf("CTRLR :\tP\tR\tY\tP_V\tR_V\tY_V\tH\tLAT\tLON\n");
printf(" P :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
printf(" I :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
printf(" D :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
printf("PosAtt:\tH\tLAT\tLON\tP\tR\tY\n");
printf(" :\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\t%6.3lf\n",
td.height, td.lateral, td.longitudinal,
td.pitch, td.roll, td.yaw);
return 0;
}