From c095709343b24661506a5a850bac5cf142748a01 Mon Sep 17 00:00:00 2001 From: Hans-Christoph Steiner Date: Sat, 28 Jun 2008 16:08:02 +0000 Subject: first somewhat 'working' version of the objectclass to get data from the Sony SIXAXIS accelerometer svn path=/trunk/externals/io/; revision=10115 --- sixaxis/sixaxis-help.pd | 156 ++++++++++++++++++++++ sixaxis/sixaxis.c | 345 ++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 501 insertions(+) create mode 100644 sixaxis/sixaxis-help.pd create mode 100644 sixaxis/sixaxis.c diff --git a/sixaxis/sixaxis-help.pd b/sixaxis/sixaxis-help.pd new file mode 100644 index 0000000..7b36070 --- /dev/null +++ b/sixaxis/sixaxis-help.pd @@ -0,0 +1,156 @@ +#N canvas 292 101 633 730 10; +#X obj 129 267 sixaxis /dev/hidraw0; +#X obj 107 178 bng 15 250 50 0 empty empty empty 0 -6 0 8 -262144 -1 +-1; +#X obj 18 178 metro 20; +#X obj 18 156 bng 15 250 50 0 empty empty empty 0 -6 0 8 -262144 -1 +-1; +#X msg 38 156 stop; +#X obj 107 129 key; +#X obj 2 2 cnv 15 550 25 empty empty sixaxis 20 12 1 16 -228992 -66577 +0; +#X text 10 44 [sixaxis] outputs raw events from the Linux Event system. +It is used for access the output of various Human Interface Devices +\, like mice \, joysticks \, tablets \, etc.; +#X text 26 105 bang to get an update when polling is stopped.; +#X obj 107 149 sel 98; +#X text 153 149 <- (type 'b' for a bang); +#X obj 182 372 pddp/print; +#N canvas 743 25 411 235 see 0; +#N canvas 108 318 543 264 route 0; +#X obj 27 14 inlet; +#X obj 72 226 outlet; +#X obj 19 226 outlet; +#X obj 172 226 outlet; +#X obj 222 204 symbol; +#X obj 222 226 outlet; +#X obj 272 204 symbol; +#X obj 272 226 outlet; +#X obj 322 204 symbol; +#X obj 322 226 outlet; +#X obj 372 204 symbol; +#X obj 372 226 outlet; +#X obj 122 225 outlet; +#X obj 422 204 symbol; +#X obj 422 226 outlet; +#X obj 472 204 symbol; +#X obj 472 226 outlet; +#X obj 26 63 route open device poll total product manufacturer transport +type vendorID productID; +#X connect 0 0 17 0; +#X connect 4 0 5 0; +#X connect 6 0 7 0; +#X connect 8 0 9 0; +#X connect 10 0 11 0; +#X connect 13 0 14 0; +#X connect 15 0 16 0; +#X connect 17 0 2 0; +#X connect 17 1 1 0; +#X connect 17 2 12 0; +#X connect 17 3 3 0; +#X connect 17 4 4 0; +#X connect 17 5 6 0; +#X connect 17 6 8 0; +#X connect 17 7 10 0; +#X connect 17 8 13 0; +#X connect 17 9 15 0; +#X restore 117 70 pd route info; +#X obj 81 96 tgl 15 0 empty empty open 0 -6 0 8 -262144 -1 -1 1 1; +#X obj 110 22 inlet; +#X obj 123 43 print info; +#X symbolatom 304 93 0 0 0 0 productID - -; +#X symbolatom 304 112 0 0 0 0 vendorID - -; +#X symbolatom 304 151 0 0 0 0 transport - -; +#X symbolatom 304 171 0 0 0 0 manufacturer - -; +#X symbolatom 186 192 0 0 0 0 product - -; +#X floatatom 97 140 5 0 0 0 device - -; +#X floatatom 97 162 5 0 0 0 poll - -; +#X symbolatom 304 131 0 0 0 0 type - -; +#X floatatom 97 182 5 0 0 0 total - -; +#X connect 0 0 1 0; +#X connect 0 1 9 0; +#X connect 0 2 10 0; +#X connect 0 3 12 0; +#X connect 0 4 8 0; +#X connect 0 5 7 0; +#X connect 0 6 6 0; +#X connect 0 7 11 0; +#X connect 0 8 5 0; +#X connect 0 9 4 0; +#X connect 2 0 0 0; +#X connect 2 0 3 0; +#X restore 246 297 pd see device info; +#X obj 130 210 tgl 30 0 empty empty empty 17 7 0 10 -4034 -1 -1 0 1 +; +#X msg 179 222 info; +#X obj 181 348 spigot; +#X obj 216 328 tgl 15 0 empty empty empty 17 7 0 10 -262144 -1 -1 0 +1; +#X obj 249 334 spigot; +#X obj 282 327 tgl 15 0 empty empty empty 17 7 0 10 -262144 -1 -1 0 +1; +#X obj 250 354 print RAW; +#X obj 61 416 route position speed acceleration accelerometer; +#X obj 341 445 print WARNING_UNSUPPORTED_MESSAGES; +#X obj 43 528 unpack 0 0 0; +#X floatatom 31 549 7 0 0 0 - - -; +#X floatatom 77 549 7 0 0 0 - - -; +#X floatatom 123 549 7 0 0 0 - - -; +#X obj 77 484 unpack 0 0 0; +#X floatatom 65 505 7 0 0 0 - - -; +#X floatatom 111 505 7 0 0 0 - - -; +#X floatatom 157 505 7 0 0 0 - - -; +#X obj 197 461 unpack 0 0 0; +#X floatatom 185 482 7 0 0 0 - - -; +#X floatatom 231 482 7 0 0 0 - - -; +#X floatatom 277 482 7 0 0 0 - - -; +#X obj 252 513 track_min; +#X obj 325 517 track_max; +#X floatatom 324 539 9 0 0 0 - - -; +#X floatatom 252 539 9 0 0 0 - - -; +#X obj 22 573 track_min; +#X obj 95 577 track_max; +#X floatatom 94 599 9 0 0 0 - - -; +#X floatatom 22 599 9 0 0 0 - - -; +#X msg 176 551 bang; +#X msg 232 217 close; +#X connect 0 0 15 0; +#X connect 0 0 17 0; +#X connect 0 0 20 0; +#X connect 0 1 12 0; +#X connect 1 0 0 0; +#X connect 2 0 0 0; +#X connect 3 0 2 0; +#X connect 4 0 2 0; +#X connect 5 0 9 0; +#X connect 9 0 1 0; +#X connect 13 0 0 0; +#X connect 14 0 0 0; +#X connect 15 0 11 0; +#X connect 16 0 15 1; +#X connect 17 0 19 0; +#X connect 18 0 17 1; +#X connect 20 0 22 0; +#X connect 20 1 26 0; +#X connect 20 2 30 0; +#X connect 20 4 21 0; +#X connect 22 0 23 0; +#X connect 22 1 24 0; +#X connect 22 1 39 0; +#X connect 22 1 38 0; +#X connect 22 2 25 0; +#X connect 26 0 27 0; +#X connect 26 1 28 0; +#X connect 26 2 29 0; +#X connect 30 0 31 0; +#X connect 30 1 32 0; +#X connect 30 1 35 0; +#X connect 30 1 34 0; +#X connect 30 2 33 0; +#X connect 34 0 37 0; +#X connect 35 0 36 0; +#X connect 38 0 41 0; +#X connect 39 0 40 0; +#X connect 42 0 39 2; +#X connect 42 0 38 2; +#X connect 43 0 0 0; diff --git a/sixaxis/sixaxis.c b/sixaxis/sixaxis.c new file mode 100644 index 0000000..b9c2232 --- /dev/null +++ b/sixaxis/sixaxis.c @@ -0,0 +1,345 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "m_pd.h" + +#define DEBUG(x) +//#define DEBUG(x) x + +#define DEFAULT_DELAY 10 +#define SIXAXIS_DEVICE "/dev/hidraw0" + +static char *version = "$Revision: 1.1 $"; + +/*------------------------------------------------------------------------------ + * GLOBAL DECLARATIONS + */ + +/* hidraw data format */ +struct sixaxis_state { + double time; + int ax, ay, az; // Raw accelerometer data + double ddx, ddy, ddz; // Acceleration + double dx, dy, dz; // Speed + double x, y, z; // Position +}; + +/* pre-generated symbols */ +static t_symbol *ps_open, *ps_device, *ps_poll, *ps_total, *ps_range; +static t_symbol *ps_accelerometer, *ps_acceleration, *ps_speed, *ps_position; + +/* mostly for status querying */ +static unsigned short device_count; + +/* previous state for calculating position, speed, acceleration */ +static struct sixaxis_state prev; + +/*------------------------------------------------------------------------------ + * CLASS DEF + */ +static t_class *sixaxis_class; + +typedef struct _sixaxis { + t_object x_obj; + t_int x_fd; + t_symbol *x_devname; + t_clock *x_clock; + short x_device_number; + short x_instance; + t_int x_device_open; + int x_read_ok; + int x_started; + int x_delay; + unsigned char buf[128]; + struct sixaxis_state x_sixaxis_state; + t_atom x_output_atoms[3]; + t_outlet *x_data_outlet; + t_outlet *x_status_outlet; +} t_sixaxis; + + + +/*------------------------------------------------------------------------------ + * SUPPORT FUNCTIONS + */ + +static void output_status(t_sixaxis *x, t_symbol *selector, t_float output_value) +{ + t_atom *output_atom = (t_atom *)getbytes(sizeof(t_atom)); + SETFLOAT(output_atom, output_value); + outlet_anything( x->x_status_outlet, selector, 1, output_atom); + freebytes(output_atom,sizeof(t_atom)); +} + +static void output_open_status(t_sixaxis *x) +{ + output_status(x, ps_open, x->x_device_open); +} + +static void output_device_number(t_sixaxis *x) +{ + output_status(x, ps_device, x->x_device_number); +} + +static void output_poll_time(t_sixaxis *x) +{ + output_status(x, ps_poll, x->x_delay); +} + +static void output_device_count(t_sixaxis *x) +{ + output_status(x, ps_total, device_count); +} + +/*------------------------------------------------------------------------------ + * CLASS METHODS + */ + +void sixaxis_stop(t_sixaxis* x) +{ + DEBUG(post("sixaxis_stop");); + + if (x->x_fd >= 0 && x->x_started) { + clock_unset(x->x_clock); + post("sixaxis: polling stopped"); + x->x_started = 0; + } +} + +static void sixaxis_close(t_sixaxis *x) +{ + DEBUG(post("sixaxis_close");); + +/* just to be safe, stop it first */ + sixaxis_stop(x); + + if(x->x_fd < 0) + return; + close(x->x_fd); + post("[sixaxis] closed %s",x->x_devname->s_name); + x->x_device_open = 0; + output_open_status(x); +} + +static int sixaxis_open(t_sixaxis *x, t_symbol *s) +{ + DEBUG(post("sixaxis_open");); + + sixaxis_close(x); + + /* set obj device name to parameter + * otherwise set to default + */ + if (s != &s_) + x->x_devname = s; + + /* open device */ + if (x->x_devname) { + /* open the device read-only, non-exclusive */ + x->x_fd = open(x->x_devname->s_name, O_RDONLY | O_NONBLOCK); + /* test if device open */ + if (x->x_fd > -1 ) { + x->x_device_open = 1; + output_open_status(x); + return 1; + } else { + pd_error(x, "[sixaxis] open %s failed", x->x_devname->s_name); + x->x_fd = -1; + x->x_device_open = 0; + output_open_status(x); + } + } + return 0; +} + +static void sixaxis_read(t_sixaxis *x) +{ + if(x->x_fd < 0) + return; + if(read(x->x_fd, &(x->buf), sizeof(x->buf)) > -1) { +// if ( nr < 0 ) { perror("read(stdin)"); exit(1); } +// if ( nr != 48 ) { fprintf(stderr, "Unsupported report\n"); exit(1); } + + struct timeval tv; + if ( gettimeofday(&tv, NULL) ) { + perror("gettimeofday"); + return; + } + x->x_sixaxis_state.time = tv.tv_sec + tv.tv_usec*1e-6; + x->x_sixaxis_state.ax = x->buf[40]<<8 | x->buf[41]; + x->x_sixaxis_state.ay = x->buf[42]<<8 | x->buf[43]; + x->x_sixaxis_state.az = x->buf[44]<<8 | x->buf[45]; + if ( ! prev.time ) { + prev.time = x->x_sixaxis_state.time; + prev.ax = x->x_sixaxis_state.ax; + prev.ay = x->x_sixaxis_state.ay; + prev.az = x->x_sixaxis_state.az; + } + double dt = x->x_sixaxis_state.time - prev.time; + double rc_dd = 2.0; // Time constant for highpass filter on acceleration + double alpha_dd = rc_dd / (rc_dd+dt); + x->x_sixaxis_state.ddx = alpha_dd*(prev.ddx + (x->x_sixaxis_state.ax-prev.ax)*0.01); + x->x_sixaxis_state.ddy = alpha_dd*(prev.ddy + (x->x_sixaxis_state.ay-prev.ay)*0.01); + x->x_sixaxis_state.ddz = alpha_dd*(prev.ddz - (x->x_sixaxis_state.az-prev.az)*0.01); + double rc_d = 2.0; // Time constant for highpass filter on speed + double alpha_d = rc_d / (rc_d+dt); + x->x_sixaxis_state.dx = alpha_d*(prev.dx + x->x_sixaxis_state.ddx*dt); + x->x_sixaxis_state.dy = alpha_d*(prev.dy + x->x_sixaxis_state.ddy*dt); + x->x_sixaxis_state.dz = alpha_d*(prev.dz + x->x_sixaxis_state.ddz*dt); + double rc = 1.0; // Time constant for highpass filter on position + double alpha = rc / (rc+dt); + x->x_sixaxis_state.x = alpha*(prev.x + x->x_sixaxis_state.dx*dt); + x->x_sixaxis_state.y = alpha*(prev.y + x->x_sixaxis_state.dy*dt); + x->x_sixaxis_state.z = alpha*(prev.z + x->x_sixaxis_state.dz*dt); + /* raw accelerometer data */ + SETFLOAT(x->x_output_atoms, x->x_sixaxis_state.ax); + SETFLOAT(x->x_output_atoms + 1, x->x_sixaxis_state.ay); + SETFLOAT(x->x_output_atoms + 2, x->x_sixaxis_state.az); + outlet_anything(x->x_data_outlet, ps_accelerometer, 3, x->x_output_atoms); + /* acceleration data */ + SETFLOAT(x->x_output_atoms, x->x_sixaxis_state.ddx); + SETFLOAT(x->x_output_atoms + 1, x->x_sixaxis_state.ddy); + SETFLOAT(x->x_output_atoms + 2, x->x_sixaxis_state.ddz); + outlet_anything(x->x_data_outlet, ps_acceleration, 3, x->x_output_atoms); + /* speed data */ + SETFLOAT(x->x_output_atoms, x->x_sixaxis_state.dx); + SETFLOAT(x->x_output_atoms + 1, x->x_sixaxis_state.dy); + SETFLOAT(x->x_output_atoms + 2, x->x_sixaxis_state.dz); + outlet_anything(x->x_data_outlet, ps_speed, 3, x->x_output_atoms); + /* position data */ + SETFLOAT(x->x_output_atoms, x->x_sixaxis_state.x); + SETFLOAT(x->x_output_atoms + 1, x->x_sixaxis_state.y); + SETFLOAT(x->x_output_atoms + 2, x->x_sixaxis_state.z); + outlet_anything(x->x_data_outlet, ps_position, 3, x->x_output_atoms); + } + if(x->x_started) { + clock_delay(x->x_clock, x->x_delay); + } +} +// double ddx, ddy, ddz; // Acceleration +// double dx, dy, dz; // Speed +// double x, y, z; // Position + +/* Actions */ + +static void sixaxis_info(t_sixaxis *x) +{ + output_open_status(x); + output_device_number(x); + output_device_count(x); + output_poll_time(x); +// TODO output ranges for sixaxis +// output_element_ranges(x); +} + +void sixaxis_start(t_sixaxis* x) +{ + DEBUG(post("sixaxis_start");); + + if (x->x_fd > -1 && !x->x_started) { + clock_delay(x->x_clock, DEFAULT_DELAY); + post("sixaxis: polling started"); + x->x_started = 1; + } else { + post("You need to set a input device (i.e /dev/input/event0)"); + } + + if(x->x_device_number > -1) + { + if(!x->x_device_open) + { + sixaxis_open(x,x->x_devname); + } + if(!x->x_started) + { + clock_delay(x->x_clock, x->x_delay); + x->x_started = 1; + } + } + +} + +static void sixaxis_float(t_sixaxis* x, t_floatarg f) +{ + DEBUG(post("sixaxis_float");); + + if (f > 0) + sixaxis_start(x); + else + sixaxis_stop(x); +} + +/* setup functions */ +static void sixaxis_free(t_sixaxis* x) +{ + DEBUG(post("sixaxis_free");); + + if (x->x_fd < 0) return; + + sixaxis_stop(x); + clock_free(x->x_clock); + close(x->x_fd); +} + +static void *sixaxis_new(t_symbol *s) +{ + t_sixaxis *x = (t_sixaxis *)pd_new(sixaxis_class); + + DEBUG(post("sixaxis_new");); + + post("[sixaxis] %s, written by Hans-Christoph Steiner ",version); + + /* init vars */ + x->x_fd = -1; + x->x_read_ok = 1; + x->x_started = 0; + x->x_delay = DEFAULT_DELAY; + x->x_devname = gensym(SIXAXIS_DEVICE); + + x->x_clock = clock_new(x, (t_method)sixaxis_read); + + /* create standard hidio-style outlets */ + x->x_data_outlet = outlet_new(&x->x_obj, 0); + x->x_status_outlet = outlet_new(&x->x_obj, 0); + + /* set to the value from the object argument, if that exists */ + if (s != &s_) + x->x_devname = s; + + return (x); +} + +void sixaxis_setup(void) +{ + DEBUG(post("sixaxis_setup");); + sixaxis_class = class_new(gensym("sixaxis"), + (t_newmethod)sixaxis_new, + (t_method)sixaxis_free, + sizeof(t_sixaxis), 0, A_DEFSYM, 0); + + /* add inlet datatype methods */ + class_addfloat(sixaxis_class,(t_method) sixaxis_float); + class_addbang(sixaxis_class,(t_method) sixaxis_read); + + /* add inlet message methods */ + class_addmethod(sixaxis_class,(t_method) sixaxis_open,gensym("open"),A_DEFFLOAT,0); + class_addmethod(sixaxis_class,(t_method) sixaxis_close,gensym("close"),0); + class_addmethod(sixaxis_class,(t_method) sixaxis_info,gensym("info"),0); + + /* pre-generate often used symbols */ + ps_open = gensym("open"); + ps_device = gensym("device"); + ps_poll = gensym("poll"); + ps_total = gensym("total"); + ps_range = gensym("range"); + ps_accelerometer = gensym("accelerometer"); + ps_acceleration = gensym("acceleration"); + ps_speed = gensym("speed"); + ps_position = gensym("position"); +} + -- cgit v1.2.1