From d65ed69eb7037b946c3e1567a363d668adcddaf6 Mon Sep 17 00:00:00 2001 From: HipsterSloth Date: Mon, 24 Sep 2018 22:32:34 -0700 Subject: [PATCH 1/4] Adding support for pairing the ZCM2 PSMove controller in Windows * Added "authenticate controller" step using BluetoothAuthenticateDeviceEx --- src/platform/psmove_port_windows.c | 189 ++++++++++++++++++++++++++++- 1 file changed, 183 insertions(+), 6 deletions(-) diff --git a/src/platform/psmove_port_windows.c b/src/platform/psmove_port_windows.c index f8758046..b27d0fe1 100644 --- a/src/platform/psmove_port_windows.c +++ b/src/platform/psmove_port_windows.c @@ -294,6 +294,163 @@ is_connection_established(const HANDLE hRadio, BLUETOOTH_DEVICE_INFO *device_inf return 1; } +struct BluetoothAuthenticationCallbackState +{ + HANDLE hRadio; + BLUETOOTH_DEVICE_INFO *deviceInfo; + HANDLE hAuthenticationCompleteEvent; +}; + +static BOOL CALLBACK +bluetooth_auth_callback( + LPVOID pvParam, + PBLUETOOTH_AUTHENTICATION_CALLBACK_PARAMS pAuthCallbackParams) +{ + struct BluetoothAuthenticationCallbackState *state= (struct BluetoothAuthenticationCallbackState *)pAuthCallbackParams; + + BLUETOOTH_AUTHENTICATE_RESPONSE AuthRes; + memset(&AuthRes, 0, sizeof(BLUETOOTH_AUTHENTICATE_RESPONSE)); + AuthRes.authMethod = pAuthCallbackParams->authenticationMethod; + AuthRes.bthAddressRemote = pAuthCallbackParams->deviceInfo.Address; + AuthRes.negativeResponse = 0; + + // Send authentication response to authenticate device + DWORD dwRet = BluetoothSendAuthenticationResponseEx(state->hRadio, &AuthRes); + if (dwRet == ERROR_SUCCESS) + { + // Flag the device as authenticated + WINPAIR_DEBUG("Bluetooth device authenticated!"); + state->deviceInfo->fAuthenticated = TRUE; + } + else + { + if (dwRet == ERROR_CANCELLED) + { + WINPAIR_DEBUG("Bluetooth device denied passkey response"); + } + else if (dwRet == E_FAIL) + { + WINPAIR_DEBUG("Failure during authentication"); + } + else if (dwRet == ERROR_NOT_READY) + { + WINPAIR_DEBUG("Device not ready"); + } + else if (dwRet == ERROR_INVALID_PARAMETER) + { + WINPAIR_DEBUG("Invalid parameter"); + } + else if (dwRet == 1244) + { + WINPAIR_DEBUG("Not authenticated"); + } + else + { + WINPAIR_DEBUG("BluetoothSendAuthenticationResponseEx failed: %d", GetLastError()); + } + } + + // Signal the thread that the authentication callback completed + if (!SetEvent(state->hAuthenticationCompleteEvent)) + { + WINPAIR_DEBUG("Failed to set event: %d", GetLastError()); + } + + return TRUE; +} + +static int +authenticate_controller( + HANDLE hRadio, + BLUETOOTH_DEVICE_INFO *device_info) +{ + int ret; + + if (device_info->fAuthenticated == 0) + { + struct BluetoothAuthenticationCallbackState callbackState; + callbackState.deviceInfo= device_info; + callbackState.hAuthenticationCompleteEvent= INVALID_HANDLE_VALUE; + callbackState.hRadio= hRadio; + + // Register authentication callback before starting authentication request + HBLUETOOTH_AUTHENTICATION_REGISTRATION hRegHandle = 0; + DWORD dwRet = BluetoothRegisterForAuthenticationEx( + device_info, &hRegHandle, &bluetooth_auth_callback, &callbackState); + + if (dwRet == ERROR_SUCCESS) + { + callbackState.hAuthenticationCompleteEvent = CreateEvent( + NULL, // default security attributes + TRUE, // manual-reset event + FALSE, // initial state is non-signaled + TEXT("AuthenticationCompleteEvent")); // object name + + // Start the authentication request + dwRet = BluetoothAuthenticateDeviceEx( + NULL, + hRadio, + device_info, + NULL, + MITMProtectionNotRequiredBonding); + + if (dwRet == ERROR_NO_MORE_ITEMS) + { + WINPAIR_DEBUG("Already paired."); + ret= 0; + } + else if (dwRet == ERROR_CANCELLED) + { + WINPAIR_DEBUG("User canceled the authentication."); + ret= 1; + } + else if (dwRet == ERROR_INVALID_PARAMETER) + { + WINPAIR_DEBUG("Invalid parameter!"); + ret= 1; + } + else + { + // Block on authentication completing + WaitForSingleObject(callbackState.hAuthenticationCompleteEvent, INFINITE); + + if (device_info->fAuthenticated) + { + WINPAIR_DEBUG("Successfully paired."); + ret= 0; + } + else + { + WINPAIR_DEBUG("Failed to authenticate!"); + ret= 1; + } + } + + if (callbackState.hAuthenticationCompleteEvent != INVALID_HANDLE_VALUE) + { + CloseHandle(callbackState.hAuthenticationCompleteEvent); + } + } + else + { + WINPAIR_DEBUG("BluetoothRegisterForAuthenticationEx failed!"); + ret= 1; + } + + if (hRegHandle != 0) + { + BluetoothUnregisterAuthentication(hRegHandle); + hRegHandle= 0; + } + } + else + { + WINPAIR_DEBUG("Already authenticated."); + ret= 0; + } + + return ret; +} static int patch_registry(const BLUETOOTH_ADDRESS *move_addr, const BLUETOOTH_ADDRESS *radio_addr) @@ -368,7 +525,7 @@ is_windows8_or_later() static void -handle_windows8_and_later(const BLUETOOTH_ADDRESS *move_addr, const BLUETOOTH_ADDRESS *radio_addr, const HANDLE hRadio) +handle_windows8_and_later(const BLUETOOTH_ADDRESS *move_addr, const BLUETOOTH_ADDRESS *radio_addr, const HANDLE hRadio, enum PSMove_Model_Type model) { unsigned int scan = 0; int connected = 0; @@ -380,6 +537,16 @@ handle_windows8_and_later(const BLUETOOTH_ADDRESS *move_addr, const BLUETOOTH_AD if (is_move_motion_controller(&device_info)) { WINPAIR_DEBUG("Found Move Motion Controller matching the given address"); + if (model == Model_ZCM2) + { + if (authenticate_controller(hRadio, &device_info) != 0) + { + WINPAIR_DEBUG("Failed to authenticate the controller"); + Sleep(SLEEP_BETWEEN_SCANS); + continue; + } + } + unsigned int conn_try; for (conn_try = 1; conn_try <= CONN_RETRIES; conn_try++) { WINPAIR_DEBUG("Connection try %d/%d", conn_try, CONN_RETRIES); @@ -441,7 +608,7 @@ handle_windows8_and_later(const BLUETOOTH_ADDRESS *move_addr, const BLUETOOTH_AD static void -handle_windows_pre8(const BLUETOOTH_ADDRESS *move_addr, const BLUETOOTH_ADDRESS *radio_addr, const HANDLE hRadio) +handle_windows_pre8(const BLUETOOTH_ADDRESS *move_addr, const BLUETOOTH_ADDRESS *radio_addr, const HANDLE hRadio, enum PSMove_Model_Type model) { int connected = 0; while (!connected) { @@ -452,6 +619,16 @@ handle_windows_pre8(const BLUETOOTH_ADDRESS *move_addr, const BLUETOOTH_ADDRESS if (is_move_motion_controller(&device_info)) { WINPAIR_DEBUG("Found Move Motion Controller matching the given address"); + if (model == Model_ZCM2) + { + if (authenticate_controller(hRadio, &device_info) != 0) + { + WINPAIR_DEBUG("Failed to authenticate the controller"); + Sleep(SLEEP_BETWEEN_SCANS); + continue; + } + } + if (device_info.fConnected) { /* enable HID service only if necessary */ WINPAIR_DEBUG("Checking HID service ..."); @@ -483,7 +660,7 @@ handle_windows_pre8(const BLUETOOTH_ADDRESS *move_addr, const BLUETOOTH_ADDRESS static int -windows_register_psmove(const char *move_addr_str, const BLUETOOTH_ADDRESS *radio_addr, const HANDLE hRadio) +windows_register_psmove(const char *move_addr_str, const BLUETOOTH_ADDRESS *radio_addr, const HANDLE hRadio, enum PSMove_Model_Type model) { /* parse controller's Bluetooth device address string */ BLUETOOTH_ADDRESS *move_addr = string_to_btaddr(move_addr_str); @@ -512,10 +689,10 @@ windows_register_psmove(const char *move_addr_str, const BLUETOOTH_ADDRESS *radi if (is_windows8_or_later()) { WINPAIR_DEBUG("Dealing with Windows 8 or later"); - handle_windows8_and_later(move_addr, radio_addr, hRadio); + handle_windows8_and_later(move_addr, radio_addr, hRadio, model); } else { WINPAIR_DEBUG("Dealing with Windows version older than 8"); - handle_windows_pre8(move_addr, radio_addr, hRadio); + handle_windows_pre8(move_addr, radio_addr, hRadio, model); } free(move_addr); @@ -688,7 +865,7 @@ psmove_port_register_psmove(const char *addr, const char *host, enum PSMove_Mode return PSMove_False; } - int res = windows_register_psmove(addr, &radioInfo.address, hRadio); + int res = windows_register_psmove(addr, &radioInfo.address, hRadio, model); CloseHandle(hRadio); return (res == 0) ? PSMove_True : PSMove_False; From 92037b3d3ea78dc15512175811790fad38c4bed2 Mon Sep 17 00:00:00 2001 From: HipsterSloth Date: Mon, 24 Sep 2018 23:00:05 -0700 Subject: [PATCH 2/4] Finishing support for ZCM2 PSMove controller * Parse gyro and accelerometer calibration values (new calibration format for ZCM2) * Separated ZCM1 vs ZCM2 input packet handling cases - psmove_connect_internal (magnetometer initialization) - psmove_poll - psmove_get_ext_data - psmove_get_buttons - psmove_is_ext_connected - psmove_send_ext_data - psmove_get_temperature - psmove_get_trigger - psmove_get_half_frame - psmove_get_accelerometer - psmove_get_gyroscope - psmove_get_magnetometer * psmove_orientation_new will default OrientationFusion_MadgwickIMU for the ZCM2 (since it doesn't have a magnetometer) --- include/psmove.h | 15 ++ src/psmove.c | 401 +++++++++++++++++++++++++++++-------- src/psmove_calibration.c | 297 +++++++++++++++++++++------ src/psmove_orientation.cpp | 12 +- src/psmove_private.h | 24 ++- 5 files changed, 598 insertions(+), 151 deletions(-) diff --git a/include/psmove.h b/include/psmove.h index 58daa627..e16e9b2e 100644 --- a/include/psmove.h +++ b/include/psmove.h @@ -380,6 +380,21 @@ ADDCALL psmove_is_remote(PSMove *move); ADDAPI char * ADDCALL psmove_get_serial(PSMove *move); +/** + * \brief Get the model type (ZCM1 or ZCM2) of a controller. + * + * There are two primary version of the PSMove controller. + * The ZCM1 is the original PSMove controller that shipped with the PS3. + * The ZCM2 is the newer PSMove controller that shipped with the PS4. + * The most important difference is that ZCM2 does not have a magnetometer. + * + * \param move A valid \ref PSMove handle + * + * \return The \ref PSMove_Model_Type type of the controller + **/ +ADDAPI enum PSMove_Model_Type +ADDCALL psmove_get_model(PSMove *move); + /** * \brief Pair a controller connected via USB with the computer. * diff --git a/src/psmove.c b/src/psmove.c index 659fdea7..cdd1e237 100644 --- a/src/psmove.c +++ b/src/psmove.c @@ -135,6 +135,16 @@ psmove_decode_16bit(char *data, int offset) return (low | (high << 8)) - 0x8000; } +static inline int +psmove_decode_16bit_twos_compliment(char *data, int offset) +{ + unsigned char low = data[offset] & 0xFF; + unsigned char high = (data[offset+1]) & 0xFF; + int value= (low | (high << 8)); + + return (value & 0x8000) ? (-(~value & 0xFFFF) + 1) : value; +} + #define NUM_PSMOVE_PIDS \ ((sizeof(PSMOVE_PIDS) / sizeof(PSMOVE_PIDS[0])) - 1) @@ -191,6 +201,61 @@ typedef struct { unsigned char mZlow; /* magnetometer Z (bits 8-1) */ unsigned char timelow; /* low byte of timestamp */ unsigned char extdata[PSMOVE_EXT_DATA_BUF_SIZE]; /* external device data (EXT port) */ +} PSMove_ZCM1_Data_Input; + +typedef struct { + unsigned char type; /* message type, must be PSMove_Req_GetInput */ + unsigned char buttons1; + unsigned char buttons2; + unsigned char buttons3; + unsigned char buttons4; + unsigned char trigger; /* trigger value; 0..255 */ + unsigned char trigger2; /* trigger value, 2nd frame */ + unsigned char _unk7; + unsigned char _unk8; + unsigned char _unk9; + unsigned char _unk10; + unsigned char timehigh; /* high byte of timestamp */ + unsigned char battery; /* battery level; 0x05 = max, 0xEE = USB charging */ + unsigned char aXlow; /* low byte of accelerometer X value */ + unsigned char aXhigh; /* high byte of accelerometer X value */ + unsigned char aYlow; + unsigned char aYhigh; + unsigned char aZlow; + unsigned char aZhigh; + unsigned char aXlow2; /* low byte of accelerometer X value, 2nd frame */ + unsigned char aXhigh2; /* high byte of accelerometer X value, 2nd frame */ + unsigned char aYlow2; + unsigned char aYhigh2; + unsigned char aZlow2; + unsigned char aZhigh2; + unsigned char gXlow; /* low byte of gyro X value */ + unsigned char gXhigh; /* high byte of gyro X value */ + unsigned char gYlow; + unsigned char gYhigh; + unsigned char gZlow; + unsigned char gZhigh; + unsigned char gXlow2; /* low byte of gyro X value, 2nd frame */ + unsigned char gXhigh2; /* high byte of gyro X value, 2nd frame */ + unsigned char gYlow2; + unsigned char gYhigh2; + unsigned char gZlow2; + unsigned char gZhigh2; + unsigned char temphigh; /* temperature (bits 12-5) */ + unsigned char templow; /* tempature (bits 4-1); */ + unsigned char timehigh2; /* same as timestamp at offsets 0x0B */ + unsigned char timelow; /* same as timestamp at offsets 0x2B */ + unsigned char _unk41; + unsigned char _unk42; + unsigned char timelow2; + +} PSMove_ZCM2_Data_Input; + +typedef struct { + union { + PSMove_ZCM1_Data_Input zcm1; + PSMove_ZCM2_Data_Input zcm2; + } data; } PSMove_Data_Input; struct _PSMove { @@ -323,14 +388,21 @@ void _psmove_read_data(PSMove *move, unsigned char *data, int length) { assert(data != NULL); - assert(length >= (sizeof(move->input) + 4)); int32_t res = psmove_poll(move); assert(res <= 0xFF); *((int32_t *)data) = res; - memcpy(data + sizeof(int32_t), &(move->input), sizeof(move->input)); + if (move->model == Model_ZCM1) + { + assert(length >= (sizeof(move->input.data.zcm1) + 4)); + memcpy(data + sizeof(int32_t), &(move->input.data.zcm1), sizeof(move->input.data.zcm1)); + } + else if (move->model == Model_ZCM2) + { + memcpy(data + sizeof(int32_t), &(move->input.data.zcm2), sizeof(move->input.data.zcm2)); + } } enum PSMove_Bool @@ -553,8 +625,16 @@ psmove_connect_internal(const wchar_t *serial, const char *path, int id, unsigne move->calibration = psmove_calibration_new(move); move->orientation = psmove_orientation_new(move); - /* Load magnetometer calibration data */ - psmove_load_magnetometer_calibration(move); + if (move->model == Model_ZCM1) + { + /* Load magnetometer calibration data */ + psmove_load_magnetometer_calibration(move); + } + else + { + /* No magnetometer on the ZCM2 */ + psmove_reset_magnetometer_calibration(move); + } return move; } @@ -909,13 +989,13 @@ _psmove_read_btaddrs(PSMove *move, PSMove_Data_BTAddr *host, PSMove_Data_BTAddr } int -_psmove_get_calibration_blob(PSMove *move, char **dest, size_t *size) +_psmove_get_zcm1_calibration_blob(PSMove *move, char **dest, size_t *size) { psmove_return_val_if_fail(move != NULL, 0); psmove_return_val_if_fail(dest != NULL, 0); psmove_return_val_if_fail(size != NULL, 0); - unsigned char calibration[PSMOVE_CALIBRATION_BLOB_SIZE]; + unsigned char calibration[PSMOVE_ZCM1_CALIBRATION_BLOB_SIZE]; unsigned char cal[PSMOVE_CALIBRATION_SIZE]; int res; @@ -963,6 +1043,57 @@ _psmove_get_calibration_blob(PSMove *move, char **dest, size_t *size) return 1; } +int +_psmove_get_zcm2_calibration_blob(PSMove *move, char **dest, size_t *size) +{ + psmove_return_val_if_fail(move != NULL, 0); + psmove_return_val_if_fail(dest != NULL, 0); + psmove_return_val_if_fail(size != NULL, 0); + + unsigned char calibration[PSMOVE_ZCM2_CALIBRATION_BLOB_SIZE]; + + unsigned char cal[PSMOVE_CALIBRATION_SIZE]; + int res; + int x; + + int dest_offset; + int src_offset; + + for (x=0; x<2; x++) { + memset(cal, 0, sizeof(cal)); + cal[0] = PSMove_Req_GetCalibration; + res = hid_get_feature_report(move->handle, cal, sizeof(cal)); +#if defined(__linux) + if(res == -1) { + psmove_WARNING("hid_get_feature_report failed, kernel issue? see %s\n", + "https://github.com/thp/psmoveapi/issues/108"); + } +#endif + psmove_return_val_if_fail(res == PSMOVE_CALIBRATION_SIZE, 0); + + if (cal[1] == 0x00) { + /* First block */ + dest_offset = 0; + src_offset = 0; + } else if (cal[1] == 0x81) { + /* Second block */ + dest_offset = PSMOVE_CALIBRATION_SIZE; + src_offset = 2; + } else { + return 0; + } + + memcpy(calibration+dest_offset, cal+src_offset, + sizeof(cal)-src_offset); + } + + *dest = (char*)malloc(sizeof(calibration)); + memcpy(*dest, calibration, sizeof(calibration)); + *size = sizeof(calibration); + + return 1; +} + char * psmove_get_serial(PSMove *move) { @@ -981,6 +1112,14 @@ psmove_get_serial(PSMove *move) return strdup(move->serial_number); } +enum PSMove_Model_Type +psmove_get_model(PSMove *move) +{ + psmove_return_val_if_fail(move != NULL, Model_ZCM1); + + return move->model; +} + int _psmove_set_btaddr(PSMove *move, PSMove_Data_BTAddr *addr) { @@ -1273,12 +1412,17 @@ psmove_poll(PSMove *move) psmove_return_val_if_fail(move != NULL, 0); /* store old sequence number before reading */ - int oldseq = (move->input.buttons4 & 0x0F); + int oldseq = + (move->model == Model_ZCM1) + ? (move->input.data.zcm1.buttons4 & 0x0F) + : (move->input.data.zcm2.buttons4 & 0x0F); switch (move->type) { case PSMove_HIDAPI: - res = hid_read(move->handle, (unsigned char*)(&(move->input)), - sizeof(move->input)); + if (move->model == Model_ZCM1) + res = hid_read(move->handle, (unsigned char*)(&(move->input.data.zcm1)), sizeof(move->input.data.zcm1)); + else if (move->model == Model_ZCM2) + res = hid_read(move->handle, (unsigned char*)(&(move->input.data.zcm2)), sizeof(move->input.data.zcm2)); break; case PSMove_MOVED: if (moved_client_send(move->client, MOVED_REQ_READ_INPUT, (char)move->remote_id, NULL, 0)) { @@ -1286,10 +1430,18 @@ psmove_poll(PSMove *move) * The input buffer is stored at offset 1 (the first byte * contains the return value of the remote psmove_poll()) **/ - memcpy(&(move->input), move->client->response_buf.read_input.data, sizeof(move->input)); + size_t input_data_size= 0; + if (move->model == Model_ZCM1) { + input_data_size= sizeof(move->input.data.zcm1); + memcpy(&(move->input.data.zcm1), move->client->response_buf.read_input.data, input_data_size); + } + else if (move->model == Model_ZCM2) { + input_data_size= sizeof(move->input.data.zcm2); + memcpy(&(move->input.data.zcm2), move->client->response_buf.read_input.data, input_data_size); + } if (move->client->response_buf.read_input.poll_return_value != 0) { - res = sizeof(move->input); + res = input_data_size; } } break; @@ -1297,16 +1449,25 @@ psmove_poll(PSMove *move) psmove_CRITICAL("Unknown device type"); } - if (res == sizeof(move->input)) { + if ((move->model == Model_ZCM1 && res == sizeof(move->input.data.zcm1)) || + (move->model == Model_ZCM2 && res == sizeof(move->input.data.zcm2))) { /* Sanity check: The first byte should be PSMove_Req_GetInput */ - psmove_return_val_if_fail(move->input.type == PSMove_Req_GetInput, 0); + if (move->model == Model_ZCM1) { + psmove_return_val_if_fail(move->input.data.zcm1.type == PSMove_Req_GetInput, 0); + } + else if (move->model == Model_ZCM2) { + psmove_return_val_if_fail(move->input.data.zcm2.type == PSMove_Req_GetInput, 0); + } /** * buttons4's 4 least significant bits contain the sequence number, * so we add 1 to signal "success" and add the sequence number for * consumers to utilize the data **/ - int seq = (move->input.buttons4 & 0x0F); + int seq = + (move->model == Model_ZCM1) + ? (move->input.data.zcm1.buttons4 & 0x0F) + : (move->input.data.zcm2.buttons4 & 0x0F); if (seq != ((oldseq + 1) % 16)) { psmove_DEBUG("Warning: Dropped frames (seq %d -> %d)\n", oldseq, seq); @@ -1328,10 +1489,18 @@ psmove_get_ext_data(PSMove *move, PSMove_Ext_Data *data) psmove_return_val_if_fail(move != NULL, PSMove_False); psmove_return_val_if_fail(data != NULL, PSMove_False); - assert(sizeof(*data) >= sizeof(move->input.extdata)); + if (move->model == Model_ZCM1) + { + assert(sizeof(*data) >= sizeof(move->input.data.zcm1.extdata)); - memcpy(data, move->input.extdata, sizeof(move->input.extdata)); - return PSMove_True; + memcpy(data, move->input.data.zcm1.extdata, sizeof(move->input.data.zcm1.extdata)); + return PSMove_True; + } + else + { + // EXT data not supported on ZCM2 + return PSMove_False; + } } unsigned int @@ -1382,10 +1551,24 @@ psmove_get_buttons(PSMove *move) * **/ - return ((move->input.buttons2) | - (move->input.buttons1 << 8) | - ((move->input.buttons3 & 0x01) << 16) | - ((move->input.buttons4 & 0xF0) << 13 /* 13 = 17 - 4 */)); + if (move->model == Model_ZCM1) + { + return ((move->input.data.zcm1.buttons2) | + (move->input.data.zcm1.buttons1 << 8) | + ((move->input.data.zcm1.buttons3 & 0x01) << 16) | + ((move->input.data.zcm1.buttons4 & 0xF0) << 13 /* 13 = 17 - 4 */)); + } + else if (move->model == Model_ZCM2) + { + return ((move->input.data.zcm2.buttons2) | + (move->input.data.zcm2.buttons1 << 8) | + ((move->input.data.zcm2.buttons3 & 0x01) << 16) | + ((move->input.data.zcm2.buttons4 & 0xF0) << 13 /* 13 = 17 - 4 */)); + } + else + { + return 0; + } } void @@ -1412,7 +1595,7 @@ psmove_is_ext_connected(PSMove *move) { psmove_return_val_if_fail(move != NULL, PSMove_False); - if((move->input.buttons4 & 0x10) != 0) { + if(move->model == Model_ZCM1 && (move->input.data.zcm1.buttons4 & 0x10) != 0) { return PSMove_True; } @@ -1429,6 +1612,9 @@ psmove_get_ext_device_info(PSMove *move, PSMove_Ext_Device_Info *ext) psmove_return_val_if_fail(move != NULL, PSMove_False); psmove_return_val_if_fail(ext != NULL, PSMove_False); + if (move->model != Model_ZCM1) + return PSMove_False; + /* Send setup Report for the following read operation */ memset(send_buf, 0, sizeof(send_buf)); send_buf[0] = PSMove_Req_SetExtDeviceInfo; @@ -1475,6 +1661,9 @@ psmove_send_ext_data(PSMove *move, const unsigned char *data, unsigned char leng psmove_return_val_if_fail(data != NULL, PSMove_False); psmove_return_val_if_fail(length > 0, PSMove_False); + if (move->model != Model_ZCM1) + return PSMove_False; + if (length > sizeof(send_buf) - 9) { psmove_DEBUG("Data too large for send buffer\n"); return PSMove_False; @@ -1503,7 +1692,12 @@ psmove_get_battery(PSMove *move) { psmove_return_val_if_fail(move != NULL, 0); - return move->input.battery; + if (move->model == Model_ZCM1) + return move->input.data.zcm1.battery; + else if (move->model == Model_ZCM2) + return move->input.data.zcm2.battery; + else + return 0; } int @@ -1521,8 +1715,20 @@ psmove_get_temperature(PSMove *move) * but it seems to default to 0. **/ - return ((move->input.temphigh << 4) | - ((move->input.templow_mXhigh & 0xF0) >> 4)); + if (move->model == Model_ZCM1) + { + return ((move->input.data.zcm1.temphigh << 4) | + ((move->input.data.zcm1.templow_mXhigh & 0xF0) >> 4)); + } + else if (move->model == Model_ZCM2) + { + return ((move->input.data.zcm2.temphigh << 4) | + ((move->input.data.zcm2.templow & 0xF0) >> 4)); + } + else + { + return 0; + } } float @@ -1572,11 +1778,15 @@ psmove_get_trigger(PSMove *move) { psmove_return_val_if_fail(move != NULL, 0); - if (move->model == Model_ZCM2) { - return move->input.trigger; - } else { - return (move->input.trigger + move->input.trigger2) / 2; + if (move->model == Model_ZCM1) { + return (move->input.data.zcm1.trigger + move->input.data.zcm1.trigger2) / 2; } + else if (move->model == Model_ZCM2) { + return move->input.data.zcm2.trigger; + } + else { + return 0; + } } void @@ -1587,34 +1797,63 @@ psmove_get_half_frame(PSMove *move, enum PSMove_Sensor sensor, psmove_return_if_fail(sensor == Sensor_Accelerometer || sensor == Sensor_Gyroscope); psmove_return_if_fail(frame == Frame_FirstHalf || frame == Frame_SecondHalf); - int base; - - switch (sensor) { - case Sensor_Accelerometer: - base = offsetof(PSMove_Data_Input, aXlow); - break; - case Sensor_Gyroscope: - base = offsetof(PSMove_Data_Input, gXlow); - break; - default: - return; - } - - if (frame == Frame_SecondHalf) { - base += 6; - } - - if (x != NULL) { - *x = psmove_decode_16bit((void*)&move->input, base + 0); - } - - if (y != NULL) { - *y = psmove_decode_16bit((void*)&move->input, base + 2); - } - - if (z != NULL) { - *z = psmove_decode_16bit((void*)&move->input, base + 4); - } + if (move->model == Model_ZCM1) { + int base; + + switch (sensor) { + case Sensor_Accelerometer: + base = offsetof(PSMove_ZCM1_Data_Input, aXlow); + break; + case Sensor_Gyroscope: + base = offsetof(PSMove_ZCM1_Data_Input, gXlow); + break; + default: + return; + } + + if (frame == Frame_SecondHalf) { + base += 6; + } + + if (x != NULL) { + *x = psmove_decode_16bit((void*)&move->input.data.zcm1, base + 0); + } + + if (y != NULL) { + *y = psmove_decode_16bit((void*)&move->input.data.zcm1, base + 2); + } + + if (z != NULL) { + *z = psmove_decode_16bit((void*)&move->input.data.zcm1, base + 4); + } + } else { + int base; + + switch (sensor) { + case Sensor_Accelerometer: + base = offsetof(PSMove_ZCM2_Data_Input, aXlow); + break; + case Sensor_Gyroscope: + base = offsetof(PSMove_ZCM2_Data_Input, gXlow); + break; + default: + return; + } + + //NOTE: Only one frame on the ZCM2 + + if (x != NULL) { + *x = psmove_decode_16bit_twos_compliment((void*)&move->input.data.zcm2, base + 0); + } + + if (y != NULL) { + *y = psmove_decode_16bit_twos_compliment((void*)&move->input.data.zcm2, base + 2); + } + + if (z != NULL) { + *z = psmove_decode_16bit_twos_compliment((void*)&move->input.data.zcm2, base + 4); + } + } } void @@ -1624,30 +1863,30 @@ psmove_get_accelerometer(PSMove *move, int *ax, int *ay, int *az) if (move->model == Model_ZCM2) { if (ax != NULL) { - *ax = (int16_t) (move->input.aXlow + (move->input.aXhigh << 8)); + *ax = (int16_t) (move->input.data.zcm2.aXlow + (move->input.data.zcm2.aXhigh << 8)); } if (ay != NULL) { - *ay = (int16_t) (move->input.aYlow + (move->input.aYhigh << 8)); + *ay = (int16_t) (move->input.data.zcm2.aYlow + (move->input.data.zcm2.aYhigh << 8)); } if (az != NULL) { - *az = (int16_t) (move->input.aZlow + (move->input.aZhigh << 8)); + *az = (int16_t) (move->input.data.zcm2.aZlow + (move->input.data.zcm2.aZhigh << 8)); } } else { if (ax != NULL) { - *ax = ((move->input.aXlow + move->input.aXlow2) + - ((move->input.aXhigh + move->input.aXhigh2) << 8)) / 2 - 0x8000; + *ax = ((move->input.data.zcm1.aXlow + move->input.data.zcm1.aXlow2) + + ((move->input.data.zcm1.aXhigh + move->input.data.zcm1.aXhigh2) << 8)) / 2 - 0x8000; } if (ay != NULL) { - *ay = ((move->input.aYlow + move->input.aYlow2) + - ((move->input.aYhigh + move->input.aYhigh2) << 8)) / 2 - 0x8000; + *ay = ((move->input.data.zcm1.aYlow + move->input.data.zcm1.aYlow2) + + ((move->input.data.zcm1.aYhigh + move->input.data.zcm1.aYhigh2) << 8)) / 2 - 0x8000; } if (az != NULL) { - *az = ((move->input.aZlow + move->input.aZlow2) + - ((move->input.aZhigh + move->input.aZhigh2) << 8)) / 2 - 0x8000; + *az = ((move->input.data.zcm1.aZlow + move->input.data.zcm1.aZlow2) + + ((move->input.data.zcm1.aZhigh + move->input.data.zcm1.aZhigh2) << 8)) / 2 - 0x8000; } } } @@ -1659,30 +1898,30 @@ psmove_get_gyroscope(PSMove *move, int *gx, int *gy, int *gz) if (move->model == Model_ZCM2) { if (gx != NULL) { - *gx = (int16_t) (move->input.gXlow + (move->input.gXhigh << 8)); + *gx = (int16_t) (move->input.data.zcm2.gXlow + (move->input.data.zcm2.gXhigh << 8)); } if (gy != NULL) { - *gy = (int16_t) (move->input.gYlow + (move->input.gYhigh << 8)); + *gy = (int16_t) (move->input.data.zcm2.gYlow + (move->input.data.zcm2.gYhigh << 8)); } if (gz != NULL) { - *gz = (int16_t) (move->input.gZlow + (move->input.gZhigh << 8)); + *gz = (int16_t) (move->input.data.zcm2.gZlow + (move->input.data.zcm2.gZhigh << 8)); } } else { if (gx != NULL) { - *gx = ((move->input.gXlow + move->input.gXlow2) + - ((move->input.gXhigh + move->input.gXhigh2) << 8)) / 2 - 0x8000; + *gx = ((move->input.data.zcm1.gXlow + move->input.data.zcm1.gXlow2) + + ((move->input.data.zcm1.gXhigh + move->input.data.zcm1.gXhigh2) << 8)) / 2 - 0x8000; } if (gy != NULL) { - *gy = ((move->input.gYlow + move->input.gYlow2) + - ((move->input.gYhigh + move->input.gYhigh2) << 8)) / 2 - 0x8000; + *gy = ((move->input.data.zcm1.gYlow + move->input.data.zcm1.gYlow2) + + ((move->input.data.zcm1.gYhigh + move->input.data.zcm1.gYhigh2) << 8)) / 2 - 0x8000; } if (gz != NULL) { - *gz = ((move->input.gZlow + move->input.gZlow2) + - ((move->input.gZhigh + move->input.gZhigh2) << 8)) / 2 - 0x8000; + *gz = ((move->input.data.zcm1.gZlow + move->input.data.zcm1.gZlow2) + + ((move->input.data.zcm1.gZhigh + move->input.data.zcm1.gZhigh2) << 8)) / 2 - 0x8000; } } } @@ -1814,18 +2053,18 @@ psmove_get_magnetometer(PSMove *move, int *mx, int *my, int *mz) } } else { if (mx != NULL) { - *mx = TWELVE_BIT_SIGNED(((move->input.templow_mXhigh & 0x0F) << 8) | - move->input.mXlow); + *mx = TWELVE_BIT_SIGNED(((move->input.data.zcm1.templow_mXhigh & 0x0F) << 8) | + move->input.data.zcm1.mXlow); } if (my != NULL) { - *my = TWELVE_BIT_SIGNED((move->input.mYhigh << 4) | - (move->input.mYlow_mZhigh & 0xF0) >> 4); + *my = TWELVE_BIT_SIGNED((move->input.data.zcm1.mYhigh << 4) | + (move->input.data.zcm1.mYlow_mZhigh & 0xF0) >> 4); } if (mz != NULL) { - *mz = TWELVE_BIT_SIGNED(((move->input.mYlow_mZhigh & 0x0F) << 8) | - move->input.mZlow); + *mz = TWELVE_BIT_SIGNED(((move->input.data.zcm1.mYlow_mZhigh & 0x0F) << 8) | + move->input.data.zcm1.mZlow); } } } diff --git a/src/psmove_calibration.c b/src/psmove_calibration.c index b887a4d1..134df24b 100644 --- a/src/psmove_calibration.c +++ b/src/psmove_calibration.c @@ -48,7 +48,7 @@ enum _PSMoveCalibrationFlag { struct _PSMoveCalibration { PSMove *move; - char usb_calibration[PSMOVE_CALIBRATION_BLOB_SIZE]; + char usb_calibration[PSMOVE_MAX_CALIBRATION_BLOB_SIZE]; int flags; char *filename; @@ -69,6 +69,9 @@ struct _PSMoveCalibration { /* Pre-calculated factors for gyroscope mapping */ float gx, gy, gz; + + /* Pre-calculated raw drift values for gyroscope mapping */ + int dx, dy, dz; }; @@ -108,13 +111,21 @@ psmove_calibration_save(PSMoveCalibration *calibration); int -psmove_calibration_decode(char *data, int offset) +psmove_calibration_decode_16bit_unsigned(char *data, int offset) { unsigned char low = data[offset] & 0xFF; unsigned char high = (data[offset+1]) & 0xFF; return (low | (high << 8)) - 0x8000; } +short +psmove_calibration_decode_16bit_signed(char *data, int offset) +{ + unsigned short low = data[offset] & 0xFF; + unsigned short high = (data[offset+1]) & 0xFF; + return (short)(low | (high << 8)); +} + unsigned int psmove_calibration_decode_12bits(char *data, int offset) { @@ -135,7 +146,7 @@ psmove_calibration_decode_float(char *data, int offset) void -psmove_calibration_parse_usb(PSMoveCalibration *calibration) +psmove_ZCM1_calibration_parse_usb(PSMoveCalibration *calibration) { assert(calibration != NULL); char *data = calibration->usb_calibration; @@ -150,9 +161,9 @@ psmove_calibration_parse_usb(PSMoveCalibration *calibration) t = psmove_calibration_decode_12bits(data, 0x02); printf("# Temperature: 0x%04X (%.0f °C)\n", t, _psmove_temperature_to_celsius(t)); for (orientation=0; orientation<6; orientation++) { - x = psmove_calibration_decode(data, 0x04 + 6*orientation); - y = psmove_calibration_decode(data, 0x04 + 6*orientation + 2); - z = psmove_calibration_decode(data, 0x04 + 6*orientation + 4); + x = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6*orientation); + y = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6*orientation + 2); + z = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6*orientation + 4); printf("# Orientation #%d: (%5d | %5d | %5d)\n", orientation, x, y, z); } @@ -161,27 +172,27 @@ psmove_calibration_parse_usb(PSMoveCalibration *calibration) t = psmove_calibration_decode_12bits(data, 0x42); printf("# Temperature: 0x%04X (%.0f °C)\n", t, _psmove_temperature_to_celsius(t)); for (orientation=0; orientation<3; orientation++) { - x = psmove_calibration_decode(data, 0x46 + 8*orientation); - y = psmove_calibration_decode(data, 0x46 + 8*orientation + 2); - z = psmove_calibration_decode(data, 0x46 + 8*orientation + 4); + x = psmove_calibration_decode_16bit_unsigned(data, 0x46 + 8*orientation); + y = psmove_calibration_decode_16bit_unsigned(data, 0x46 + 8*orientation + 2); + z = psmove_calibration_decode_16bit_unsigned(data, 0x46 + 8*orientation + 4); printf("# Gyro %c, 80 rpm: (%5d | %5d | %5d)\n", "XYZ"[orientation], x, y, z); } printf("\n"); t = psmove_calibration_decode_12bits(data, 0x28); - x = psmove_calibration_decode(data, 0x2a); - y = psmove_calibration_decode(data, 0x2a + 2); - z = psmove_calibration_decode(data, 0x2a + 4); + x = psmove_calibration_decode_16bit_unsigned(data, 0x2a); + y = psmove_calibration_decode_16bit_unsigned(data, 0x2a + 2); + z = psmove_calibration_decode_16bit_unsigned(data, 0x2a + 4); printf("# Temperature: 0x%04X (%.0f °C)\n", t, _psmove_temperature_to_celsius(t)); printf("# Gyro, 0 rpm (@0x2a): (%5d | %5d | %5d)\n", x, y, z); printf("\n"); t = psmove_calibration_decode_12bits(data, 0x30); - x = psmove_calibration_decode(data, 0x32); - y = psmove_calibration_decode(data, 0x32 + 2); - z = psmove_calibration_decode(data, 0x32 + 4); + x = psmove_calibration_decode_16bit_unsigned(data, 0x32); + y = psmove_calibration_decode_16bit_unsigned(data, 0x32 + 2); + z = psmove_calibration_decode_16bit_unsigned(data, 0x32 + 4); printf("# Temperature: 0x%04X (%.0f °C)\n", t, _psmove_temperature_to_celsius(t)); printf("# Gyro, 0 rpm (@0x32): (%5d | %5d | %5d)\n", x, y, z); @@ -206,6 +217,42 @@ psmove_calibration_parse_usb(PSMoveCalibration *calibration) printf("# float @0x7a: %f\n", psmove_calibration_decode_float(data, 0x7a)); } +void +psmove_ZCM2_calibration_parse_usb(PSMoveCalibration *calibration) +{ + assert(calibration != NULL); + char *data = calibration->usb_calibration; + int orientation; + int x, y, z; + + printf("\n"); + + /* https://github.com/nitsch/moveonpc/wiki/Calibration-data-CECH%E2%80%90ZCM2 */ + + for (orientation=0; orientation<6; orientation++) { + x = psmove_calibration_decode_16bit_signed(data, 0x04 + 6*orientation); + y = psmove_calibration_decode_16bit_signed(data, 0x04 + 6*orientation + 2); + z = psmove_calibration_decode_16bit_signed(data, 0x04 + 6*orientation + 4); + printf("# Orientation #%d: (%5d | %5d | %5d)\n", orientation, x, y, z); + } + + printf("\n"); + + x = psmove_calibration_decode_16bit_signed(data, 0x26); + y = psmove_calibration_decode_16bit_signed(data, 0x26 + 2); + z = psmove_calibration_decode_16bit_signed(data, 0x26 + 4); + printf("# Gyro Bias?, 0 rpm (@0x26): (%5d | %5d | %5d)\n", x, y, z); + + printf("\n"); + + for (orientation=0; orientation<6; orientation++) { + x = psmove_calibration_decode_16bit_signed(data, 0x30 + 6*orientation); + y = psmove_calibration_decode_16bit_signed(data, 0x30 + 6*orientation + 2); + z = psmove_calibration_decode_16bit_signed(data, 0x30 + 6*orientation + 4); + printf("# Gyro %c, 90 rpm: (%5d | %5d | %5d)\n", "XYZXYZ"[orientation], x, y, z); + } +} + void psmove_calibration_get_usb_accel_values(PSMoveCalibration *calibration, int *x1, int *x2, int *y1, int *y2, int *z1, int *z2) @@ -216,45 +263,100 @@ psmove_calibration_get_usb_accel_values(PSMoveCalibration *calibration, int orientation; - /* Minimum (negative) value of each axis */ - orientation = 1; - *x1 = psmove_calibration_decode(data, 0x04 + 6*orientation); - orientation = 5; - *y1 = psmove_calibration_decode(data, 0x04 + 6*orientation + 2); - orientation = 2; - *z1 = psmove_calibration_decode(data, 0x04 + 6*orientation + 4); - - /* Maximum (positive) values of each axis */ - orientation = 3; - *x2 = psmove_calibration_decode(data, 0x04 + 6*orientation); - orientation = 4; - *y2 = psmove_calibration_decode(data, 0x04 + 6*orientation + 2); - orientation = 0; - *z2 = psmove_calibration_decode(data, 0x04 + 6*orientation + 4); + enum PSMove_Model_Type model_type= psmove_get_model(calibration->move); + + if (model_type == Model_ZCM1) + { + /* Minimum (negative) value of each axis */ + orientation = 1; + *x1 = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6*orientation); + orientation = 5; + *y1 = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6*orientation + 2); + orientation = 2; + *z1 = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6*orientation + 4); + + /* Maximum (positive) values of each axis */ + orientation = 3; + *x2 = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6*orientation); + orientation = 4; + *y2 = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6*orientation + 2); + orientation = 0; + *z2 = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6*orientation + 4); + } + else if (model_type == Model_ZCM2) + { + /* Minimum (negative) value of each axis */ + orientation = 1; + *x1 = psmove_calibration_decode_16bit_signed(data, 0x02 + 6*orientation); + orientation = 3; + *y1 = psmove_calibration_decode_16bit_signed(data, 0x02 + 6*orientation + 2); + orientation = 5; + *z1 = psmove_calibration_decode_16bit_signed(data, 0x02 + 6*orientation + 4); + + /* Maximum (positive) values of each axis */ + orientation = 0; + *x2 = psmove_calibration_decode_16bit_signed(data, 0x02 + 6*orientation); + orientation = 2; + *y2 = psmove_calibration_decode_16bit_signed(data, 0x02 + 6*orientation + 2); + orientation = 4; + *z2 = psmove_calibration_decode_16bit_signed(data, 0x02 + 6*orientation + 4); + } } void -psmove_calibration_get_usb_gyro_values(PSMoveCalibration *calibration, +psmove_calibration_get_zcm1_usb_gyro_values(PSMoveCalibration *calibration, int *x, int *y, int *z) { assert(calibration != NULL); assert(psmove_calibration_supported(calibration)); char *data = calibration->usb_calibration; - int bx, by, bz; /* Bias(?) values, need to sustract those */ - bx = psmove_calibration_decode(data, 0x2a); - by = psmove_calibration_decode(data, 0x2a + 2); - bz = psmove_calibration_decode(data, 0x2a + 4); + int bx, by, bz; /* Bias(?) values, need to subtract those */ + bx = psmove_calibration_decode_16bit_unsigned(data, 0x2a); + by = psmove_calibration_decode_16bit_unsigned(data, 0x2a + 2); + bz = psmove_calibration_decode_16bit_unsigned(data, 0x2a + 4); int orientation; orientation = 0; - *x = psmove_calibration_decode(data, 0x46 + 8*orientation) - bx; + *x = psmove_calibration_decode_16bit_unsigned(data, 0x46 + 8*orientation) - bx; orientation = 1; - *y = psmove_calibration_decode(data, 0x46 + 8*orientation + 2) - by; + *y = psmove_calibration_decode_16bit_unsigned(data, 0x46 + 8*orientation + 2) - by; orientation = 2; - *z = psmove_calibration_decode(data, 0x46 + 8*orientation + 4) - bz; + *z = psmove_calibration_decode_16bit_unsigned(data, 0x46 + 8*orientation + 4) - bz; +} + +void +psmove_calibration_get_zcm2_usb_gyro_values(PSMoveCalibration *calibration, + int *x1, int *x2, int *y1, int *y2, int *z1, int *z2, + int *dx, int *dy, int *dz) +{ + assert(calibration != NULL); + assert(psmove_calibration_supported(calibration)); + char *data = calibration->usb_calibration; + + int orientation; + + /* Minimum (negative) value of each axis */ + orientation = 3; + *x1 = psmove_calibration_decode_16bit_signed(data, 0x30 + 6*orientation); + orientation = 4; + *y1 = psmove_calibration_decode_16bit_signed(data, 0x30 + 6*orientation + 2); + orientation = 5; + *z1 = psmove_calibration_decode_16bit_signed(data, 0x30 + 6*orientation + 4); + + /* Maximum (positive) values of each axis */ + orientation = 0; + *x2 = psmove_calibration_decode_16bit_signed(data, 0x30 + 6*orientation); + orientation = 1; + *y2 = psmove_calibration_decode_16bit_signed(data, 0x30 + 6*orientation + 2); + orientation = 2; + *z2 = psmove_calibration_decode_16bit_signed(data, 0x30 + 6*orientation + 4); + + *dx = psmove_calibration_decode_16bit_signed(data, 0x26); + *dy = psmove_calibration_decode_16bit_signed(data, 0x26 + 2); + *dz = psmove_calibration_decode_16bit_signed(data, 0x26 + 4); } void @@ -264,7 +366,13 @@ psmove_calibration_dump_usb(PSMoveCalibration *calibration) assert(calibration != NULL); - for (j=0; jusb_calibration); j++) { + enum PSMove_Model_Type model= psmove_get_model(calibration->move); + size_t calibration_blob_size= + (model == Model_ZCM1) + ? PSMOVE_ZCM1_CALIBRATION_BLOB_SIZE + : PSMOVE_ZCM2_CALIBRATION_BLOB_SIZE; + + for (j=0; jusb_calibration[j]); if (j % 16 == 15) { printf("\n"); @@ -274,7 +382,10 @@ psmove_calibration_dump_usb(PSMoveCalibration *calibration) } printf("\n"); - psmove_calibration_parse_usb(calibration); + if (model == Model_ZCM1) + psmove_ZCM1_calibration_parse_usb(calibration); + else if (model == Model_ZCM2) + psmove_ZCM2_calibration_parse_usb(calibration); printf("\n"); } @@ -287,6 +398,8 @@ psmove_calibration_new(PSMove *move) char *serial; int i; + enum PSMove_Model_Type model= psmove_get_model(move); + PSMoveCalibration *calibration = (PSMoveCalibration*)calloc(1, sizeof(PSMoveCalibration)); @@ -341,7 +454,7 @@ psmove_calibration_new(PSMove *move) /** * - * Calculation of accelermeter mapping (as factor of gravity, 1g): + * Calculation of accelerometer mapping (as factor of gravity, 1g): * * 2 * (raw - low) * calibrated = ---------------- - 1 @@ -393,20 +506,20 @@ psmove_calibration_new(PSMove *move) * with: * * raw ..... Raw sensor reading - * rpm80 ... Sensor reading at 80 RPM (from calibration blob) + * rpmXX ... Sensor reading at XX RPM (from calibration blob) * rpm60 ... Sensor reading at 60 RPM (1 rotation per second) * * Or combined: * - * 80 * raw * 2 PI + * XX * raw * 2 PI * calibrated = ----------------- - * 60 * rpm80 + * 60 * rpmXX * * Now define: * - * 2 * PI * 80 + * 2 * PI * XX * f = ------------- - * 60 * rpm80 + * 60 * rpmXX * * Then we get: * @@ -414,14 +527,52 @@ psmove_calibration_new(PSMove *move) * **/ - int gx80, gy80, gz80; - psmove_calibration_get_usb_gyro_values(calibration, - &gx80, &gy80, &gz80); - - float factor = (float)(2.f * M_PI * 80.f) / 60.f; - calibration->gx = factor / (float)gx80; - calibration->gy = factor / (float)gy80; - calibration->gz = factor / (float)gz80; + const float k_rpm_to_rad_per_sec = (2.0f * (float)M_PI) / 60.0f; + + if (model == Model_ZCM1) + { + /* ZCM1 gyro calibrations are at +80RPM */ + int gx80, gy80, gz80; + psmove_calibration_get_zcm1_usb_gyro_values(calibration, + &gx80, &gy80, &gz80); + + const float k_calibration_rpm= 80.f; + const float factor = k_calibration_rpm * k_rpm_to_rad_per_sec; + + calibration->gx = factor / (float)gx80; + calibration->gy = factor / (float)gy80; + calibration->gz = factor / (float)gz80; + + // Per frame drift taken into account using adjusted gain values + calibration->dx = 0; + calibration->dy = 0; + calibration->dz = 0; + } else { + /* ZCM1 gyro calibrations are at +90RPM and -90RPM + + Note on the bias values (dx, dy, and dz). + Given the slightly asymmetrical min and max 90RPM readings you might think + that there is a bias in the gyros that you should compute by finding + the y-intercept value (the y-intercept of the gyro reading/angular speed line) + using the formula b= y_hi - m*x_hi, but this results in pretty bad + controller drift. We get much better results ignoring the y-intercept + and instead use the presumed "drift" values stored at 0x26 + */ + int gx90_low, gx90_high, gy90_low, gy90_high, gz90_low, gz90_high; + psmove_calibration_get_zcm2_usb_gyro_values(calibration, + &gx90_low, &gx90_high, &gy90_low, &gy90_high, &gz90_low, &gz90_high, + &calibration->dx, &calibration->dy, &calibration->dz); + + const float k_calibration_rpm= 90.f; + const float calibration_hi= k_calibration_rpm * k_rpm_to_rad_per_sec; + const float calibration_low= -k_calibration_rpm * k_rpm_to_rad_per_sec; + const float factor = calibration_hi - calibration_low; + + // Compute the gain value (the slope of the gyro reading/angular speed line) + calibration->gx = factor / (float)(gx90_high - gx90_low); + calibration->gy = factor / (float)(gy90_high - gy90_low); + calibration->gz = factor / (float)(gz90_high - gz90_low); + } } else { /* No calibration data - pass-through input data */ calibration->ax = 1.f; @@ -435,6 +586,10 @@ psmove_calibration_new(PSMove *move) calibration->gx = 1.f; calibration->gy = 1.f; calibration->gz = 1.f; + + calibration->dx = 0; + calibration->dy = 0; + calibration->dz = 0; } return calibration; @@ -448,13 +603,27 @@ psmove_calibration_read_from_usb(PSMoveCalibration *calibration) char *data; size_t size; - if (_psmove_get_calibration_blob(calibration->move, &data, &size)) { - assert(size == PSMOVE_CALIBRATION_BLOB_SIZE); - memcpy(calibration->usb_calibration, data, size); - free(data); - calibration->flags |= CalibrationFlag_HaveUSB; - return 1; - } + enum PSMove_Model_Type model= psmove_get_model(calibration->move); + if (model == Model_ZCM1) + { + if (_psmove_get_zcm1_calibration_blob(calibration->move, &data, &size) == 1) { + assert(size == PSMOVE_ZCM1_CALIBRATION_BLOB_SIZE); + memcpy(calibration->usb_calibration, data, size); + free(data); + calibration->flags |= CalibrationFlag_HaveUSB; + return 1; + } + } + else if (model == Model_ZCM2) + { + if (_psmove_get_zcm2_calibration_blob(calibration->move, &data, &size) == 1) { + assert(size == PSMOVE_ZCM2_CALIBRATION_BLOB_SIZE); + memcpy(calibration->usb_calibration, data, size); + free(data); + calibration->flags |= CalibrationFlag_HaveUSB; + return 1; + } + } return 0; } @@ -500,15 +669,15 @@ psmove_calibration_map_gyroscope(PSMoveCalibration *calibration, psmove_return_if_fail(raw_input != NULL); if (gx) { - *gx = (float)raw_input[0] * calibration->gx; + *gx = (float)(raw_input[0] - calibration->dx) * calibration->gx; } if (gy) { - *gy = (float)raw_input[1] * calibration->gy; + *gy = (float)(raw_input[1] - calibration->dy) * calibration->gy; } if (gz) { - *gz = (float)raw_input[2] * calibration->gz; + *gz = (float)(raw_input[2] - calibration->dz) * calibration->gz; } } diff --git a/src/psmove_orientation.cpp b/src/psmove_orientation.cpp index 90ac9641..540eeaad 100644 --- a/src/psmove_orientation.cpp +++ b/src/psmove_orientation.cpp @@ -162,8 +162,16 @@ psmove_orientation_new(PSMove *move) orientation_state->quaternion = *k_psmove_quaternion_identity; orientation_state->reset_quaternion = *k_psmove_quaternion_identity; - /* Initialize data specific to the selected filter */ - psmove_orientation_set_fusion_type(orientation_state, OrientationFusion_ComplementaryMARG); + /* Initialize data specific to the selected filter */ + if (psmove_get_model(move) == Model_ZCM1) + { + psmove_orientation_set_fusion_type(orientation_state, OrientationFusion_ComplementaryMARG); + } + else + { + // No magnetometer on the ZCM2 + psmove_orientation_set_fusion_type(orientation_state, OrientationFusion_MadgwickIMU); + } /* Set the transform used re-orient the calibration data used by the orientation fusion algorithm */ psmove_orientation_set_calibration_transform(orientation_state, k_psmove_identity_pose_laying_flat); diff --git a/src/psmove_private.h b/src/psmove_private.h index 2f3f5533..5f4cf059 100644 --- a/src/psmove_private.h +++ b/src/psmove_private.h @@ -104,8 +104,14 @@ struct PSMove_RGBValue { /* Buffer size for calibration data */ #define PSMOVE_CALIBRATION_SIZE 49 -/* Three blocks, minus 2x the header (2 bytes) for the 2nd and 3rd block */ -#define PSMOVE_CALIBRATION_BLOB_SIZE (PSMOVE_CALIBRATION_SIZE*3 - 2*2) +/* Three blocks, minus header (2 bytes) for blocks 2,3 */ +#define PSMOVE_ZCM1_CALIBRATION_BLOB_SIZE (PSMOVE_CALIBRATION_SIZE*3 - 2*2) + +/* Three blocks, minus header (2 bytes) for block 2 */ +#define PSMOVE_ZCM2_CALIBRATION_BLOB_SIZE (PSMOVE_CALIBRATION_SIZE*2 - 2*1) + +/* The maximum calibration blob size */ +#define PSMOVE_MAX_CALIBRATION_BLOB_SIZE PSMOVE_ZCM1_CALIBRATION_BLOB_SIZE /* System-wide data directory */ #define PSMOVE_SYSTEM_DATA_DIR "/etc/psmoveapi" @@ -139,14 +145,24 @@ ADDAPI const char * ADDCALL _psmove_get_device_path(PSMove *move); /** - * [PRIVATE API] Get the calibration data from a connected USB controller. + * [PRIVATE API] Get the ZCM1 calibration data from a connected USB controller. + * + * The pointer *dest will be set to a newly-allocated byte array + * of a certain size (which will be saved in *size) and the caller + * has to free this field with free() + **/ +ADDAPI int +ADDCALL _psmove_get_zcm1_calibration_blob(PSMove *move, char **dest, size_t *size); + +/** + * [PRIVATE API] Get the ZCM2 calibration data from a connected USB controller. * * The pointer *dest will be set to a newly-allocated byte array * of a certain size (which will be saved in *size) and the caller * has to free this field with free() **/ ADDAPI int -ADDCALL _psmove_get_calibration_blob(PSMove *move, char **dest, size_t *size); +ADDCALL _psmove_get_zcm2_calibration_blob(PSMove *move, char **dest, size_t *size); /** * [PRIVATE API] Translate a raw temperature value to degrees Celsius From 2e473980d7ba7e881718dfb24efc4dbfba0dabb9 Mon Sep 17 00:00:00 2001 From: Derek Quam Date: Tue, 2 Oct 2018 12:17:28 -0500 Subject: [PATCH 3/4] Fixed bugs in bluetooth authentication of ZCM2U on Windows Handle unknown errors from BluetoothAuthenticateDeviceEx. Not handling these errors could cause a hang. Fixed an incorrect cast of the callback parameters in bluetooth_auth_callback. --- src/platform/psmove_port_windows.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/platform/psmove_port_windows.c b/src/platform/psmove_port_windows.c index b27d0fe1..ce2e4133 100644 --- a/src/platform/psmove_port_windows.c +++ b/src/platform/psmove_port_windows.c @@ -306,7 +306,7 @@ bluetooth_auth_callback( LPVOID pvParam, PBLUETOOTH_AUTHENTICATION_CALLBACK_PARAMS pAuthCallbackParams) { - struct BluetoothAuthenticationCallbackState *state= (struct BluetoothAuthenticationCallbackState *)pAuthCallbackParams; + struct BluetoothAuthenticationCallbackState *state= (struct BluetoothAuthenticationCallbackState *)pvParam; BLUETOOTH_AUTHENTICATE_RESPONSE AuthRes; memset(&AuthRes, 0, sizeof(BLUETOOTH_AUTHENTICATE_RESPONSE)); @@ -409,6 +409,10 @@ authenticate_controller( WINPAIR_DEBUG("Invalid parameter!"); ret= 1; } + else if (dwRet != ERROR_SUCCESS) { + WINPAIR_DEBUG("BluetoothAuthenticateDeviceEx failed: %d", GetLastError()); + ret= 1; + } else { // Block on authentication completing From 989d0794d5a50e12620ab268c6aa03644c4dc17d Mon Sep 17 00:00:00 2001 From: Derek Quam Date: Wed, 10 Oct 2018 13:52:31 -0500 Subject: [PATCH 4/4] Allow BluetoothAuthenticateDeviceEx errors to fall through again. --- src/platform/psmove_port_windows.c | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/platform/psmove_port_windows.c b/src/platform/psmove_port_windows.c index ce2e4133..1ffac635 100644 --- a/src/platform/psmove_port_windows.c +++ b/src/platform/psmove_port_windows.c @@ -409,10 +409,6 @@ authenticate_controller( WINPAIR_DEBUG("Invalid parameter!"); ret= 1; } - else if (dwRet != ERROR_SUCCESS) { - WINPAIR_DEBUG("BluetoothAuthenticateDeviceEx failed: %d", GetLastError()); - ret= 1; - } else { // Block on authentication completing