spnav-csharp-wrapper/spnav.c

205 lines
5.6 KiB
C
Raw Permalink 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
2018-05-30 09:30:18 +00:00
#define DEBUG_PRINT(...) \
do { \
fprintf(stderr, __VA_ARGS__); \
} while (false)
2018-05-22 23:25:02 +00:00
#else
2018-05-30 09:30:18 +00:00
#define DEBUG_PRINT(...) \
do { \
} while (false)
2018-05-22 23:25:02 +00:00
#endif
2018-05-30 22:10:16 +00:00
#define EVENT_BUF 64
2018-05-22 23:25:02 +00:00
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-23 14:18:19 +00:00
static int SPNAV_DEADZONE_THRESHOLD = 5;
2018-05-30 09:30:18 +00:00
/* HID device for SpaceNavigator mouse */
2018-05-22 23:25:02 +00:00
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;
2018-05-30 09:30:18 +00:00
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-30 09:30:18 +00:00
int read_event(hid_device *device, spnav_event *ev, int ms) {
2018-05-30 22:10:16 +00:00
unsigned char buf[EVENT_BUF];
int nbytes;
if (ms == -1) {
nbytes = hid_read(device, buf, sizeof(buf));
} else {
nbytes = hid_read_timeout(device, buf, sizeof(buf), ms);
}
2018-05-22 23:25:02 +00:00
if (nbytes < 0) {
DEBUG_PRINT("hid_read_timeout() error");
return -1;
} else if (nbytes == 0) {
ev->type = 0;
return 0;
}
ev->type = buf[0];
2018-05-30 22:10:16 +00:00
// Fill spnav_event struct
2018-05-22 23:25:02 +00:00
switch (ev->type) {
case TRANSLATION:
2018-05-30 09:30:18 +00:00
if (in_deadzone(buf, SPNAV_DEADZONE_THRESHOLD)) {
ev->type = 0;
return ev->type;
}
ev->motion.type = 1;
2018-05-30 09:30:18 +00:00
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.y = -(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 14:18:19 +00:00
if (in_deadzone(buf, SPNAV_DEADZONE_THRESHOLD)) {
ev->type = 0;
return ev->type;
}
ev->motion.type = 1;
2018-05-30 09:30:18 +00:00
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.ry = (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",
2018-05-30 09:30:18 +00:00
nbytes, sizeof(led_data));
2018-05-22 23:25:02 +00:00
return -1;
}
return nbytes;
2018-05-22 23:25:02 +00:00
}
int spnav_open(unsigned short vendor_id, unsigned short product_id) {
2018-05-22 23:25:02 +00:00
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,
2018-05-30 09:30:18 +00:00
// and optionally the Serial number.
device = hid_open(vendor_id, product_id, NULL);
2018-05-22 23:25:02 +00:00
if (device == NULL) {
DEBUG_PRINT("hid_open() failed!");
return -1;
}
IS_OPEN = true;
set_led(device, 1);
return 0;
}
int spnav_close() {
if (!IS_OPEN) {
2018-05-30 22:10:16 +00:00
DEBUG_PRINT("Device is not opened!\n");
2018-05-22 23:25:02 +00:00
return -1;
}
set_led(device, 0);
hid_close(device);
2018-05-30 22:10:16 +00:00
DEBUG_PRINT("Connection to HID device closed!\n");
2018-05-22 23:25:02 +00:00
hid_exit();
IS_OPEN = false;
return 0;
}
2018-05-30 22:10:16 +00:00
int spnav_set_nonblocking(bool nonblock) {
int ret = hid_set_nonblocking(device, nonblock);
if (ret == -1) {
DEBUG_PRINT("Call to hid_set_nonblocking() failed\n");
return -1;
}
DEBUG_PRINT("Nonblocking state is now %d\n", nonblock);
return 0;
}
2018-05-22 23:25:02 +00:00
int spnav_wait_event(spnav_event *event) {
if (device == NULL) {
DEBUG_PRINT("spnav_wait_event(): device not connected.\n");
return -1;
}
2018-05-30 09:30:18 +00:00
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;
}
2018-05-30 09:30:18 +00:00
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) {
2018-05-23 14:18:19 +00:00
if (sens <= 0.0) {
DEBUG_PRINT("Invalid sensitivity value %f\n", sens);
2018-05-23 13:10:11 +00:00
return -1;
}
SPNAV_SENS = sens;
2018-05-23 14:18:19 +00:00
DEBUG_PRINT("Sensitivity set to %f\n", SPNAV_SENS);
return 0;
}
int spnav_deadzone(int value) {
if (value < 0) {
DEBUG_PRINT("Invalid deadzone value %d\n", value);
return -1;
}
SPNAV_DEADZONE_THRESHOLD = value;
DEBUG_PRINT("Deadzone threshold set to %d\n", value);
2018-05-23 13:10:11 +00:00
return 0;
}