void do_input(int fd, unsigned char* buf, struct dev_input input)
{
b1 = buf[3];
b2 = buf[4];
b3 = buf[5];
lx = buf[7] - 128;
ly = buf[8] - 128;
rx = buf[9] - 128;
ry = buf[10] - 128;
//deadzones
if (lx > -10 && lx < 10) lx = 0;
if (ly > -10 && ly < 10) ly = 0;
if (rx > -11 && rx < 11) rx = 0;
if (ry > -11 && ry < 11) ry = 0;
//lr3 enable/disable
if ((b1 & SIXAXIS_KEY_L3) && b1 != last_b1)
lr3_axis = !lr3_axis;
if ((b1 & SIXAXIS_KEY_R3) && b1 != last_b1)
lr3_buttons = !lr3_buttons;
last_b1 = b1;
//buttons
if (!input.use_lr3 || (input.use_lr3 && lr3_buttons)) {
//part1
if (last_ib1 != b1) {
if (input.key_select) uinput_send(fd, EV_KEY, input.key_select, b1 & 0x01 ? 1 : 0);
if (input.key_l3) uinput_send(fd, EV_KEY, input.key_l3, b1 & 0x02 ? 1 : 0);
if (input.key_r3) uinput_send(fd, EV_KEY, input.key_r3, b1 & 0x04 ? 1 : 0);
if (input.key_start) uinput_send(fd, EV_KEY, input.key_start, b1 & 0x08 ? 1 : 0);
if (input.key_up) uinput_send(fd, EV_KEY, input.key_up, b1 & 0x10 ? 1 : 0);
if (input.key_right) uinput_send(fd, EV_KEY, input.key_right, b1 & 0x20 ? 1 : 0);
if (input.key_down) uinput_send(fd, EV_KEY, input.key_down, b1 & 0x40 ? 1 : 0);
if (input.key_left) uinput_send(fd, EV_KEY, input.key_left, b1 & 0x80 ? 1 : 0);
}
//part2
if (last_ib2 != b2) {
if (input.key_l2) uinput_send(fd, EV_KEY, input.key_l2, b2 & 0x01 ? 1 : 0);
if (input.key_r2) uinput_send(fd, EV_KEY, input.key_r2, b2 & 0x02 ? 1 : 0);
if (input.key_l1) uinput_send(fd, EV_KEY, input.key_l1, b2 & 0x04 ? 1 : 0);
if (input.key_r1) uinput_send(fd, EV_KEY, input.key_r1, b2 & 0x08 ? 1 : 0);
if (input.key_tri) uinput_send(fd, EV_KEY, input.key_tri, b2 & 0x10 ? 1 : 0);
if (input.key_cir) uinput_send(fd, EV_KEY, input.key_cir, b2 & 0x20 ? 1 : 0);
if (input.key_cro) uinput_send(fd, EV_KEY, input.key_cro, b2 & 0x40 ? 1 : 0);
if (input.key_squ) uinput_send(fd, EV_KEY, input.key_squ, b2 & 0x80 ? 1 : 0);
}
//part3
if (last_ib3 != b3) {
if (input.key_ps) uinput_send(fd, EV_KEY, input.key_ps, b3 & 0x01 ? 1 : 0);
}
}
//axis
if (!input.use_lr3 || (input.use_lr3 && lr3_axis)) {
int rel;
bool rw_do;
if (rw_timer%(input.axis_speed*2) == 0)
rw_do = true;
else
rw_do = false;
if (input.axis_l_type == INPUT_TYPE_KEYS)
{
uinput_send(fd, EV_KEY, input.axis_l_right, (lx > 100));
uinput_send(fd, EV_KEY, input.axis_l_left, (lx < -100));
uinput_send(fd, EV_KEY, input.axis_l_up, (ly > 100));
uinput_send(fd, EV_KEY, input.axis_l_down, (ly < -100));
}
else if (input.axis_l_type == INPUT_TYPE_MOUSE)
{
rel = input.axis_l_right;
if (rel == REL_X || rel == REL_Y) {
uinput_send(fd, EV_REL, rel, lx/4/input.axis_speed);
} else if (rw_do && (rel == REL_WHEEL || rel == REL_HWHEEL)) {
lx = lx/20;
if (rel == REL_WHEEL) lx = -lx; //Inverted
uinput_send(fd, EV_REL, rel, lx);
}
rel = input.axis_l_up;
if (rel == REL_X || rel == REL_Y) {
uinput_send(fd, EV_REL, rel, ly/4/input.axis_speed);
} else if (rw_do && (rel == REL_WHEEL || rel == REL_HWHEEL)) {
ly = ly/20;
if (rel == REL_WHEEL) ly = -ly; //Inverted
uinput_send(fd, EV_REL, rel, ly);
}
}
if (input.axis_r_type == INPUT_TYPE_KEYS)
{
uinput_send(fd, EV_KEY, input.axis_r_right, (rx > 100));
uinput_send(fd, EV_KEY, input.axis_r_left, (rx < -100));
uinput_send(fd, EV_KEY, input.axis_r_up, (ry > 100));
uinput_send(fd, EV_KEY, input.axis_r_down, (ry < -100));
}
else if (input.axis_r_type == INPUT_TYPE_MOUSE)
{
rel = input.axis_r_right;
if (rel == REL_X || rel == REL_Y) {
uinput_send(fd, EV_REL, rel, rx/4/input.axis_speed);
} else if (rw_do && (rel == REL_WHEEL || rel == REL_HWHEEL)) {
rx = rx/20;
if (rel == REL_WHEEL) rx = -rx; //Inverted
uinput_send(fd, EV_REL, rel, rx);
}
rel = input.axis_r_up;
if (rel == REL_X || rel == REL_Y) {
uinput_send(fd, EV_REL, rel, ry/4/input.axis_speed);
} else if (rw_do && (rel == REL_WHEEL || rel == REL_HWHEEL)) {
ry = ry/20;
if (rel == REL_WHEEL) ry = -ry; //Inverted
uinput_send(fd, EV_REL, rel, ry);
}
}
}
if (b1 > 0 || b2 > 0 || b3 > 0 || lx != 0 || ly != 0 || rx != 0 || ry != 0) {
set_active(true);
}
last_ib1 = b1;
last_ib2 = b2;
last_ib3 = b3;
uinput_send(fd, EV_SYN, SYN_REPORT, 0);
if (rw_timer > 0xff)
rw_timer = 0;
else
rw_timer += 1;
}
void do_joystick(int fd, unsigned char* buf, struct dev_joystick joystick)
{
newH.time = tv.tv_sec + tv.tv_usec*1e-6;
newH.ax = buf[42]<<8 | buf[43];
newH.ay = buf[44]<<8 | buf[45];
newH.az = buf[46]<<8 | buf[47];
if ( ! prev.time ) {
prev.time = newH.time;
prev.ax = newH.ax;
prev.ay = newH.ay;
prev.az = newH.az;
}
dt = newH.time - prev.time; //(time constants were recuced by half)
if (joystick.accon) {
rc_dd = 1.0; // Time constant for highpass filter on acceleration
alpha_dd = rc_dd / (rc_dd+dt);
newH.ddx = alpha_dd*(prev.ddx + (newH.ax-prev.ax)*0.01);
newH.ddy = alpha_dd*(prev.ddy + (newH.ay-prev.ay)*0.01);
newH.ddz = alpha_dd*(prev.ddz - (newH.az-prev.az)*0.01);
}
if (joystick.speed) {
rc_d = 1.0; // Time constant for highpass filter on speed
alpha_d = rc_d / (rc_d+dt);
newH.dx = alpha_d*(prev.dx + newH.ddx*dt);
newH.dy = alpha_d*(prev.dy + newH.ddy*dt);
newH.dz = alpha_d*(prev.dz + newH.ddz*dt);
}
if (joystick.pos) {
rc = 0.5; // Time constant for highpass filter on position
alpha = rc / (rc+dt);
newH.x = alpha*(prev.x + newH.dx*dt);
newH.y = alpha*(prev.y + newH.dy*dt);
newH.z = alpha*(prev.z + newH.dz*dt);
}
prev = newH;
b1 = buf[3];
b2 = buf[4];
b3 = buf[5];
lx = buf[7] - 128;
ly = buf[8] - 128;
rx = buf[9] - 128;
ry = buf[10] - 128;
acx = - (buf[42]<<8 | buf[43]); //reversed
acy = buf[44]<<8 | buf[45];
acz = buf[46]<<8 | buf[47];
gyro = 0; // FIXME - What is the gyro suppose to do?
up = buf[15];
right = buf[16];
down = buf[17];
left = buf[18];
l2 = buf[19];
r2 = buf[20];
l1 = buf[21];
r1 = buf[22];
tri = buf[23];
cir = buf[24];
cro = buf[25];
squ = buf[26];
posX = (int)(newH.x*1000);
posY = (int)(newH.y*1000);
posZ = (int)(newH.z*1000);
accX = (int)(newH.ddx*1000);
accY = (int)(newH.ddy*1000);
accZ = (int)(newH.ddz*1000);
velX = (int)(newH.dx*1000);
velY = (int)(newH.dy*1000);
velZ = (int)(newH.dz*1000);
//deadzones
if (lx > -10 && lx < 10) lx = 0;
if (ly > -10 && ly < 10) ly = 0;
if (rx > -11 && rx < 11) rx = 0;
if (ry > -11 && ry < 11) ry = 0;
if (acx < -508 && acx > -516) acx = -512; //acx is reversed
if (acy > 508 && acy < 516) acy = 512;
if (acz > 508 && acz < 516) acz = 512;
if (posX > -30 && posX < 30) posX = 0;
if (posY > -30 && posY < 30) posY = 0;
if (posZ > -30 && posZ < 30) posZ = 0;
if (accX > -30 && accX < 30) accX = 0;
if (accY > -30 && accY < 30) accY = 0;
if (accZ > -30 && accZ < 30) accZ = 0;
if (velX > -30 && velX < 30) velX = 0;
if (velY > -30 && velY < 30) velY = 0;
if (velZ > -30 && velZ < 30) velZ = 0;
if (joystick.buttons) {
//part1
if (last_jb1 != b1) {
uinput_send(fd, EV_KEY, BTN_JOYSTICK + 0, b1 & 0x01 ? 1 : 0);
uinput_send(fd, EV_KEY, BTN_JOYSTICK + 1, b1 & 0x02 ? 1 : 0);
uinput_send(fd, EV_KEY, BTN_JOYSTICK + 2, b1 & 0x04 ? 1 : 0);
uinput_send(fd, EV_KEY, BTN_JOYSTICK + 3, b1 & 0x08 ? 1 : 0);
uinput_send(fd, EV_KEY, BTN_JOYSTICK + 4, b1 & 0x10 ? 1 : 0);
uinput_send(fd, EV_KEY, BTN_JOYSTICK + 5, b1 & 0x20 ? 1 : 0);
uinput_send(fd, EV_KEY, BTN_JOYSTICK + 6, b1 & 0x40 ? 1 : 0);
uinput_send(fd, EV_KEY, BTN_JOYSTICK + 7, b1 & 0x80 ? 1 : 0);
}
//part2
if (last_jb2 != b2) {
uinput_send(fd, EV_KEY, BTN_JOYSTICK + 8, b2 & 0x01 ? 1 : 0);
uinput_send(fd, EV_KEY, BTN_JOYSTICK + 9, b2 & 0x02 ? 1 : 0);
uinput_send(fd, EV_KEY, BTN_JOYSTICK + 10, b2 & 0x04 ? 1 : 0);
uinput_send(fd, EV_KEY, BTN_JOYSTICK + 11, b2 & 0x08 ? 1 : 0);
uinput_send(fd, EV_KEY, BTN_JOYSTICK + 12, b2 & 0x10 ? 1 : 0);
uinput_send(fd, EV_KEY, BTN_JOYSTICK + 13, b2 & 0x20 ? 1 : 0);
uinput_send(fd, EV_KEY, BTN_JOYSTICK + 14, b2 & 0x40 ? 1 : 0);
uinput_send(fd, EV_KEY, BTN_JOYSTICK + 15, b2 & 0x80 ? 1 : 0);
}
//part3
if (last_jb3 != b3) {
uinput_send(fd, EV_KEY, BTN_JOYSTICK + 16, b3 & 0x01 ? 1 : 0);
}
if (b1 > 0 || b2 > 0 || b3 > 0) {
set_active(true);
}
}
//axis
if (joystick.axis) {
uinput_send(fd, EV_ABS, 0, lx);
uinput_send(fd, EV_ABS, 1, ly);
uinput_send(fd, EV_ABS, 2, rx);
uinput_send(fd, EV_ABS, 3, ry);
if (lx != 0 || ly != 0 || rx != 0 || ry != 0) {
set_active(true);
}
}
//accelerometer RAW
if (joystick.accel) {
uinput_send(fd, EV_ABS, 4, acx);
uinput_send(fd, EV_ABS, 5, acy);
uinput_send(fd, EV_ABS, 6, acz);
uinput_send(fd, EV_ABS, 7, gyro);
}
//buttons (sensible, as axis)
if (joystick.sbuttons) {
uinput_send(fd, EV_ABS, 8, up);
uinput_send(fd, EV_ABS, 9, right);
uinput_send(fd, EV_ABS, 10, down);
uinput_send(fd, EV_ABS, 11, left);
uinput_send(fd, EV_ABS, 12, l2);
uinput_send(fd, EV_ABS, 13, r2);
uinput_send(fd, EV_ABS, 14, l1);
uinput_send(fd, EV_ABS, 15, r1);
uinput_send(fd, EV_ABS, 16+AXIS_PADDING, tri);
uinput_send(fd, EV_ABS, 17+AXIS_PADDING, cir);
uinput_send(fd, EV_ABS, 18+AXIS_PADDING, cro);
uinput_send(fd, EV_ABS, 19+AXIS_PADDING, squ);
if (up > 0 || right > 0 || down > 0 || left > 0 || l2 > 0 || r2 > 0 || l1 > 0 || r1 > 0 || tri > 0 || cir > 0 || cro > 0 || squ > 0 ) {
set_active(true);
}
}
//acceleration
if (joystick.accon) {
uinput_send(fd, EV_ABS, 20+AXIS_PADDING, accX);
uinput_send(fd, EV_ABS, 21+AXIS_PADDING, accY);
uinput_send(fd, EV_ABS, 22+AXIS_PADDING, accZ);
}
//speed
if (joystick.speed) {
uinput_send(fd, EV_ABS, 23+AXIS_PADDING, velX);
uinput_send(fd, EV_ABS, 24+AXIS_PADDING, velY);
uinput_send(fd, EV_ABS, 25+AXIS_PADDING, velZ);
}
//position
if (joystick.pos) {
uinput_send(fd, EV_ABS, 26+AXIS_PADDING, posX);
uinput_send(fd, EV_ABS, 27+AXIS_PADDING, posY);
uinput_send(fd, EV_ABS, 28+AXIS_PADDING, posZ);
}
last_jb1 = b1;
last_jb2 = b2;
last_jb3 = b3;
uinput_send(fd, EV_SYN, SYN_REPORT, 0);
}
int main(int argc, char **argv)
{
int fd, nr;
unsigned char buf[128];
struct uinput_fd *ufd;
struct device_settings settings;
if (argc < 2) {
std::cout << "Usage: " << argv[0] << " /dev/hidrawX" << std::endl;
return 1;
}
if ((fd = open(argv[1], O_RDONLY|O_NONBLOCK)) < 0) {
std::cerr << "sixad-3in1::open(hidrawX) - failed to open hidraw device" << std::endl;
return 1;
}
nr=read(fd, buf, sizeof(buf));
if (nr < 0 && errno != EAGAIN) {
std::cerr << "sixad-3in1::read(fd) - failed to read from device" << std::endl;
return 1;
}
if (nr != -1 && nr != 19) {
std::cerr << "sixad-3in1::read(fd) - not a 3in1 keymote (nr = " << nr << ")" << std::endl;
return 1;
}
open_log("sixad-3in1");
memset(&settings, 0, sizeof(device_settings));
settings.led.enabled = 0;
settings.joystick.enabled = 0;
settings.remote.enabled = 0;
settings.input.enabled = 1;
settings.input.key_select = 29;
settings.input.key_l3 = 0;
settings.input.key_r3 = 0;
settings.input.key_start = 56;
settings.input.key_up = 103;
settings.input.key_right = 106;
settings.input.key_down = 108;
settings.input.key_left = 105;
settings.input.key_l2 = 102;
settings.input.key_r2 = 107;
settings.input.key_l1 = 67;
settings.input.key_r1 = 87;
settings.input.key_tri = 14;
settings.input.key_cir = 273;
settings.input.key_squ = 28;
settings.input.key_cro = 272;
settings.input.key_ps = 42;
settings.input.axis_l_type = 3;
settings.input.axis_l_up = 1;
settings.input.axis_l_right = 0;
settings.input.axis_l_down = 0;
settings.input.axis_l_left = 0;
settings.input.axis_r_type = 3;
settings.input.axis_r_up = 8;
settings.input.axis_r_right = 6;
settings.input.axis_r_down = 0;
settings.input.axis_r_left = 0;
settings.input.axis_speed = 9;
settings.input.use_lr3 = 1;
settings.rumble.enabled = 0;
ufd = uinput_open(DEV_TYPE_3IN1, "3in1", settings);
if (ufd->js != 0 || ufd->mk == 0) {
syslog(LOG_ERR, "Error! something is not right...");
return 1;
}
syslog(LOG_INFO, "Connected 'Brooklyn 3in1 KeyMote'");
int b0, b1, lx, ly, rx, ry;
int kUp, kDown, kLeft, kRight;
int last_b1 = 0;
int last_ib0 = 0;
int last_ib1 = 0;
int last_lx = 0;
int last_ly = 0;
int last_rx = 0;
int last_ry = 0;
int last_kUp = 0;
int last_kDown = 0;
int last_kLeft = 0;
int last_kRight = 0;
bool lr3_axis = true;
bool lr3_buttons = true;
int rw_timer = 0;
while (true) {
nr=read(fd, buf, sizeof(buf));
if (nr == 19) {
// read successful
b0 = buf[0];
b1 = buf[1];
lx = buf[3] - 128;
ly = buf[4] - 128;
rx = buf[5] - 128;
ry = buf[6] - 128;
kRight = buf[7];
kLeft = buf[8];
kUp = buf[9];
kDown = buf[10];
//min value is -127
if (lx < -127) lx = -127;
if (ly < -127) ly = -127;
if (rx < -127) rx = -127;
if (ry < -127) ry = -127;
} else {
if (errno != EAGAIN) {
std::cerr << "sixad-3in1::read(fd, buf) - failed to read from device" << std::endl;
break;
}
b0 = last_ib0;
b1 = last_ib1;
lx = last_lx;
ly = last_ly;
rx = last_rx;
ry = last_ry;
kRight = last_kRight;
kLeft = last_kLeft;
kUp = last_kUp;
kDown = last_kDown;
}
//lr3 enable/disable
if ((b1 & KEYMOTE_KEY_L3) && b1 != last_b1)
lr3_axis = !lr3_axis;
if ((b1 & KEYMOTE_KEY_R3) && b1 != last_b1)
lr3_buttons = !lr3_buttons;
last_b1 = b1;
//buttons
if (lr3_buttons) {
//part1
if (last_ib0 != b0) {
uinput_send(ufd->mk, EV_KEY, settings.input.key_l2, b0 & KEYMOTE_KEY_L2 ? 1 : 0);
uinput_send(ufd->mk, EV_KEY, settings.input.key_r2, b0 & KEYMOTE_KEY_R2 ? 1 : 0);
uinput_send(ufd->mk, EV_KEY, settings.input.key_l1, b0 & KEYMOTE_KEY_L1 ? 1 : 0);
uinput_send(ufd->mk, EV_KEY, settings.input.key_r1, b0 & KEYMOTE_KEY_R1 ? 1 : 0);
uinput_send(ufd->mk, EV_KEY, settings.input.key_tri, b0 & KEYMOTE_KEY_TRIANGLE ? 1 : 0);
uinput_send(ufd->mk, EV_KEY, settings.input.key_cir, b0 & KEYMOTE_KEY_CIRCLE ? 1 : 0);
uinput_send(ufd->mk, EV_KEY, settings.input.key_cro, b0 & KEYMOTE_KEY_CROSS ? 1 : 0);
uinput_send(ufd->mk, EV_KEY, settings.input.key_squ, b0 & KEYMOTE_KEY_SQUARE ? 1 : 0);
}
//part2
if (last_ib1 != b1) {
uinput_send(ufd->mk, EV_KEY, settings.input.key_select, b1 & KEYMOTE_KEY_SELECT ? 1 : 0);
uinput_send(ufd->mk, EV_KEY, settings.input.key_start, b1 & KEYMOTE_KEY_START ? 1 : 0);
}
uinput_send(ufd->mk, EV_KEY, settings.input.key_up, kUp ? 1 : 0);
uinput_send(ufd->mk, EV_KEY, settings.input.key_right, kRight ? 1 : 0);
uinput_send(ufd->mk, EV_KEY, settings.input.key_down, kDown ? 1 : 0);
uinput_send(ufd->mk, EV_KEY, settings.input.key_left, kLeft ? 1 : 0);
}
//axis
if (lr3_axis) {
bool rw_do;
if (rw_timer%(settings.input.axis_speed*2) == 0)
rw_do = true;
else
rw_do = false;
uinput_send(ufd->mk, EV_REL, REL_X, lx/4/settings.input.axis_speed);
uinput_send(ufd->mk, EV_REL, REL_Y, ly/4/settings.input.axis_speed);
if (rw_do) {
rx = rx/20;
ry = ry/20;
ry = -ry; //Inverted
uinput_send(ufd->mk, EV_REL, REL_HWHEEL, rx);
uinput_send(ufd->mk, EV_REL, REL_WHEEL, ry);
}
}
last_ib0 = b0;
last_ib1 = b1;
last_lx = lx;
last_ly = ly;
last_rx = rx;
last_ry = ry;
last_kUp = kUp;
last_kDown = kDown;
last_kLeft = kLeft;
last_kRight = kRight;
uinput_send(ufd->mk, EV_SYN, SYN_REPORT, 0);
if (rw_timer > 0xff)
rw_timer = 0;
else
rw_timer += 1;
if (nr != 19)
usleep(10000);
}
uinput_close(ufd->mk, 0);
std::cerr << "sixad-3in1::read(buf) - connection has been broken" << std::endl;
delete ufd;
return 0;
}
