From 0192517a2dcc2ed53724de861a60628e9d725833 Mon Sep 17 00:00:00 2001 From: polygraphene Date: Tue, 21 Aug 2018 23:04:13 +0900 Subject: [PATCH] Add sample scripts. Fix parameter of Madgwick filter. Add magnetometer. --- .../VRControllerFreePIEPlugin.cs | 21 +- samples/test.py | 97 ++++++++ samples/withalvr.py | 208 ++++++++++++++++++ 3 files changed, 317 insertions(+), 9 deletions(-) create mode 100644 samples/test.py create mode 100644 samples/withalvr.py diff --git a/FreePIEVRController/VRControllerFreePIEPlugin.cs b/FreePIEVRController/VRControllerFreePIEPlugin.cs index 4ad1e80..1f620c1 100644 --- a/FreePIEVRController/VRControllerFreePIEPlugin.cs +++ b/FreePIEVRController/VRControllerFreePIEPlugin.cs @@ -275,10 +275,11 @@ void CallbackGearVR(GattCharacteristic sender, GattValueChangedEventArgs eventAr int dataCount = (buf.Length - FOOTER_SIZE) / DATA_SIZE; var mscale = 0.06 * 0.06; - var mx = Get16(DATA_SIZE * dataCount + 0, buf) * mscale; // 48 ~ 54 - var my = Get16(DATA_SIZE * dataCount + 2, buf) * mscale; - var mz = Get16(DATA_SIZE * dataCount + 4, buf) * mscale; - var mmag = Math.Sqrt(mx * mx + my * my + mz * mz); + magnetometer[0] = Get16(DATA_SIZE * dataCount + 0, buf) * mscale; // 48 ~ 54 + magnetometer[1] = Get16(DATA_SIZE * dataCount + 2, buf) * mscale; + magnetometer[2] = Get16(DATA_SIZE * dataCount + 4, buf) * mscale; + var mmag = Math.Sqrt(magnetometer[0] * magnetometer[0] + + magnetometer[1] * magnetometer[1] + magnetometer[2] * magnetometer[2]); for (int i = 0; i < dataCount; i++) { @@ -304,7 +305,7 @@ void CallbackGearVR(GattCharacteristic sender, GattValueChangedEventArgs eventAr gyroscope[1] = gy; gyroscope[2] = gz; - AHRS.Update((float)gx, (float)gy, (float)gz, (float)ax, (float)ay, (float)az, (float)mx, (float)my, (float)mz); + AHRS.Update((float)gx, (float)gy, (float)gz, (float)ax, (float)ay, (float)az, (float)magnetometer[0], (float)magnetometer[1], (float)magnetometer[2]); } // We need to fix the difference of coordinate system between AHRS and VR app. @@ -371,7 +372,7 @@ async Task ConnectDaydream(DeviceInformation info) service = await AsTask(GattDeviceService.FromIdAsync(info.Id)); if (service == null) { - Console.WriteLine("Error: Other device is using the device."); + Console.WriteLine("Error: Another program is using the device."); return false; } @@ -417,7 +418,7 @@ async Task ConnectGearVR(DeviceInformation info) service = await AsTask(GattDeviceService.FromIdAsync(info.Id)); if (service == null) { - Console.WriteLine("Error: Other device is using the device."); + Console.WriteLine("Error: Another program is using the device."); return false; } @@ -470,7 +471,7 @@ async Task ConnectGearVR(DeviceInformation info) if (status == GattCommunicationStatus.Success) { Console.WriteLine("Connected."); - AHRS = new AHRS.MadgwickAHRS(1f / Constants.GEARVR_HZ, 0.1f); + AHRS = new AHRS.MadgwickAHRS(1f / Constants.GEARVR_HZ, 0.01f); type = Constants.TYPE_GEARVR; bdaddr = BDAddrToString(service.Device.BluetoothAddress); deviceId = service.Device.DeviceId; @@ -516,12 +517,14 @@ private Task AsTask(IAsyncOperation operation) // Controller orientation in quaternion(x,y,z,w) rad. public double[] quaternion { get; private set; } = new double[4]; - // Orientation raw (x,y,z) + // Orientation raw (x,y,z) (only for Daydream) public double[] orientation { get; private set; } = new double[3]; // Acceleration in (x,y,z) ms^-2 public double[] acceleration { get; private set; } = new double[3]; // Gyroscope public double[] gyroscope { get; private set; } = new double[3]; + // Magnetometer (only for GearVR) + public double[] magnetometer { get; private set; } = new double[3]; // Position (caluclated by arm model) public double[] position { get; private set; } = new double[3]; // Touch X,Y diff --git a/samples/test.py b/samples/test.py new file mode 100644 index 0000000..d0c1fbe --- /dev/null +++ b/samples/test.py @@ -0,0 +1,97 @@ +# only Daydream +diagnostics.watch(vrcontroller[0].orientation[0]) +diagnostics.watch(vrcontroller[0].orientation[1]) +diagnostics.watch(vrcontroller[0].orientation[2]) + +diagnostics.watch(vrcontroller[0].quaternion[0]) +diagnostics.watch(vrcontroller[0].quaternion[1]) +diagnostics.watch(vrcontroller[0].quaternion[2]) +diagnostics.watch(vrcontroller[0].quaternion[3]) + +diagnostics.watch(vrcontroller[0].position[0]) +diagnostics.watch(vrcontroller[0].position[1]) +diagnostics.watch(vrcontroller[0].position[2]) + +diagnostics.watch(vrcontroller[0].touch) +diagnostics.watch(vrcontroller[0].click) +diagnostics.watch(vrcontroller[0].home) +diagnostics.watch(vrcontroller[0].app) +diagnostics.watch(vrcontroller[0].volup) +diagnostics.watch(vrcontroller[0].voldown) +# only GearVR +diagnostics.watch(vrcontroller[0].trigger) +diagnostics.watch(vrcontroller[0].button[0]) + +# Constants for button index +diagnostics.watch(vrcontroller[0].CLICK) +diagnostics.watch(vrcontroller[0].HOME) +diagnostics.watch(vrcontroller[0].APP) +diagnostics.watch(vrcontroller[0].TRIGGER) + +diagnostics.watch(vrcontroller[0].acceleration[0]) +diagnostics.watch(vrcontroller[0].acceleration[1]) +diagnostics.watch(vrcontroller[0].acceleration[2]) + +diagnostics.watch(vrcontroller[0].gyroscope[0]) +diagnostics.watch(vrcontroller[0].gyroscope[1]) +diagnostics.watch(vrcontroller[0].gyroscope[2]) + +# only GearVR +diagnostics.watch(vrcontroller[0].magnetometer[0]) +diagnostics.watch(vrcontroller[0].magnetometer[1]) +diagnostics.watch(vrcontroller[0].magnetometer[2]) + +diagnostics.watch(vrcontroller[0].trackpad[0]) +diagnostics.watch(vrcontroller[0].trackpad[1]) + +diagnostics.watch(vrcontroller[0].version) +diagnostics.watch(vrcontroller[0].type) +diagnostics.watch(vrcontroller[0].bdaddr) +diagnostics.watch(vrcontroller[0].deviceId) +diagnostics.watch(vrcontroller[0].temperature) + +# only Daydream +diagnostics.watch(vrcontroller[1].orientation[0]) +diagnostics.watch(vrcontroller[1].orientation[1]) +diagnostics.watch(vrcontroller[1].orientation[2]) + +diagnostics.watch(vrcontroller[1].quaternion[0]) +diagnostics.watch(vrcontroller[1].quaternion[1]) +diagnostics.watch(vrcontroller[1].quaternion[2]) +diagnostics.watch(vrcontroller[1].quaternion[3]) + +diagnostics.watch(vrcontroller[1].position[0]) +diagnostics.watch(vrcontroller[1].position[1]) +diagnostics.watch(vrcontroller[1].position[2]) + +diagnostics.watch(vrcontroller[1].touch) +diagnostics.watch(vrcontroller[1].click) +diagnostics.watch(vrcontroller[1].home) +diagnostics.watch(vrcontroller[1].app) +diagnostics.watch(vrcontroller[1].volup) +diagnostics.watch(vrcontroller[1].voldown) +# only GearVR +diagnostics.watch(vrcontroller[1].trigger) +diagnostics.watch(vrcontroller[1].button[0]) + +diagnostics.watch(vrcontroller[1].acceleration[0]) +diagnostics.watch(vrcontroller[1].acceleration[1]) +diagnostics.watch(vrcontroller[1].acceleration[2]) + +diagnostics.watch(vrcontroller[1].gyroscope[0]) +diagnostics.watch(vrcontroller[1].gyroscope[1]) +diagnostics.watch(vrcontroller[1].gyroscope[2]) + +# only GearVR +diagnostics.watch(vrcontroller[1].magnetometer[0]) +diagnostics.watch(vrcontroller[1].magnetometer[1]) +diagnostics.watch(vrcontroller[1].magnetometer[2]) + +diagnostics.watch(vrcontroller[1].trackpad[0]) +diagnostics.watch(vrcontroller[1].trackpad[1]) + +diagnostics.watch(vrcontroller[1].version) +diagnostics.watch(vrcontroller[1].type) +diagnostics.watch(vrcontroller[1].bdaddr) +diagnostics.watch(vrcontroller[1].deviceId) +diagnostics.watch(vrcontroller[1].temperature) diff --git a/samples/withalvr.py b/samples/withalvr.py new file mode 100644 index 0000000..a6673a3 --- /dev/null +++ b/samples/withalvr.py @@ -0,0 +1,208 @@ +import math, time + +global prev, pressed, mode, offset, message_time + +def sign(x): return 1 if x >= 0 else -1 + +# conjugate quaternion +def conj(q): + return [-q[0], -q[1], -q[2], q[3]] + +# multiplication of quaternion +def multiply(a, b): + x0, y0, z0, w0 = a + x1, y1, z1, w1 = b + return [x1 * w0 - y1 * z0 + z1 * y0 + w1 * x0, + x1 * z0 + y1 * w0 - z1 * x0 + w1 * y0, + -x1 * y0 + y1 * x0 + z1 * w0 + w1 * z0, + -x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0] + +# convert quaternion to euler +def quaternion2euler(q): + yaw_pitch_roll = [0.0, 0.0, 0.0] + # roll (x-axis rotation) + sinr = +2.0 * (q[3] * q[0] + q[1] * q[2]) + cosr = +1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1]) + yaw_pitch_roll[2] = math.atan2(sinr, cosr) + + # pitch (y-axis rotation) + sinp = +2.0 * (q[3] * q[1] - q[2] * q[0]) + if (abs(sinp) >= 1): + yaw_pitch_roll[1] = math.copysign(M_PI / 2, sinp) + else: + yaw_pitch_roll[1] = math.asin(sinp) + + # yaw (z-axis rotation) + siny = +2.0 * (q[3] * q[2] + q[0] * q[1]); + cosy = +1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]); + yaw_pitch_roll[0] = math.atan2(siny, cosy); + + return yaw_pitch_roll + +# convert euler to quaternion +def euler2quaternion(yaw_pitch_roll): + cy = math.cos(yaw_pitch_roll[0] * 0.5); + sy = math.sin(yaw_pitch_roll[0] * 0.5); + cr = math.cos(yaw_pitch_roll[2] * 0.5); + sr = math.sin(yaw_pitch_roll[2] * 0.5); + cp = math.cos(yaw_pitch_roll[1] * 0.5); + sp = math.sin(yaw_pitch_roll[1] * 0.5); + + return [cy * sr * cp - sy * cr * sp, + cy * cr * sp + sy * sr * cp, + sy * cr * cp - cy * sr * sp, + cy * cr * cp + sy * sr * sp] + +def quaternion_rotate_vec(q, vec): + return multiply(multiply(q, vec), conj(q)) + +# rotate specified vector using yaw_pitch_roll +def rotatevec(yaw_pitch_roll, vec): + q = euler2quaternion(yaw_pitch_roll) + return quaternion_rotate_vec(q, vec) + +if starting: + prev = [[False] * 30, [False] * 30] + pressed = [[False] * 30, [False] * 30] + mode = [0, 0] + offset = [0.0, 0.0, 0.0] + message_time = 0.0 + alvr.two_controllers = True + +# change target controller +if keyboard.getPressed(Key.Z): + controller = 1 - controller + +map = [["system", Key.G], ["application_menu", Key.X], ["trigger", Key.T], ["a", Key.V], ["b", Key.B], ["x", Key.N], ["y", Key.M] +, ["grip", Key.F1], ["trackpad_click", Key.F2], ["back", Key.F3], ["guide", Key.F4], ["start", Key.F5] +, ["dpad_left", Key.F6], ["dpad_up", Key.F7], ["dpad_right", Key.F8], ["dpad_down", Key.F9], ["trackpad_touch", Key.F10]] + +for k in map: + alvr.buttons[0][alvr.Id(k[0])] = keyboard.getKeyDown(k[1]) + alvr.buttons[1][alvr.Id(k[0])] = keyboard.getKeyDown(k[1]) + +if time.time() - message_time > 2: + # remove message after 2 seconds + alvr.message = "" + +for c in range(0, 2): + for i in range(0, vrcontroller[c].BUTTONS): + if prev[c][i] != vrcontroller[c].button[i]: + prev[c][i] = vrcontroller[c].button[i] + pressed[c][i] = prev[c][i] + else: + pressed[c][i] = False + + if pressed[c][vrcontroller[0].APP]: + mode[c] = (mode[c] + 1) % 3 + # show messageo on display + alvr.message = "mode left " + str(mode[0]) + "\nmode right " + str(mode[1]) + message_time = time.time() + + if mode[c] == 0: + # trackpad guesture mode + alvr.buttons[c][alvr.Id("trigger")] = alvr.buttons[c][alvr.Id("trigger")] or vrcontroller[c].trigger + #alvr.buttons[c][alvr.Id("application_menu")] = alvr.buttons[c][alvr.Id("application_menu")] or alvr.input_buttons[alvr.InputId("back")] + + if vrcontroller[c].click: + if vrcontroller[c].trackpad[0] + vrcontroller[c].trackpad[1] > 0.0: + if vrcontroller[c].trackpad[0] - vrcontroller[c].trackpad[1] > 0.0: + # right + alvr.buttons[c][alvr.Id("system")] = True + else: + # top + alvr.buttons[c][alvr.Id("trackpad_click")] = True + alvr.buttons[c][alvr.Id("trackpad_touch")] = True + else: + if vrcontroller[c].trackpad[0] - vrcontroller[c].trackpad[1] > 0.0: + # bottom + alvr.buttons[c][alvr.Id("grip")] = True + else: + # left + alvr.buttons[c][alvr.Id("application_menu")] = True + elif mode[c] == 1: + # fly mode (buggy) + # press upper half of trackpad to forward. bottom half to back + if vrcontroller[c].click: + outvec = quaternion_rotate_vec(vrcontroller[c].quaternion, [0, 0, -1, 0]) + speed = 0.002 * sign(vrcontroller[c].trackpad[1]) + offset[0] += speed * outvec[0] + offset[1] += speed * outvec[1] + offset[2] += speed * outvec[2] + if vrcontroller[c].trigger and vrcontroller[c].click: + offset = [0.0, 0.0, 0.0] + + alvr.buttons[c][alvr.Id("trigger")] = alvr.buttons[c][alvr.Id("trigger")] or vrcontroller[c].trigger + elif mode[c] == 2: + # passthrough mode + alvr.buttons[c][alvr.Id("trackpad_click")] = alvr.buttons[c][alvr.Id("trackpad_click")] or vrcontroller[c].click + alvr.buttons[c][alvr.Id("trackpad_touch")] = alvr.buttons[c][alvr.Id("trackpad_touch")] or vrcontroller[c].touch + alvr.buttons[c][alvr.Id("trigger")] = alvr.buttons[c][alvr.Id("trigger")] or vrcontroller[c].trigger + alvr.trackpad[c][0] = vrcontroller[c].trackpad[0] + alvr.trackpad[c][1] = vrcontroller[c].trackpad[1] + alvr.buttons[c][alvr.Id("system")] = vrcontroller[c].volup + alvr.buttons[c][alvr.Id("grip")] = vrcontroller[c].voldown + + # You need to set trigger value correctly to get trigger click work + alvr.trigger[c] = 1.0 if alvr.buttons[c][alvr.Id("trigger")] else 0.0 + + alvr.override_head_position = True + + alvr.head_position[0] = alvr.input_head_position[0] + offset[0] + alvr.head_position[1] = alvr.input_head_position[1] + offset[1] + alvr.head_position[2] = alvr.input_head_position[2] + offset[2] + + alvr.override_controller_position = True + + alvr.controller_position[c][0] = vrcontroller[c].position[0] * 3 + offset[0] + alvr.controller_position[c][1] = vrcontroller[c].position[1] * 3 + offset[1] + alvr.controller_position[c][2] = vrcontroller[c].position[2] * 3 + offset[2] + #alvr.controller_position[1-controller][0] = alvr.input_controller_position[0] + offset[0] + 0.1 + #alvr.controller_position[1-controller][1] = alvr.input_controller_position[1] + offset[1] + 0.1 + #alvr.controller_position[1-controller][2] = alvr.input_controller_position[2] + offset[2] + 0.1 + #alvr.controller_orientation[1-controller][0] = android[0].yaw + #alvr.controller_orientation[1-controller][1] = android[0].pitch + #alvr.controller_orientation[1-controller][2] = android[0].roll + + alvr.override_controller_orientation = True + yaw_pitch_roll = quaternion2euler(vrcontroller[c].quaternion) + alvr.controller_orientation[c][0] = yaw_pitch_roll[0] + alvr.controller_orientation[c][1] = yaw_pitch_roll[1] + alvr.controller_orientation[c][2] = yaw_pitch_roll[2] + +if True: + # watch variables on FreePIE debugger + diagnostics.watch(alvr.input_head_orientation[0]) + diagnostics.watch(alvr.input_head_orientation[1]) + diagnostics.watch(alvr.input_head_orientation[2]) + + diagnostics.watch(alvr.input_controller_orientation[0]) + diagnostics.watch(alvr.input_controller_orientation[1]) + diagnostics.watch(alvr.input_controller_orientation[2]) + + diagnostics.watch(alvr.input_head_position[0]) + diagnostics.watch(alvr.input_head_position[1]) + diagnostics.watch(alvr.input_head_position[2]) + + diagnostics.watch(alvr.input_controller_position[0]) + diagnostics.watch(alvr.input_controller_position[1]) + diagnostics.watch(alvr.input_controller_position[2]) + + diagnostics.watch(alvr.input_trackpad[0]) + diagnostics.watch(alvr.input_trackpad[1]) + + diagnostics.watch(alvr.input_buttons[0]) + diagnostics.watch(alvr.input_buttons[1]) + diagnostics.watch(alvr.input_buttons[2]) + diagnostics.watch(alvr.input_buttons[3]) + diagnostics.watch(alvr.input_buttons[4]) + diagnostics.watch(alvr.input_buttons[5]) + + diagnostics.watch(alvr.head_position[0]) + diagnostics.watch(alvr.head_position[1]) + diagnostics.watch(alvr.head_position[2]) + + diagnostics.watch(alvr.buttons[c][alvr.Id("trigger")]) + diagnostics.watch(vrcontroller[1].magnetometer[0]) + diagnostics.watch(vrcontroller[1].magnetometer[1]) + diagnostics.watch(vrcontroller[1].magnetometer[2])