SQSensor.cpp

 
#include <map>
#include <vector>
#include <QGuiApplication>
#include <QScreen>
 
#include "SSensor.h"
#include "SQSensor.h"
#include "tools/SO3Util.h"
#include <scolPlugin.h>
 
#ifdef SCOL_STATIC
# include <QtPlugin>
 
# if defined(APPLE_IOS)
Q_IMPORT_PLUGIN(IOSSensorPlugin)
# elif defined(LINUX) || defined(RPI)
Q_IMPORT_PLUGIN(LinuxSensorPlugin)
# else
Q_IMPORT_PLUGIN(genericSensorPlugin)
# endif
 
#endif
 
/* * * * * * * * * * * * * EVENT CALLBACK * * * * * * * * * * * * */
 
SSensorManager* SQSensorManager::mSingletonInstance;
 
 
/* * * * * * * * * * * * * SENSOR * * * * * * * * * * * * */
 
SQSensor::SQSensor() : QObject(QCoreApplication::instance()), SSensor()
{
  mQSensor = NULL;
}
 
SQSensor::SQSensor(SSensorType type) : QObject(QCoreApplication::instance()), SSensor(type)
{
  switch (type)
  {
    case SSensorType::SSENSOR_TYPE_ACCELEROMETER:
    {
      mQSensor = new QAccelerometer(this);
      //static_cast<QAccelerometer*>(mQSensor)->setAccelerationMode(QAccelerometer::Combined);
      break;
    }
      
    case SSensorType::SSENSOR_TYPE_GYROSCOPE:
    {
      mQSensor = new QGyroscope(this);
      break;
    }
      
    case SSensorType::SSENSOR_TYPE_MAGNETIC_FIELD:
    {
      mQSensor = new QMagnetometer(this);
      break;
    }
      
    case SSensorType::SSENSOR_TYPE_LIGHT:
    {
      mQSensor = new QLightSensor(this);
      break;
    }
      
    case SSensorType::SSENSOR_TYPE_PROXIMITY:
    {
      mQSensor = new QProximitySensor(this);
      break;
    }
    
    default:
      mQSensor = NULL;
  }
  
  if (mQSensor)
  {
    connect(mQSensor, SIGNAL(readingChanged()), this, SLOT(updateData()));
    mQSensor->connectToBackend();
  }
}
 
SQSensor::~SQSensor()
{
  if (mQSensor)
    mQSensor->stop();
  
  SAFE_DELETE(mQSensor);
}
 
 
void SQSensor::updateData()
{
  if (mEnabled && mQSensor->isActive())
  {
    SSensorData formatedData;
    memset(&formatedData, 0, sizeof(formatedData));
    SSensorManager* manager = SSensorManager::GetInstance();
    
    QSensorReading* reading = mQSensor->reading();
    formatedData.delta = GetElapsedTime(reading->timestamp());
 
    switch (mType)
    {
      case SSensorType::SSENSOR_TYPE_ACCELEROMETER:
      {
        QAccelerometerReading* reading = static_cast<QAccelerometerReading*>(reading);
        formatedData.vec3.x = reading->x();
        formatedData.vec3.y = reading->y();
        formatedData.vec3.z = reading->z();
        
        CallSensorCb(formatedData);
        if (manager->IsEnabled(SSENSOR_TYPE_ORIENTATION))
        {
          if (OrientationTracker* orientationTracker = manager->GetOrientationTracker())
          {
            orientationTracker->processAcceleration(formatedData.vec3, GetTickCountNano());
          }
        }
        break;
      }
      case SSensorType::SSENSOR_TYPE_GYROSCOPE:
      {
        QGyroscopeReading* reading = static_cast<QGyroscopeReading*>(reading);
        //convert to radian
        formatedData.vec3.x = reading->x() * DEG_TO_RAD;
        formatedData.vec3.y = reading->y() * DEG_TO_RAD;
        formatedData.vec3.z = reading->z() * DEG_TO_RAD;
 
        CallSensorCb(formatedData);
        //qDebug("Current device gyro: x=%f y=%f z=%f", formatedData.vec3.x, formatedData.vec3.y, formatedData.vec3.z);
        if (manager->IsEnabled(SSENSOR_TYPE_ORIENTATION))
        {
          if (OrientationTracker* orientationTracker = manager->GetOrientationTracker())
          {
            orientationTracker->processGyro(formatedData.vec3, GetTickCountNano());
          }
        }
        break;
      }
      case SSensorType::SSENSOR_TYPE_MAGNETIC_FIELD:
      {
        QMagnetometerReading* reading = static_cast<QMagnetometerReading*>(reading);
        formatedData.vec3.x = reading->x();
        formatedData.vec3.y = reading->y();
        formatedData.vec3.z = reading->z();
 
        CallSensorCb(formatedData);
        /*if (manager->IsEnabled(SSENSOR_TYPE_ORIENTATION))
        {
           if (OrientationTracker* orientationTracker = manager->GetOrientationTracker())
           {
             orientationTracker->processMag(formatedData.vec3, GetTickCountNano());
           }
         }*/
        break;
      }
      case SSensorType::SSENSOR_TYPE_LIGHT:
      {
        QLightReading* reading = static_cast<QLightReading*>(reading);
        formatedData.fval = reading->lux();
 
        CallSensorCb(formatedData);
        break;
      }
      case SSensorType::SSENSOR_TYPE_PROXIMITY:
      {
        QProximityReading* reading = static_cast<QProximityReading*>(reading);
        formatedData.fval = (reading->close()) ? 1.0f : 0.0f;
 
        CallSensorCb(formatedData);
        break;
      }
      default:
        break;
        
      // >>>>> ADD HERE NEW SENSOR TYPES TO BE SUPPORTED <<<<<
    }
    SetData(formatedData);
  }
}
 
