Spatial.cpp

Go to the documentation of this file.

#include "Spatial.h"

namespace Devices
{
        int CCONV Spatial::AttachHandler(CPhidgetHandle IFK, void* usrptr)
        {
                Spatial* spatial = static_cast<Spatial*>(usrptr);

                int serialNo;
                const char* name;

                CPhidget_getDeviceName(IFK, &name);
                CPhidget_getSerialNumber(IFK, &serialNo);

                if (spatial->config->serialno == -1)
                        spatial->config->serialno = serialNo;

                char msg[99];
                sprintf_s(msg, 99, "Spatial::AttachHandler %s (%i) attached!", name, serialNo);
                spatial->logger->Log(msg);
                return 0;
        }

        int CCONV Spatial::DetachHandler(CPhidgetHandle IFK, void* usrptr)
        {
                Spatial* spatial = static_cast<Spatial*>(usrptr);

                int serialNo;
                const char* name;

                CPhidget_getDeviceName(IFK, &name);
                CPhidget_getSerialNumber(IFK, &serialNo);

                char msg[99];
                sprintf_s(msg, 99, "Spatial::DetachHandler %s (%i) detached!", name, serialNo);
                spatial->logger->Log(msg, Logger::Error);

                return 0;
        }

        int CCONV Spatial::ErrorHandler(CPhidgetHandle IFK, void* usrptr, int ErrorCode, const char* unknown)
        {
                Spatial* spatial = static_cast<Spatial*>(usrptr);
                char msg[99];
                sprintf_s(msg, 99, "Spatial::ErrorHandler Error handled. %s", unknown);
                spatial->logger->Log(msg, Logger::Error, ErrorCode);
                return 0;
        }

        int CCONV Spatial::SpatialDataHandler(CPhidgetSpatialHandle IFK, void* usrptr, CPhidgetSpatial_SpatialEventDataHandle* data, int count)
        {
                Spatial* spatial = static_cast<Spatial*>(usrptr);

                for (int i = 0; i < count; i++)
                {
                        spatial->cumulativeAcceleration += D3DXVECTOR3(
                                static_cast<float>(data[i]->acceleration[spatial->xyzMapping[0]]) + spatial->config->accelerationAxes[spatial->xyzMapping[0]].bias,
                                static_cast<float>(data[i]->acceleration[spatial->xyzMapping[1]]) + spatial->config->accelerationAxes[spatial->xyzMapping[1]].bias,
                                static_cast<float>(data[i]->acceleration[spatial->xyzMapping[2]]) + spatial->config->accelerationAxes[spatial->xyzMapping[2]].bias);
                        spatial->cumulativeAccelSamples++;
                }

                return 0;
        }

        //Display the properties of the attached phidget to the screen.  We will be displaying the name, serial number and version of the attached device.
        //Will also display the number of inputs, outputs, and analog inputs on the interface kit as well as the state of the ratiometric flag
        //and the current analog sensor sensitivity.
        int Spatial::display_properties(CPhidgetHandle phid) const
        {
                int serialNo, version;
                const char* ptr;
                int numAccelAxes, numGyroAxes, numCompassAxes, dataRateMax, dataRateMin;

                CPhidget_getDeviceType(phid, &ptr);
                CPhidget_getSerialNumber(phid, &serialNo);
                CPhidget_getDeviceVersion(phid, &version);
                CPhidgetSpatial_getAccelerationAxisCount(reinterpret_cast<CPhidgetSpatialHandle>(phid), &numAccelAxes);
                CPhidgetSpatial_getGyroAxisCount(reinterpret_cast<CPhidgetSpatialHandle>(phid), &numGyroAxes);
                CPhidgetSpatial_getCompassAxisCount(reinterpret_cast<CPhidgetSpatialHandle>(phid), &numCompassAxes);
                CPhidgetSpatial_getDataRateMax(reinterpret_cast<CPhidgetSpatialHandle>(phid), &dataRateMax);
                CPhidgetSpatial_getDataRateMin(reinterpret_cast<CPhidgetSpatialHandle>(phid), &dataRateMin);

                char msg[99];
                sprintf_s(msg, 99, "Spatial::display_properties Type: %s", ptr);
                logger->Log(msg);
                sprintf_s(msg, 99, "Spatial::display_properties Serial Number: %i", serialNo);
                logger->Log(msg);
                sprintf_s(msg, 99, "Spatial::display_properties Version: %i", version);
                logger->Log(msg);
                sprintf_s(msg, 99, "Spatial::display_properties Number of Accel Axes: %i", numAccelAxes);
                logger->Log(msg);
                sprintf_s(msg, 99, "Spatial::display_properties Number of Gyro Axes: %i", numGyroAxes);
                logger->Log(msg);
                sprintf_s(msg, 99, "Spatial::display_properties Number of Compass Axes: %i", numCompassAxes);
                logger->Log(msg);
                sprintf_s(msg, 99, "Spatial::display_properties Maximum Datarate: %d", dataRateMax);
                logger->Log(msg);
                sprintf_s(msg, 99, "Spatial::display_properties Minimum Datarate: %d", dataRateMin);
                logger->Log(msg);

                return 0;
        }

