Thanks to visit codestin.com
Credit goes to github.com

Skip to content
This repository was archived by the owner on May 3, 2021. It is now read-only.

Commit 2368fc5

Browse files
HipsterSlothHipsterSloth
authored
Merge pull request #585 from cboulay/ZCM2U
Adding support for new PSMove ZCM2 Controller
2 parents 86ca648 + 4c343b6 commit 2368fc5

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

52 files changed

+5539
-3806
lines changed

‎.gitmodules‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,3 +36,6 @@
3636
[submodule "thirdparty/libstem_gamepad"]
3737
path = thirdparty/libstem_gamepad
3838
url = https://github.com/cboulay/libstem_gamepad.git
39+
[submodule "thirdparty/lockfreequeue"]
40+
path = thirdparty/lockfreequeue
41+
url = https://github.com/cameron314/readerwriterqueue.git

‎src/psmoveconfigtool/AppStage_ControllerSettings.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -192,7 +192,7 @@ const int k_default_ds4_orientation_filter_index = 3; // OrientationKalman
192192
const int k_default_ds4_gyro_gain_index = 4; // 2000deg/s
193193

194194
const char* k_controller_position_filter_names[] = { "PassThru", "LowPassOptical", "LowPassIMU", "LowPassExponential", "ComplimentaryOpticalIMU", "PositionKalman" };
195-
const char* k_psmove_orientation_filter_names[] = { "PassThru", "MadgwickARG", "MadgwickMARG", "ComplementaryMARG", "OrientationKalman" };
195+
const char* k_psmove_orientation_filter_names[] = { "PassThru", "MadgwickARG", "MadgwickMARG", "ComplementaryMARG", "ComplementaryOpticalARG", "OrientationKalman" };
196196
const char* k_ds4_orientation_filter_names[] = { "PassThru", "MadgwickARG", "ComplementaryOpticalARG", "OrientationKalman" };
197197
const char* k_ds4_gyro_gain_setting_labels[] = { "125deg/s", "250deg/s", "500deg/s", "1000deg/s", "2000deg/s", "custom"};
198198

‎src/psmoveconfigtool/AppStage_GyroscopeCalibration.cpp‎

