SSensor.cpp

#include "SSensor.h"
#include <set>
 
#ifdef ANDROID
#include "SASensor.h"
#elif _WIN32
#include "SWSensor.h"
#else
#include "SQSensor.h"
#endif
 
#include "tools/SO3Util.h"
 
extern int SENSOR_DATA_CB;
 
#if defined(OSX) || defined(APPLE_IOS)
//Tickcount function
#include <boost/chrono/chrono.hpp>
 
int64_t GetTickCountNano()
{
  return boost::chrono::duration_cast<boost::chrono::nanoseconds>(boost::chrono::steady_clock::now().time_since_epoch()).count();
}
#elif defined(_WIN32)
int64_t GetTickCountNano()
{
  int msec = GetTickCount();
  return (int64_t)msec * (int64_t)1e-6;
}
#else
int64_t GetTickCountNano()
{
  struct timespec now;
  clock_gettime(CLOCK_MONOTONIC, &now);
  return (int64_t)now.tv_sec * 1000000000LL + (int64_t)now.tv_nsec;
}
 
#endif
 
/* * * * * * * * * * * * * STATIC SENSOR MANAGER * * * * * * * * * * * * */
 
SSensorManager* SSensorManager::mSingletonInstance;
 
SSensorManager* SSensorManager::GetInstance()
{
  if (!mSingletonInstance)
  {
#ifdef ANDROID
    mSingletonInstance = new SASensorManager();
#elif _WIN32
    mSingletonInstance = new SWSensorManager();
#else
    mSingletonInstance = new SQSensorManager();
#endif
  }
  return mSingletonInstance;
}
 
void SSensorManager::Kill(){
  SAFE_DELETE(mSingletonInstance);
}
 
 
/* * * * * * * * * * * * * Axis Sensor Callback * * * * * * * * * * * * */
SSensorCb::SSensorCb(SSensorType type): mType(type)
{
}
 
SSensorCb::~SSensorCb()
{
 
}
 
SSensorType SSensorCb::GetType()
{
  return mType;
}
 
 
/* * * * * * * * * * * * * * * * Sensor * * * * * * * * * * * * * * * * */
SSensor::SSensor()
{
  mType = SSENSOR_TYPE_UNKNOWN;
  mEnabled = false;
  mPaused = false;
  mLastTimeStamp = 0;
}
 
SSensor::SSensor(SSensorType type)
{
  mType = type;
  mEnabled = false;
  mPaused = false;
  mLastTimeStamp = 0;
}
 
SSensor::~SSensor()
{
 
}
 
SSensorType SSensor::GetType()
{
  return mType;
}
 
bool SSensor::IsEnabled()
{
  return mEnabled;
}
 
void SSensor::SetEnabled(bool enabled)
{
  if (mEnabled != enabled)
  {
    mEnabled = enabled;
    mLastTimeStamp = 0;
  }
}
 
bool SSensor::IsPaused()
{
  return mPaused;
}
 
void SSensor::SetPaused(bool paused)
{
  if (mPaused != paused)
  {
    mPaused = paused;
    mLastTimeStamp = 0;
  }
}
 
SSensorCb* SSensor::AddCallBack()
{
  SSensorCb* sensorCb = new SSensorCb(mType);
  mAxisCallbacks.push_back(sensorCb);
 
  return sensorCb;
}
 
void SSensor::RemoveCallBack(SSensorCb* sensorCb)
{
  if (sensorCb == 0)
    return;
 
  mAxisCallbacks.remove(sensorCb);
  SAFE_DELETE(sensorCb);
}
 
double SSensor::GetElapsedTime(int64_t timeStamp)
{
  double delta = 0.0;
  if (mLastTimeStamp != 0)
  {
    delta = (timeStamp - mLastTimeStamp) / 1e9;
  }
  mLastTimeStamp = timeStamp;
  return delta;
}
 
