337 lines
12 KiB
C++
337 lines
12 KiB
C++
//======================================================================================================
|
|
// Copyright 2016, NaturalPoint Inc.
|
|
//======================================================================================================
|
|
|
|
#include "dllcommon.h"
|
|
#include "ExampleDevice.h"
|
|
|
|
// stl
|
|
#include <string>
|
|
#include <list>
|
|
#include <thread>
|
|
using namespace std;
|
|
|
|
// OptiTrack Peripheral Device API
|
|
#include "AnalogChannelDescriptor.h"
|
|
#include "IDeviceManager.h"
|
|
using namespace OptiTrackPluginDevices;
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// Device Factory
|
|
//
|
|
const char* ExampleDeviceFactory::Name() const
|
|
{
|
|
// REQUIRED: factory name
|
|
return "Example";
|
|
}
|
|
|
|
unique_ptr<IDevice> ExampleDeviceFactory::Create() const
|
|
{
|
|
// Create device
|
|
ExampleDevice* pDevice = new ExampleDevice();
|
|
|
|
// REQUIRED: Set device name/model/serial
|
|
pDevice->SetProperty( AnalogSystem::cPluginDeviceBase::kNamePropName, (char*) DeviceName() ); // device name
|
|
pDevice->SetProperty( cPluginDeviceBase::kDisplayNamePropName, (char*) DeviceName() ); // device display name
|
|
pDevice->SetProperty( cPluginDeviceBase::kModelPropName, "ExampleModel" ); // device model
|
|
char szValue[MAX_PATH];
|
|
sprintf_s( szValue, "%s-serial", DeviceName() );
|
|
pDevice->SetProperty( cPluginDeviceBase::kSerialPropName, szValue ); // device serial (must be unique)
|
|
|
|
// OPTIONAL: Add data channels
|
|
int channelIndex;
|
|
channelIndex = pDevice->AddChannelDescriptor( "ExampleChannel1", ChannelType_Float );
|
|
channelIndex = pDevice->AddChannelDescriptor( "ExampleChannel2", ChannelType_Float );
|
|
|
|
// OPTIONAL: Set some property values
|
|
pDevice->SetProperty( cPluginDeviceBase::kRatePropName, 100.0 );
|
|
pDevice->SetProperty(cPluginDeviceBase::kMocapRateMultiplePropName, 2.0);
|
|
pDevice->SetProperty( ExampleDeviceProp_Property1, "NewValue" );
|
|
|
|
for (int i = 0; i <= channelIndex; i++)
|
|
{
|
|
// data channel enabled by default
|
|
pDevice->ChannelDescriptor(i)->SetProperty(ChannelProp_Enabled, true);
|
|
|
|
// hide unused channel properties
|
|
pDevice->ChannelDescriptor(i)->ModifyProperty(ChannelProp_Units, true, true);
|
|
pDevice->ChannelDescriptor(i)->ModifyProperty(ChannelProp_MinVoltage, true, true);
|
|
pDevice->ChannelDescriptor(i)->ModifyProperty(ChannelProp_MaxVoltage, true, true);
|
|
pDevice->ChannelDescriptor(i)->ModifyProperty(ChannelProp_TerminalName, true, true);
|
|
pDevice->ChannelDescriptor(i)->ModifyProperty(ChannelProp_TerminalType, true, true);
|
|
pDevice->ChannelDescriptor(i)->ModifyProperty(ChannelProp_MaxVoltage, true, true);
|
|
}
|
|
|
|
// Transfer ownership of the created device to the host, Motive.
|
|
unique_ptr<IDevice> ptrDevice( pDevice );
|
|
return ptrDevice;
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// Device
|
|
//
|
|
ExampleDevice::ExampleDevice() :
|
|
mCollecting( false ),
|
|
mCollectionThread( nullptr )
|
|
{
|
|
SetDeviceProperties();
|
|
}
|
|
|
|
ExampleDevice::~ExampleDevice()
|
|
{
|
|
}
|
|
|
|
void ExampleDevice::SetDeviceProperties()
|
|
{
|
|
// OPTIONAL: Add custom properties
|
|
AddProperty( ExampleDeviceProp_Property1, 33.0, "Settings" );
|
|
ModifyProperty( ExampleDeviceProp_Property1, false, false );
|
|
AddProperty( ExampleDeviceProp_Property2, "ExampleValue", "Custom Settings" );
|
|
ModifyProperty( ExampleDeviceProp_Property2, true, false );
|
|
|
|
// OPTIONAL: Hide unrelated properties
|
|
ModifyProperty(cPluginDeviceBase::kOrderPropName, true, true); // device serial
|
|
ModifyProperty(cPluginDeviceBase::kMasterSerialPropName, true, true); // master device serial
|
|
ModifyProperty(cPluginDeviceBase::kUseExternalClockPropName, true, true); // clock sync devices
|
|
ModifyProperty(cPluginDeviceBase::kExternalTriggerTerminalPropName, true, true); // trigger sync devices
|
|
ModifyProperty(cPluginDeviceBase::kCalOffsetPropName, true, true); // force plates
|
|
ModifyProperty(cPluginDeviceBase::kCalSquareRotationPropName, true, true); // force plates
|
|
ModifyProperty(cPluginDeviceBase::kZeroPropName, true, true); // zero
|
|
ModifyProperty(cPluginDeviceBase::kAssetPropName, true, true); // gloves. paired skeleton
|
|
ModifyProperty(cPluginDeviceBase::kUserDataPropName, true, true); // reserved
|
|
}
|
|
|
|
// OPTIONAL: handle property change events
|
|
void ExampleDevice::OnPropertyChanged( const char* propertyName )
|
|
{
|
|
LogError( MessageType_StatusInfo, "[Example Device] Property changed (%s)", propertyName );
|
|
}
|
|
|
|
// OPTIONAL: handle channel property change events
|
|
void ExampleDevice::OnChannelPropertyChanged( const char* channelName, const char* propertyName )
|
|
{
|
|
AnalogChannelDescriptor* pChannel = ChannelDescriptor( channelName );
|
|
|
|
// Example : Handle when a user changes a channel's enabled state in Motive
|
|
if( strcmp( propertyName, ChannelProp_Enabled ) == 0 )
|
|
{
|
|
bool enabled;
|
|
pChannel->GetProperty( ChannelProp_Enabled, enabled );
|
|
LogError( MessageType_StatusInfo, "[Example Device ] Channel Enabled State Changed (Channel:%s Enabled:%d)", propertyName, (int) enabled );
|
|
}
|
|
}
|
|
|
|
bool ExampleDevice::Configure()
|
|
{
|
|
bool success = Deconfigure();
|
|
if( !success )
|
|
return false;
|
|
|
|
// update device's buffer allocation based on current channel and data type configuration
|
|
success = __super::Configure();
|
|
if( !success )
|
|
return false;
|
|
|
|
// user specified enabled channels in Motive
|
|
int nChannels = 0;
|
|
for( int i = 0; i < ChannelDescriptorCount(); i++ )
|
|
{
|
|
AnalogChannelDescriptor* pChannelDesc = ChannelDescriptor( i );
|
|
bool enabled;
|
|
pChannelDesc->GetProperty( ChannelProp_Enabled, enabled );
|
|
if( enabled )
|
|
{
|
|
// optional - do something when channel enabled state changes
|
|
}
|
|
}
|
|
|
|
return success;
|
|
}
|
|
|
|
bool ExampleDevice::Deconfigure()
|
|
{
|
|
bool success = __super::Deconfigure();
|
|
return success;
|
|
}
|
|
|
|
bool ExampleDevice::StartCapture()
|
|
{
|
|
bool success = __super::StartCapture();
|
|
|
|
// REQUIRED: Start collecting data
|
|
// EXAMPLE: Start collecting from device using a polling thread
|
|
mCollecting = true;
|
|
mCollectionThread = CreateThread( nullptr, 0, CollectionThread, this, 0, nullptr );
|
|
if( mCollectionThread == nullptr )
|
|
{
|
|
return false;
|
|
mCollecting = false;
|
|
}
|
|
|
|
return success;
|
|
}
|
|
|
|
bool ExampleDevice::StopCapture()
|
|
{
|
|
bool success = __super::StopCapture();
|
|
|
|
// REQUIRED: Stop collecting data. Terminate hardware device polling thread
|
|
mCollecting = false;
|
|
DWORD waitResult = WaitForSingleObject( mCollectionThread, 1000 );
|
|
if( waitResult == WAIT_OBJECT_0 )
|
|
{
|
|
CloseHandle( mCollectionThread );
|
|
mCollectionThread = nullptr;
|
|
success = true;
|
|
}
|
|
else if( waitResult == WAIT_TIMEOUT )
|
|
{
|
|
BOOL result = TerminateThread( mCollectionThread, 0 );
|
|
success = ( result == TRUE );
|
|
}
|
|
else
|
|
{
|
|
success = false;
|
|
}
|
|
|
|
return success;
|
|
}
|
|
|
|
// EXAMPLE: Hardware device polling thread
|
|
unsigned long ExampleDevice::CollectionThread( void* Context )
|
|
{
|
|
ExampleDevice* pThis = static_cast<ExampleDevice*>( Context );
|
|
return pThis->DoCollectionThread();
|
|
}
|
|
|
|
|
|
unsigned long ExampleDevice::DoCollectionThread()
|
|
{
|
|
// simulated hardware device data structure
|
|
const int maxFrames = 10;
|
|
const int maxChannels = 5;
|
|
float deviceData[maxFrames][maxChannels];
|
|
int channelsRead, framesRead;
|
|
|
|
// simulate hardware device timing on windows
|
|
std::chrono::high_resolution_clock::time_point start, end;
|
|
std::chrono::milliseconds actualFrameDurationMS;
|
|
|
|
double deviceRate;
|
|
GetProperty( cPluginDeviceBase::kRatePropName, deviceRate );
|
|
double requestedRateMS = ( 1.0f / deviceRate ) * 1000.0f;
|
|
double adjustment = 0.0;
|
|
double durationDeltaMS = 0.0;
|
|
|
|
while( mCollecting )
|
|
{
|
|
start = std::chrono::high_resolution_clock::now();
|
|
|
|
// simulate reading data from hardware device
|
|
ReadDataFromHardware( deviceData, channelsRead, framesRead );
|
|
|
|
if( ( framesRead > 0 ) && ( channelsRead > 0 ) )
|
|
{
|
|
// copy data from hardware device to PluginDevice frame buffer
|
|
for( int iFrame = 0; iFrame < framesRead; iFrame++ )
|
|
{
|
|
// Retrieve the next available data slot from this Device's ring buffer,
|
|
// which is automatically allocated during the base Configure() routine
|
|
// based on selected data type and number of data channels.
|
|
//
|
|
// A DeviceFrameID is required for later use by the host/sync system during retrieval.
|
|
// The DeviceFrameID can come from the hardware itself, or from a simple software counter.
|
|
// It is used to frame align (synchronize) the host system with this device
|
|
|
|
// establish device FrameID
|
|
long deviceFrameID = this->FrameCounter(); // use software frame counter
|
|
|
|
// get frame from device buffer
|
|
AnalogFrame<float>* pFrame = this->BeginFrameUpdate( deviceFrameID );
|
|
if( pFrame )
|
|
{
|
|
// fill in frame header
|
|
int flags = 0;
|
|
pFrame->SetID( deviceFrameID );
|
|
pFrame->SetFlag( flags );
|
|
|
|
// fill in frame data
|
|
memcpy( pFrame->ChannelData(), &deviceData[iFrame], channelsRead * sizeof( float ) );
|
|
|
|
// update device buffer and unlock
|
|
this->EndFrameUpdate();
|
|
|
|
// update frame counter
|
|
this->SetFrameCounter( deviceFrameID + 1 );
|
|
|
|
// test: check value copy
|
|
float src = deviceData[iFrame][0];
|
|
const AnalogFrame<float>* pTestFrame = this->GetFrameByDeviceFrameID( pFrame->ID() );
|
|
float dst = pTestFrame->ChannelData()[0];
|
|
if( src != dst )
|
|
{
|
|
this->LogError( MessageType_StatusError, "[Example Device] Error in data copy (source:%d, target:%d", src, dst );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
this->LogError( MessageType_StatusError, "[Example Device] Unable to get frame from buffer (frameID:%d)", deviceFrameID );
|
|
}
|
|
}
|
|
}
|
|
|
|
// End frame update. Sleep the thread for the remaining frame period.
|
|
std::this_thread::sleep_for(std::chrono::milliseconds((int)(requestedRateMS - adjustment)));
|
|
end = std::chrono::high_resolution_clock::now();
|
|
actualFrameDurationMS = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
|
|
durationDeltaMS = actualFrameDurationMS.count() - requestedRateMS;
|
|
|
|
// calculating offset adjustment from the delta to apply to the next frame.
|
|
if (durationDeltaMS > 1.0)
|
|
adjustment = -1.0;
|
|
else if (durationDeltaMS > 2.0)
|
|
adjustment = -2.0;
|
|
else if (durationDeltaMS < -2.0)
|
|
adjustment = 2.0;
|
|
else if (durationDeltaMS < -1.0)
|
|
adjustment = 1.0;
|
|
else
|
|
adjustment = 0.0;
|
|
if (fabs(durationDeltaMS) > 1.0)
|
|
{
|
|
this->LogError(MessageType_StatusInfo, "[Example Device] Device timing resolution off by %3.1f ms (requested:%3.1f, actual:%3.1f)", durationDeltaMS, requestedRateMS, (double) actualFrameDurationMS.count());
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
template <size_t N, size_t M>
|
|
void ExampleDevice::ReadDataFromHardware( float( &deviceData )[M][N], int& channelsRead, int& framesRead )
|
|
{
|
|
// EXAMPLE: generate 1 sample with 2 channels : sine/cosine wave
|
|
|
|
framesRead = 1;
|
|
channelsRead = this->ActiveChannelCount();
|
|
for( int frame = 0; frame < framesRead; frame++ )
|
|
{
|
|
for( int index = 0; index < channelsRead; index++ )
|
|
{
|
|
int channel = ChannelID( index );
|
|
float counter = (float) this->FrameCounter() / 10.0f;
|
|
float val;
|
|
if( channel == 0 )
|
|
{
|
|
val = (float) this->FrameCounter();
|
|
}
|
|
else
|
|
{
|
|
val = (float) sin( counter );
|
|
}
|
|
deviceData[frame][index] = val;
|
|
}
|
|
}
|
|
} |