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); }
char * psmove_port_get_host_bluetooth_address() { PSMove_Data_BTAddr btaddr; PSMove_Data_BTAddr blank; memset(blank, 0, sizeof(PSMove_Data_BTAddr)); memset(btaddr, 0, sizeof(PSMove_Data_BTAddr)); hci_for_each_dev(HCI_UP, _psmove_linux_bt_dev_info, (long)btaddr); if(memcmp(btaddr, blank, sizeof(PSMove_Data_BTAddr))==0) { fprintf(stderr, "WARNING: Can't determine Bluetooth address.\n" "Make sure Bluetooth is turned on.\n"); return NULL; } return _psmove_btaddr_to_string(btaddr); }
static moved_client_list * moved_client_list_discover(moved_client_list *result) { int fd = (int)socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); if (fd == -1) { return nullptr; } psmove_port_set_socket_timeout_ms(fd, 10); int enabled = 1; setsockopt(fd, SOL_SOCKET, SO_BROADCAST, (const char *)&enabled, sizeof(enabled)); struct sockaddr_in addr; addr.sin_family = AF_INET; addr.sin_port = htons(MOVED_UDP_PORT); addr.sin_addr.s_addr = INADDR_BROADCAST; union PSMoveMovedRequest request; union PSMoveMovedRequest response; memset(&request, 0, sizeof(request)); request.header.request_sequence = 0x47110000; request.header.command_id = MOVED_REQ_DISCOVER; request.header.controller_id = 0; for (int i=0; i<5; i++) { request.header.request_sequence++; int res = sendto(fd, (const char *)&request, sizeof(request), /*flags=*/0, (struct sockaddr *)&addr, sizeof(addr)); if (res == -1) { psmove_WARNING("Could not send discovery request"); } psmove_port_sleep_ms(20); } psmove_port_sleep_ms(50); std::map<std::string, bool> discovered; int failures = 0; while (failures < 10) { struct sockaddr_in addr_server; socklen_t addr_server_len = sizeof(addr_server); memset(&addr_server, 0, sizeof(addr_server)); addr_server.sin_family = AF_INET; int res = recvfrom(fd, (char *)&response, sizeof(response), /*flags=*/0, (struct sockaddr *)&addr_server, &addr_server_len); if (res == sizeof(response)) { char temp[1024]; inet_ntop(addr.sin_family, &addr_server.sin_addr, temp, sizeof(temp)); std::string hostname(temp); discovered[hostname] = true; psmove_DEBUG("Discovered daemon: '%s' (seq=0x%08x)\n", temp, response.header.request_sequence); failures = 0; } else { failures++; } } psmove_port_close_socket(fd); for (auto it: discovered) { const char *hostname = it.first.c_str(); printf("Using remote host '%s' (from auto-discovery)\n", hostname); moved_client *client = moved_client_create(hostname); result = moved_client_list_insert(result, client); if (moved_client_send(client, MOVED_REQ_GET_HOST_BTADDR, 0, nullptr, 0)) { char *serial = _psmove_btaddr_to_string(*((PSMove_Data_BTAddr *)client->response_buf.get_host_btaddr.btaddr)); printf("Bluetooth host address of '%s' is '%s'\n", hostname, serial); free(serial); } } return result; }
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; }