SSensorData SQSensor::GetData()
{
  return mData;
}
 
void SQSensor::SetData(SSensorData newData)
{
  mData = newData;
}
 
QSensor* SQSensor::GetQSensor()
{
  return mQSensor;
}
 
void SQSensor::SetQSensor(QSensor* aSensor)
{
  mQSensor = aSensor;
}
 
 
/* * * * * * * * * * * * * SENSOR MANAGER * * * * * * * * * * * * */
 
SQSensorManager::SQSensorManager() : SSensorManager()
{
  mSensorList = std::set<SSensor*>();
  QList<QByteArray> sensorTypes = QSensor::sensorTypes();
  
  if (sensorTypes.contains(QAccelerometer::type))
  {
    MMechostr(MSKDEBUG, "SSensorManager() : an accelerometer has been found\n");
    SQSensor* sensor = new SQSensor(SSensorType::SSENSOR_TYPE_ACCELEROMETER);
    mSensorList.insert(sensor);
  }
 
  if (sensorTypes.contains(QGyroscope::type))
  {
    MMechostr(MSKDEBUG, "SSensorManager() : a gyrometer has been found\n");
    SQSensor* sensor = new SQSensor(SSensorType::SSENSOR_TYPE_GYROSCOPE);
    mSensorList.insert(sensor);
  }
 
  if (sensorTypes.contains(QMagnetometer::type))
  {
    MMechostr(MSKDEBUG, "SSensorManager() : a magnetometer has been found\n");
    SQSensor* sensor = new SQSensor(SSensorType::SSENSOR_TYPE_MAGNETIC_FIELD);
    mSensorList.insert(sensor);
  }
 
  if (sensorTypes.contains(QLightSensor::type))
  {
    MMechostr(MSKDEBUG, "SSensorManager() : a light sensor has been found\n");
    SQSensor* sensor = new SQSensor(SSensorType::SSENSOR_TYPE_LIGHT);
    mSensorList.insert(sensor);
  }
 
  if (sensorTypes.contains(QIRProximitySensor::type))
  {
    MMechostr(MSKDEBUG, "SSensorManager() : a proximity sensor has been found\n");
    SQSensor* sensor = new SQSensor(SSensorType::SSENSOR_TYPE_PROXIMITY);
    mSensorList.insert(sensor);
  }
}
 
SQSensorManager::~SQSensorManager()
{
  for (std::set<SSensor*>::iterator it = mSensorList.begin(); it != mSensorList.end(); ++it)
  {
    SQSensor* sensor = (SQSensor*) (*it);
    SAFE_DELETE(sensor);
  }
 
  mSingletonInstance = NULL;
}
 
int SQSensorManager::SetSensorEnable(SSensorType sensorType, bool state)
{
  boost::shared_lock< boost::shared_mutex > lock(mGlobalMutex);
  // SOFTWARE SENSORS
  if (sensorType == SSENSOR_TYPE_ORIENTATION && IsAvailable(SSENSOR_TYPE_ORIENTATION))
  {
    if (state)
    {
      int orient = ROTATION_0;
      Qt::ScreenOrientation qrot = QGuiApplication::primaryScreen()->primaryOrientation();
      switch(qrot)
      {
          case Qt::LandscapeOrientation:
          orient = ROTATION_90;
          break;
          
          case Qt::InvertedPortraitOrientation:
          orient = ROTATION_180;
          break;
          
          case Qt::InvertedLandscapeOrientation:
          orient = ROTATION_270;
          break;
          
        default:
          orient = ROTATION_0;
      }
      
      GetOrientationTracker()->startTracking((DeviceOrientation)orient);
    }
    else
      GetOrientationTracker()->stopTracking();
 
    SetSensorEnable(SSENSOR_TYPE_MAGNETIC_FIELD, state);
    return SetSensorEnable(SSENSOR_TYPE_ACCELEROMETER, state) + SetSensorEnable(SSENSOR_TYPE_GYROSCOPE, state);
  }
  // PHYSICAL SENSORS
  else
  {
    SSensor* sensor = GetSensorByType(sensorType);
    if (sensor)
    {
      QSensor* aSensor = ((SQSensor*) sensor)->GetQSensor();
      if (aSensor != NULL)
      {
        sensor->SetEnabled(state);
        if (state)
        {
          aSensor->setDataRate(100); //hz
          aSensor->start();
        }
        else
        {
          aSensor->stop();
        }
        return 1;
      }
      else
      {
        // ERROR: NO ASENSOR FOUND
        return -1;
      }
    }
    else
    {
      // ERROR: NO SENSOR FOUND
        return -2;
    }
  }
}
 
void SQSensorManager::Vibrate(long millis)
{
}
 
void SQSensorManager::StartVibration()
{
}
 
void SQSensorManager::VibratePattern(std::vector<int> pattern, bool loop)
{
  //for (unsigned int i = 0; i < pattern.size(); i++)
    //t[i] = (jlong)pattern[i];
}
 
void SQSensorManager::StopVibration()
{
}
 
void SQSensorManager::StartLocationService()
{
 
}
 
void SQSensorManager::StopLocationService()
{
 
}
 
bool SQSensorManager::GetLocation(float &longitude, float &latitude, float &altitude)
{
  return false;
}