Finish Motion in Setsu

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<QString, SetsuTrackingId> k =  { setsu_device_get_path(event->dev), event->tracking_id };
+			QPair<QString, SetsuTrackingId> 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;
 		}

setsu/demo/motion.c

@@ -8,9 +8,29 @@
 #include <string.h>
 #include <assert.h>
 #include <signal.h>
+#include <math.h>
 
 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[6 * (1 + NAME_LEN + BAR_LENGTH) + 1];
-	char buf[256];
+	size_t i = 0;
-	*buf = 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<max; bi++)
+		{
+#define BAR_VAL(x) ((b > 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;
 	}

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; i<TOUCHES_MAX; i++)
 			{
 				if(!touches[i].down)
 				{
 					touches[i].down = true;
-					touches[i].tracking_id = event->tracking_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; i<TOUCHES_MAX; i++)
 			{
-				if(touches[i].down && touches[i].tracking_id == event->tracking_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].x = event->touch.x;
-							touches[i].y = event->y;
+							touches[i].y = event->touch.y;
 							break;
 						case SETSU_EVENT_TOUCH_UP:
 							touches[i].down = false;

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

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.touch.tracking_id = dev->touchpad.slots[i].tracking_id;
-					event.x = (uint32_t)(dev->touchpad.slots[i].x - dev->touchpad.min_x);
+					event.touch.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.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