void SSensor::CallSensorCb(SSensorData data)
{
  for (SSensorCb* sensorCb : mAxisCallbacks)
  {
    SSensorData* ndata = new SSensorData;
    ndata->fval = data.fval;
    ndata->vec3 = Vector3d(data.vec3);
    ndata->delta = data.delta;
 
    OBJpostEvent(SENSOR_DATA_CB, SCOL_PTR sensorCb, SCOL_PTR ndata);
  }
}
 
 
/* * * * * * * * * * * * * * * SENSOR MANAGER * * * * * * * * * * * * * * */
 
SSensorManager::SSensorManager()
{
  mOrientationTracker = new OrientationTracker();
}
 
SSensorManager::~SSensorManager()
{
  SAFE_DELETE(mOrientationTracker);
}
 
bool SSensorManager::IsAvailable(SSensorType sensorType)
{
  if (sensorType == SSENSOR_TYPE_ORIENTATION)
    return IsAvailable(SSENSOR_TYPE_ACCELEROMETER) && IsAvailable(SSENSOR_TYPE_GYROSCOPE);
  else
    return (GetSensorByType(sensorType) != 0);
}
 
bool SSensorManager::IsEnabled(SSensorType sensorType)
{
  if (sensorType == SSENSOR_TYPE_ORIENTATION)
  {
    return IsEnabled(SSENSOR_TYPE_ACCELEROMETER) && IsEnabled(SSENSOR_TYPE_GYROSCOPE);
  }
  else
  {
    SSensor* sensor = GetSensorByType(sensorType);
    return (sensor)? sensor->IsEnabled() : false;
  }
}
 
SSensor* SSensorManager::GetSensorByType(SSensorType sensorType)
{
  for (std::set<SSensor*>::iterator it = mSensorList.begin(); it != mSensorList.end(); ++it)
  {
    if ((*it) && (*it)->GetType() == sensorType)
    {
      return *it;
    }
  }
  return NULL;
}
 
void SSensorManager::PauseSensorManager()
{
  //TODO remove update thread
  for (std::set<SSensor*>::iterator it = mSensorList.begin(); it != mSensorList.end(); ++it)
  {
    SSensor* sensor = *it;
    if (sensor && sensor->IsEnabled())
    {
      SetSensorEnable(sensor->GetType(), false);
      sensor->SetPaused(true);
    }
  }
}
 
void SSensorManager::ResumeSensorManager()
{
  //TODO add update thread again
  for (std::set<SSensor*>::iterator it = mSensorList.begin(); it != mSensorList.end(); ++it)
  {
    SSensor* sensor = *it;
    if (sensor && sensor->IsPaused())
    {
      SetSensorEnable(sensor->GetType(), true);
      sensor->SetPaused(false);
    }
  }
}
 
std::set<SSensor*> SSensorManager::GetSensorList()
{
  return mSensorList;
}
 
OrientationTracker* SSensorManager::GetOrientationTracker()
{
  return mOrientationTracker;
}
 
Quaternion SSensorManager::GetDeviceOrientation(double timeOffsetInSeconds)
{
  if (mOrientationTracker && IsEnabled(SSENSOR_TYPE_ORIENTATION))
  {
    boost::shared_lock< boost::shared_mutex > lock(mGlobalMutex);
    return mOrientationTracker->lastHeadView(timeOffsetInSeconds);
  }
  else
  {
    // TODO: ERROR NO ORIENTATION EKF
    return Quaternion::IDENTITY;
  }
}
 
SSensorCb* SSensorManager::AddSensorCallBack(SSensorType sensorType)
{
  SSensor* sensor = GetSensorByType(sensorType);
  if (sensor == 0)
    return 0;
 
  return sensor->AddCallBack();
}
 
void SSensorManager::RemoveSensorCallBack(SSensorCb* sensorCb)
{
  if (sensorCb == 0)
    return;
 
  SSensor* sensor = GetSensorByType(sensorCb->GetType());
  if (sensor == 0)
    return;
 
  sensor->RemoveCallBack(sensorCb);
}