        void Spatial::Initialize(Logger* prmLogger, SpatialConfig* prmConfig, Bus* prmBus)
        {
                logger = prmLogger;
                config = prmConfig;
                bus = prmBus;

                int result;
                const char* err;

                // Declare an spatial handle
                spatial = nullptr;
                cumulativeAcceleration = D3DXVECTOR3(0, 0, 0);
                cumulativeAccelTime = 0.0f;
                cumulativeAccelSamples = 0;
                bus->PlatformAcceleration = D3DXVECTOR3(logf(-1), logf(-1), logf(-1));

                for (UINT i = 0; i < config->accelerationAxes.size(); i++)
                {
                        if (config->accelerationAxes[i].axis == 'X') xyzMapping[0] = i;
                        if (config->accelerationAxes[i].axis == 'Y') xyzMapping[1] = i;
                        if (config->accelerationAxes[i].axis == 'Z') xyzMapping[2] = i;
                }

                CPhidgetSpatial_create(&spatial);

                //Set the handlers to be run when the device is plugged in or opened from software, unplugged or closed from software, or generates an error.
                CPhidget_set_OnAttach_Handler(reinterpret_cast<CPhidgetHandle>(spatial), AttachHandler, this);
                CPhidget_set_OnDetach_Handler(reinterpret_cast<CPhidgetHandle>(spatial), DetachHandler, this);
                CPhidget_set_OnError_Handler(reinterpret_cast<CPhidgetHandle>(spatial), ErrorHandler, this);

                //Registers a callback that will run according to the set data rate that will return the spatial data changes
                //Requires the handle for the Spatial, the callback handler function that will be called, 
                //and an arbitrary pointer that will be supplied to the callback function (may be NULL)
                CPhidgetSpatial_set_OnSpatialData_Handler(spatial, SpatialDataHandler, this);

                //open the interfacekit for device connections
                CPhidget_open(reinterpret_cast<CPhidgetHandle>(spatial), -1);

                //get the program to wait for an interface kit device to be attached
                logger->Log("Spatial::ctor Waiting for spatial to be attached....");
                result = CPhidget_waitForAttachment(reinterpret_cast<CPhidgetHandle>(spatial), 5000);
                if (result)
                {
                        CPhidget_getErrorDescription(result, &err);
                        char msg[99];
                        sprintf_s(msg, 99, "Spatial::ctor Problem waiting for attachment: %s", err);
                        logger->Log(msg, Logger::Fatal);
                        config->enable = false;
                        return;
                }

                result = CPhidgetSpatial_setDataRate(spatial, config->sampleRateMs);
                if (result)
                {
                        CPhidget_getErrorDescription(result, &err);
                        char msg[99];
                        sprintf_s(msg, 99, "Spatial::ctor Problem %s setting sample rate: %i", err, config->sampleRateMs);
                        logger->Log(msg, Logger::Warn);
                }

                // Log the properties of the attached spatial device
                display_properties(reinterpret_cast<CPhidgetHandle>(spatial));
        }

        void Spatial::Destroy() const
        {
                logger->Log("Spatial::Destroy...");
                CPhidget_close(reinterpret_cast<CPhidgetHandle>(spatial));
                CPhidget_delete(reinterpret_cast<CPhidgetHandle>(spatial));
        }

        void Spatial::FrameMove(float fElapsed)
        {
                cumulativeAccelTime += fElapsed;
                if (cumulativeAccelTime > 1.0f)
                {
                        if (cumulativeAccelSamples > 0)
                        {
                                bus->PlatformAcceleration = cumulativeAcceleration / cumulativeAccelTime / static_cast<float>(cumulativeAccelSamples);
                                cumulativeAcceleration = D3DXVECTOR3(0, 0, 0);
                                cumulativeAccelSamples = 0;
                        }
                        else
                        {
                                bus->PlatformAcceleration = D3DXVECTOR3(-999, -999, -999);
                        }
                        cumulativeAccelTime = 0.0f;
                }
        }
}