Switched NPS joystick support to SDL for cross-platform use

conf/Makefile.sim

@@ -47,9 +47,9 @@ Q=@
 # End of configuration part.
 #
 
-ifneq ($(SIM_TYPE),JSBSIM)
+#ifneq ($(SIM_TYPE),JSBSIM)
 INCLUDES += -I `ocamlc -where`
-endif
+#endif
 
 CFLAGS  = -W -Wall
 CFLAGS += $(INCLUDES)

conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile

@@ -23,6 +23,7 @@ nps.CFLAGS  += -DSITL -DUSE_NPS
 nps.CFLAGS  += `pkg-config glib-2.0 --cflags`
 nps.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lglibivy -lgsl -lgslcblas
 nps.CFLAGS  += -I$(NPSDIR) -I$(SRC_FIRMWARE) -I$(SRC_BOARD) -I../simulator -I$(PAPARAZZI_HOME)/conf/simulator/nps
+nps.LDFLAGS += `sdl-config --libs`
 
 # use the paparazzi-jsbsim package if it is installed, otherwise look for JSBsim under /opt/jsbsim
 ifndef JSBSIM_PKG

sw/simulator/nps/nps_main.c

@@ -1,3 +1,25 @@
+/*
+ * Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
+ * Copyright (C) 2012 The Paparazzi Team
+ *
+ * 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.
+ */
+
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -166,7 +188,7 @@ static gboolean nps_main_periodic(gpointer data __attribute__ ((unused))) {
     gettimeofday(&tv_now, NULL);
     t2 = time_to_double(&tv_now);
     /* add the pause to initial real time */
-    irt += t2 - t1;
+    irt += t2 - t1; 
     nps_main.real_initial_time += t2 - t1;
     /* convert to scaled initial real time */
     nps_main.scaled_initial_time = t2 - (t2 - irt)/nps_main.host_time_factor;
@@ -227,7 +249,7 @@ static bool_t nps_main_parse_options(int argc, char** argv) {
 "   -h                                     Display this help\n"
 "   --fg_host <flight gear host>           e.g. 127.0.0.1\n"
 "   --fg_port <flight gear port>           e.g. 5501\n"
-"   -j --js_dev <joystick device or index> e.g. /dev/input/js0 or 0\n"
+"   -j --js_dev <optional joystick index>  e.g. 1 (default 0)\n"
 "   --spektrum_dev <spektrum device>       e.g. /dev/ttyUSB0\n"
 "   --rc_script <number>                   e.g. 0\n";
 
@@ -237,13 +259,13 @@ static bool_t nps_main_parse_options(int argc, char** argv) {
     static struct option long_options[] = {
       {"fg_host", 1, NULL, 0},
       {"fg_port", 1, NULL, 0},
-      {"js_dev", 1, NULL, 0},
+      {"js_dev", 2, NULL, 0},
       {"spektrum_dev", 1, NULL, 0},
       {"rc_script", 1, NULL, 0},
       {0, 0, 0, 0}
     };
     int option_index = 0;
-    int c = getopt_long(argc, argv, "j:h",
+    int c = getopt_long(argc, argv, "jh",
                         long_options, &option_index);
     if (c == -1)
       break;
@@ -256,7 +278,9 @@ static bool_t nps_main_parse_options(int argc, char** argv) {
       case 1:
         nps_main.fg_port = atoi(optarg); break;
       case 2:
-        nps_main.js_dev = strdup(optarg); break;
+        if (optarg == NULL) {nps_main.js_dev = strdup("0");}
+        else {nps_main.js_dev = strdup(optarg);}
+        break;
       case 3:
         nps_main.spektrum_dev = strdup(optarg); break;
       case 4:
@@ -265,7 +289,8 @@ static bool_t nps_main_parse_options(int argc, char** argv) {
       break;
 
     case 'j':
-      nps_main.js_dev = strdup(optarg);
+      if (optarg == NULL) {nps_main.js_dev = strdup("0");}
+      else {nps_main.js_dev = strdup(optarg);}
       break;
 
     case 'h':

sw/simulator/nps/nps_radio_control.c

@@ -78,6 +78,7 @@ bool_t nps_radio_control_available(double time) {
     nps_radio_control.next_update += RADIO_CONTROL_DT;
 
     if (nps_radio_control.type == JOYSTICK) {
+      nps_radio_control_joystick_update();
       nps_radio_control.throttle = nps_joystick.throttle;
       nps_radio_control.roll = nps_joystick.roll;
       nps_radio_control.pitch = nps_joystick.pitch;

sw/simulator/nps/nps_radio_control_joystick.c

@@ -1,21 +1,69 @@
+/*
+ * Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
+ * Copyright (C) 2012 The Paparazzi Team
+ *
+ * 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.
+ */
+
+/**
+ *  @file nps_radio_control_joystick.c
+ *  Direct RC control for NPS with a standard joystick using SDL.
+ *
+ *  Simple DirectMedia Layer library is used for cross-platform support.
+ *  Joystick button and axes are mapped to RC commands directly with defines.
+ *
+ *  Currently it doesn't support different RC configurations.
+ */
+
 #include "nps_radio_control.h"
 #include "nps_radio_control_joystick.h"
 
-#include <glib.h>
 #include <stdio.h>
 #include <string.h>
-#include <sys/types.h>
-#include <glib.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <sys/ioctl.h>
-#include <linux/joystick.h>
+#include <stdlib.h>
+#include <SDL/SDL.h>
 
-NpsJoystick nps_joystick;
+// axes indice
+#define JS_ROLL     0
+#define JS_PITCH    1
+#define JS_YAW      2
+#define JS_THROTTLE 3
+#define JS_NB_AXIS  4
+
+// buttons to switch modes
+#define JS_MODE_MANUAL 4
+#define JS_MODE_AUTO1  5
+#define JS_MODE_AUTO2  6
+#define JS_NB_BUTTONS  3
 
-static gboolean on_js_data_received(GIOChannel *source, GIOCondition condition, gpointer data);
+NpsJoystick nps_joystick;
+SDL_Joystick *sdl_joystick;
+SDL_Event sdl_event;
 
+/**
+ *  Initializes SDL and the joystick.
+ *
+ *  Function exits with -1 if fails
+ *
+ *  @param device_index  string integer of desired joystick device
+ *
+ *  @returns  0 on success
+ */
 int nps_radio_control_joystick_init(const char* device) {
 
   nps_joystick.throttle = 0.5;
@@ -24,74 +72,117 @@ int nps_radio_control_joystick_init(const char* device) {
   nps_joystick.yaw = 0.;
   nps_joystick.mode = MODE_SWITCH_AUTO2;
 
-  int fd = open(device, O_RDONLY | O_NONBLOCK);
-  if (fd == -1) {
-    printf("opening joystick device %s : %s\n", device, strerror(errno));
-    return -1;
+  //Convert device index string to integer
+  int device_index = atoi(device);
+
+  //Initialize SDL with joystick support and event support (through video)
+  if (SDL_Init(SDL_INIT_JOYSTICK|SDL_INIT_VIDEO) < 0)
+  {
+    printf("Could not initialize SDL: %s.\n", SDL_GetError());
+    exit(-1);
+  }
+
+  //Quit SDL at exit
+  atexit(SDL_Quit);
+
+  //Start the event handler, disable all but joystick button events and quit handler
+  SDL_EventState(SDL_ACTIVEEVENT,SDL_IGNORE);
+  SDL_EventState(SDL_KEYDOWN,SDL_IGNORE);
+  SDL_EventState(SDL_KEYUP,SDL_IGNORE);
+  SDL_EventState(SDL_MOUSEMOTION,SDL_IGNORE);
+  SDL_EventState(SDL_MOUSEBUTTONDOWN,SDL_IGNORE);
+  SDL_EventState(SDL_MOUSEBUTTONUP,SDL_IGNORE);
+  SDL_EventState(SDL_JOYAXISMOTION,SDL_IGNORE);
+  SDL_EventState(SDL_JOYBALLMOTION,SDL_IGNORE);
+  SDL_EventState(SDL_JOYHATMOTION,SDL_IGNORE);
+  //SDL_EventState(SDL_JOYBUTTONDOWN,SDL_IGNORE);
+  SDL_EventState(SDL_JOYBUTTONUP,SDL_IGNORE);
+  SDL_EventState(SDL_VIDEORESIZE,SDL_IGNORE);
+  SDL_EventState(SDL_VIDEOEXPOSE,SDL_IGNORE);
+  //SDL_EventState(SDL_QUIT,SDL_IGNORE);
+  SDL_EventState(SDL_USEREVENT,SDL_IGNORE);
+  SDL_EventState(SDL_SYSWMEVENT,SDL_IGNORE);
+
+  //Check there are actually joysticks attached
+  if (!SDL_NumJoysticks())
+  {
+    printf("No joysticks attached! Quitting.\n");
+    exit(-1);
+  }
+
+  //Open the desired joystick and make sure it will work
+  sdl_joystick = SDL_JoystickOpen(device_index);
+  if (!sdl_joystick)
+  {
+    printf("Joystick corresponding to SDL Index %d failed to open! Exiting.\n", device_index);
+    exit(-1);
   }
-  GIOChannel* channel = g_io_channel_unix_new(fd);
-  g_io_channel_set_encoding(channel, NULL, NULL);
-  g_io_add_watch (channel, G_IO_IN , on_js_data_received, NULL);
+  else if (SDL_JoystickNumAxes(sdl_joystick) < JS_NB_AXIS)
+  {
+    printf("Selected joystick does not support enough axes!\n");
+    SDL_JoystickClose(sdl_joystick);
+    exit(-1);
+  }
+  else if (SDL_JoystickNumButtons(sdl_joystick) < JS_NB_BUTTONS)
+  {
+    printf("Selected joystick does not support enough buttons!\n");
+    SDL_JoystickClose(sdl_joystick);
+    exit(-1);
+  }
+  else
+  {
+    printf("Using joystick named: %s\n",SDL_JoystickName(device_index));
+  }
+
   return 0;
 }
 
+/**
+ *  Updates joystick buttons from events, directly reads current axis positions..
+ */
+void nps_radio_control_joystick_update(void) {
 
-#define JS_ROLL     0
-#define JS_PITCH    1
-#define JS_YAW      2
-#define JS_THROTTLE 3
-#define JS_NB_AXIS  7
+  nps_joystick.throttle = ((float)(SDL_JoystickGetAxis(sdl_joystick,JS_THROTTLE)) - 32767.)/-65534.;
+  nps_joystick.roll = (float)(SDL_JoystickGetAxis(sdl_joystick,JS_ROLL))/32767.;
+  nps_joystick.pitch = (float)(SDL_JoystickGetAxis(sdl_joystick,JS_PITCH))/32767.;
+  nps_joystick.yaw = (float)(SDL_JoystickGetAxis(sdl_joystick,JS_YAW))/32767.;
 
-// buttons to switch modes
-#define JS_MODE_MANUAL 4
-#define JS_MODE_AUTO1  5
-#define JS_MODE_AUTO2  6
+  while(SDL_PollEvent(&sdl_event))
+  {
+    switch(sdl_event.type)
+    {
+      case SDL_JOYBUTTONDOWN:
+      {
+	switch(sdl_event.jbutton.button)
+	{
+	  case JS_MODE_MANUAL:
+          nps_joystick.mode = MODE_SWITCH_MANUAL;
+	  break;
+
+	  case JS_MODE_AUTO1:
+	  nps_joystick.mode = MODE_SWITCH_AUTO1;
+	  break;
 
-static gboolean on_js_data_received(GIOChannel *source,
-				    GIOCondition condition __attribute__ ((unused)),
-				    gpointer data __attribute__ ((unused))) {
-
-  struct js_event js;
-  gsize len;
-  GError *err = NULL;
-  g_io_channel_read_chars(source, (gchar*)(&js), sizeof(struct js_event), &len, &err);
-
-  if (js.type == JS_EVENT_AXIS) {
-    if (js.number < JS_NB_AXIS) {
-      switch (js.number) {
-      case JS_THROTTLE:
-        nps_joystick.throttle = ((float)js.value - 32767.)/-65534.;
-        //printf("joystick throttle %d\n",js.value);
-        break;
-      case JS_ROLL:
-        nps_joystick.roll = (float)js.value/32767.;
-        //printf("joystick roll %d %f\n",js.value, nps_joystick.roll);
-        break;
-      case JS_PITCH:
-        nps_joystick.pitch = (float)js.value/32767.;
-        //printf("joystick pitch %d %f\n",js.value, nps_joystick.pitch);
-        break;
-      case JS_YAW:
-        //nps_joystick.yaw = 0.;
-        nps_joystick.yaw = (float)js.value/32767.;
-        //printf("joystick yaw %d %f\n",js.value, nps_joystick.yaw);
-        break;
+	  case JS_MODE_AUTO2:
+	  nps_joystick.mode = MODE_SWITCH_AUTO2;
+	  break;
+
+	  default:
+	  //ignore
+	  break;
+        }
       }
-    }
-  }
-  if (js.type == JS_EVENT_BUTTON) {
-    switch (js.number) {
-    case JS_MODE_MANUAL:
-      nps_joystick.mode = MODE_SWITCH_MANUAL;
       break;
-    case JS_MODE_AUTO1:
-      nps_joystick.mode = MODE_SWITCH_AUTO1;
+
+      case SDL_QUIT:
+      printf("Quitting...\n");
+      exit(-1);
       break;
-    case JS_MODE_AUTO2:
-      nps_joystick.mode = MODE_SWITCH_AUTO2;
+
+      default:
+      //do nothing
+      printf("unknown SDL event!!!\n");
       break;
     }
   }
-
-  return TRUE;
 }