private static void ControllerUpdatePositions( PSMoveWorkerSettings WorkerSettings, IntPtr psmove_tracker, // PSMoveTracker* IntPtr psmove_fusion, // PSMoveFusion* IntPtr psmove, // PSMove* PSMoveRawControllerData_Base controllerData) { // Find the sphere position in the camera PSMoveAPI.psmove_tracker_update(psmove_tracker, psmove); PSMoveTracker_Status curr_status = PSMoveAPI.psmove_tracker_get_status(psmove_tracker, psmove); // Can we actually see the controller this frame? controllerData.IsSeenByTracker = curr_status == PSMoveTracker_Status.Tracker_TRACKING; // Update the position of the controller if (controllerData.IsSeenByTracker) { float xcm = 0.0f, ycm = 0.0f, zcm = 0.0f; PSMoveAPI.psmove_fusion_get_transformed_location(psmove_fusion, psmove, ref xcm, ref ycm, ref zcm); // [Store the controller position] // Remember the position the ps move controller in either its native space // or in a transformed space if a transform file existed. controllerData.PSMovePosition = new Vector3( xcm + WorkerSettings.PSMoveOffset.X, ycm + WorkerSettings.PSMoveOffset.Y, zcm + WorkerSettings.PSMoveOffset.Z); } }
private static bool WorkerContextUpdateControllerConnections(WorkerContext context) { bool controllerCountChanged = false; if (context.moveCountCheckTimer.ElapsedMilliseconds >= WorkerContext.CONTROLLER_COUNT_POLL_INTERVAL) { // Update the number int newcount = PSMoveAPI.psmove_count_connected(); if (context.PSMoveCount != newcount) { UnityEngine.Debug.Log(string.Format("PSMove Controllers count changed: {0} -> {1}.", context.PSMoveCount, newcount)); context.PSMoveCount = newcount; controllerCountChanged = true; } // Refresh the connection and tracking state of every controller entry for (int psmove_id = 0; psmove_id < context.PSMoves.Length; psmove_id++) { if (psmove_id < context.PSMoveCount) { if (context.PSMoves[psmove_id] == IntPtr.Zero) { // The controller should be connected context.PSMoves[psmove_id] = PSMoveAPI.psmove_connect_by_id(psmove_id); if (context.PSMoves[psmove_id] != IntPtr.Zero) { PSMoveAPI.psmove_enable_orientation(context.PSMoves[psmove_id], PSMove_Bool.PSMove_True); System.Diagnostics.Debug.Assert(PSMoveAPI.psmove_has_orientation(context.PSMoves[psmove_id]) == PSMove_Bool.PSMove_True); context.WorkerControllerDataArray[psmove_id].IsConnected = true; } else { context.WorkerControllerDataArray[psmove_id].IsConnected = false; UnityEngine.Debug.LogError(string.Format("Failed to connect to PSMove controller {0}", psmove_id)); } } if (context.PSMoves[psmove_id] != IntPtr.Zero && context.WorkerControllerDataArray[psmove_id].IsTrackingEnabled == false && context.WorkerSettings.bTrackerEnabled && WorkerContextIsTrackingSetup(context)) { int happyTrackerCount = 0; // Attempt to enable any trackers that haven't successfully calibrated the controller yet for (int tracker_index = 0; tracker_index < context.TrackerCount; ++tracker_index) { PSMoveTracker_Status tracker_status = PSMoveAPI.psmove_tracker_get_status( context.PSMoveTrackers[tracker_index], context.PSMoves[psmove_id]); if (tracker_status == PSMoveTracker_Status.Tracker_CALIBRATED || tracker_status == PSMoveTracker_Status.Tracker_TRACKING) { ++happyTrackerCount; } else { // The controller is connected, but not tracking yet // Enable tracking for this controller with next available color. if (PSMoveAPI.psmove_tracker_enable( context.PSMoveTrackers[tracker_index], context.PSMoves[psmove_id]) == PSMoveTracker_Status.Tracker_CALIBRATED) { ++happyTrackerCount; } else { UnityEngine.Debug.LogError(string.Format("Failed to enable tracking for PSMove controller {0} on tracker {1}", psmove_id, tracker_index)); } } } if (happyTrackerCount >= context.TrackerCount) { context.WorkerControllerDataArray[psmove_id].IsTrackingEnabled = true; } } } else { // The controller should no longer be tracked if (context.PSMoves[psmove_id] != IntPtr.Zero) { PSMoveAPI.psmove_disconnect(context.PSMoves[psmove_id]); context.PSMoves[psmove_id] = IntPtr.Zero; context.WorkerControllerDataArray[psmove_id].IsTrackingEnabled = false; context.WorkerControllerDataArray[psmove_id].IsConnected = false; } } } // Remember the last time we polled the move count context.moveCountCheckTimer.Reset(); context.moveCountCheckTimer.Start(); } return(controllerCountChanged); }
public void ThreadUpdate() { using (new PSMoveHitchWatchdog("PSMoveWorker_ThreadUpdate", 34 * PSMoveHitchWatchdog.MICROSECONDS_PER_MILLISECOND)) { // Setup or teardown tracking based on the updated tracking state if (WorkerSettings.bTrackerEnabled && !WorkerContextIsTrackingSetup(Context)) { WorkerContextSetupTracking(WorkerSettings, Context); } else if (!WorkerSettings.bTrackerEnabled && WorkerContextIsTrackingSetup(Context)) { WorkerContextTeardownTracking(Context); } // Setup or tear down controller connections based on the number of active controllers WorkerContextUpdateControllerConnections(Context); // Renew the image on camera, if tracking is enabled if (WorkerContextIsTrackingSetup(Context)) { using (new PSMoveHitchWatchdog("PSMoveWorker_UpdateImage", 33 * PSMoveHitchWatchdog.MICROSECONDS_PER_MILLISECOND)) { for (int tracker_index = 0; tracker_index < Context.TrackerCount; ++tracker_index) { PSMoveAPI.psmove_tracker_update_image(Context.PSMoveTrackers[tracker_index]); // Sometimes libusb crashes here. } } } // Update the raw positions on the local controller data if (WorkerContextIsTrackingSetup(Context)) { for (int psmove_id = 0; psmove_id < Context.PSMoveCount; psmove_id++) { PSMoveRawControllerData_TLS localControllerData = WorkerControllerDataArray[psmove_id]; if (WorkerSettings.bTrackerEnabled) { ControllerUpdatePositions( WorkerSettings, Context.PSMoveTrackers, Context.PSMoveFusions, Context.TrackerCount, Context.PSMovePositionFilter, Context.PSMoves[psmove_id], localControllerData); } else { localControllerData.IsSeenByTracker = false; } } } // Do bluetooth IO: Orientation, Buttons, Rumble for (int psmove_id = 0; psmove_id < Context.PSMoveCount; psmove_id++) { //TODO: Is it necessary to keep polling until no frames are left? while (PSMoveAPI.psmove_poll(Context.PSMoves[psmove_id]) > 0) { PSMoveRawControllerData_TLS localControllerData = WorkerControllerDataArray[psmove_id]; // Update the controller status (via bluetooth) PSMoveAPI.psmove_poll(Context.PSMoves[psmove_id]); // Necessary to poll yet again? // Store the controller sensor data ControllerUpdateSensors(Context.PSMoves[psmove_id], localControllerData); // Store the controller orientation ControllerUpdateOrientations(Context.PSMoves[psmove_id], localControllerData); // Store the button state ControllerUpdateButtonState(Context.PSMoves[psmove_id], localControllerData); // Now read in requested changes from Component. e.g., RumbleRequest, CycleColourRequest localControllerData.WorkerRead(); // Set the controller rumble (uint8; 0-255) PSMoveAPI.psmove_set_rumble(Context.PSMoves[psmove_id], localControllerData.RumbleRequest); // Push the updated rumble state to the controller PSMoveAPI.psmove_update_leds(Context.PSMoves[psmove_id]); if (localControllerData.CycleColourRequest) { if (WorkerSettings.bTrackerEnabled) { UnityEngine.Debug.Log("PSMoveWorker:: CYCLE COLOUR"); // Attempt to cycle the color for each tracker and re-acquire tracking int happyTrackerCount = 0; for (int tracker_index = 0; tracker_index < Context.TrackerCount; ++tracker_index) { PSMoveAPI.psmove_tracker_cycle_color( Context.PSMoveTrackers[tracker_index], Context.PSMoves[psmove_id]); PSMoveTracker_Status tracker_status = PSMoveAPI.psmove_tracker_get_status( Context.PSMoveTrackers[tracker_index], Context.PSMoves[psmove_id]); if (tracker_status == PSMoveTracker_Status.Tracker_CALIBRATED || tracker_status == PSMoveTracker_Status.Tracker_TRACKING) { ++happyTrackerCount; } } // If not all trackers re-acquired, // mark the controller as no having tracking enabled, // and let WorkerContextUpdateControllerConnections() try // and reacquire next update. if (happyTrackerCount < Context.TrackerCount) { Context.WorkerControllerDataArray[psmove_id].IsTrackingEnabled = false; } } else { UnityEngine.Debug.LogWarning("PSMoveWorker:: CYCLE COLOUR ignored! Tracking is disabled!"); } localControllerData.CycleColourRequest = false; } // Publish Position, Orientation, and Button state to the concurrent data // This also publishes updated CycleColourRequest. localControllerData.WorkerPost(); } } } }