int psmove_calibration_save(PSMoveCalibration *calibration) { psmove_return_val_if_fail(calibration != NULL, 0); FILE *fp = psmove_file_open(calibration->filename, "wb"); if (fp == NULL) { psmove_CRITICAL("Unable to write USB calibration"); return 0; } if (fwrite(calibration->usb_calibration, sizeof(calibration->usb_calibration), 1, fp) != 1) { psmove_CRITICAL("Unable to write USB calibration"); psmove_file_close(fp); return 0; } if (fwrite(&(calibration->flags), sizeof(calibration->flags), 1, fp) != 1) { psmove_CRITICAL("Unable to write USB calibration"); psmove_file_close(fp); return 0; } psmove_file_close(fp); return 1; }
int psmove_calibration_load(PSMoveCalibration *calibration) { psmove_return_val_if_fail(calibration != NULL, 0); FILE *fp = psmove_file_open(calibration->filename, "rb"); if (fp == NULL) { // use system file in case local is not available fp = psmove_file_open(calibration->system_filename, "rb"); if (fp == NULL) { return 0; } } if (fread(calibration->usb_calibration, sizeof(calibration->usb_calibration), 1, fp) != 1) { psmove_CRITICAL("Unable to read USB calibration"); psmove_file_close(fp); return 0; } if (fread(&(calibration->flags), sizeof(calibration->flags), 1, fp) != 1) { psmove_CRITICAL("Unable to read USB calibration"); psmove_file_close(fp); return 0; } psmove_file_close(fp); return 1; }
PSMoveCalibration * psmove_calibration_new(PSMove *move) { PSMove_Data_BTAddr addr; char *serial; int i; PSMoveCalibration *calibration = (PSMoveCalibration*)calloc(1, sizeof(PSMoveCalibration)); calibration->move = move; if (psmove_connection_type(move) == Conn_USB) { _psmove_read_btaddrs(move, NULL, &addr); serial = _psmove_btaddr_to_string(addr); } else { serial = psmove_get_serial(move); } if (!serial) { psmove_CRITICAL("Could not determine serial from controller"); free(calibration); return NULL; } for (i=0; i<strlen(serial); i++) { if (serial[i] == ':') { serial[i] = '_'; } } char *template = malloc(strlen(serial) +
char * psmove_port_get_host_bluetooth_address() { PSMove_Data_BTAddr btaddr; HANDLE hRadio; if (windows_get_first_bluetooth_radio(&hRadio) != 0 || !hRadio) { psmove_WARNING("Failed to find a Bluetooth radio"); return NULL; } BLUETOOTH_RADIO_INFO radioInfo; radioInfo.dwSize = sizeof(BLUETOOTH_RADIO_INFO); if (BluetoothGetRadioInfo(hRadio, &radioInfo) != ERROR_SUCCESS) { psmove_CRITICAL("BluetoothGetRadioInfo"); CloseHandle(hRadio); return NULL; } int i; for (i=0; i<6; i++) { btaddr[i] = radioInfo.address.rgBytes[i]; } CloseHandle(hRadio); return _psmove_btaddr_to_string(btaddr); }
void psmove_port_register_psmove(const char *addr, const char *host) { // FIXME: Host is ignored for now HANDLE hRadio; if (windows_get_first_bluetooth_radio(&hRadio) != 0 || !hRadio) { psmove_WARNING("Failed to find a Bluetooth radio"); return NULL; } BLUETOOTH_RADIO_INFO radioInfo; radioInfo.dwSize = sizeof(BLUETOOTH_RADIO_INFO); if (BluetoothGetRadioInfo(hRadio, &radioInfo) != ERROR_SUCCESS) { psmove_CRITICAL("BluetoothGetRadioInfo"); CloseHandle(hRadio); return NULL; } windows_register_psmove(addr, &radioInfo.address, hRadio); CloseHandle(hRadio); }
PSMoveCalibration * psmove_calibration_new(PSMove *move) { PSMove_Data_BTAddr addr; char *serial; int i; PSMoveCalibration *calibration = (PSMoveCalibration*)calloc(1, sizeof(PSMoveCalibration)); calibration->move = move; if (psmove_connection_type(move) == Conn_USB) { _psmove_read_btaddrs(move, NULL, &addr); serial = _psmove_btaddr_to_string(addr); } else { serial = psmove_get_serial(move); } if (!serial) { psmove_CRITICAL("Could not determine serial from controller"); free(calibration); return NULL; } for (i = 0; i<(int)strlen(serial); i++) { if (serial[i] == ':') { serial[i] = '_'; } } size_t calibration_filename_length = strlen(serial) + strlen(PSMOVE_CALIBRATION_EXTENSION) + 1; char *calibration_filename = (char *)malloc(calibration_filename_length); strcpy(calibration_filename, serial); strcat(calibration_filename, PSMOVE_CALIBRATION_EXTENSION); calibration->filename = psmove_util_get_file_path(calibration_filename); calibration->system_filename = psmove_util_get_system_file_path(calibration_filename); free(calibration_filename); free(serial); /* Try to load the calibration data from disk, or from USB */ psmove_calibration_load(calibration); if (!psmove_calibration_supported(calibration)) { if (psmove_connection_type(move) == Conn_USB) { psmove_DEBUG("Storing calibration from USB\n"); psmove_calibration_read_from_usb(calibration); psmove_calibration_save(calibration); } } /* Pre-calculate values used for mapping input */ if (psmove_calibration_supported(calibration)) { /* Accelerometer reading (high/low) for each axis */ int axlow, axhigh, aylow, ayhigh, azlow, azhigh; psmove_calibration_get_usb_accel_values(calibration, &axlow, &axhigh, &aylow, &ayhigh, &azlow, &azhigh); /** * * Calculation of accelerometer mapping (as factor of gravity, 1g): * * 2 * (raw - low) * calibrated = ---------------- - 1 * (high - low) * * with: * * raw .... Raw sensor reading * low .... Raw reading at -1g * high ... Raw reading at +1g * * Now define: * * 2 * f = -------------- * (high - low) * * And combine constants: * * c = - (f * low) - 1 * * Then we get: * * calibrated = f * raw + c * **/ /* Accelerometer factors "f" */ calibration->ax = 2.f / (float)(axhigh - axlow); calibration->ay = 2.f / (float)(ayhigh - aylow); calibration->az = 2.f / (float)(azhigh - azlow); /* Accelerometer constants "c" */ calibration->bx = - (calibration->ax * (float)axlow) - 1.f; calibration->by = - (calibration->ay * (float)aylow) - 1.f; calibration->bz = - (calibration->az * (float)azlow) - 1.f; /** * Calculation of gyroscope mapping (in radiant per second): * * raw * calibrated = -------- * 2 PI * rpm60 * * 60 * rpm80 * rpm60 = ------------ * 80 * * with: * * raw ..... Raw sensor reading * rpm80 ... Sensor reading at 80 RPM (from calibration blob) * rpm60 ... Sensor reading at 60 RPM (1 rotation per second) * * Or combined: * * 80 * raw * 2 PI * calibrated = ----------------- * 60 * rpm80 * * Now define: * * 2 * PI * 80 * f = ------------- * 60 * rpm80 * * Then we get: * * calibrated = f * raw * **/ int gx80, gy80, gz80; psmove_calibration_get_usb_gyro_values(calibration, &gx80, &gy80, &gz80); float factor = (float)(2.0 * M_PI * 80.0) / 60.0; calibration->gx = factor / (float)gx80; calibration->gy = factor / (float)gy80; calibration->gz = factor / (float)gz80; } else { /* No calibration data - pass-through input data */ calibration->ax = 1.f; calibration->ay = 1.f; calibration->az = 1.f; calibration->bx = 0.f; calibration->by = 0.f; calibration->bz = 0.f; calibration->gx = 1.f; calibration->gy = 1.f; calibration->gz = 1.f; } return calibration; }