diff --git a/dev_in.c b/dev_in.c index 99554b5..d81aa3f 100644 --- a/dev_in.c +++ b/dev_in.c @@ -5,8 +5,6 @@ #include "message.h" #include "dev_evdev.h" #include "dev_iio.h" -#include -#include typedef enum dev_in_type { DEV_IN_TYPE_NONE, @@ -61,6 +59,9 @@ static int map_message_from_iio(dev_in_iio_t *const in_iio, in_message_t *const uint8_t data[32]; + struct timeval read_time; + gettimeofday(&read_time, NULL); + res = read(dev_iio_get_buffer_fd(in_iio->iiodev), &data[0], sizeof(data)); if (res == -1) { res = errno; @@ -68,7 +69,28 @@ static int map_message_from_iio(dev_in_iio_t *const in_iio, in_message_t *const goto send_message_from_iio_err; } - res = 0; + if (res != 24) { + fprintf(stderr, "Invalid read lenght\n"); + res = -EIO; + goto send_message_from_iio_err; + } + + uint16_t *const scan_elements = (uint16_t*)&data[0]; + + messages[0].data.gamepad_set.element = GAMEPAD_ACCELEROMETER; + messages[0].data.gamepad_set.status.gyro.sample_time = read_time; + messages[0].data.gamepad_set.status.accel.x = scan_elements[0]; + messages[0].data.gamepad_set.status.accel.y = scan_elements[1]; + messages[0].data.gamepad_set.status.accel.z = scan_elements[2]; + + messages[1].type = GAMEPAD_SET_ELEMENT; + messages[1].data.gamepad_set.element = GAMEPAD_GYROSCOPE; + messages[1].data.gamepad_set.status.gyro.sample_time = read_time; + messages[1].data.gamepad_set.status.gyro.x = scan_elements[3]; + messages[1].data.gamepad_set.status.gyro.y = scan_elements[4]; + messages[1].data.gamepad_set.status.gyro.z = scan_elements[5]; + + res = 2; send_message_from_iio_err: return res; diff --git a/dev_out.c b/dev_out.c index 458ceca..dcf1f9e 100644 --- a/dev_out.c +++ b/dev_out.c @@ -134,6 +134,20 @@ static void handle_incoming_message_gamepad_set( break; } + case GAMEPAD_GYROSCOPE: { + inout_gamepad->last_gyro_motion_time = msg_payload->status.gyro.sample_time; + inout_gamepad->raw_gyro[0] = msg_payload->status.gyro.x; + inout_gamepad->raw_gyro[1] = msg_payload->status.gyro.y; + inout_gamepad->raw_gyro[2] = msg_payload->status.gyro.z; + break; + } + case GAMEPAD_ACCELEROMETER: { + inout_gamepad->last_accel_motion_time = msg_payload->status.accel.sample_time; + inout_gamepad->raw_accel[0] = msg_payload->status.accel.x; + inout_gamepad->raw_accel[1] = msg_payload->status.accel.y; + inout_gamepad->raw_accel[2] = msg_payload->status.accel.z; + break; + } default: { fprintf(stderr, "Unknown gamepad element: %d\n", msg_payload->element); break; diff --git a/message.h b/message.h index 91bc3b2..d16e371 100644 --- a/message.h +++ b/message.h @@ -32,14 +32,35 @@ typedef enum in_message_gamepad_btn { GAMEPAD_DPAD_X, GAMEPAD_DPAD_Y, + + GAMEPAD_GYROSCOPE, + GAMEPAD_ACCELEROMETER, } in_gamepad_element_t; +typedef struct in_message_gamepad_gyro { + struct timeval sample_time; + + uint16_t x; + uint16_t y; + uint16_t z; +} in_message_gamepad_gyro_t; + +typedef struct in_message_gamepad_accel { + struct timeval sample_time; + + uint16_t x; + uint16_t y; + uint16_t z; +} in_message_gamepad_accel_t; + typedef struct in_message_gamepad_set_element { in_gamepad_element_t element; union { uint8_t btn; int32_t joystick_pos; int8_t dpad; // -1 | 0 | +1 + in_message_gamepad_accel_t accel; + in_message_gamepad_gyro_t gyro; } status; } in_message_gamepad_set_element_t;