From b37b7c29e3de879f5b83bc9ed73bef39e20d18ff Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Thu, 2 Oct 2008 15:48:22 +0000
Subject: [PATCH] joystick driver with sample main code (based on martin's
 driver)

---
 sw/ground_segment/joystick/Makefile     |  39 +++
 sw/ground_segment/joystick/README       |  27 ++
 sw/ground_segment/joystick/main_stick.c |  89 +++++++
 sw/ground_segment/joystick/usb_stick.c  | 338 ++++++++++++++++++++++++
 sw/ground_segment/joystick/usb_stick.h  |  64 +++++
 5 files changed, 557 insertions(+)
 create mode 100644 sw/ground_segment/joystick/Makefile
 create mode 100644 sw/ground_segment/joystick/README
 create mode 100644 sw/ground_segment/joystick/main_stick.c
 create mode 100644 sw/ground_segment/joystick/usb_stick.c
 create mode 100644 sw/ground_segment/joystick/usb_stick.h

diff --git a/sw/ground_segment/joystick/Makefile b/sw/ground_segment/joystick/Makefile
new file mode 100644
index 0000000000..f182a1162e
--- /dev/null
+++ b/sw/ground_segment/joystick/Makefile
@@ -0,0 +1,39 @@
+# 
+#   $Id$
+#   Copyright (C) 2004 Pascal Brisset, Antoine Drouin
+#
+# This file is part of paparazzi.
+#
+# paparazzi is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2, or (at your option)
+# any later version.
+#
+# paparazzi is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with paparazzi; see the file COPYING.  If not, write to
+# the Free Software Foundation, 59 Temple Place - Suite 330,
+# Boston, MA 02111-1307, USA.  
+# 
+
+# Quiet compilation
+Q=@
+
+all: main_stick ctrlstick ffjoystick
+
+main_stick: main_stick.c usb_stick.c
+	gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs`  `pcre-config --libs` -lglibivy
+
+ctrlstick: ctrlstick.c
+	gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs`  `pcre-config --libs` -lglibivy
+
+ffjoystick: ffjoystick.c
+	gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs`  `pcre-config --libs` -lglibivy -lm
+	
+clean:
+	rm -f *.opt *.out *~ core *.o *.bak .depend *.cm* main_stick ctrlstick ffjoystick
+
diff --git a/sw/ground_segment/joystick/README b/sw/ground_segment/joystick/README
new file mode 100644
index 0000000000..6c6fb0bde1
--- /dev/null
+++ b/sw/ground_segment/joystick/README
@@ -0,0 +1,27 @@
+#
+#   $Id$
+#   Copyright (C) 2003 Pascal Brisset Antoine Drouin
+#
+# This file is part of paparazzi.
+#
+# paparazzi is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2, or (at your option)
+# any later version.
+#
+# paparazzi is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with paparazzi; see the file COPYING.  If not, write to
+# the Free Software Foundation, 59 Temple Place - Suite 330,
+# Boston, MA 02111-1307, USA.  
+# 
+
+
+Tools for controling an aircraft with a ground joystick
+
+
+
diff --git a/sw/ground_segment/joystick/main_stick.c b/sw/ground_segment/joystick/main_stick.c
new file mode 100644
index 0000000000..4066c4ee4e
--- /dev/null
+++ b/sw/ground_segment/joystick/main_stick.c
@@ -0,0 +1,89 @@
+/*
+ * main_stick
+ *
+ * send joystick control to paparazzi through ivy 
+ *
+ * based on Force Feedback: Constant Force Stress Test
+ * Copyright (C) 2001 Oliver Hamann
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include "usb_stick.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <glib.h>
+#include <Ivy/ivy.h>
+#include <Ivy/ivyglibloop.h>
+
+#define TIMEOUT_PERIOD  100
+
+#define DEFAULT_AC_ID       1
+
+/* Options */
+char * device_name    = NULL;
+int aircraft_id       = DEFAULT_AC_ID;
+
+void parse_args(int argc, char * argv[])
+{
+  int i;
+
+  for (i = 1; i < argc; i++) {
+    if      (!strcmp(argv[i],"-d") && i<argc-1) device_name = argv[++i];
+    else if (!strcmp(argv[i],"-a") && i<argc-1) aircraft_id = atoi(argv[++i]);
+    else if (!strcmp(argv[i],"-h")) goto l_help;
+  }
+  return;
+
+l_help:
+  printf("Usage:\n");
+  printf("  %s <option> [<option>...]\n",argv[0]);
+  printf("Options:\n");
+  printf("  -d <string>  device name\n");
+  printf("  -a <int>     aircraft id (default: %d)\n",DEFAULT_AC_ID);
+  printf("  -h           display this help\n");
+  exit(1);
+}
+
+
+static gboolean joystick_periodic(gpointer data __attribute__ ((unused))) {
+
+	stick_read();
+
+	IvySendMsg("dl JOYSTICK_RAW %d %d %d %d", aircraft_id, stick_axis_values[0], stick_axis_values[1], stick_axis_values[2]);
+
+	return 1;
+}
+
+
+int main ( int argc, char** argv) {
+
+  GMainLoop *ml =  g_main_loop_new(NULL, FALSE);
+  
+  parse_args(argc, argv);
+  
+  IvyInit ("IvyCtrlJoystick", "IvyCtrlJoystick READY", NULL, NULL, NULL, NULL);
+  IvyStart("127.255.255.255");
+
+  if (stick_init(device_name) != 0) return 0;
+  
+  g_timeout_add(TIMEOUT_PERIOD, joystick_periodic, NULL);
+  
+  g_main_loop_run(ml);
+
+  return 0;
+}
diff --git a/sw/ground_segment/joystick/usb_stick.c b/sw/ground_segment/joystick/usb_stick.c
new file mode 100644
index 0000000000..5fcee84e20
--- /dev/null
+++ b/sw/ground_segment/joystick/usb_stick.c
@@ -0,0 +1,338 @@
+/*
+ * $Id: 
+ *
+ * joystick lib
+ *
+ * based on Force Feedback: Constant Force Stress Test
+ * Copyright (C) 2001 Oliver Hamann
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#define STICK_DBG 1
+
+#include "usb_stick.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <glib.h>
+#include <linux/input.h>
+#include <sys/ioctl.h>
+#include <Ivy/ivy.h>
+#include <Ivy/ivyglibloop.h>
+
+#ifdef STICK_DBG
+#define dbgprintf fprintf
+#else
+#define dbgprintf(x ...)
+#endif
+
+#define MIN_BUTTON_CODE   BTN_JOYSTICK
+#define MAX_BUTTON_CODE   BTN_THUMBR+1
+#define MIN_ABS_CODE      ABS_X
+#define MAX_ABS_CODE      ABS_MAX+1
+
+#define ABS_MAX_VALUE     254
+#define ABS_MID_VALUE     127
+
+#define BUTTON_COUNT      STICK_BUTTON_COUNT
+#define AXIS_COUNT        STICK_AXIS_COUNT
+
+/* Helper for testing large bit masks */
+#define TEST_BIT(bit,bits) (((bits[bit>>5]>>(bit&0x1f))&1)!=0)
+
+/* Global variables about the initialized device */
+int stick_device_handle;
+
+int8_t stick_axis_values[AXIS_COUNT] = {0, 0, 0, 0, 0, 0};
+int16_t stick_button_values = 0;
+
+int axis_code[AXIS_COUNT];
+int axis_count = 0;
+int button_code[BUTTON_COUNT];
+int button_count = 0;
+
+int32_t axis_min[AXIS_COUNT], axis_max[AXIS_COUNT];
+
+struct stick_code_param_ stick_init_param = {
+  0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
+  0, {0, 0, 0, 0, 0, 0}
+};
+
+//struct ff_effect effect;
+
+
+int init_hid_device(char* device_name)
+{
+  int cnt;
+  unsigned long key_bits[32],abs_bits[32];
+  //	unsigned long ff_bits[32];
+  int valbuf[16];
+  char name[256] = "Unknown";
+
+  button_count = 0;
+  axis_count = 0;
+
+  /* Open event device read only (with write permission for ff) */
+  stick_device_handle = open(device_name,O_RDONLY|O_NONBLOCK);
+  if (stick_device_handle<0) {
+    dbgprintf(stderr,"ERROR: can not open %s (%s) [%s:%d]\n",
+        device_name,strerror(errno),__FILE__,__LINE__);
+    return(1);
+  }
+
+  /* Which buttons has the device? */
+  memset(key_bits,0,32*sizeof(unsigned long));
+  if (ioctl(stick_device_handle,EVIOCGBIT(EV_KEY,32*sizeof(unsigned long)),key_bits)<0) {
+    dbgprintf(stderr,"ERROR: can not get key bits (%s) [%s:%d]\n",
+        strerror(errno),__FILE__,__LINE__);
+    return(1);
+  }
+
+  /* Store buttons */
+  if (stick_init_param.button_count > 0) {
+    for (cnt = 0; cnt < MIN(stick_init_param.button_count,BUTTON_COUNT); cnt++) {
+      if (!TEST_BIT(stick_init_param.button_code[cnt], key_bits)) {
+        dbgprintf(stderr,"ERROR: no suitable custom button %d found [%s:%d]\n",cnt,__FILE__,__LINE__);
+        return 1;
+      }
+    }
+    button_count = stick_init_param.button_count;
+    memcpy(button_code,stick_init_param.button_code,BUTTON_COUNT*sizeof(int));
+  }
+  else {
+    for (cnt = MIN_BUTTON_CODE; cnt < MAX_BUTTON_CODE; cnt++) {
+      if (TEST_BIT(cnt, key_bits)) {
+        button_code[button_count++] = cnt;
+        dbgprintf(stderr,"Available button: %d (0x%x)\n",cnt,cnt);
+      }
+      if (button_count == BUTTON_COUNT) break;
+    }
+    if (button_count == 0) {
+      dbgprintf(stderr,"ERROR: no suitable buttons found [%s:%d]\n",__FILE__,__LINE__);
+    }
+  }
+
+  /* Which axis has the device? */
+  memset(abs_bits,0,32*sizeof(unsigned long));
+  if (ioctl(stick_device_handle,EVIOCGBIT(EV_ABS,32*sizeof(unsigned long)),abs_bits)<0) {
+    dbgprintf(stderr,"ERROR: can not get abs bits (%s) [%s:%d]\n",
+        strerror(errno),__FILE__,__LINE__);
+    return(1);
+  }
+
+  /* Store axis */
+  if (stick_init_param.axis_count > 0) {
+    for (cnt = 0; cnt < MIN(stick_init_param.axis_count,AXIS_COUNT); cnt++) {
+      if (!TEST_BIT(stick_init_param.axis_code[cnt], abs_bits)) {
+        dbgprintf(stderr,"ERROR: no suitable custom axis %d found [%s:%d]\n",cnt,__FILE__,__LINE__);
+        return 1;
+      }
+    }
+    axis_count = stick_init_param.axis_count;
+    memcpy(axis_code,stick_init_param.axis_code,AXIS_COUNT*sizeof(int));
+  }
+  else {
+    for (cnt = MIN_ABS_CODE; cnt < MAX_ABS_CODE; cnt++) {
+      if (TEST_BIT(cnt, abs_bits)) {
+        axis_code[axis_count++] = cnt;
+        dbgprintf(stderr,"Available axis: %d (0x%x)\n",cnt,cnt);
+      }
+      if (axis_count == AXIS_COUNT) break;
+    }
+    // at least 2 axis are needed in auto detection
+    if (axis_count < 2) {
+      dbgprintf(stderr,"ERROR: no suitable axis found [%s:%d]\n",__FILE__,__LINE__);
+      return(1);
+    }
+  }
+
+  /* Axis param */
+  for (cnt = 0; cnt < axis_count; cnt++)
+  {
+    /* get axis value range */
+    if (ioctl(stick_device_handle,EVIOCGABS(axis_code[cnt]),valbuf)<0) {
+      dbgprintf(stderr,"ERROR: can not get axis %d value range (%s) [%s:%d]\n",
+          cnt,strerror(errno),__FILE__,__LINE__);
+      return(1);
+    }
+    axis_min[cnt]=valbuf[1];
+    axis_max[cnt]=valbuf[2];
+    if (axis_min[cnt]>=axis_max[cnt]) {
+      dbgprintf(stderr,"ERROR: bad axis %d value range (%d,%d) [%s:%d]\n",
+          cnt,axis_min[cnt],axis_max[cnt],__FILE__,__LINE__);
+      return(1);
+    }
+  }
+
+#if 0
+  /* Now get some information about force feedback */
+  memset(ff_bits,0,32*sizeof(unsigned long));
+  if (ioctl(device_handle,EVIOCGBIT(EV_FF ,32*sizeof(unsigned long)),ff_bits)<0) {
+    dbgprintf(stderr,"ERROR: can not get ff bits (%s) [%s:%d]\n",
+        strerror(errno),__FILE__,__LINE__);
+    return(1);
+  }
+
+  /* force feedback supported? */
+  if (!TEST_BIT(FF_CONSTANT,ff_bits)) {
+    dbgprintf(stderr,"ERROR: device (or driver) has no force feedback support [%s:%d]\n",
+        __FILE__,__LINE__);
+    return(1);
+  }
+
+  /* Switch off auto centering */
+  memset(&event,0,sizeof(event));
+  event.type=EV_FF;
+  event.code=FF_AUTOCENTER;
+  event.value=0;
+  if (write(device_handle,&event,sizeof(event))!=sizeof(event)) {
+    dbgprintf(stderr,"ERROR: failed to disable auto centering (%s) [%s:%d]\n",
+        strerror(errno),__FILE__,__LINE__);
+    return(1);
+  }
+
+  /* Initialize constant force effect */
+  memset(&effect,0,sizeof(effect));
+  effect.type=FF_CONSTANT;
+  effect.id=-1;
+  effect.trigger.button=0;
+  effect.trigger.interval=0;
+  effect.replay.length=0xffff;
+  effect.replay.delay=0;
+  effect.u.constant.level=0;
+  effect.direction=0xC000;
+  effect.u.constant.envelope.attack_length=0;
+  effect.u.constant.envelope.attack_level=0;
+  effect.u.constant.envelope.fade_length=0;
+  effect.u.constant.envelope.fade_level=0;
+
+  /* Upload effect */
+  if (ioctl(device_handle,EVIOCSFF,&effect)==-1) {
+    dbgprintf(stderr,"ERROR: uploading effect failed (%s) [%s:%d]\n",
+        strerror(errno),__FILE__,__LINE__);
+    return(1);
+  }
+
+  /* Start effect */
+  memset(&event,0,sizeof(event));
+  event.type=EV_FF;
+  event.code=effect.id;
+  event.value=1;
+  if (write(device_handle,&event,sizeof(event))!=sizeof(event)) {
+    dbgprintf(stderr,"ERROR: starting effect failed (%s) [%s:%d]\n",
+        strerror(errno),__FILE__,__LINE__);
+    return(1);
+  }
+#endif	
+
+  ioctl(stick_device_handle, EVIOCGNAME(sizeof(name)), name);
+  printf("Input device name: \"%s\" on device \"%s\"\n", name, device_name);
+
+  return(0);
+}
+
+
+int stick_read( void ) {
+
+  int cnt;
+  struct input_event event;
+
+  /* Get events */
+  while (read(stick_device_handle,&event,sizeof(event))==sizeof(event)) {
+
+    switch (event.type) {
+      case EV_KEY:
+        for (cnt = 0; cnt < button_count; cnt++) {
+          if (event.code == button_code[cnt]) {
+            if (event.value) stick_button_values |= (1 << cnt); // Set bit
+            else stick_button_values &= ~(1 << cnt); // Clear bit
+            break;
+          }
+        }
+        break;
+      case EV_ABS:
+        for (cnt = 0; cnt < axis_count; cnt++) {
+          if (event.code == axis_code[cnt]) {
+            stick_axis_values[cnt] = (((event.value) - axis_min[cnt]))*ABS_MAX_VALUE / (axis_max[cnt] - axis_min[cnt]) - ABS_MID_VALUE;
+            break;
+          }
+        }
+        break;
+      default: break;
+    }
+
+  }
+
+  dbgprintf(stderr,"buttons %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d | ",
+      (stick_button_values >> 0) & 1,
+      (stick_button_values >> 1) & 1,
+      (stick_button_values >> 2) & 1,
+      (stick_button_values >> 3) & 1,
+      (stick_button_values >> 4) & 1,
+      (stick_button_values >> 5) & 1,
+      (stick_button_values >> 6) & 1,
+      (stick_button_values >> 7) & 1,
+      (stick_button_values >> 8) & 1,
+      (stick_button_values >> 9) & 1,
+      (stick_button_values >> 10) & 1,
+      (stick_button_values >> 11) & 1,
+      (stick_button_values >> 12) & 1,
+      (stick_button_values >> 13) & 1,
+      (stick_button_values >> 14) & 1,
+      (stick_button_values >> 15) & 1);
+  dbgprintf(stderr,"axis %d %d %d %d %d %d\n",
+      stick_axis_values[0],
+      stick_axis_values[1],
+      stick_axis_values[2],
+      stick_axis_values[3],
+      stick_axis_values[4],
+      stick_axis_values[5]);
+
+  return 0;
+}
+
+
+int stick_init( char * device_name ) {
+
+  char devname[256];
+  int cnt = 0;
+
+  /* test device_name, else look for a suitable device */
+  if (device_name != NULL) {
+    if (init_hid_device(device_name) != 0) cnt = STICK_INPUT_DEV_MAX;
+  }
+  else {
+    for (cnt = 0; cnt < STICK_INPUT_DEV_MAX; cnt++) {
+      sprintf(devname, STICK_DEVICE_NAME "%d", cnt);
+      if (init_hid_device(devname) == 0) break;
+    }
+  }
+
+  /* return 1 if no device found */
+  if (cnt == STICK_INPUT_DEV_MAX) {
+    fprintf(stderr,"ERROR: no suitable joystick found [%s:%d]\n",
+        __FILE__,__LINE__);
+    return(1);
+  }  
+
+  return 0;
+}
+
diff --git a/sw/ground_segment/joystick/usb_stick.h b/sw/ground_segment/joystick/usb_stick.h
new file mode 100644
index 0000000000..e52204c349
--- /dev/null
+++ b/sw/ground_segment/joystick/usb_stick.h
@@ -0,0 +1,64 @@
+/*
+ * $Id: 
+ *
+ * joystick lib
+ *
+ * based on Force Feedback: Constant Force Stress Test
+ * Copyright (C) 2001 Oliver Hamann
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef USB_STICK_H
+#define USB_STICK_H
+
+#include <linux/input.h>
+
+/* Max number of axis and buttons */
+#define STICK_BUTTON_COUNT    16
+#define STICK_AXIS_COUNT      6
+
+/* Default values for the options */
+#define STICK_INPUT_DEV_MAX   15
+#define STICK_DEVICE_NAME     "/dev/input/event"
+
+/* Global variables about the initialized device */
+extern int stick_device_handle;
+extern int8_t stick_axis_values[STICK_AXIS_COUNT];
+extern int16_t stick_button_values;
+
+/* Structure for custom configuration
+ * if button_count > 0, button_code is used (valid range:[0x120, 0x13e])
+ * if axis_count > 0, axis_code is used (valid range: [0x00, 0x3f])
+ * codes are defined in "linux/input.h"
+ */
+struct stick_code_param_ {
+  int button_count;
+  int button_code[STICK_BUTTON_COUNT];
+  int axis_count;
+  int axis_code[STICK_AXIS_COUNT];
+};
+extern struct stick_code_param_ stick_init_param;
+
+/* Init: device_name = NULL for automatic search
+ * return 1 if no joystick found 
+ */
+extern int stick_init( char * device_name );
+
+/* Update values */
+extern int stick_read( void );
+
+#endif
+