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);
}
int
moved_client_send(moved_client *client, enum PSMoveMovedCmd cmd, int controller_id,
const uint8_t *data, size_t len)
{
int retry_count = 0;
client->request_buf.header.request_sequence++;
client->request_buf.header.command_id = (uint16_t)cmd;
client->request_buf.header.controller_id = (uint16_t)controller_id;
if (data != NULL) {
memset(client->request_buf.payload, 0, sizeof(client->request_buf.payload));
memcpy(client->request_buf.payload, data, std::min(len, sizeof(client->request_buf.payload)));
}
while (retry_count < MOVED_MAX_RETRIES) {
int res = sendto(client->socket, (const char *)client->request_buf.bytes, sizeof(client->request_buf),
/*flags=*/0, (struct sockaddr *)&(client->moved_addr), sizeof(client->moved_addr));
if (res == sizeof(client->request_buf)) {
while (1) {
res = recv(client->socket, (char *)client->response_buf.bytes, sizeof(client->response_buf),
/*flags=*/0);
if (res != sizeof(client->response_buf)) {
break;
}
if (client->request_buf.header.request_sequence == client->response_buf.header.request_sequence) {
break;
} else {
// Invalid sequence number received, retry reading a (newer?) packet
}
}
if (res != sizeof(client->response_buf)) {
if (client->request_buf.header.command_id == MOVED_REQ_SET_LEDS) {
// Writing LEDs should be fast, do not wait for a response here
return 0;
}
retry_count++;
continue;
}
return 1;
}
}
psmove_WARNING("Command %d to %s, controller %d failed (errno=%d)\n",
(int)cmd, client->hostname, controller_id, errno);
return 0;
}
void
camera_control_set_parameters(CameraControl* cc,
int autoE, int autoG, int autoWB,
int exposure, int gain,
int wbRed, int wbGreen, int wbBlue,
int contrast, int brightness)
{
#if defined(CAMERA_CONTROL_USE_PS3EYE_DRIVER)
// TODO: Implement setting those parameters on cc->eye
psmove_WARNING("Unimplemented: Setting of PS3EYEDriver parameters\n");
#else
macosx_camera_set_exposure_lock(1);
#endif
}
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);
}
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;
}
CameraControl *
camera_control_new_with_settings(int cameraID, int width, int height, int framerate, int cam_type)
{
CameraControl* cc = (CameraControl*) calloc(1, sizeof(CameraControl));
cc->cameraID = cameraID;
if (framerate <= 0) {
framerate = PSMOVE_TRACKER_DEFAULT_FPS;
}
if (cam_type == PSMove_Camera_PS3EYE_BLUEDOT)
{
cc->focl_x = (float)PS3EYE_FOCAL_LENGTH_BLUE;
cc->focl_y = (float)PS3EYE_FOCAL_LENGTH_BLUE;
}
else if (cam_type == PSMove_Camera_PS3EYE_REDDOT)
{
cc->focl_x = (float)PS3EYE_FOCAL_LENGTH_RED;
cc->focl_y = (float)PS3EYE_FOCAL_LENGTH_RED;
}
else if (cam_type == PSMove_Camera_Unknown)
{
cc->focl_x = (float)PS3EYE_FOCAL_LENGTH_BLUE;
cc->focl_y = (float)PS3EYE_FOCAL_LENGTH_BLUE;
}
// Needed for cbb tracker. Will be overwritten by camera calibration files if they exist.
#if defined(CAMERA_CONTROL_USE_CL_DRIVER)
// Windows 32-bit. Either CL_SDK or Registry_requiring
int cams = CLEyeGetCameraCount();
if (cams <= cameraID) {
free(cc);
return NULL;
}
GUID cguid = CLEyeGetCameraUUID(cameraID);
cc->camera = CLEyeCreateCamera(cguid,
CLEYE_COLOR_PROCESSED, CLEYE_VGA, framerate);
CLEyeCameraGetFrameDimensions(cc->camera, &width, &height);
// Depending on color mode chosen, create the appropriate OpenCV image
cc->frame4ch = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 4);
cc->frame3ch = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 3);
CLEyeCameraStart(cc->camera);
#elif defined(CAMERA_CONTROL_USE_PS3EYE_DRIVER)
// Mac or Windows
// Initialize PS3EYEDriver
ps3eye_init();
int cams = ps3eye_count_connected();
psmove_DEBUG("Found %i ps3eye(s) with CAMERA_CONTROL_USE_PS3EYE_DRIVER.\n", cams);
if (cams <= cameraID) {
free(cc);
return NULL;
}
if (width <= 0 || height <= 0) {
get_metrics(&width, &height);
}
psmove_DEBUG("Attempting to open ps3eye with cameraId, width, height, framerate: %d, %d, %d, %d.\n", cameraID, width, height, framerate);
cc->eye = ps3eye_open(cameraID, width, height, framerate);
if (cc->eye == NULL) {
psmove_WARNING("Failed to open camera ID %d", cameraID);
free(cc);
return NULL;
}
cc->framebgr = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 3);
#else
// Assume webcam accessible from OpenCV.
char *video = psmove_util_get_env_string(PSMOVE_TRACKER_FILENAME_ENV);
if (video) {
psmove_DEBUG("Using '%s' as video input.\n", video);
cc->capture = cvCaptureFromFile(video);
free(video);
} else {
cc->capture = cvCaptureFromCAM(cc->cameraID);
if (width <= 0 || height <= 0) {
get_metrics(&width, &height);
}
cvSetCaptureProperty(cc->capture, CV_CAP_PROP_FRAME_WIDTH, width);
cvSetCaptureProperty(cc->capture, CV_CAP_PROP_FRAME_HEIGHT, height);
}
#endif
cc->width = width;
cc->height = height;
cc->deinterlace = PSMove_False;
return cc;
}
int
moved_client_send(moved_client *client, char req, char id, const unsigned char *data)
{
int retry_count = 0;
client->request_buf[0] = req;
client->request_buf[1] = id;
if (data != NULL) {
memcpy(client->request_buf+2, data, sizeof(client->request_buf)-2);
}
while (retry_count < MOVED_MAX_RETRIES) {
if (sendto(client->socket, (char*)client->request_buf,
sizeof(client->request_buf), 0,
(struct sockaddr *)&(client->moved_addr),
sizeof(client->moved_addr)) >= 0)
{
switch (req) {
case MOVED_REQ_COUNT_CONNECTED:
if (recv(client->socket, (char*)client->read_response_buf,
sizeof(client->read_response_buf), 0) == -1) {
retry_count++;
continue;
}
return client->read_response_buf[0];
break;
case MOVED_REQ_READ:
case MOVED_REQ_SERIAL:
if (recv(client->socket, (char*)client->read_response_buf,
sizeof(client->read_response_buf), 0) == -1) {
retry_count++;
continue;
}
return 1;
break;
case MOVED_REQ_WRITE:
return 1;
break;
default:
psmove_WARNING("Unknown request ID: %d\n", req);
return 0;
break;
}
}
}
switch (req) {
case MOVED_REQ_COUNT_CONNECTED:
psmove_WARNING("Could not get device count from %s (errno=%d)\n",
client->hostname, errno);
break;
case MOVED_REQ_READ:
psmove_WARNING("Could not read data from %s (errno=%d)\n",
client->hostname, errno);
break;
default:
psmove_WARNING("Request ID %d to %s failed (errno=%d)\n",
req, client->hostname, errno);
break;
}
return 0;
}
