spnav-csharp-wrapper/spnav.c

169 lines
4.6 KiB
C
Raw Normal View History

2018-05-22 23:25:02 +00:00
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include "spnav.h"
#include "hidapi.h"
#ifdef DEBUG
#define DEBUG_PRINT(...) do{ fprintf( stderr, __VA_ARGS__ ); } while( false )
#else
#define DEBUG_PRINT(...) do{ } while ( false )
#endif
enum {
TRANSLATION = 1,
ROTATION = 2,
BTN = 3
};
static bool IS_OPEN = false;
2018-05-23 13:10:11 +00:00
/* Sensitivity is multiplied with every motion (1.0 normal). */
static double SPNAV_SENS = 1.0;
2018-05-22 23:25:02 +00:00
/* HID device for SpaceNavigator mouse */
hid_device *device = NULL;
int 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;
}
}
2018-05-23 13:06:49 +00:00
bool in_deadzone(unsigned char *data, int threshold) {
/* data[0] is the event type */
int i;
for (i=1; i<SPNAV_NAXIS; i++) {
2018-05-23 13:06:49 +00:00
if (data[i] > threshold) {
return false;
}
}
DEBUG_PRINT("in_deadzone\n");
return true;
}
2018-05-22 23:25:02 +00:00
int read_event(hid_device *device, spnav_event* ev, int ms) {
unsigned char buf[64];
int nbytes = hid_read_timeout(device, buf, sizeof(buf), ms);
if (nbytes < 0) {
DEBUG_PRINT("hid_read_timeout() error");
return -1;
} else if (nbytes == 0) {
ev->type = 0;
return 0;
}
ev->type = buf[0];
switch (ev->type) {
case TRANSLATION:
2018-05-23 13:06:49 +00:00
if (in_deadzone(buf, 4)) {
ev->type = 0;
return ev->type;
}
ev->motion.type = 1;
2018-05-23 13:10:11 +00:00
ev->motion.x = (int) SPNAV_SENS * convert_input((buf[1] & 0x0000ff), buf[2]);
ev->motion.y = (int) SPNAV_SENS * convert_input((buf[3] & 0x0000ff), buf[4]);
ev->motion.z = (int) SPNAV_SENS * convert_input((buf[5] & 0x0000ff), buf[6]);
2018-05-22 23:25:02 +00:00
// DEBUG_PRINT("Translation x=%d, y=%d, z=%d\n", ev->motion.x, ev->motion.y, ev->motion.z);
break;
case ROTATION:
2018-05-23 13:06:49 +00:00
if (in_deadzone(buf, 4)) {
ev->type = 0;
return ev->type;
}
ev->motion.type = 1;
2018-05-23 13:10:11 +00:00
ev->motion.rx = (int) SPNAV_SENS * convert_input((buf[1] & 0x0000ff), buf[2]);
ev->motion.ry = (int) SPNAV_SENS * convert_input((buf[3] & 0x0000ff), buf[4]);
ev->motion.rz = (int) SPNAV_SENS * convert_input((buf[5] & 0x0000ff), buf[6]);
2018-05-22 23:25:02 +00:00
// DEBUG_PRINT("Rotation rx=%d, ry=%d, rz=%d\n", ev->motion.rx, ev->motion.ry, ev->motion.rz);
break;
case BTN:
ev->button.type = 2;
ev->button.press = buf[1] == 0x01;
ev->button.bnum = buf[1];
//DEBUG_PRINT("Buttons: %d %d\n", /* btn 1 */buf[1] & 0x01, /* btn 2 */ buf[1] & 0x02);
2018-05-22 23:25:02 +00:00
break;
}
return ev->type;
2018-05-22 23:25:02 +00:00
}
int set_led(hid_device *dev, char state) {
const unsigned char led_data[2] = {0x04, state};
int nbytes = hid_write(dev, led_data, sizeof(led_data));
if (nbytes != sizeof(led_data)) {
DEBUG_PRINT("set_led(): hid_write() has written %d bytes (should be %ld)\n",
nbytes, sizeof(led_data));
return -1;
}
return nbytes;
2018-05-22 23:25:02 +00:00
}
int spnav_open() {
DEBUG_PRINT("spnav_open()\n");
/* Connexion already opened */
if (IS_OPEN) {
DEBUG_PRINT("Connexion already opened!\n");
return -1;
}
// Initialize the hidapi library
hid_init();
// Open the device using the VID, PID,
// and optionally the Serial number.
device = hid_open(SPNAV_VENDOR_ID, SPNAV_PRODUCT_ID, NULL);
if (device == NULL) {
DEBUG_PRINT("hid_open() failed!");
return -1;
}
IS_OPEN = true;
set_led(device, 1);
return 0;
}
int spnav_close() {
DEBUG_PRINT("spnav_close()\n");
if (!IS_OPEN) {
return -1;
}
set_led(device, 0);
hid_close(device);
hid_exit();
IS_OPEN = false;
return 0;
}
int spnav_wait_event(spnav_event *event) {
if (device == NULL) {
DEBUG_PRINT("spnav_wait_event(): device not connected.\n");
return -1;
}
return read_event(device, event, -1);;
2018-05-22 23:25:02 +00:00
}
int spnav_wait_event_timeout(spnav_event *event, int milliseconds) {
2018-05-23 00:41:20 +00:00
if (device == NULL) {
DEBUG_PRINT("spnav_wait_event_timeout(): device not connected.\n");
return -1;
}
return read_event(device, event, milliseconds);;
2018-05-23 00:41:20 +00:00
}
2018-05-23 13:10:11 +00:00
int spnav_sensitivity(double sens) {
if (sens < 1.0) {
DEBUG_PRINT("Invalid sensitivity value %f", sens);
return -1;
}
SPNAV_SENS = sens;
return 0;
}
2018-05-22 23:25:02 +00:00
/*
int spnav_wait_event(spnav_event *event);
int spnav_poll_event(spnav_event *event, int timeout); */