void PSMoveQt::onTimeout() { int ax, ay, az, gx, gy, gz, mx, my, mz, buttons, battery; while (psmove_poll(_move)) { setTrigger(psmove_get_trigger(_move)); psmove_get_gyroscope(_move, &gx, &gy, &gz); if (gx != _gx || gy != _gy || gz != _gz) { _gx = gx; _gy = gy; _gz = gz; emit gyroChanged(); } psmove_get_accelerometer(_move, &ax, &ay, &az); if (ax != _ax || ay != _ay || az != _az) { _ax = ax; _ay = ay; _az = az; emit accelerometerChanged(); } psmove_get_magnetometer(_move, &mx, &my, &mz); if (mx != _mx || my != _my || mz != _mz) { _mx = mx; _my = my; _mz = mz; emit magnetometerChanged(); } buttons = psmove_get_buttons(_move); if (buttons != _buttons) { unsigned int pressed, released; psmove_get_button_events(_move, &pressed, &released); for (int i=1; i<=PSMoveQt::T; i <<= 1) { if (pressed & i) { emit buttonPressed(i); } else if (released & i) { emit buttonReleased(i); } } _buttons = buttons; } battery = psmove_get_battery(_move); if (battery != _battery) { bool charging = (battery == Batt_CHARGING || _battery == Batt_CHARGING); _battery = battery; if (charging) { emit chargingChanged(); } else if (_battery != Batt_CHARGING) { emit batteryChanged(_battery); } } } }
void PSMoveAPI::update() { for (auto &c: controllers) { if (c->connected && !c->sent_connect) { if (receiver->connect != nullptr) { // Send initial connect event receiver->connect(&c->controller, user_data); } c->sent_connect = true; } if (!c->connected && !c->sent_disconnect) { if (receiver->disconnect != nullptr) { // Send disconnect event receiver->disconnect(&c->controller, user_data); } c->sent_disconnect = true; } if (!c->connected) { // Done with this controller (TODO: Can we check reconnect?) continue; } auto read_move = c->read_move(); if (read_move == nullptr) { // We don't have a handle that supports reading (USB-only connection); // we call update exactly once (no new data will be reported), so that // the update method can change the LED and rumble values if (receiver->update != nullptr) { receiver->update(&c->controller, user_data); } } else { while (psmove_poll(read_move)) { if (receiver->update != nullptr) { int previous = c->controller.buttons; c->controller.buttons = psmove_get_buttons(read_move); c->controller.pressed = c->controller.buttons & ~previous; c->controller.released = previous & ~c->controller.buttons; c->controller.trigger = float(psmove_get_trigger(read_move)) / 255.f; psmove_get_accelerometer_frame(read_move, Frame_SecondHalf, &c->controller.accelerometer.x, &c->controller.accelerometer.y, &c->controller.accelerometer.z); psmove_get_gyroscope_frame(read_move, Frame_SecondHalf, &c->controller.gyroscope.x, &c->controller.gyroscope.y, &c->controller.gyroscope.z); psmove_get_magnetometer_vector(read_move, &c->controller.magnetometer.x, &c->controller.magnetometer.y, &c->controller.magnetometer.z); c->controller.battery = psmove_get_battery(read_move); receiver->update(&c->controller, user_data); } } } auto write_move = c->write_move(); if (write_move != nullptr) { psmove_set_leds(write_move, uint32_t(255 * c->controller.color.r), uint32_t(255 * c->controller.color.g), uint32_t(255 * c->controller.color.b)); psmove_set_rumble(write_move, uint32_t(255 * c->controller.rumble)); if (psmove_update_leds(write_move) == Update_Failed) { if (read_move != nullptr && write_move != read_move) { // Disconnected from USB, but we read from Bluetooth, so we // can just give up the USB connection and use Bluetooth psmove_disconnect(c->move_usb), c->move_usb = nullptr; // Now that USB is gone, clear the controller's USB flag c->controller.usb = 0; } else { c->connected = false; } } } } }
int main(int argc, char* argv[]) { PSMove *move; enum PSMove_Connection_Type ctype; int i; if (!psmove_init(PSMOVE_CURRENT_VERSION)) { fprintf(stderr, "PS Move API init failed (wrong version?)\n"); exit(1); } i = psmove_count_connected(); printf("Connected controllers: %d\n", i); move = psmove_connect(); if (move == NULL) { printf("Could not connect to default Move controller.\n" "Please connect one via USB or Bluetooth.\n"); exit(1); } char *serial = psmove_get_serial(move); printf("Serial: %s\n", serial); free(serial); ctype = psmove_connection_type(move); switch (ctype) { case Conn_USB: printf("Connected via USB.\n"); break; case Conn_Bluetooth: printf("Connected via Bluetooth.\n"); break; case Conn_Unknown: printf("Unknown connection type.\n"); break; } for (i=0; i<10; i++) { psmove_set_leds(move, 0, 255*(i%3==0), 0); psmove_set_rumble(move, 255*(i%2)); psmove_update_leds(move); psmove_usleep(10000 * (i % 10)); } for (i=250; i>=0; i-=5) { psmove_set_leds(move, i, i, 0); psmove_set_rumble(move, 0); psmove_update_leds(move); } /* Enable rate limiting for LED updates */ psmove_set_rate_limiting(move, 1); psmove_set_leds(move, 0, 0, 0); psmove_set_rumble(move, 0); psmove_update_leds(move); while (ctype != Conn_USB && !(psmove_get_buttons(move) & Btn_PS)) { int res = psmove_poll(move); if (res) { if (psmove_get_buttons(move) & Btn_TRIANGLE) { printf("Triangle pressed, with trigger value: %d\n", psmove_get_trigger(move)); psmove_set_rumble(move, psmove_get_trigger(move)); } else { psmove_set_rumble(move, 0x00); } psmove_set_leds(move, 0, 0, psmove_get_trigger(move)); int x, y, z; psmove_get_accelerometer(move, &x, &y, &z); printf("accel: %5d %5d %5d\n", x, y, z); psmove_get_gyroscope(move, &x, &y, &z); printf("gyro: %5d %5d %5d\n", x, y, z); psmove_get_magnetometer(move, &x, &y, &z); printf("magnetometer: %5d %5d %5d\n", x, y, z); printf("buttons: %x\n", psmove_get_buttons(move)); int battery = psmove_get_battery(move); if (battery == Batt_CHARGING) { printf("battery charging\n"); } else if (battery == Batt_CHARGING_DONE) { printf("battery fully charged (on charger)\n"); } else if (battery >= Batt_MIN && battery <= Batt_MAX) { printf("battery level: %d / %d\n", battery, Batt_MAX); } else { printf("battery level: unknown (%x)\n", battery); } printf("raw temperature: %d\n", psmove_get_temperature(move)); printf("celsius temperature: %f\n", psmove_get_temperature_in_celsius(move)); psmove_update_leds(move); } } psmove_disconnect(move); psmove_shutdown(); return 0; }
int main(int argc, char* argv[]) { PSMove *move; enum PSMove_Connection_Type ctype; int i; i = psmove_count_connected(); printf("Connected controllers: %d\n", i); move = psmove_connect(); if (move == NULL) { printf("Could not connect to default Move controller.\n" "Please connect one via USB or Bluetooth.\n"); exit(1); } ctype = psmove_connection_type(move); switch (ctype) { case Conn_USB: printf("Connected via USB.\n"); break; case Conn_Bluetooth: printf("Connected via Bluetooth.\n"); break; case Conn_Unknown: printf("Unknown connection type.\n"); break; } if (ctype == Conn_USB) { PSMove_Data_BTAddr addr; psmove_read_btaddrs(move, &addr, NULL); printf("Current BT Host: "); for (i=0; i<6; i++) { printf("%02x ", addr[i]); } printf("\n"); #if 0 /* This is the easy method (pair to this host): */ if (psmove_pair(move)) { printf("Paired. Press the PS Button now :)\n"); } else { printf("psmove_pair() failed :/\n"); } /* This is the advanced method: */ /* Example BT Address: 01:23:45:67:89:ab */ const PSMove_Data_BTAddr newhost = { 0x01, 0x23, 0x45, 0x67, 0x89, 0xab, }; if (!psmove_set_btaddr(move, &newhost)) { printf("Could not set BT address!\n"); } #endif } for (i=0; i<10; i++) { psmove_set_leds(move, 0, 255*(i%3==0), 0); psmove_set_rumble(move, 255*(i%2)); psmove_update_leds(move); usleep(10000*(i%10)); } for (i=250; i>=0; i-=5) { psmove_set_leds(move, i, i, 0); psmove_set_rumble(move, 0); psmove_update_leds(move); } /* Enable rate limiting for LED updates */ psmove_set_rate_limiting(move, 1); psmove_set_leds(move, 0, 0, 0); psmove_set_rumble(move, 0); psmove_update_leds(move); while (!(psmove_get_buttons(move) & Btn_PS)) { int res = psmove_poll(move); if (res) { if (psmove_get_buttons(move) & Btn_TRIANGLE) { printf("Triangle pressed, with trigger value: %d\n", psmove_get_trigger(move)); psmove_set_rumble(move, psmove_get_trigger(move)); } else { psmove_set_rumble(move, 0x00); } psmove_set_leds(move, 0, 0, psmove_get_trigger(move)); int x, y, z; psmove_get_accelerometer(move, &x, &y, &z); printf("accel: %5d %5d %5d\n", x, y, z); psmove_get_gyroscope(move, &x, &y, &z); printf("gyro: %5d %5d %5d\n", x, y, z); psmove_get_magnetometer(move, &x, &y, &z); printf("magnetometer: %5d %5d %5d\n", x, y, z); printf("buttons: %x\n", psmove_get_buttons(move)); int battery = psmove_get_battery(move); if (battery == Batt_CHARGING) { printf("battery charging\n"); } else if (battery >= Batt_MIN && battery <= Batt_MAX) { printf("battery level: %d / %d\n", battery, Batt_MAX); } else { printf("battery level: unknown (%x)\n", battery); } printf("temperature: %d\n", psmove_get_temperature(move)); psmove_update_leds(move); } } psmove_disconnect(move); return 0; }
uint32 FPSMoveWorker::Run() { if (!psmove_init(PSMOVE_CURRENT_VERSION)) { UE_LOG(LogPSMove, Error, TEXT("PS Move API init failed (wrong version?)")); return -1; } // I want the psmoves and psmove_tracker to be local variables in the thread. // Initialize an empty array of psmove controllers PSMove* psmoves[FPSMoveWorker::k_max_controllers]; memset(psmoves, 0, sizeof(psmoves)); // Initialize and configure the psmove_tracker PSMoveTracker *psmove_tracker = psmove_tracker_new(); // Unfortunately the API does not have a way to change the resolution and framerate. PSMoveFusion *psmove_fusion = psmove_fusion_new(psmove_tracker, 1., 1000.); int tracker_width = 640; int tracker_height = 480; if (psmove_tracker) { UE_LOG(LogPSMove, Log, TEXT("PSMove tracker initialized.")); //Set exposure. TODO: Expose this to component. psmove_tracker_set_exposure(psmove_tracker, Exposure_LOW); //Exposure_LOW, Exposure_MEDIUM, Exposure_HIGH psmove_tracker_set_smoothing(psmove_tracker, 0, 1); psmove_tracker_set_mirror(psmove_tracker, PSMove_True); psmove_tracker_get_size(psmove_tracker, &tracker_width, &tracker_height); UE_LOG(LogPSMove, Log, TEXT("Camera Dimensions: %d x %d"), tracker_width, tracker_height); } else { UE_LOG(LogPSMove, Log, TEXT("PSMove tracker failed to initialize.")); } //Initial wait before starting. FPlatformProcess::Sleep(0.03); float xcm, ycm, zcm, oriw, orix, oriy, oriz; while (StopTaskCounter.GetValue() == 0) { // Get positional data from tracker if (psmove_tracker) { // Setup or tear down controller connections based on the number of active controllers UpdateControllerConnections(psmove_tracker, psmoves); // Renew the image on camera if (PSMoveCount > 0) { psmove_tracker_update_image(psmove_tracker); // Sometimes libusb crashes here. psmove_tracker_update_cbb(psmove_tracker, NULL); // Passing null (instead of m_moves[i]) updates all controllers. } } else { FPlatformProcess::Sleep(0.001); } for (int i = 0; i < PSMoveCount; i++) { FPSMoveRawControllerData_TLS &localControllerData = WorkerControllerDataArray[i]; //-------------- // Read the published data from the component //-------------- localControllerData.WorkerRead(); // Get positional data from tracker if (psmove_tracker) { localControllerData.IsTracked = psmove_tracker_get_status(psmove_tracker, psmoves[i]) == Tracker_TRACKING; psmove_fusion_get_transformed_location(psmove_fusion, psmoves[i], &xcm, &ycm, &zcm); if (localControllerData.IsTracked && xcm && ycm && zcm && !isnan(xcm) && !isnan(ycm) && !isnan(zcm) && xcm == xcm && ycm == ycm && zcm == zcm) { localControllerData.PosX = xcm; localControllerData.PosY = ycm; localControllerData.PosZ = zcm; } else { localControllerData.IsTracked = false; } //UE_LOG(LogPSMove, Log, TEXT("X: %f, Y: %f, Z: %f"), xcm, ycm, zcm); if (localControllerData.ResetPoseRequest && localControllerData.IsTracked) { psmove_tracker_reset_location(psmove_tracker, psmoves[i]); } // If we are to change the tracked colour. if (localControllerData.UpdateLedRequest) { // Stop tracking the controller with the existing color psmove_tracker_disable(psmove_tracker, psmoves[i]); localControllerData.IsTracked = false; localControllerData.IsCalibrated = false; psmove_tracker_set_dimming(psmove_tracker, 0.0); // Set dimming to 0 to trigger blinking calibration. psmove_set_leds(psmoves[i], 0, 0, 0); // Turn off the LED to make sure it isn't trackable until new colour set. psmove_update_leds(psmoves[i]); FColor newFColor = localControllerData.LedColourRequest.Quantize(); if (psmove_tracker_enable_with_color(psmove_tracker, psmoves[i], newFColor.R, newFColor.G, newFColor.B) == Tracker_CALIBRATED) { this->WorkerControllerDataArray[i].IsCalibrated = true; } else { UE_LOG(LogPSMove, Error, TEXT("Failed to change tracking color for PSMove controller %d"), i); } localControllerData.LedColourWasUpdated = true; } else { localControllerData.LedColourWasUpdated = false; } } else { FPlatformProcess::Sleep(0.001); } // Do bluetooth IO: Orientation, Buttons, Rumble //TODO: Is it necessary to keep polling until no frames are left? while (psmove_poll(psmoves[i]) > 0) { // Update the controller status (via bluetooth) psmove_poll(psmoves[i]); // Get the controller orientation (uses IMU). psmove_get_orientation(psmoves[i], &oriw, &orix, &oriy, &oriz); localControllerData.OriW = oriw; localControllerData.OriX = orix; localControllerData.OriY = oriy; localControllerData.OriZ = oriz; //UE_LOG(LogPSMove, Log, TEXT("Ori w,x,y,z: %f, %f, %f, %f"), oriw, orix, oriy, oriz); // Get the controller button state localControllerData.Buttons = psmove_get_buttons(psmoves[i]); // Bitwise; tells if each button is down. psmove_get_button_events(psmoves[i], &localControllerData.Pressed, &localControllerData.Released); // i.e., state change // Get the controller trigger value (uint8; 0-255) localControllerData.TriggerValue = psmove_get_trigger(psmoves[i]); // Set the controller rumble (uint8; 0-255) psmove_set_rumble(psmoves[i], localControllerData.RumbleRequest); } if (localControllerData.ResetPoseRequest) { psmove_reset_orientation(psmoves[i]); //TODO: reset yaw only localControllerData.PoseWasReset = true; } else { localControllerData.PoseWasReset = false; } //-------------- // Publish the updated worker data to the component //-------------- localControllerData.WorkerPost(); } //Sleeping the thread seems to crash libusb. //FPlatformProcess::Sleep(0.005); } // Delete the controllers for (int i = 0; i<PSMoveCount; i++) { psmove_disconnect(psmoves[i]); } // Delete the fusion if (psmove_fusion) { psmove_fusion_free(psmove_fusion); } // Delete the tracker if (psmove_tracker) { psmove_tracker_free(psmove_tracker); } psmove_shutdown(); return 0; }