Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

joystick driver with sample main code (based on martin's driver)

Browse Source
This commit is contained in:
Gautier Hattenberger
2008-10-02 15:48:22 +00:00
parent 93bb313832
commit b37b7c29e3
5 changed files with 557 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/ground_segment/joystick/Makefile
+39
View File
@@ -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
sw/ground_segment/joystick/README
+27
View File
@@ -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
sw/ground_segment/joystick/main_stick.c
+89
View File
@@ -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;
}
sw/ground_segment/joystick/usb_stick.c
+338
View File
@@ -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;
}
sw/ground_segment/joystick/usb_stick.h
+64
View File
@@ -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