Lines changed: 35 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -33,34 +33,49 @@ const int k_desired_scale_sample_count = 1000;
3333
//-- definitions -----
3434
struct GyroscopeNoiseSamples
3535
{
36-
PSMVector3f omega_samples[k_desired_noise_sample_count];
36+
PSMVector3i raw_gyro_samples[k_desired_noise_sample_count];
37+
PSMVector3f calibrated_omega_samples[k_desired_noise_sample_count];
3738
PSMVector3f drift_rotation;
3839
std::chrono::time_point<std::chrono::high_resolution_clock> sampleStartTime;
3940
int sample_count;
4041

41-
float variance; // Max sensor variance (raw_sensor_units/s/s for DS4, rad/s/s for PSMove)
42-
float drift; // Max drift rate (raw_sensor_units/s for DS4, rad/s for PSMove)
42+
PSMVector3f raw_bias; // The average bias in the raw gyro measurement per frame
43+
float angular_drift_variance; // Max sensor variance (raw_sensor_units/s/s for DS4, rad/s/s for PSMove)
44+
float angular_drift_rate; // Max drift rate (raw_sensor_units/s for DS4, rad/s for PSMove)
4345

4446
void clear()
4547
{
4648
drift_rotation= *k_psm_float_vector3_zero;
4749
sample_count= 0;
48-
variance= 0.f;
49-
drift= 0.f;
50+
raw_bias= *k_psm_float_vector3_zero;
51+
angular_drift_variance= 0.f;
52+
angular_drift_rate= 0.f;
5053
}
5154

5255
void computeStatistics(std::chrono::duration<float, std::milli> sampleDurationMilli)
5356
{
5457
const float sampleDurationSeconds= sampleDurationMilli.count() / 1000.f;
5558
const float N = static_cast<float>(sample_count);
5659

60+
// Compute the mean of the raw gyro measurements
61+
// If this is a non-zero value then the gyro has some per frame
62+
// bias we need to subtract off.
63+
raw_bias= *k_psm_float_vector3_zero;
64+
for (int sample_index = 0; sample_index < sample_count; sample_index++)
65+
{
66+
PSMVector3f raw_sample= PSM_Vector3iCastToFloat(&raw_gyro_samples[sample_index]);
67+
68+
raw_bias= PSM_Vector3fAdd(&raw_bias, &raw_sample);
69+
}
70+
raw_bias= PSM_Vector3fUnsafeScalarDivide(&raw_bias, N);
71+
5772
// Compute the mean of the error samples, where "error" = abs(omega_sample)
58-
// If we took the mean of the signed omega samples we'd get a value very
59-
// close to zero since the the gyro at rest over a short period has mean-zero noise
73+
// If the gyro has little to no bias then the mean of the signed omega samples
74+
// would be very close to zero since the the gyro at rest over a short period has mean-zero noise
6075
PSMVector3f mean_omega_error= *k_psm_float_vector3_zero;
6176
for (int sample_index = 0; sample_index < sample_count; sample_index++)
6277
{
63-
PSMVector3f error_sample= PSM_Vector3fAbs(&omega_samples[sample_index]);
78+
PSMVector3f error_sample= PSM_Vector3fAbs(&calibrated_omega_samples[sample_index]);
6479

6580
mean_omega_error= PSM_Vector3fAdd(&mean_omega_error, &error_sample);
6681
}
@@ -70,7 +85,7 @@ struct GyroscopeNoiseSamples
7085
PSMVector3f var_omega= *k_psm_float_vector3_zero;
7186
for (int sample_index = 0; sample_index < sample_count; sample_index++)
7287
{
73-
PSMVector3f error_sample= PSM_Vector3fAbs(&omega_samples[sample_index]);
88+
PSMVector3f error_sample= PSM_Vector3fAbs(&calibrated_omega_samples[sample_index]);
7489
PSMVector3f diff_from_mean= PSM_Vector3fSubtract(&error_sample, &mean_omega_error);
7590
PSMVector3f diff_from_mean_sqrd= PSM_Vector3fSquare(&diff_from_mean);
7691

@@ -79,12 +94,12 @@ struct GyroscopeNoiseSamples
7994
var_omega= PSM_Vector3fUnsafeScalarDivide(&var_omega, N - 1);
8095

8196
// Use the max variance of all three axes (should be close)
82-
variance= PSM_Vector3fMaxValue(&var_omega);
97+
angular_drift_variance= PSM_Vector3fMaxValue(&var_omega);
8398

8499
// Compute the max drift rate we got across a three axis
85100
PSMVector3f drift_rate= PSM_Vector3fUnsafeScalarDivide(&drift_rotation, sampleDurationSeconds);
86101
PSMVector3f drift_rate_abs= PSM_Vector3fAbs(&drift_rate);
87-
drift= PSM_Vector3fMaxValue(&drift_rate_abs);
102+
angular_drift_rate= PSM_Vector3fMaxValue(&drift_rate_abs);
88103
}
89104
};
90105

@@ -274,7 +289,8 @@ void AppStage_GyroscopeCalibration::update()
274289
// Record the next noise sample
275290
if (m_gyroNoiseSamples->sample_count < k_desired_noise_sample_count)
276291
{
277-
m_gyroNoiseSamples->omega_samples[m_gyroNoiseSamples->sample_count]= m_lastCalibratedGyroscope;
292+
m_gyroNoiseSamples->raw_gyro_samples[m_gyroNoiseSamples->sample_count]= m_lastRawGyroscope;
293+
m_gyroNoiseSamples->calibrated_omega_samples[m_gyroNoiseSamples->sample_count]= m_lastCalibratedGyroscope;
278294
++m_gyroNoiseSamples->sample_count;
279295
}
280296

@@ -286,8 +302,9 @@ void AppStage_GyroscopeCalibration::update()
286302

287303
// Update the gyro config on the service
288304
request_set_gyroscope_calibration(
289-
m_gyroNoiseSamples->drift,
290-
m_gyroNoiseSamples->variance);
305+
m_gyroNoiseSamples->raw_bias,
306+
m_gyroNoiseSamples->angular_drift_rate,
307+
m_gyroNoiseSamples->angular_drift_variance);
291308

292309
setState(eCalibrationMenuState::measureComplete);
293310
}
@@ -714,6 +731,7 @@ void AppStage_GyroscopeCalibration::handle_tracking_space_settings_response(
714731
}
715732

