vtkPlusPhidgetSpatialTracker.h

Go to the documentation of this file.

/*=Plus=header=begin======================================================
Program: Plus
Copyright (c) Laboratory for Percutaneous Surgery. All rights reserved.
See License.txt for details.
=========================================================Plus=header=end*/

#ifndef __vtkPlusPhidgetSpatialTracker_h
#define __vtkPlusPhidgetSpatialTracker_h

#include "vtkPlusDataCollectionExport.h"

#include "vtkPlusDevice.h"

class AhrsAlgo;

// Number of parameters required by CPhidgetSpatial_setCompassCorrectionParameters
#define PHIDGET_NUMBER_OF_COMPASS_CORRECTION_PARAMETERS 13

class vtkPlusDataCollectionExport vtkPlusPhidgetSpatialTracker : public vtkPlusDevice
{
public:

  static vtkPlusPhidgetSpatialTracker *New();
  vtkTypeMacro( vtkPlusPhidgetSpatialTracker,vtkPlusDevice );
  void PrintSelf( ostream& os, vtkIndent indent );

  virtual bool IsTracker() const { return true; }

  PlusStatus InternalConnect();

  virtual PlusStatus InternalDisconnect();

  PlusStatus Probe();

  virtual PlusStatus ReadConfiguration(vtkXMLDataElement* config); 

  virtual PlusStatus WriteConfiguration(vtkXMLDataElement* config);

  /*
  * Override the default resettable behavior of the device
  * because Phidget trackers can be reset
  */
  virtual bool IsResettable();

  virtual PlusStatus Reset();

  vtkSetMacro(ZeroGyroscopeOnConnect, bool);

  vtkSetMacro(SerialNumber, int);
  vtkGetMacro(SerialNumber, int);

  friend class PhidgetSpatialCallbackClass;

protected:

  vtkPlusPhidgetSpatialTracker();
  ~vtkPlusPhidgetSpatialTracker();

  PlusStatus InternalStartRecording();

  PlusStatus InternalStopRecording();

  void ZeroGyroscope();

  vtkSetVector2Macro(AhrsAlgorithmGain, double);
  vtkSetVector2Macro(FilteredTiltSensorAhrsAlgorithmGain, double);
  
  vtkSetVectorMacro(CompassCorrectionParameters, double, PHIDGET_NUMBER_OF_COMPASS_CORRECTION_PARAMETERS);

  bool IsCompassCorrectionParametersDefined();

private:  // Functions.

  vtkPlusPhidgetSpatialTracker( const vtkPlusPhidgetSpatialTracker& );
  void operator=( const vtkPlusPhidgetSpatialTracker& ); 

  //void Get3x3RotMatrixFromIMUQuat(double rotMatrix[3][3], AhrsAlgo* AhrsAlgo);

private:  // Variables.  

  void* SpatialDeviceHandle;

  // Device-specific identifier that can be used to choose a particular device when multiple devices are connected
  // If the default value (-1) is kept then it connects to the first available device.
  int SerialNumber;

  unsigned int FrameNumber;
  double TrackerTimeToSystemTimeSec; // time_System = time_Tracker + TrackerTimeToSystemTimeSec
  bool TrackerTimeToSystemTimeComputed; // the time offset is always computed when the first frame is received after start tracking  

  vtkMatrix4x4* LastAccelerometerToTrackerTransform;
  vtkMatrix4x4* LastGyroscopeToTrackerTransform;
  vtkMatrix4x4* LastMagnetometerToTrackerTransform;
  vtkMatrix4x4* LastTiltSensorToTrackerTransform;
  vtkMatrix4x4* LastFilteredTiltSensorToTrackerTransform;
  vtkMatrix4x4* LastOrientationSensorToTrackerTransform;

  vtkPlusDataSource* AccelerometerTool;
  vtkPlusDataSource* GyroscopeTool;
  vtkPlusDataSource* MagnetometerTool;
  vtkPlusDataSource* TiltSensorTool;
  vtkPlusDataSource* FilteredTiltSensorTool;
  vtkPlusDataSource* OrientationSensorTool;

  enum AHRS_METHOD
  {
    AHRS_MADGWICK,
    AHRS_MAHONY
  };

  AhrsAlgo* FilteredTiltSensorAhrsAlgo;

  AhrsAlgo* AhrsAlgo;

  bool AhrsUseMagnetometer;

  double AhrsAlgorithmGain[2];
  double FilteredTiltSensorAhrsAlgorithmGain[2];

  double AhrsLastUpdateTime;
  double FilteredTiltSensorAhrsLastUpdateTime;

  int TiltSensorWestAxisIndex;
  int FilteredTiltSensorWestAxisIndex;

  bool ZeroGyroscopeOnConnect;

  double CompassCorrectionParameters[PHIDGET_NUMBER_OF_COMPASS_CORRECTION_PARAMETERS];
};

#endif