diff --git a/gui/src/streamsession.cpp b/gui/src/streamsession.cpp index 52915f0..97a1305 100644 --- a/gui/src/streamsession.cpp +++ b/gui/src/streamsession.cpp @@ -429,7 +429,7 @@ void StreamSession::HandleSetsuEvent(SetsuEvent *event) case SETSU_EVENT_TOUCH_UP: for(auto it=setsu_ids.begin(); it!=setsu_ids.end(); it++) { - if(it.key().first == setsu_device_get_path(event->dev) && it.key().second == event->tracking_id) + if(it.key().first == setsu_device_get_path(event->dev) && it.key().second == event->touch.tracking_id) { chiaki_controller_state_stop_touch(&setsu_state, it.value()); setsu_ids.erase(it); @@ -439,18 +439,18 @@ void StreamSession::HandleSetsuEvent(SetsuEvent *event) SendFeedbackState(); break; case SETSU_EVENT_TOUCH_POSITION: { - QPair k = { setsu_device_get_path(event->dev), event->tracking_id }; + QPair k = { setsu_device_get_path(event->dev), event->touch.tracking_id }; auto it = setsu_ids.find(k); if(it == setsu_ids.end()) { - int8_t cid = chiaki_controller_state_start_touch(&setsu_state, event->x, event->y); + int8_t cid = chiaki_controller_state_start_touch(&setsu_state, event->touch.x, event->touch.y); if(cid >= 0) setsu_ids[k] = (uint8_t)cid; else break; } else - chiaki_controller_state_set_touch_pos(&setsu_state, it.value(), event->x, event->y); + chiaki_controller_state_set_touch_pos(&setsu_state, it.value(), event->touch.x, event->touch.y); SendFeedbackState(); break; } diff --git a/setsu/demo/motion.c b/setsu/demo/motion.c index f66e691..0b0f88b 100644 --- a/setsu/demo/motion.c +++ b/setsu/demo/motion.c @@ -8,9 +8,29 @@ #include #include #include +#include Setsu *setsu; +#define NAME_LEN 8 +const char * const names[] = { + "accel x ", + "accel y ", + "accel z ", + " gyro x ", + " gyro y ", + " gyro z " +}; +union +{ + struct + { + float accel_x, accel_y, accel_z; + float gyro_x, gyro_y, gyro_z; + }; + float v[6]; +} vals; +uint32_t timestamp; bool dirty = false; bool log_mode; volatile bool should_quit; @@ -22,14 +42,44 @@ void sigint(int s) should_quit = true; } +#define BAR_LENGTH 100 +#define BAR_MAX 2.0f +#define BAR_MAX_GYRO 180.0f + void print_state() { -#if 0 - char buf[256]; - *buf = 0; + char buf[6 * (1 + NAME_LEN + BAR_LENGTH) + 1]; + size_t i = 0; + for(size_t b=0; b<6; b++) + { + buf[i++] = '\n'; + memcpy(buf + i, names[b], NAME_LEN); + i += NAME_LEN; + buf[i++] = '['; + size_t max = BAR_LENGTH-2; + for(size_t bi=0; bi 2 ? BAR_MAX_GYRO : BAR_MAX) * (2.0f * (float)(x) / (float)max - 1.0f)) + float cur = BAR_VAL(bi); + float prev = BAR_VAL((int)bi - 1); + if(prev < 0.0f && cur >= 0.0f) + { + buf[i++] = '|'; + continue; + } + bool cov = ((vals.v[b] < 0.0f) == (cur < 0.0f)) && fabsf(vals.v[b]) > fabsf(cur); + float next = BAR_VAL(bi + 1); +#define MARK_VAL (b > 2 ? 90.0f : 1.0f) + bool mark = cur < -MARK_VAL && next >= -MARK_VAL || prev < MARK_VAL && cur >= MARK_VAL; + buf[i++] = cov ? (mark ? '#' : '=') : (mark ? '.' : ' '); +#undef BAR_VAL + } + buf[i++] = ']'; + } + buf[i++] = '\0'; + assert(i == sizeof(buf)); printf("\033[2J%s", buf); fflush(stdout); -#endif } void event(SetsuEvent *event, void *user) @@ -49,7 +99,19 @@ void event(SetsuEvent *event, void *user) break; LOG("Device removed: %s\n", event->path); break; - // TODO: motion events + case SETSU_EVENT_MOTION: + LOG("Motion: %f, %f, %f / %f, %f, %f / %u\n", + event->motion.accel_x, event->motion.accel_y, event->motion.accel_z, + event->motion.gyro_x, event->motion.gyro_y, event->motion.gyro_z, + (unsigned int)event->motion.timestamp); + vals.accel_x = event->motion.accel_x; + vals.accel_y = event->motion.accel_y; + vals.accel_z = event->motion.accel_z; + vals.gyro_x = event->motion.gyro_x; + vals.gyro_y = event->motion.gyro_y; + vals.gyro_z = event->motion.gyro_z; + timestamp = event->motion.timestamp; + dirty = true; default: break; } diff --git a/setsu/demo/touchpad.c b/setsu/demo/touchpad.c index 18757d4..b51d628 100644 --- a/setsu/demo/touchpad.c +++ b/setsu/demo/touchpad.c @@ -80,13 +80,13 @@ void event(SetsuEvent *event, void *user) LOG("Device removed: %s\n", event->path); break; case SETSU_EVENT_TOUCH_DOWN: - LOG("Down for %s, tracking id %d\n", setsu_device_get_path(event->dev), event->tracking_id); + LOG("Down for %s, tracking id %d\n", setsu_device_get_path(event->dev), event->touch.tracking_id); for(size_t i=0; itracking_id; + touches[i].tracking_id = event->touch.tracking_id; break; } } @@ -94,19 +94,19 @@ void event(SetsuEvent *event, void *user) case SETSU_EVENT_TOUCH_POSITION: case SETSU_EVENT_TOUCH_UP: if(event->type == SETSU_EVENT_TOUCH_UP) - LOG("Up for %s, tracking id %d\n", setsu_device_get_path(event->dev), event->tracking_id); + LOG("Up for %s, tracking id %d\n", setsu_device_get_path(event->dev), event->touch.tracking_id); else LOG("Position for %s, tracking id %d: %u, %u\n", setsu_device_get_path(event->dev), - event->tracking_id, (unsigned int)event->x, (unsigned int)event->y); + event->touch.tracking_id, (unsigned int)event->touch.x, (unsigned int)event->touch.y); for(size_t i=0; itracking_id) + if(touches[i].down && touches[i].tracking_id == event->touch.tracking_id) { switch(event->type) { case SETSU_EVENT_TOUCH_POSITION: - touches[i].x = event->x; - touches[i].y = event->y; + touches[i].x = event->touch.x; + touches[i].y = event->touch.y; break; case SETSU_EVENT_TOUCH_UP: touches[i].down = false; diff --git a/setsu/include/setsu.h b/setsu/include/setsu.h index a2ca4ed..36ecf03 100644 --- a/setsu/include/setsu.h +++ b/setsu/include/setsu.h @@ -46,7 +46,10 @@ typedef enum { SETSU_EVENT_BUTTON_DOWN, /* Event will have dev and button set. */ - SETSU_EVENT_BUTTON_UP + SETSU_EVENT_BUTTON_UP, + + /* Event will have motion set. */ + SETSU_EVENT_MOTION } SetsuEventType; #define SETSU_BUTTON_0 (1u << 0) @@ -72,8 +75,14 @@ typedef struct setsu_event_t { SetsuTrackingId tracking_id; uint32_t x, y; - }; + } touch; SetsuButton button; + struct + { + float accel_x, accel_y, accel_z; // unit is 1G + float gyro_x, gyro_y, gyro_z; // unit is deg/sec + uint32_t timestamp; // microseconds + } motion; }; }; }; @@ -90,6 +99,7 @@ const char *setsu_device_get_path(SetsuDevice *dev); uint32_t setsu_device_touchpad_get_width(SetsuDevice *dev); uint32_t setsu_device_touchpad_get_height(SetsuDevice *dev); + #ifdef __cplusplus } #endif diff --git a/setsu/src/setsu.c b/setsu/src/setsu.c index 85bd41b..6395313 100644 --- a/setsu/src/setsu.c +++ b/setsu/src/setsu.c @@ -67,6 +67,12 @@ typedef struct setsu_device_t } touchpad; struct { + int accel_res_x, accel_res_y, accel_res_z; + int gyro_res_x, gyro_res_y, gyro_res_z; + int accel_x, accel_y, accel_z; + int gyro_x, gyro_y, gyro_z; + uint32_t timestamp; + bool dirty; } motion; }; } SetsuDevice; @@ -341,7 +347,13 @@ SetsuDevice *setsu_connect(Setsu *setsu, const char *path, SetsuDeviceType type) } break; case SETSU_DEVICE_TYPE_MOTION: - // TODO: init to defaults + dev->motion.accel_res_x = libevdev_get_abs_resolution(dev->evdev, ABS_X); + dev->motion.accel_res_y = libevdev_get_abs_resolution(dev->evdev, ABS_Y); + dev->motion.accel_res_z = libevdev_get_abs_resolution(dev->evdev, ABS_Z); + dev->motion.gyro_res_x = libevdev_get_abs_resolution(dev->evdev, ABS_RX); + dev->motion.gyro_res_y = libevdev_get_abs_resolution(dev->evdev, ABS_RY); + dev->motion.gyro_res_z = libevdev_get_abs_resolution(dev->evdev, ABS_RZ); + dev->motion.accel_y = dev->motion.accel_res_y; // 1G down break; } @@ -573,7 +585,45 @@ static void device_event(Setsu *setsu, SetsuDevice *dev, struct input_event *ev, } break; case SETSU_DEVICE_TYPE_MOTION: - // TODO: handle the events + switch(ev->type) + { + case EV_ABS: + switch(ev->code) + { + case ABS_X: + dev->motion.accel_x = ev->value; + dev->motion.dirty = true; + break; + case ABS_Y: + dev->motion.accel_y = ev->value; + dev->motion.dirty = true; + break; + case ABS_Z: + dev->motion.accel_z = ev->value; + dev->motion.dirty = true; + break; + case ABS_RX: + dev->motion.gyro_x = ev->value; + dev->motion.dirty = true; + break; + case ABS_RY: + dev->motion.gyro_y = ev->value; + dev->motion.dirty = true; + break; + case ABS_RZ: + dev->motion.gyro_z = ev->value; + dev->motion.dirty = true; + break; + } + break; + case EV_MSC: + if(ev->code == MSC_TIMESTAMP) + { + dev->motion.timestamp = ev->value; + dev->motion.dirty = true; + } + break; + } break; } } @@ -591,23 +641,23 @@ static void device_drain(Setsu *setsu, SetsuDevice *dev, SetsuEventCb cb, void * if(dev->touchpad.slots[i].tracking_id_prev != -1) { BEGIN_EVENT(SETSU_EVENT_TOUCH_UP); - event.tracking_id = dev->touchpad.slots[i].tracking_id_prev; + event.touch.tracking_id = dev->touchpad.slots[i].tracking_id_prev; SEND_EVENT(); dev->touchpad.slots[i].tracking_id_prev = -1; } if(dev->touchpad.slots[i].downed) { BEGIN_EVENT(SETSU_EVENT_TOUCH_DOWN); - event.tracking_id = dev->touchpad.slots[i].tracking_id; + event.touch.tracking_id = dev->touchpad.slots[i].tracking_id; SEND_EVENT(); dev->touchpad.slots[i].downed = false; } if(dev->touchpad.slots[i].pos_dirty) { BEGIN_EVENT(SETSU_EVENT_TOUCH_POSITION); - event.tracking_id = dev->touchpad.slots[i].tracking_id; - event.x = (uint32_t)(dev->touchpad.slots[i].x - dev->touchpad.min_x); - event.y = (uint32_t)(dev->touchpad.slots[i].y - dev->touchpad.min_y); + event.touch.tracking_id = dev->touchpad.slots[i].tracking_id; + event.touch.x = (uint32_t)(dev->touchpad.slots[i].x - dev->touchpad.min_x); + event.touch.y = (uint32_t)(dev->touchpad.slots[i].y - dev->touchpad.min_y); SEND_EVENT(); dev->touchpad.slots[i].pos_dirty = false; } @@ -630,7 +680,19 @@ static void device_drain(Setsu *setsu, SetsuDevice *dev, SetsuEventCb cb, void * dev->touchpad.buttons_prev = dev->touchpad.buttons_cur; break; case SETSU_DEVICE_TYPE_MOTION: - // TODO + if(dev->motion.dirty) + { + BEGIN_EVENT(SETSU_EVENT_MOTION); + event.motion.accel_x = (float)dev->motion.accel_x / (float)dev->motion.accel_res_x; + event.motion.accel_y = (float)dev->motion.accel_y / (float)dev->motion.accel_res_y; + event.motion.accel_z = (float)dev->motion.accel_z / (float)dev->motion.accel_res_z; + event.motion.gyro_x = (float)dev->motion.gyro_x / (float)dev->motion.gyro_res_x; + event.motion.gyro_y = (float)dev->motion.gyro_y / (float)dev->motion.gyro_res_y; + event.motion.gyro_z = (float)dev->motion.gyro_z / (float)dev->motion.gyro_res_z; + event.motion.timestamp = dev->motion.timestamp; + SEND_EVENT(); + dev->motion.dirty = false; + } break; } #undef BEGIN_EVENT