Skip to content
Snippets Groups Projects
Commit 438c0e8a authored by David Wehr's avatar David Wehr
Browse files
parents 5f1199a8 0f528c51
No related branches found
No related tags found
No related merge requests found
Showing
with 690 additions and 1070 deletions
*.asv
*.autosave
\ No newline at end of file
This diff is collapsed.
No preview for this file type
No preview for this file type
......@@ -3,8 +3,8 @@
# Generic Variables
GCC=gcc
GXX=g++
CFLAGS= -Wall -Wpedantic -Wextra -Werror -std=c99 -g -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-but-set-variable
CXXFLAGS= -Wall -Wpedantic -Wextra -Werror -Wno-reorder -Wno-unused-variable -std=c++0x -g
CFLAGS= -Wall -pedantic -Wextra -Werror -std=gnu99 -g -Wno-unused-parameter -Wno-unused-variable -Wno-unused-function -Wno-unused-but-set-variable
CXXFLAGS= -Wall -pedantic -Wextra -Werror -Wno-reorder -Wno-unused-variable -std=c++0x -g
INCLUDES = $(foreach dir, $(INCDIR), -I$(dir))
INCDIR=inc src/vrpn src/vrpn/quat src/vrpn/build $(BESRCDIR) $(CLISRCDIR) $(FESRCDIR)
LIBS= -lpthread -lbluetooth -lvrpn -lquat -Lsrc/vrpn/build -Lsrc/vrpn/build/quat
......
......@@ -20,3 +20,44 @@ If you wish to change the way the backend communicates to the quad and vice vers
This provides a list of environment variables which you can set and use for your computer or time of use.
Because the backend must be ran with sudo privledges, you will need to preserve the env. vars. with sudo rights.
Hence the "-E" flag.
## Modifying
See MODIFYING for the software architecture/organization and how to add new functionality.
## Using
First, the backend daemon must be running. Run the backend with ./BackEnd. Note
the environment variables in config.h, especially the backend socket path. The backend
requires root for bluetooth, but can run as a normal user with TCP, as long as the
socket path is writable by the user.
Once the backend is running, various CLI tools can be used to view the state of the
quad and send commands. Each of these tools is given as the first argument
to the CLI program, for example, to monitor the quad, use `cli monitor`. Note that
the backend socket environment variable must be set to the same value as it
was for the backend. The CLI program also supports busybox-style symbolic links.
For example, if there is a symlink named `monitor` that points to the `cli` binary,
running the `monitor` program will effectively run `cli monitor`.
The names of the binaries is subject to change.
### Example
In one terminal or screen, run the backend:
`UCART_SOCKET=./ucart.socket ./BackEnd`
This will activate the quad and the backend, and the backend will be available for
connections from the frontend tools. One useful tool is the monitor. In another
terminal window, run the monitor forever:
`UCART_SOCKET=./ucart.socket ./cli monitor -f`
This will begin a periodic monitoring that updates 10 times per second.
Finally, in a third window, export the socket path:
`export UCART_SOCKET=./ucart.socket`
and then run any other tools to modify the quad, for example modifying PID constants:
`./cli setpid --pitch -p 1.000`
......@@ -40,6 +40,9 @@
#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 *);
......@@ -267,6 +270,7 @@ int main(int argc, char **argv)
}
}
} else if (get_client_index(fd) > -1) {
/* It is a socket to a frontend */
client_recv(fd);
}
}
......@@ -625,7 +629,40 @@ static void client_recv(int fd) {
printf("Client(%d) : '%s'\n",fd, buffer);
unsigned char * packet;
if(formatCommand(buffer, &packet) == -1) {
printf("Could not recognize command '%s'\n", buffer);
/* 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!");
......
......@@ -2,29 +2,29 @@
#define __CLI_H
#include "cli_monitor.h"
// #include "cli_setpid.h"
#include "cli_setpid.h"
#include "cli_getpid.h"
// #include "cli_getimu.h"
enum CommandNameIds{
CMD_MONITOR,
CMD_SETPID,
CMD_GETPID,
CMD_SETPID,
CMD_GETIMU,
MAX_COMMANDS
};
typedef int (*cli_function_ptr)(struct backend_conn *, int, char **);
static cli_function_ptr cli_functions[2] = {
static cli_function_ptr cli_functions[] = {
&cli_monitor,
&cli_getpid
// &cli_setpid,
// &cli_getimu
&cli_getpid,
&cli_setpid
//&cli_getimu
};
static char* commandNames[MAX_COMMANDS] = {
"monitor",
"getPid",
"setPid",
"getpid",
"setpid",
"getImu"
};
......
......@@ -7,12 +7,20 @@
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;
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, &getYawV, 1},
{"pitchv", no_argument, &getYawV, 1},
{"yawv", no_argument, &getYawV, 1},
{"height", no_argument, &getHeight, 1},
{"lat", no_argument, &getLat, 1},
{"long", no_argument, &getLong, 1},
{0, 0, 0, 0}
};
......@@ -34,33 +42,33 @@ int cli_getpid(struct backend_conn * conn, int argc, char **argv) {
int result;
if(getAll) {
pid_data.pid = ROLL;
pid_data.controller = PID_ROLL;
if ((result = getValues(conn, &pid_data))) {
return result;
}
pid_data.pid = PITCH;
pid_data.controller = PID_PITCH;
if ((result = getValues(conn, &pid_data))) {
return result;
}
pid_data.pid = YAW;
pid_data.controller = PID_YAW;
if ((result = getValues(conn, &pid_data))) {
return result;
}
} else {
if(getPitch) {
pid_data.pid = PITCH;
pid_data.controller = PID_PITCH;
if ((result = getValues(conn, &pid_data))) {
return result;
}
}
if(getRoll) {
pid_data.pid = ROLL;
pid_data.controller = PID_ROLL;
if ((result = getValues(conn, &pid_data))) {
return result;
}
}
if(getYaw) {
pid_data.pid = YAW;
pid_data.controller = PID_YAW;
if ((result = getValues(conn, &pid_data))) {
return result;
}
......@@ -77,16 +85,16 @@ int getValues(struct backend_conn * conn, struct frontend_pid_data * pid_data) {
return 0;
switch(pid_data->pid) {
case PITCH :
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 ROLL :
case PID_ROLL :
printf("Roll Constants: P = %f\tI = %f\tD = %f\n",
pid_data->p, pid_data->i, pid_data->d);
break;
case YAW :
case PID_YAW :
printf("Yaw Constants: P = %f\tI = %f\tD = %f\n",
pid_data->p, pid_data->i, pid_data->d);
break;
......@@ -94,4 +102,4 @@ int getValues(struct backend_conn * conn, struct frontend_pid_data * pid_data) {
break;
}
return 0;
}
\ No newline at end of file
}
......@@ -5,49 +5,62 @@
#include <getopt.h>
#include <time.h>
#include <unistd.h>
#include <err.h>
#include "cli_monitor.h"
static void timespec_diff(struct timespec *start, struct timespec *result);
#include "frontend_tracker.h"
int cli_monitor(struct backend_conn * conn, int argc, char **argv) {
int c, result;
int timeFlag = 0;
int countFlag = 0;
int count, rate = 10;
int forever = 0;
static struct timespec startTime;
static struct timespec elapsedTime;
static struct timespec lastChecked;
static int nsecs, rate = 10;
while ((c = getopt(argc, argv, "t:r:")) != -1) {
while ((c = getopt(argc, argv, "fc:r:")) != -1) {
switch(c) {
case 't' :
nsecs = atoi(optarg);
timeFlag = 1;
case 'c' :
count = atoi(optarg);
countFlag = 1;
break;
case 'r' :
rate = atoi(optarg) + 1;
break;
case 'f' :
forever = 1;
break;
default :
break;
}
}
if(timeFlag) {
clock_gettime(CLOCK_REALTIME, &startTime);
clock_gettime(CLOCK_REALTIME, &lastChecked);
while((result = monitor(conn)) == 0) {
timespec_diff(&startTime, &elapsedTime);
if(elapsedTime.tv_sec > nsecs) {
break;
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;
}
while(1) {
timespec_diff(&lastChecked, &elapsedTime);
if(elapsedTime.tv_nsec >= (long) ((SECOND_IN_NANO * 2) / rate)) {
clock_gettime(CLOCK_REALTIME, &lastChecked);
break;
}
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);
......@@ -57,29 +70,33 @@ int cli_monitor(struct backend_conn * conn, int argc, char **argv) {
}
int monitor(struct backend_conn * conn) {
static char * line = "monitor\n";
printf("monitoring\n");
int size;
if((size = ucart_backendWrite(conn, line)) < 0 ) {
return 1;
/* Get tracker data */
struct frontend_tracker_data td;
if (frontend_track(conn, &td)) {
errx(1, "Error reading tracker data");
}
//TODO : HANDLE RETURN 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;
}
static void timespec_diff(struct timespec *start, struct timespec *result)
{
struct timespec stop;
clock_gettime(CLOCK_REALTIME, &stop);
if ((stop.tv_nsec - start->tv_nsec) < 0) {
result->tv_sec = stop.tv_sec - start->tv_sec - 1;
result->tv_nsec = stop.tv_nsec - start->tv_nsec + 1000000000;
} else {
result->tv_sec = stop.tv_sec - start->tv_sec;
result->tv_nsec = stop.tv_nsec - start->tv_nsec;
}
return;
}
\ No newline at end of file
#ifndef _CLI_SETPID_H
#define _CLI_SETPID_H
#include "frontend_setpid.h"
int cli_setpid(struct backend_conn * conn, int argc, char ** argv);
#endif
......@@ -10,7 +10,7 @@
* Example:
*
* struct frontend_pid_data pid_data;
* pid_data.pid = PITCH;
* pid_data.pid = PID_PITCH;
* if (frontend_getpid(conn, &pid_data)) {
* error
* } else {
......@@ -23,14 +23,14 @@ int frontend_getpid(
struct backend_conn * conn, struct frontend_pid_data * pid_data) {
char line[100] = "";
switch (pid_data->pid) {
case PITCH :
switch (pid_data->controller) {
case PID_PITCH :
strncpy(line, "getpitchp\ngetpitchd\n", 20);
break;
case ROLL :
case PID_ROLL :
strncpy(line, "getrollp\ngetrolld\n", 18);
break;
case YAW :
case PID_YAW :
strncpy(line, "getyawp\ngetyawd\n", 17);
break;
default :
......@@ -43,4 +43,4 @@ int frontend_getpid(
}
return 0;
}
\ No newline at end of file
}
#include <err.h>
#include <stdio.h>
#include "frontend_setpid.h"
#include "pid_common.h"
#include "frontend_common.h"
int frontend_setpid(
struct backend_conn * conn,
struct frontend_pid_data * pid_data,
int mask)
{
if (conn == NULL) {
return -1;
}
char * controller;
switch (pid_data->controller) {
case PID_PITCH:
controller = "pitch";
break;
case PID_ROLL:
controller = "roll";
break;
case PID_YAW:
controller = "yaw";
break;
/* What is the "throttle" pid constant? */
default:
warnx("Unsupported PID constant");
return -1;
}
char buffer[2048];
/* Set the P, I, and D */
if (mask & SET_P) {
if (snprintf(buffer, 2048,
"set%sp %f\n",
controller,
pid_data->p) >= 2048) {
errx(0, "Buffer to small to format!");
}
ucart_backendWrite(conn, buffer);
}
if (mask & SET_I) {
if (snprintf(buffer, 2048,
"set%si %f\n",
controller,
pid_data->i) >= 2048) {
errx(0, "Buffer to small to format!");
}
ucart_backendWrite(conn, buffer);
}
if (mask & SET_D) {
if (snprintf(buffer, 2048,
"set%sd %f\n",
controller,
pid_data->d) >= 2048) {
errx(0, "Buffer to small to format!");
}
ucart_backendWrite(conn, buffer);
}
return 0;
}
#ifndef _FRONTEND_SETPID_H
#define _FRONTEND_SETPID_H
#include "pid_common.h"
#include "frontend_common.h"
int frontend_setpid(
struct backend_conn * conn,
struct frontend_pid_data * pid_data,
int mask);
#define SET_P 0x01
#define SET_I 0x02
#define SET_D 0x04
#define SET_ALL (SET_P | SET_I | SET_D)
#endif
#include <err.h>
#include <string.h>
#include <stdio.h>
#include "frontend_tracker.h"
#define MAGIC "TRACKERDATA"
int frontend_track(
struct backend_conn * conn,
struct frontend_tracker_data * data)
{
ucart_backendWrite(conn, MAGIC "\n");
char * line;
for (;;) {
line = ucart_backendGetline(conn);
if (line == NULL) {
warnx("Line not returned from backend");
return 1;
}
if (strncmp(line, MAGIC, strlen(MAGIC)) == 0) {
break;
}
}
if (strncmp(line, MAGIC " ERROR", strlen(MAGIC " ERROR")) == 0) {
warnx("Backend returned an error: %s", strstr(line, "ERROR"));
return 1;
}
/* Line format: Height Lat Long Pitch Roll Yaw */
sscanf(line, MAGIC " %lf %lf %lf %lf %lf %lf ",
&data->height,
&data->lateral,
&data->longitudinal,
&data->pitch,
&data->roll,
&data->yaw);
return 0;
}
#ifndef _FRONTEND_TRACKER_H
#define _FRONTEND_TRACKER_H
#include "frontend_common.h"
/* Struct containing pos/att data */
struct frontend_tracker_data {
double height;
double lateral;
double longitudinal;
double pitch;
double roll;
double yaw;
};
/* Get pos/att data from the tracking system
*
* conn: IN Connection to quad
* data: OUT Data is written to this struct
*
* Returns 0 on success, nonzero on error
*
*/
int frontend_track(struct backend_conn * conn,
struct frontend_tracker_data *data);
#endif
#ifndef PID_COMMON_H
#define PID_COMMON_H
enum frontend_pid {
PITCH,
ROLL,
YAW,
NUM_PIDS
enum pid_controller {
PID_PITCH,
PID_ROLL,
PID_YAW,
PID_PITCH_VELOCITY,
PID_ROLL_VELOCITY,
PID_YAW_VELOCITY,
PID_HEIGHT, /* Z */
PID_LAT, /* X */
PID_LONG, /* Y */
PID_NUM_PIDS
};
struct frontend_pid_data {
enum frontend_pid pid;
enum pid_controller controller;
float p;
float i;
......
#### joe made this: http://goel.io/joe
output/
PASSWORD
lib/python2.7/site-packages/
pelicanconf.pyc
*.pyc
lib/
bin/
include/
pip-selfcheck.json
#####=== Jekyll ===#####
_site/
.sass-cache/
#####=== Sass ===#####
.sass-cache
*.css.map
#####=== OSX ===#####
.DS_Store
.AppleDouble
.LSOverride
# Icon must end with two \r
Icon
# Thumbnails
._*
# Files that might appear on external disk
.Spotlight-V100
.Trashes
# Directories potentially created on remote AFP share
.AppleDB
.AppleDesktop
Network Trash Folder
Temporary Items
.apdisk
#####=== Windows ===#####
# Windows image file caches
Thumbs.db
ehthumbs.db
# Folder config file
Desktop.ini
# Recycle Bin used on file shares
$RECYCLE.BIN/
# Windows Installer files
*.cab
*.msi
*.msm
*.msp
# Windows shortcuts
*.lnk
#####=== Linux ===#####
*~
# KDE directory preferences
.directory
# Linux trash folder which might appear on any partition or disk
.Trash-*
#####=== JetBrains ===#####
# Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm
*.iml
## Directory-based project format:
.idea/
# if you remove the above rule, at least ignore the following:
# User-specific stuff:
# .idea/workspace.xml
# .idea/tasks.xml
# .idea/dictionaries
# Sensitive or high-churn files:
# .idea/dataSources.ids
# .idea/dataSources.xml
# .idea/sqlDataSources.xml
# .idea/dynamic.xml
# .idea/uiDesigner.xml
# Gradle:
# .idea/gradle.xml
# .idea/libraries
# Mongo Explorer plugin:
# .idea/mongoSettings.xml
## File-based project format:
*.ipr
*.iws
## Plugin-specific files:
# IntelliJ
out/
# mpeltonen/sbt-idea plugin
.idea_modules/
# JIRA plugin
atlassian-ide-plugin.xml
# Crashlytics plugin (for Android Studio and IntelliJ)
com_crashlytics_export_strings.xml
crashlytics.properties
crashlytics-build.properties
# Bundler
.bundle
vendor
Gemfile.lock
PASSWORD
\ No newline at end of file
# A sample Gemfile
source "https://rubygems.org"
gem "jekyll"
This diff is collapsed.
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment