Tracker::Tracker() : m_moves(NULL), m_count(psmove_count_connected()), mocapRecorder("psmovedata") {
// PSMove *move;
// 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);
// auto 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;
// }
std::cout << "PSMOVE : Initialising Tracker..." << std::endl;
m_tracker = psmove_tracker_new();
std::cout << "PSMOVE : Tracker Initialised" << std::endl;
std::cout << "PSMOVE : Initialising Fusion..." << std::endl;
m_fusion = psmove_fusion_new(m_tracker, 1., 1000.);
std::cout << "PSMOVE : Fusion Initialised" << std::endl;
psmove_tracker_set_mirror(m_tracker, PSMove_False);
psmove_tracker_set_exposure(m_tracker, Exposure_HIGH);
m_moves = (PSMove**) calloc(m_count, sizeof(PSMove*));
m_items = (int*) calloc(m_count, sizeof(int));
for (int i = 0; i < m_count; i++) {
std::cout << "PSMOVE : Initialising Controller " << i << "..." << std::endl;
m_moves[i] = psmove_connect_by_id(i);
m_items[i] = WIRE_CUBE;
psmove_enable_orientation(m_moves[i], PSMove_True);
assert(psmove_has_orientation(m_moves[i]));
std::cout << "PSMOVE : Calibrating Controller " << i << "..." << std::endl;
while (psmove_tracker_enable(m_tracker, m_moves[i]) != Tracker_CALIBRATED)
;
std::cout << "PSMOVE : Controller Calibrated " << i << "..." << std::endl;
}
// psmove_tracker_set_dimming(m_tracker,1);
std::cout << "PSMOVE INITIALISED" << std::endl;
}
Tracker::Tracker()
: m_moves(NULL),
m_count(psmove_count_connected()),
m_tracker(psmove_tracker_new()),
m_fusion(psmove_fusion_new(m_tracker, 1., 1000.))
{
psmove_tracker_set_mirror(m_tracker, PSMove_True);
psmove_tracker_set_exposure(m_tracker, Exposure_HIGH);
m_moves = (PSMove**)calloc(m_count, sizeof(PSMove*));
m_items = (int*)calloc(m_count, sizeof(int));
for (int i=0; i<m_count; i++) {
m_moves[i] = psmove_connect_by_id(i);
m_items[i] = WIRE_CUBE;
psmove_enable_orientation(m_moves[i], PSMove_True);
assert(psmove_has_orientation(m_moves[i]));
while (psmove_tracker_enable(m_tracker, m_moves[i]) != Tracker_CALIBRATED);
}
}
bool FPSMoveWorker::UpdateControllerConnections(
PSMoveTracker *Tracker,
PSMove **PSMoves)
{
bool controllerCountChanged = false;
uint32 currentTime = FPlatformTime::Cycles();
float millisecondsSinceCheck = FPlatformTime::ToMilliseconds(currentTime - this->LastMoveCountCheckTime);
if (millisecondsSinceCheck >= CONTROLLER_COUNT_POLL_INTERVAL)
{
// Update the number
int newcount = psmove_count_connected();
if (this->PSMoveCount != newcount)
{
UE_LOG(LogPSMove, Log, TEXT("PSMove Controllers count changed: %d -> %d."), this->PSMoveCount, newcount);
this->PSMoveCount = newcount;
controllerCountChanged = true;
}
// Refresh the connection and tracking state of every controller entry
for (int psmove_id = 0; psmove_id < FPSMoveWorker::k_max_controllers; psmove_id++)
{
if (psmove_id < this->PSMoveCount)
{
if (PSMoves[psmove_id] == NULL)
{
// The controller should be connected
PSMoves[psmove_id] = psmove_connect_by_id(psmove_id);
if (PSMoves[psmove_id] != NULL)
{
psmove_enable_orientation(PSMoves[psmove_id], PSMove_True);
assert(psmove_has_orientation(PSMoves[psmove_id]));
this->WorkerControllerDataArray[psmove_id].IsConnected = true;
}
else
{
this->WorkerControllerDataArray[psmove_id].IsConnected = false;
UE_LOG(LogPSMove, Error, TEXT("Failed to connect to PSMove controller %d"), psmove_id);
}
}
if (PSMoves[psmove_id] != NULL && this->WorkerControllerDataArray[psmove_id].IsCalibrated == false)
{
PSMoveTracker_Status tracking_status = psmove_tracker_enable(Tracker, PSMoves[psmove_id]);
psmove_tracker_set_auto_update_leds(Tracker, PSMoves[psmove_id], PSMove_True);
if (tracking_status == Tracker_CALIBRATED)
{
this->WorkerControllerDataArray[psmove_id].IsCalibrated = true;
}
else
{
UE_LOG(LogPSMove, Error, TEXT("Failed to enable tracking for PSMove controller %d (result status: %d)"), psmove_id, (int32)tracking_status);
}
}
}
else
{
// The controller should no longer be tracked
if (PSMoves[psmove_id] != NULL)
{
psmove_disconnect(PSMoves[psmove_id]);
PSMoves[psmove_id] = NULL;
this->WorkerControllerDataArray[psmove_id].IsTracked = false;
this->WorkerControllerDataArray[psmove_id].IsCalibrated = false;
this->WorkerControllerDataArray[psmove_id].IsConnected = false;
}
}
}
// Remember the last time we polled the move count
this->LastMoveCountCheckTime = currentTime;
}
return controllerCountChanged;
}