thib8956/spacenav-driver
1
0
Fork 0
Code Issues Pull Requests Releases 1 Wiki Activity

Reformat code

Browse Source
This commit is contained in:
Thibaud Gasser 2018-05-30 11:30:18 +02:00
parent 5b29976da9
commit 0daaf52c7c
3 changed files with 57 additions and 175 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

155
main.c
View File

@ -5,105 +5,6 @@
#define SPNAV_VENDOR_ID 0x046d #define SPNAV_VENDOR_ID 0x046d
#define SPNAV_PRODUCT_ID 0xc626 #define SPNAV_PRODUCT_ID 0xc626
// enum {
// TRANSLATION = 1,
// ROTATION = 2,
// BUTTON = 3
// };
// struct event_motion {
// int type;
// int x, y, z;
// int rx, ry, rz;
// unsigned int period;
// int *data;
// };
// struct event_button {
// int type;
// int press;
// int bnum;
// };
// typedef union spnav_event {
// int type;
// struct event_motion motion;
// struct event_button button;
// } spnav_event;
// static bool INTERRUPTED = false;
// int spnav_convert_input(int first, unsigned char val) {
// switch (val) {
// case 0:
// return first;
// case 1:
// return first + 255;
// case 254:
// return -512 + first;
// case 255:
// return -255 + first;
// default:
// return 0;
// }
// }
// spnav_event read_event(hid_device *device, int ms) {
// spnav_event ev;
// unsigned char buf[64];
// int nbytes = hid_read_timeout(device, buf, sizeof(buf), ms);
// if (nbytes < 0) {
// // TODO : error handling
// wprintf(L"error hid_read");
// } else if (nbytes == 0) {
// ev.type = 0;
// }
// ev.type = buf[0];
// switch (ev.type) {
// case TRANSLATION:
// ev.type = 0;
// ev.motion.x = spnav_convert_input((buf[1] & 0x0000ff), buf[2]);
// ev.motion.y = spnav_convert_input((buf[3] & 0x0000ff), buf[4]);
// ev.motion.z = spnav_convert_input((buf[5] & 0x0000ff), buf[6]);
// //wprintf(L"Translation x=%d, y=%d, z=%d\n", ev.motion.x, ev.motion.y, ev.motion.z);
// break;
// case ROTATION:
// ev.type = 1;
// ev.motion.rx = spnav_convert_input((buf[1] & 0x0000ff), buf[2]);
// ev.motion.ry = spnav_convert_input((buf[3] & 0x0000ff), buf[4]);
// ev.motion.rz = spnav_convert_input((buf[5] & 0x0000ff), buf[6]);
// //wprintf(L"Rotation rx=%d, ry=%d, rz=%d\n", ev.motion.rx, ev.motion.ry, ev.motion.rz);
// break;
// case BUTTON:
// wprintf(L"Buttons: %d %d\n", /* btn 1 */buf[1] & 0x01, /* btn 2 */ buf[1] & 0x02);
// break;
// }
// return ev;
// }
// void set_led(hid_device *dev, char state) {
// const unsigned char led_data[2] = {0x04, state};
// // led_data[0] = 0x04;
// // led_data[1] = state;
// int nbytes = hid_write(dev, led_data, sizeof(led_data));
// wprintf(L"Bytes written : %d\n", nbytes);
// }
// void spnav_stop(hid_device *dev) {
// set_led(dev, 0);
// hid_close(dev);
// hid_exit();
// }
// void sighandler(int signo) {
// if (signo == SIGINT) {
// wprintf(L"Stopping...\n");
// INTERRUPTED = true;
// }
// }
#include <signal.h> #include <signal.h>
#include <stdbool.h> #include <stdbool.h>
#include "spnav.h" #include "spnav.h"
@ -116,54 +17,28 @@ void sighandler(int signo) {
} }
} }
int main(int argc, char const *argv[]) int main(int argc, char const* argv[]) {
{
signal(SIGINT, sighandler); signal(SIGINT, sighandler);
/* spnav_event ev;
// Initialize the hidapi library spnav_open();
hid_init(); spnav_sensitivity(0.1);
// Open the device using the VID, PID, spnav_deadzone(10);
// and optionally the Serial number. for (;;) {
hid_device *handle = hid_open(SPNAV_VENDOR_ID, SPNAV_PRODUCT_ID, NULL); spnav_wait_event_timeout(&ev, 400);
set_led(handle, 1); switch (ev.type) {
case MOTION:
while (1) {
spnav_event ev = read_event(handle, 400);
if (ev.type == 0) {
wprintf(L"x=%d y=%d z=%d rx=%d ry=%d rz=%d\n",
ev.motion.x, ev.motion.y, ev.motion.z, ev.motion.rx, ev.motion.ry, ev.motion.rz);
} else {
wprintf(L"ev.type : %d\n", ev.type);
}
if (INTERRUPTED) {
break;
}
}
spnav_stop(handle);
return 0;
*/
spnav_event ev;
spnav_open();
spnav_sensitivity(0.1);
spnav_deadzone(10);
for (;;) {
spnav_wait_event_timeout(&ev, 400);
switch (ev.type) {
case MOTION:
printf("x=%d, y=%d, z=%d ", ev.motion.x, ev.motion.y, ev.motion.z); printf("x=%d, y=%d, z=%d ", ev.motion.x, ev.motion.y, ev.motion.z);
printf("rx=%d, ry=%d, rz=%d\n", ev.motion.rx, ev.motion.ry, ev.motion.rz); printf("rx=%d, ry=%d, rz=%d\n", ev.motion.rx, ev.motion.ry, ev.motion.rz);
break; break;
case BUTTON: case BUTTON:
printf("bnum=%d, pressed=%d\n", ev.button.bnum, ev.button.press); printf("bnum=%d, pressed=%d\n", ev.button.bnum, ev.button.press);
break; break;
} }
if (INTERRUPTED) { if (INTERRUPTED) {
break; break;
} }
} }
spnav_close(); spnav_close();
} }

