SWSensor.cpp

#ifdef _WIN32
 
#include <windows.h>
#include <initguid.h>
#include <propkeydef.h>
#include "Sensors.h"
#include "Sensorsapi.h"
 
#include <chrono>
#include "SSensor.h"
#include "SWSensor.h"
 
  /* * * * * * * * * * * * * SENSOR * * * * * * * * * * * * */
 
SSensorManager* SWSensorManager::mSingletonInstance;
 
SWSensor::SWSensor() : SSensor()
{
  mISensor = NULL;
}
 
SWSensor::SWSensor(SSensorType type) : SSensor(type)
{
  mISensor = NULL;
}
 
SWSensor::~SWSensor()
{
  SAFE_RELEASE(mISensor);
}
 
SSensorData SWSensor::GetData()
{
  SSensorData sensorData;
  memset(&sensorData, 0, sizeof(sensorData));
 
  if (mEnabled && mISensor)
  {
    SENSOR_TYPE_ID iSensorType;
    mISensor->GetType(&iSensorType);
    if (IsEqualGUID(iSensorType, SENSOR_TYPE_ACCELEROMETER_3D))
    {
      // GET DATA
      ISensorDataReport* sensorReport(NULL);
      HRESULT hr = S_OK;
      hr = mISensor->GetData(&sensorReport);
      if (!SUCCEEDED(hr))
      {
        // TODO: ERROR! NO DATA AVAILABLE
      }
      PROPVARIANT x, y, z, tm;
      hr = sensorReport->GetSensorValue(SENSOR_DATA_TYPE_TIMESTAMP, &tm);
      hr = hr && sensorReport->GetSensorValue(SENSOR_DATA_TYPE_ACCELERATION_X_G, &x);
      hr = hr && sensorReport->GetSensorValue(SENSOR_DATA_TYPE_ACCELERATION_Y_G, &y);
      hr = hr && sensorReport->GetSensorValue(SENSOR_DATA_TYPE_ACCELERATION_Z_G, &z);
      if (!SUCCEEDED(hr))
      {
        // TODO: ERROR! NO DATA AVAILABLE
      }
 
      sensorData.delta = GetElapsedTime(std::chrono::duration<int64_t, std::nano>(std::chrono::system_clock::now().time_since_epoch()).count()); //tm.filetime
      sensorData.vec3 = Vector3d(x.dblVal, y.dblVal, z.dblVal);
      CallSensorCb(sensorData);
    }
    else if (IsEqualGUID(iSensorType, SENSOR_TYPE_COMPASS_3D))
    {
      // GET DATA
      ISensorDataReport* sensorReport(NULL);
      HRESULT hr = S_OK;
      hr = mISensor->GetData(&sensorReport);
      if (!SUCCEEDED(hr))
      {
        // TODO: ERROR! NO DATA AVAILABLE
      }
      PROPVARIANT x, y, z, tm;
      hr = sensorReport->GetSensorValue(SENSOR_DATA_TYPE_TIMESTAMP, &tm);
      hr = hr && sensorReport->GetSensorValue(SENSOR_DATA_TYPE_MAGNETIC_HEADING_X_DEGREES, &x);
      hr = hr && sensorReport->GetSensorValue(SENSOR_DATA_TYPE_MAGNETIC_HEADING_Y_DEGREES, &y);
      hr = hr && sensorReport->GetSensorValue(SENSOR_DATA_TYPE_MAGNETIC_HEADING_Z_DEGREES, &z);
      if (!SUCCEEDED(hr))
      {
        // TODO: ERROR! NO DATA AVAILABLE
      }
 
      sensorData.delta = GetElapsedTime(std::chrono::duration<int64_t, std::nano>(std::chrono::system_clock::now().time_since_epoch()).count()); //tm.filetime
      sensorData.vec3 = Vector3d(x.dblVal, y.dblVal, z.dblVal);
      CallSensorCb(sensorData);
    }
    else if (IsEqualGUID(iSensorType, SENSOR_TYPE_GYROMETER_3D))
    {
      // GET DATA
      ISensorDataReport* sensorReport(NULL);
      HRESULT hr = S_OK;
      hr = mISensor->GetData(&sensorReport);
      if (!SUCCEEDED(hr))
      {
        // TODO: ERROR! NO DATA AVAILABLE
      }
      PROPVARIANT x, y, z, tm;
      hr = sensorReport->GetSensorValue(SENSOR_DATA_TYPE_TIMESTAMP, &tm);
      hr = hr && sensorReport->GetSensorValue(SENSOR_DATA_TYPE_ANGULAR_ACCELERATION_X_DEGREES_PER_SECOND_SQUARED, &x);
      hr = hr && sensorReport->GetSensorValue(SENSOR_DATA_TYPE_ANGULAR_ACCELERATION_Y_DEGREES_PER_SECOND_SQUARED, &y);
      hr = hr && sensorReport->GetSensorValue(SENSOR_DATA_TYPE_ANGULAR_ACCELERATION_Z_DEGREES_PER_SECOND_SQUARED, &z);
      if (!SUCCEEDED(hr))
      {
        // TODO: ERROR! NO DATA AVAILABLE
      }
 
      sensorData.delta = GetElapsedTime(std::chrono::duration<int64_t, std::nano>(std::chrono::system_clock::now().time_since_epoch()).count()); //tm.filetime
      sensorData.vec3 = Vector3d(x.dblVal, y.dblVal, z.dblVal);
      CallSensorCb(sensorData);
    }
    else if (IsEqualGUID(iSensorType, SENSOR_TYPE_AMBIENT_LIGHT))
    {
      // GET DATA
      ISensorDataReport* sensorReport(NULL);
      HRESULT hr = S_OK;
      hr = mISensor->GetData(&sensorReport);
      if (!SUCCEEDED(hr))
      {
        // TODO: ERROR! NO DATA AVAILABLE
      }
      PROPVARIANT l, tm;
      hr = sensorReport->GetSensorValue(SENSOR_DATA_TYPE_TIMESTAMP, &tm);
      hr = hr && sensorReport->GetSensorValue(SENSOR_DATA_TYPE_LIGHT_LEVEL_LUX, &l);
      if (!SUCCEEDED(hr))
      {
        // TODO: ERROR! NO DATA AVAILABLE
      }
 
      sensorData.delta = GetElapsedTime(std::chrono::duration<int64_t, std::nano>(std::chrono::system_clock::now().time_since_epoch()).count()); //tm.filetime
      sensorData.fval = (float) l.dblVal;
      CallSensorCb(sensorData);
    }
 
    // >>>>> ADD HERE NEW SENSOR TYPES TO BE SUPPORTED <<<<<
 
    else
    {
      // TODO ERROR: INVALID SENSOR TYPE 
    }
  }
 
  return sensorData;
}
 
