diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index e935ddeec0..03c68288a4 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -24,6 +24,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 diff --git a/sw/simulator/nps/nps_main.c b/sw/simulator/nps/nps_main.c index ac662de85d..4a2012c446 100644 --- a/sw/simulator/nps/nps_main.c +++ b/sw/simulator/nps/nps_main.c @@ -1,3 +1,25 @@ +/* + * Copyright (C) 2009 Antoine Drouin + * 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 #include #include @@ -227,7 +249,7 @@ static bool_t nps_main_parse_options(int argc, char** argv) { " -h Display this help\n" " --fg_host e.g. 127.0.0.1\n" " --fg_port e.g. 5501\n" -" -j --js_dev e.g. /dev/input/js0 or 0\n" +" -j --js_dev e.g. 1 (default 0)\n" " --spektrum_dev e.g. /dev/ttyUSB0\n" " --rc_script 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': diff --git a/sw/simulator/nps/nps_radio_control.c b/sw/simulator/nps/nps_radio_control.c index aa2df95460..cd28f2e6d5 100644 --- a/sw/simulator/nps/nps_radio_control.c +++ b/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; diff --git a/sw/simulator/nps/nps_radio_control_joystick.c b/sw/simulator/nps/nps_radio_control_joystick.c index e2de8f0d34..035e72636f 100644 --- a/sw/simulator/nps/nps_radio_control_joystick.c +++ b/sw/simulator/nps/nps_radio_control_joystick.c @@ -1,21 +1,69 @@ +/* + * Copyright (C) 2009 Antoine Drouin + * 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 #include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include + +// 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 NpsJoystick nps_joystick; +SDL_Joystick *sdl_joystick; +SDL_Event sdl_event; -static gboolean on_js_data_received(GIOChannel *source, GIOCondition condition, gpointer data); - +/** + * 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); } - 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); + + //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); + } + 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; -static gboolean on_js_data_received(GIOChannel *source, - GIOCondition condition __attribute__ ((unused)), - gpointer data __attribute__ ((unused))) { + case JS_MODE_AUTO1: + nps_joystick.mode = MODE_SWITCH_AUTO1; + break; - struct js_event js; - gsize len; - GError *err = NULL; - g_io_channel_read_chars(source, (gchar*)(&js), sizeof(struct js_event), &len, &err); + case JS_MODE_AUTO2: + nps_joystick.mode = MODE_SWITCH_AUTO2; + break; - 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; + 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; - break; - case JS_MODE_AUTO2: - nps_joystick.mode = MODE_SWITCH_AUTO2; - break; - } - } - return TRUE; + case SDL_QUIT: + printf("Quitting...\n"); + exit(-1); + break; + + default: + //do nothing + printf("unknown SDL event!!!\n"); + break; + } + } } diff --git a/sw/simulator/nps/nps_radio_control_joystick.h b/sw/simulator/nps/nps_radio_control_joystick.h index 388f10c592..9b32ba581c 100644 --- a/sw/simulator/nps/nps_radio_control_joystick.h +++ b/sw/simulator/nps/nps_radio_control_joystick.h @@ -1,7 +1,30 @@ +/* + * Copyright (C) 2009 Antoine Drouin + * 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. + */ + #ifndef NPS_RADIO_CONTROL_JOYSTICK_H #define NPS_RADIO_CONTROL_JOYSTICK_H extern int nps_radio_control_joystick_init(const char* device); +extern void nps_radio_control_joystick_update(void); struct NpsJoystick { double throttle; @@ -13,5 +36,4 @@ struct NpsJoystick { extern struct NpsJoystick nps_joystick; - -#endif /* NPS_RADIO_CONTROL_SPEKTRUM_H */ +#endif /* NPS_RADIO_CONTROL_JOYSTICK_H */