41
spnav.c
View File

@ -4,10 +4,16 @@
#include "spnav.h" #include "spnav.h"
#include "hidapi.h" #include "hidapi.h"
#define DEBUG
#ifdef DEBUG #ifdef DEBUG
#define DEBUG_PRINT(...) do{ fprintf( stderr, __VA_ARGS__ ); } while( false ) #define DEBUG_PRINT(...) \
do { \
fprintf(stderr, __VA_ARGS__); \
} while (false)
#else #else
#define DEBUG_PRINT(...) do{ } while ( false ) #define DEBUG_PRINT(...) \
do { \
} while (false)
#endif #endif
enum { enum {
@ -41,7 +47,7 @@ int convert_input(int first, unsigned char val) {
bool in_deadzone(unsigned char *data, int threshold) { bool in_deadzone(unsigned char *data, int threshold) {
/* data[0] is the event type */ /* data[0] is the event type */
int i; int i;
for (i=1; i<SPNAV_NAXIS; i++) { for (i = 1; i < SPNAV_NAXIS; i++) {
if (data[i] > threshold) { if (data[i] > threshold) {
return false; return false;
} }
@ -50,7 +56,7 @@ bool in_deadzone(unsigned char *data, int threshold) {
return true; return true;
} }
int read_event(hid_device *device, spnav_event* ev, int ms) { int read_event(hid_device *device, spnav_event *ev, int ms) {
unsigned char buf[64]; unsigned char buf[64];
int nbytes = hid_read_timeout(device, buf, sizeof(buf), ms); int nbytes = hid_read_timeout(device, buf, sizeof(buf), ms);
if (nbytes < 0) { if (nbytes < 0) {
@ -64,14 +70,14 @@ int read_event(hid_device *device, spnav_event* ev, int ms) {
switch (ev->type) { switch (ev->type) {
case TRANSLATION: case TRANSLATION:
if (in_deadzone(buf, SPNAV_DEADZONE_THRESHOLD)) { if (in_deadzone(buf, SPNAV_DEADZONE_THRESHOLD)) {
ev->type = 0; ev->type = 0;
return ev->type; return ev->type;
} }
ev->motion.type = 1; ev->motion.type = 1;
ev->motion.x = (int) (SPNAV_SENS * convert_input((buf[1] & 0x0000ff), buf[2])); ev->motion.x = (int)(SPNAV_SENS * convert_input((buf[1] & 0x0000ff), buf[2]));
ev->motion.z = - (int) (SPNAV_SENS * convert_input((buf[3] & 0x0000ff), buf[4])); ev->motion.z = -(int)(SPNAV_SENS * convert_input((buf[3] & 0x0000ff), buf[4]));
ev->motion.y = - (int) (SPNAV_SENS * convert_input((buf[5] & 0x0000ff), buf[6])); ev->motion.y = -(int)(SPNAV_SENS * convert_input((buf[5] & 0x0000ff), buf[6]));
// DEBUG_PRINT("Translation x=%d, y=%d, z=%d\n", ev->motion.x, ev->motion.y, ev->motion.z); // DEBUG_PRINT("Translation x=%d, y=%d, z=%d\n", ev->motion.x, ev->motion.y, ev->motion.z);
break; break;
case ROTATION: case ROTATION:
@ -80,9 +86,9 @@ int read_event(hid_device *device, spnav_event* ev, int ms) {
return ev->type; return ev->type;
} }
ev->motion.type = 1; ev->motion.type = 1;
ev->motion.rx = -(int) (SPNAV_SENS * convert_input((buf[1] & 0x0000ff), buf[2])); ev->motion.rx = -(int)(SPNAV_SENS * convert_input((buf[1] & 0x0000ff), buf[2]));
ev->motion.rz = -(int) (SPNAV_SENS * convert_input((buf[3] & 0x0000ff), buf[4])); ev->motion.rz = -(int)(SPNAV_SENS * convert_input((buf[3] & 0x0000ff), buf[4]));
ev->motion.ry = (int) (SPNAV_SENS * convert_input((buf[5] & 0x0000ff), buf[6])); ev->motion.ry = (int)(SPNAV_SENS * convert_input((buf[5] & 0x0000ff), buf[6]));
// DEBUG_PRINT("Rotation rx=%d, ry=%d, rz=%d\n", ev->motion.rx, ev->motion.ry, ev->motion.rz); // DEBUG_PRINT("Rotation rx=%d, ry=%d, rz=%d\n", ev->motion.rx, ev->motion.ry, ev->motion.rz);
break; break;
case BTN: case BTN:
@ -101,7 +107,7 @@ int set_led(hid_device *dev, char state) {
int nbytes = hid_write(dev, led_data, sizeof(led_data)); int nbytes = hid_write(dev, led_data, sizeof(led_data));
if (nbytes != sizeof(led_data)) { if (nbytes != sizeof(led_data)) {
DEBUG_PRINT("set_led(): hid_write() has written %d bytes (should be %ld)\n", DEBUG_PRINT("set_led(): hid_write() has written %d bytes (should be %ld)\n",
nbytes, sizeof(led_data)); nbytes, sizeof(led_data));
return -1; return -1;
} }
return nbytes; return nbytes;
@ -117,7 +123,7 @@ int spnav_open() {
// Initialize the hidapi library // Initialize the hidapi library
hid_init(); hid_init();
// Open the device using the VID, PID, // Open the device using the VID, PID,
// and optionally the Serial number. // and optionally the Serial number.
device = hid_open(SPNAV_VENDOR_ID, SPNAV_PRODUCT_ID, NULL); device = hid_open(SPNAV_VENDOR_ID, SPNAV_PRODUCT_ID, NULL);
if (device == NULL) { if (device == NULL) {
DEBUG_PRINT("hid_open() failed!"); DEBUG_PRINT("hid_open() failed!");
@ -146,7 +152,8 @@ int spnav_wait_event(spnav_event *event) {
DEBUG_PRINT("spnav_wait_event(): device not connected.\n"); DEBUG_PRINT("spnav_wait_event(): device not connected.\n");
return -1; return -1;
} }
return read_event(device, event, -1);; return read_event(device, event, -1);
;
} }
int spnav_wait_event_timeout(spnav_event *event, int milliseconds) { int spnav_wait_event_timeout(spnav_event *event, int milliseconds) {
@ -154,7 +161,8 @@ int spnav_wait_event_timeout(spnav_event *event, int milliseconds) {
DEBUG_PRINT("spnav_wait_event_timeout(): device not connected.\n"); DEBUG_PRINT("spnav_wait_event_timeout(): device not connected.\n");
return -1; return -1;
} }
return read_event(device, event, milliseconds);; return read_event(device, event, milliseconds);
;
} }
int spnav_sensitivity(double sens) { int spnav_sensitivity(double sens) {
@ -176,6 +184,3 @@ int spnav_deadzone(int value) {
DEBUG_PRINT("Deadzone threshold set to %d\n", value); DEBUG_PRINT("Deadzone threshold set to %d\n", value);
return 0; return 0;
} }
/*
int spnav_wait_event(spnav_event *event);
int spnav_poll_event(spnav_event *event, int timeout); */

32
spnav.h
View File

@ -1,16 +1,18 @@
#ifndef SPNAV_H__ #ifndef SPNAV_H__
#define SPNAV_H__ #define SPNAV_H__
#include <stdbool.h>
#define SPNAV_VENDOR_ID 0x046d #define SPNAV_VENDOR_ID 0x046d
#define SPNAV_PRODUCT_ID 0xc626 #define SPNAV_PRODUCT_ID 0xc626
#define SPNAV_NAXIS 6 #define SPNAV_NAXIS 6
#ifdef _WIN32 #ifdef _WIN32
#define SPNAV_API_EXPORT __declspec(dllexport) #define SPNAV_API_EXPORT __declspec(dllexport)
#define SPNAV_API_CALL #define SPNAV_API_CALL
#else #else
#define SPNAV_API_EXPORT /**< API export macro */ #define SPNAV_API_EXPORT /**< API export macro */
#define SPNAV_API_CALL /**< API call macro */ #define SPNAV_API_CALL /**< API call macro */
#endif #endif
#define SPNAV_API_EXPORT_CALL SPNAV_API_EXPORT SPNAV_API_CALL /**< API export and call macro*/ #define SPNAV_API_EXPORT_CALL SPNAV_API_EXPORT SPNAV_API_CALL /**< API export and call macro*/
@ -22,23 +24,23 @@ enum event_type {
}; };
struct event_motion { struct event_motion {
int type; int type;
int x, y, z; int x, y, z;
int rx, ry, rz; int rx, ry, rz;
unsigned int period; unsigned int period;
int *data; int *data;
}; };
struct event_button { struct event_button {
int type; int type;
bool press; bool press;
int bnum; int bnum;
}; };
typedef union spnav_event { typedef union spnav_event {
int type; int type;
struct event_motion motion; struct event_motion motion;
struct event_button button; struct event_button button;
} spnav_event; } spnav_event;
#ifdef __cplusplus #ifdef __cplusplus