void SWSensor::SetISensor(ISensor* isensor)
{
  mISensor = isensor;
}
 
ISensor* SWSensor::GetISensor()
{
  return mISensor;
}
 
  /* * * * * * * * * * * * * SENSOR MANAGER * * * * * * * * * * * * */
 
SWSensorManager::SWSensorManager() : SSensorManager()
{
  CoInitialize(NULL);
 
        // ISensorManager creation
  HRESULT hr = CoCreateInstance(CLSID_SensorManager, 
                   NULL, CLSCTX_INPROC_SERVER, // is agregate? | context in which launch the code
                   IID_ISensorManager, (void**)&mISensorManager);
  if (hr == S_OK)
  {
    // Making list of available ISensors
    ISensorCollection * sensorList;
    hr = mISensorManager->GetSensorsByCategory(SENSOR_CATEGORY_ALL, &sensorList);
    if (hr == S_OK)
    {
      ULONG sensorCount(0);
      sensorList->GetCount(&sensorCount);
      MMechostr(MSKDEBUG, "SWSensorManager() : %d sensors has been found", sensorCount);
 
      // Setting sensor list for scol
      for (unsigned int i = 0; i < sensorCount; i++)
      {
        ISensor* wsensor;
        hr = sensorList->GetAt(i, &wsensor);
        if (hr == S_OK)
        {
          SENSOR_TYPE_ID iSensorType;
          wsensor->GetType(&iSensorType);
 
          if (IsEqualGUID(iSensorType, SENSOR_TYPE_ACCELEROMETER_3D))
          {
            MMechostr(MSKDEBUG, "SWSensorManager() : an accelerometer has been found (%d)", i);
            SWSensor* sensor = new SWSensor(SSENSOR_TYPE_ACCELEROMETER);
            sensor->SetISensor(wsensor);
            mSensorList.insert(sensor);
            SetSensorDelay(sensor, 10); // 100Hz in ms
          }
          else if (IsEqualGUID(iSensorType, SENSOR_TYPE_COMPASS_3D))
          {
            MMechostr(MSKDEBUG, "SWSensorManager() : an magnetometer has been found (%d)", i);
            SWSensor* sensor = new SWSensor(SSENSOR_TYPE_MAGNETIC_FIELD);
            sensor->SetISensor(wsensor);
            mSensorList.insert(sensor);
            SetSensorDelay(sensor, 10); // 100Hz in ms
          }
          else if (IsEqualGUID(iSensorType, SENSOR_TYPE_GYROMETER_3D))
          {
            MMechostr(MSKDEBUG, "SWSensorManager() : an gyrometer has been found (%d)", i);
            SWSensor* sensor = new SWSensor(SSENSOR_TYPE_GYROSCOPE);
            sensor->SetISensor(wsensor);
            mSensorList.insert(sensor);
            SetSensorDelay(sensor, 10); // 100Hz in ms
          }
          else if (IsEqualGUID(iSensorType, SENSOR_TYPE_AMBIENT_LIGHT))
          {
            MMechostr(MSKDEBUG, "SWSensorManager() : a light sensor has been found (%d)", i);
            SWSensor* sensor = new SWSensor(SSENSOR_TYPE_LIGHT);
            sensor->SetISensor(wsensor);
            mSensorList.insert(sensor);
            SetSensorDelay(sensor, 10); // 100Hz in ms
          }
 
          // >>>>> ADD HERE NEW SENSOR TYPES TO BE SUPPORTED <<<<<
 
          else
          {
            // TODO ERROR: INVALID SENSOR TYPE 
            MMechostr(MSKDEBUG, "SSensorManager() : an unknown sensor has been found (%d)", i);
          }
        }
        else
        {
          // TODO: ERROR ON ISENSOR ACCESS
        }
      }
      SAFE_RELEASE(sensorList);
    }
    else
    {
      // TODO: ERROR ON SENSOR LIST
    }
  }
  else
  {
    // TODO: ERROR ON ISENSOR MANAGER
  }
}
 