716733
void AppStage_GyroscopeCalibration::request_set_gyroscope_calibration(
734+
const PSMVector3f &raw_bias,
717735
const float drift,
718736
const float variance)
719737
{
@@ -725,6 +743,9 @@ void AppStage_GyroscopeCalibration::request_set_gyroscope_calibration(
725743

726744
calibration->set_controller_id(m_controllerView->ControllerID);
727745

746+
calibration->mutable_raw_bias()->set_i(raw_bias.x);
747+
calibration->mutable_raw_bias()->set_j(raw_bias.y);
748+
calibration->mutable_raw_bias()->set_k(raw_bias.z);
728749
calibration->set_drift(drift);
729750
calibration->set_variance(variance);
730751
calibration->set_gyro_gain_setting(""); // keep existing gain

‎src/psmoveconfigtool/AppStage_GyroscopeCalibration.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,8 @@ class AppStage_GyroscopeCalibration : public AppStage
5555
static void handle_tracking_space_settings_response(
5656
const PSMResponseMessage *response_message,
5757
void *userdata);
58-
void request_set_gyroscope_calibration(const float raw_drift, const float raw_variance);
58+
void request_set_gyroscope_calibration(
59+
const PSMVector3f &raw_bias, const float raw_drift, const float raw_variance);
5960
static void handle_acquire_controller(
6061
const PSMResponseMessage *response,
6162
void *userdata);

‎src/psmovemath/MathEigen.cpp‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,26 @@ const Eigen::Quaternionf g_eigen_quaternion_zero = Eigen::Quaternionf( 0, 0, 0,
1212
const Eigen::Quaternionf *k_eigen_quaternion_zero = &g_eigen_quaternion_zero;
1313

1414
//-- public methods -----
15+
Eigen::Quaternionf
16+
eigen_quaternion_from_ZY(
17+
const Eigen::Vector3f &in_Z,
18+
const Eigen::Vector3f &in_Y)
19+
{
20+
Eigen::Vector3f Z= in_Z;
21+
22+
eigen_vector3f_normalize_with_default(Z, Eigen::Vector3f::UnitZ());
23+
Eigen::Vector3f X= Z.cross(in_Y);
24+
eigen_vector3f_normalize_with_default(X, Eigen::Vector3f::UnitX());
25+
Eigen::Vector3f Y= X.cross(Z);
26+
27+
Eigen::Matrix3f mat;
28+
mat.col(0)= X;
29+
mat.col(1)= Y;
30+
mat.col(2)= Z;
31+
32+
return Eigen::Quaternionf(mat).normalized();
33+
}
34+
1535
// Creates a quaternion that rotates clockwise about the axis for a positive angle
1636
// when appied with psmove_vector_clockwise_rotate()
1737
Eigen::Quaternionf

‎src/psmovemath/MathEigen.h‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -70,9 +70,7 @@ namespace Eigen
7070

7171
//-- interface -----
7272
Eigen::Quaternionf
73-
eigen_quaternion_from_forward_up(
74-
const Eigen::Vector3f &forward,
75-
const Eigen::Vector3f &up);
73+
eigen_quaternion_from_ZY(const Eigen::Vector3f &Z, const Eigen::Vector3f &Y);
7674

7775
// Creates a quaternion that rotates clockwise about the axis for a positive angle
7876
// when appied with psmove_vector_clockwise_rotate()

‎src/psmoveprotocol/PSMoveProtocol.proto‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -255,11 +255,13 @@ message Request {
255255
// Parameters for SET_CONTROLLER_GYROSCOPE_CALIBRATION
256256
message RequestSetControllerGyroscopeCalibration {
257257
int32 controller_id = 1;
258-
float variance= 2;
259-
float drift= 3;
260-
string gyro_gain_setting= 4;
258+
FloatVector raw_bias= 2;
259+
float variance= 3;
260+
float drift= 4;
261+
string gyro_gain_setting= 5;
261262
}
262263
RequestSetControllerGyroscopeCalibration set_controller_gyroscope_calibration_request = 15;
264+
263265
// Parameters for SET_OPTICAL_NOISE_CALIBRATION
264266
message RequestSetOpticalNoiseCalibration {
265267
int32 controller_id = 1;

‎src/psmoveprotocol/ProtocolVersion.h‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,8 @@
1212
#define PSM_RELEASE_VERSION_PRODUCT 0
1313
#define PSM_RELEASE_VERSION_MAJOR 9
1414
#define PSM_RELEASE_VERSION_PHASE alpha
15-
#define PSM_RELEASE_VERSION_MINOR 8
16-
#define PSM_RELEASE_VERSION_RELEASE 11
15+
#define PSM_RELEASE_VERSION_MINOR 9
16+
#define PSM_RELEASE_VERSION_RELEASE 0
1717
#define PSM_RELEASE_VERSION_HOTFIX 0
1818

1919
/// "Product.Major-Phase Minor.Release.Hotfix"
@@ -25,8 +25,8 @@
2525
#define PSM_PROTOCOL_VERSION_PRODUCT 0
2626
#define PSM_PROTOCOL_VERSION_MAJOR 9
2727
#define PSM_PROTOCOL_VERSION_PHASE alpha
28-
#define PSM_PROTOCOL_VERSION_MINOR 8
29-
#define PSM_PROTOCOL_VERSION_RELEASE 11
28+
#define PSM_PROTOCOL_VERSION_MINOR 9
29+
#define PSM_PROTOCOL_VERSION_RELEASE 0
3030
#define PSM_PROTOCOL_VERSION_HOTFIX 0
3131

3232
/// "Product.Major-Phase Minor.Release.Hotfix"

‎src/psmoveservice/CMakeLists.txt‎

Lines changed: 33 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -69,6 +69,27 @@ file(GLOB PSMOVESERVICE_FILTER_SRC
6969
)
7070
source_group("Filter" FILES ${PSMOVESERVICE_FILTER_SRC})
7171

72+
list(APPEND PSMOVESERVICE_PLATFORM_SRC
73+
${CMAKE_CURRENT_LIST_DIR}/Platform/BluetoothQueries.h
74+
${CMAKE_CURRENT_LIST_DIR}/Platform/BluetoothRequests.h
75+
${CMAKE_CURRENT_LIST_DIR}/Platform/BluetoothRequests.cpp)
76+
IF(${CMAKE_SYSTEM_NAME} MATCHES "Windows")
77+
list(APPEND PSMOVESERVICE_PLATFORM_SRC
78+
${CMAKE_CURRENT_LIST_DIR}/Platform/BluetoothRequestsWin32.cpp
79+
${CMAKE_CURRENT_LIST_DIR}/Platform/BluetoothQueriesWin32.cpp
80+
${CMAKE_CURRENT_LIST_DIR}/Platform/PlatformDeviceAPIWin32.h
81+
${CMAKE_CURRENT_LIST_DIR}/Platform/PlatformDeviceAPIWin32.cpp)
82+
ELSEIF(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
83+
list(APPEND PSMOVESERVICE_PLATFORM_SRC
84+
${CMAKE_CURRENT_LIST_DIR}/Platform/BluetoothRequestsOSX.mm
85+
${CMAKE_CURRENT_LIST_DIR}/Platform/BluetoothQueriesOSX.mm)
86+
ELSE()
87+
list(APPEND PSMOVESERVICE_PLATFORM_SRC
88+
${CMAKE_CURRENT_LIST_DIR}/Platform/BluetoothRequestsLinux.cpp
89+
${CMAKE_CURRENT_LIST_DIR}/Platform/BluetoothQueriesLinux.cpp)
90+
ENDIF()
91+
source_group("Platform" FILES ${PSMOVESERVICE_PLATFORM_SRC})
92+
7293
file(GLOB PSMOVESERVICE_SERVER_SRC
7394
"${CMAKE_CURRENT_LIST_DIR}/Server/*.cpp"
7495
"${CMAKE_CURRENT_LIST_DIR}/Server/*.h"
@@ -83,6 +104,12 @@ file(GLOB PSMOVESERVICE_TRACKER_SRC
83104
)
84105
source_group("Tracker" FILES ${PSMOVESERVICE_TRACKER_SRC})
85106

107+
file(GLOB PSMOVESERVICE_UTILS_SRC
108+
"${CMAKE_CURRENT_LIST_DIR}/Utils/*.cpp"
109+
"${CMAKE_CURRENT_LIST_DIR}/Utils/*.h"
110+
)
111+
source_group("Utils" FILES ${PSMOVESERVICE_UTILS_SRC})
112+
86113
set(PSMOVESERVICE_SRC
87114
${PSMOVESERVICE_CONFIG_SRC}
88115
${PSMOVESERVICE_CONTROLLER_SRC}
@@ -93,8 +120,10 @@ set(PSMOVESERVICE_SRC
93120
${PSMOVESERVICE_DEVICE_VIEW_SRC}
94121
${PSMOVESERVICE_HMD_SRC}
95122
${PSMOVESERVICE_FILTER_SRC}
123+
${PSMOVESERVICE_PLATFORM_SRC}
96124
${PSMOVESERVICE_SERVER_SRC}
97125
${PSMOVESERVICE_TRACKER_SRC}
126+
${PSMOVESERVICE_UTILS_SRC}
98127
)
99128

100129
list(APPEND PSMOVE_SERVICE_INCL_DIRS
@@ -114,9 +143,13 @@ list(APPEND PSMOVE_SERVICE_INCL_DIRS
114143
${CMAKE_CURRENT_LIST_DIR}/PSMoveTracker
115144
${CMAKE_CURRENT_LIST_DIR}/PSMoveTracker/PSEye
116145
${CMAKE_CURRENT_LIST_DIR}/Server
146+
${CMAKE_CURRENT_LIST_DIR}/Utils
117147
${CMAKE_CURRENT_LIST_DIR}/VirtualController
118148
)
119149

150+
# Lockfree Queue
151+
list(APPEND PSMOVE_SERVICE_INCL_DIRS ${ROOT_DIR}/thirdparty/lockfreequeue)
152+
120153
# Eigen math library
121154
list(APPEND PSMOVE_SERVICE_INCL_DIRS ${EIGEN3_INCLUDE_DIR})
122155

@@ -143,24 +176,6 @@ list(APPEND PSMOVE_SERVICE_INCL_DIRS ${HIDAPI_INCLUDE_DIRS})
143176
list(APPEND PSMOVESERVICE_SRC ${HIDAPI_SRC})
144177
list(APPEND PSMOVE_SERVICE_REQ_LIBS ${HIDAPI_LIBS})
145178

146-
# bluetooth
147-
list(APPEND PSMOVESERVICE_SRC
148-
${CMAKE_CURRENT_LIST_DIR}/Platform/BluetoothRequests.h
149-
${CMAKE_CURRENT_LIST_DIR}/Platform/BluetoothQueries.h)
150-
IF(${CMAKE_SYSTEM_NAME} MATCHES "Windows")
151-
list(APPEND PSMOVESERVICE_SRC
152-
${CMAKE_CURRENT_LIST_DIR}/Platform/BluetoothRequestsWin32.cpp
153-
${CMAKE_CURRENT_LIST_DIR}/Platform/BluetoothQueriesWin32.cpp)
154-
ELSEIF(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
155-
list(APPEND PSMOVESERVICE_SRC
156-
${CMAKE_CURRENT_LIST_DIR}/Platform/BluetoothRequestsOSX.mm
157-
${CMAKE_CURRENT_LIST_DIR}/Platform/BluetoothQueriesOSX.mm)
158-
ELSE()
159-
list(APPEND PSMOVESERVICE_SRC
160-
${CMAKE_CURRENT_LIST_DIR}/Platform/BluetoothRequestsLinux.cpp
161-
${CMAKE_CURRENT_LIST_DIR}/Platform/BluetoothQueriesLinux.cpp)
162-
ENDIF()
163-
164179
# LibUSB for device management
165180
find_package(USB1 REQUIRED)
166181
list(APPEND PSMOVE_SERVICE_INCL_DIRS ${LIBUSB_INCLUDE_DIR})
@@ -170,17 +185,6 @@ list(APPEND PSMOVE_SERVICE_REQ_LIBS ${LIBUSB_LIBRARIES})
170185
list(APPEND PSMOVE_SERVICE_INCL_DIRS ${LIBSTEM_GAMEPAD_INCLUDE_DIRS})
171186
list(APPEND PSMOVESERVICE_SRC ${LIBSTEM_GAMEPAD_SRC})
172187

173-
# Platform Specific Device Management
174-
IF(${CMAKE_SYSTEM_NAME} MATCHES "Windows")
175-
# Windows utilities for querying driver infomation (provider name)
176-
list(APPEND PSMOVE_SERVICE_INCL_DIRS ${ROOT_DIR}/src/psmoveservice/Platform)
177-
list(APPEND PSMOVESERVICE_SRC
178-
${CMAKE_CURRENT_LIST_DIR}/Platform/PlatformDeviceAPIWin32.h
179-
${CMAKE_CURRENT_LIST_DIR}/Platform/PlatformDeviceAPIWin32.cpp)
180-
ELSEIF(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
181-
ELSE()
182-
ENDIF()
183-
184188
# PSMoveDataFrame
185189
list(APPEND PSMOVE_SERVICE_INCL_DIRS ${ROOT_DIR}/src/psmoveprotocol/)
186190
list(APPEND PSMOVE_SERVICE_REQ_LIBS PSMoveProtocol)

0 commit comments

Comments
 (0)