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;
}
