From Skybotix
#include <sys/time.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
//IO stuff
#include <sys/ioctl.h>
//#include <sys/time.h>
//#include <sys/types.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>
#include <stdint.h>
//
//Joystick Stuff:
#include <linux/input.h>
#include <linux/joystick.h>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#ifdef WIN32
#define sscanf sscanf_s
#define strcpy strcpy_s
#define strncpy strncpy_s
#endif
#include <string>
#include <vector>
#include <sbapi.h>
#include <sbsimple.h>
#include "PID.h"
#define D2R(X) ((X)*M_PI/180.0)
#define R2D(X) ((X)*180.0/M_PI)
#define MAX(X,Y) (((X)<(Y))?(Y):(X))
#define MIN(X,Y) (((X)<(Y))?(X):(Y))
//#define DEBUG(c) printf("Executing "#c"\n")
//#define DEBUG(c) res=0;printf("Executing "#c"\n");c;printf("Result %d\n",res)
#define DEBUG(c) res=0;c;if (res) printf("Result of "#c": %d\n",res)
#define CRITICAL(c) res=0;c;if (res) {printf("Result of "#c": %d\n",res); return res;}
static int end = 0;
void sighdl(int n) {
end ++;
}
#define JS_DEVICE "/dev/input/js0"
#define NAME_LENGTH 128
//Value of axis is hold in an signed 16-Bit integer by the operating system
//15 bit for data and 1 bit for direction
#define joystick_max_value 32767.0
class SBController
{
protected:
SBApiSimpleContext *simple;
unsigned long sensorList;
int res;
unsigned int naxes, nbuttons;
int axis[32],button[32];
int joyfd;
pthread_t tid;
public:
SBController(SBApiSimpleContext *s) : simple(s), sensorList(0), res(0) {
sbSimpleDefaultContext(simple);
naxes = nbuttons = joyfd = -1;
tid = -1;
}
~SBController() {
if ((int)tid!=-1) {
pthread_cancel(tid);
pthread_join(tid,NULL);
tid = -1;
}
}
static void *joythread(void*arg) {
SBController *ctrl = (SBController*)arg;
while (1) {
if (ctrl->readjoy()) break;
usleep(10000);
}
return NULL;
}
int readjoy() {
struct js_event js;
if (read(joyfd, &js, sizeof(struct js_event)) != sizeof(struct js_event)) {
return -1;
}
if (js.number<32) {
switch(js.type & ~JS_EVENT_INIT){
case JS_EVENT_BUTTON:
button[js.number] = js.value;
break;
case JS_EVENT_AXIS:
axis[js.number] = js.value;
break;
}
}
if ((*simple->endP)) {
return -1;
}
return 0;
}
int initjoystick(const std::string & devname) {
int version = 0;
char name[NAME_LENGTH] ;
if ((joyfd = open(devname.c_str(), O_RDONLY)) < 0){
perror("Couldn't get File-Descriptor of Joystick");
return -1;
}
//Now get some information about the Joystick:
ioctl(joyfd, JSIOCGVERSION, &version);
ioctl(joyfd, JSIOCGAXES, &naxes);
ioctl(joyfd, JSIOCGBUTTONS, &nbuttons);
ioctl(joyfd, JSIOCGNAME(NAME_LENGTH), name);
naxes = naxes & 0xFF;
nbuttons = nbuttons & 0xFF;
printf("Joystick found:\n");
printf("Name: %s\n",name);
printf("Number of Buttons: %d\n",nbuttons);
printf("Number of Axes: %d\n",naxes);
printf("Driver-Version: %d.%d.%d\n", version>>16,
(version>>8)&0xFF,version&0xFF);
pthread_create(&tid,NULL,joythread,this);
return 0;
}
int initialise(const std::string & devname) {
res = 0;
simple->device[0] = 0;
strncpy(simple->device,devname.c_str(),255);
simple->device[255] = 0;
simple->commPort = 5123; // 8123 to test the repeater
if (simple->device[0] == '/') {
simple->commType = SB_COMMTYPE_SERIAL;
} else {
simple->commType = SB_COMMTYPE_UDP;
sscanf(devname.c_str(),"%[^:]:%d",simple->device,&simple->commPort);
}
simple->initNavState = SB_NAV_STOP;
simple->cmdTimeout = 30000;
simple->ctrlTimeout = 10000;
simple->sensorList = &sensorList;
simple->altCtrlMode = SB_CTRL_REL;
simple->yawCtrlMode = SB_CTRL_VEL;
#if 0
simple->rollCtrlMode = SB_CTRL_MANUAL;// SB_CTRL_POS;
simple->pitchCtrlMode = SB_CTRL_MANUAL;// SB_CTRL_POS;
#else
simple->rollCtrlMode = SB_CTRL_POS;
simple->pitchCtrlMode = SB_CTRL_POS;
#endif
simple->oaMode = SB_OA_NONE;
DEBUG(res = sbSimpleInitialise(simple));
printf("Channel connected, continuing\n");
return res;
}
int terminate() {
res = 0;
DEBUG(res = sbSimpleTerminate(simple));
return res;
}
double now() {
struct timeval tv;
gettimeofday(&tv,NULL);
return tv.tv_sec + 1e-6*tv.tv_usec;
}
int joyctrl() {
double desHeight = 0;
bool firstctrl = true;
while (1) {
if ((*simple->endP)) {
break;
}
DEBUG(res = sbSimpleWaitState(simple,NULL,1.0));
if (button[5] && (simple->state.mode.oavoid==0)) {
sbConfigureOAMode(&simple->control,SB_OA_HORIZONTAL);
printf("Started obstacle avoidance\n");
}
if (button[6] && (simple->state.mode.oavoid!=0)) {
sbConfigureOAMode(&simple->control,SB_OA_NONE);
printf("Stopped obstacle avoidance\n");
}
if (simple->state.errorFlags) {
printf("An error has been detected on the PIC: %02X\n",
simple->state.errorFlags);
DEBUG(res = sbSimpleReachNavState(simple,SB_NAV_STOP,30.0));
break;
}
switch (simple->state.mode.navigation) {
case SB_NAV_STOP:
desHeight = 0;
firstctrl = true;
if (button[0]) {
DEBUG(res = sbSimpleReachNavState(simple,SB_NAV_IDLE,30.0));
}
break;
case SB_NAV_IDLE:
desHeight = 0;
firstctrl = true;
if (button[0]) {
DEBUG(res = sbSimpleReachNavState(simple,SB_NAV_CTRLLED,30.0));
}
if (button[1]) {
DEBUG(res = sbSimpleReachNavState(simple,SB_NAV_STOP,30.0));
}
break;
case SB_NAV_TAKEOFF:
case SB_NAV_LAND:
case SB_NAV_HOVER:
case SB_NAV_SINK:
firstctrl = true;
desHeight = simple->state.zrange;
if (button[1]) {
DEBUG(res = sbSimpleReachNavState(simple,SB_NAV_IDLE,30.0));
}
break;
case SB_NAV_CTRLLED:
{
double desYaw = 0;
double desRoll = 0;
double desPitch = 0;
if (firstctrl) {
desHeight = simple->state.zrange;
printf("Initial control: desHeight = %f\n",desHeight);
firstctrl = false;
}
if (button[0]) {
DEBUG(res = sbSimpleReachNavState(simple,SB_NAV_IDLE,30.0));
break;
}
if (button[1]) {
desHeight -= 5e-2;
printf("Height: %f\n",desHeight);
}
if (button[2]) {
desHeight += 5e-2;
printf("Height: %f\n",desHeight);
}
if (button[3]) { desYaw = -80*M_PI/180; }
if (button[4]) { desYaw = +80*M_PI/180.; }
desYaw /= 17.; // IMU BUG
desPitch = (0.5*axis[1]) / joystick_max_value;
desRoll = (0.5*axis[0]) / joystick_max_value;
DEBUG(res = sbSimpleControl(simple,desRoll,desPitch,desYaw,desHeight));
printf("Battery: %f GyroZ: %f\n",
simple->state.battery,
simple->state.gyro[2]);
break;
}
default:
break;
}
usleep(50000);
}
return 0;
}
};
// #define SIMULATION
int main(int argc, const char *argv[])
{
int res;
SBApiSimpleContext simple;
SBController api(&simple);
CRITICAL(res = api.initjoystick((argc<3)?JS_DEVICE:(argv[2])));
CRITICAL(res = api.initialise((argc<2)?("localhost"):(argv[1])));
DEBUG(res = api.joyctrl());
DEBUG(res = api.terminate());
return 0;
}