SWSensorManager::~SWSensorManager()
{
  CoUninitialize();
 
  for (std::set<SSensor*>::iterator it = mSensorList.begin(); it != mSensorList.end(); ++it)
  {
    SWSensor* sensor = (SWSensor*) (*it);
    SAFE_DELETE(sensor);
  }
 
  mSingletonInstance = NULL;
}
 
int SWSensorManager::SetSensorDelay(SSensor* sensor, unsigned long millisecondsDelay)
{
  if (sensor)
  {
    ISensor* wsensor = ((SWSensor*)sensor)->GetISensor();
 
    IPortableDeviceValues* propsToSet = NULL;
    IPortableDeviceValues* propsReturn = NULL;
 
    HRESULT hr = CoCreateInstance(__uuidof(PortableDeviceValues), NULL, CLSCTX_INPROC_SERVER, IID_PPV_ARGS(&propsToSet));
    if (SUCCEEDED(hr))
    {
      // Set new delay to set
      hr = propsToSet->SetUnsignedIntegerValue(SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL, millisecondsDelay);
      if (SUCCEEDED(hr))
      {
        // Apply modification on sensor
        hr = wsensor->SetProperties(propsToSet, &propsReturn);
        if (hr == S_FALSE)
        {
          HRESULT hrError = S_OK;
          hr = propsReturn->GetErrorValue(SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL, &hrError);
          if (SUCCEEDED(hr))
          {
            wprintf_s(L"\nSetting current report interval failed with error 0x%X\n", hrError);
            hr = hrError;
          }
        }
        else if (hr == E_ACCESSDENIED)
        {
          // No permission. Take appropriate action.
        }
      }
    }
 
    SAFE_RELEASE(propsToSet);
    SAFE_RELEASE(propsReturn);
 
    return hr;
  }
  else
  {
    // TODO: ERROR INVALID SENSOR
    return -1;
  }
}
 
int SWSensorManager::SetSensorEnable(SSensorType sensorType, bool state)
{
  if (IsAvailable(sensorType))
    return 0;
  else
    // ERROR: THE SENSOR IS NOT AVAILABLE
    return -1;
}
 
void SWSensorManager::Vibrate(long millis)
{
 
}
 
void SWSensorManager::StartVibration()
{
 
}
 
void SWSensorManager::StopVibration()
{
 
}
 
void SWSensorManager::VibratePattern(std::vector<int> pattern, bool loop)
{
 
}
 
void SWSensorManager::StartLocationService()
{
 
}
 
void SWSensorManager::StopLocationService()
{
 
}
 
bool SWSensorManager::GetLocation(float &longitude, float &latitude, float &altitude)
{
  return false;
}
 
#endif