diff --git a/Base/Devices/imstkVRPNArduinoDeviceClient.cpp b/Base/Devices/imstkVRPNArduinoDeviceClient.cpp index 5321beada7ba637beb01e1e3f9caa3f0fbaf89a1..65b707914c7c7654db25b45157044a30239e505f 100644 --- a/Base/Devices/imstkVRPNArduinoDeviceClient.cpp +++ b/Base/Devices/imstkVRPNArduinoDeviceClient.cpp @@ -57,8 +57,9 @@ VRPNArduinoDeviceClient::analogChangeHandler(void *userData, const _vrpn_ANALOGC { auto deviceClient = reinterpret_cast<VRPNArduinoDeviceClient*>(userData); - deviceClient->m_ypr << a.channel[0], a.channel[1], a.channel[2]; - deviceClient->m_accel << a.channel[3], a.channel[4], a.channel[5]; + //deviceClient->m_ypr << a.channel[0], a.channel[1], a.channel[2]; + //deviceClient->m_accel << a.channel[3], a.channel[4], a.channel[5]; + deviceClient->m_roll = a.channel[0]; } diff --git a/Base/Devices/imstkVRPNArduinoDeviceClient.h b/Base/Devices/imstkVRPNArduinoDeviceClient.h index ff0cd469253541a710b566f2a5399ce8bae8ca65..8f205f52824cf027bf51c201c12500856ab23828 100644 --- a/Base/Devices/imstkVRPNArduinoDeviceClient.h +++ b/Base/Devices/imstkVRPNArduinoDeviceClient.h @@ -56,6 +56,11 @@ public: virtual ~VRPNArduinoDeviceClient() {} + Vec3d& getYPR() {return m_ypr;} + + Vec3d& getAcceleration(){return m_accel;} + float getRoll(){return m_roll;} + protected: /// /// \brief Initialize device client module @@ -84,9 +89,11 @@ private: /// static void VRPN_CALLBACK analogChangeHandler(void *userData, const _vrpn_ANALOGCB a); + std::shared_ptr<vrpn_Analog_Remote> m_vrpnAnalog; //!< VRPN position/orientation interface Vec3d m_ypr = Vec3d::Zero(); Vec3d m_accel = Vec3d::Zero(); + float m_roll = 0; }; } diff --git a/Base/SceneElements/Controllers/imstkFLSCameraController.cpp b/Base/SceneElements/Controllers/imstkFLSCameraController.cpp index d41c378c45c3dcfdb394bb850204b805c8b5dc5c..0aa0705269ae123d513859e9797d13b72a3c5234 100644 --- a/Base/SceneElements/Controllers/imstkFLSCameraController.cpp +++ b/Base/SceneElements/Controllers/imstkFLSCameraController.cpp @@ -52,6 +52,15 @@ FLSCameraController::getCameraAngulationOffset() const return m_cameraAngulationOffset; } +void +FLSCameraController::setArduinoDevice(std::shared_ptr<VRPNArduinoDeviceClient> aClient) +{ + arduinoActive = true; + arduinoClient = aClient; + + m_rollOffset = arduinoClient->getRoll(); +} + void FLSCameraController::setCameraAngulationTranslationOffset(const double t) { @@ -67,6 +76,7 @@ FLSCameraController::getCameraAngulationTranslationOffset() const void FLSCameraController::runModule() { + this->setCameraHeadAngleOffset(arduinoClient->getRoll()); auto roff = Quatd(Eigen::AngleAxisd(m_cameraHeadAngleOffset*PI / 180., Vec3d(0., 0., 1.))); roff *= m_cameraAngulationRotOffset; this->setCameraRotationOffset(roff); diff --git a/Base/SceneElements/Controllers/imstkFLSCameraController.h b/Base/SceneElements/Controllers/imstkFLSCameraController.h index ca96c5c1b42f12ed7bc645af1ae299cf0f30d12a..d4f88d8e173b44085b4a97259aa63225c141992b 100644 --- a/Base/SceneElements/Controllers/imstkFLSCameraController.h +++ b/Base/SceneElements/Controllers/imstkFLSCameraController.h @@ -23,6 +23,7 @@ #define imstkFLSCameraController_h #include "imstkCameraController.h" +#include "imstkVRPNArduinoDeviceClient.h" namespace imstk { @@ -65,6 +66,8 @@ public: void setCameraAngulationTranslationOffset(const double t); double getCameraAngulationTranslationOffset() const; + void setArduinoDevice(std::shared_ptr<VRPNArduinoDeviceClient> aClient); + protected: /// @@ -73,9 +76,11 @@ protected: void runModule() override; double m_cameraHeadAngleOffset = 0; ///< camera head angle offset (in deg) - double m_cameraAngulationOffset = 0.; ///< Rotation offset for the camera via telescope angulation Quatd m_cameraAngulationRotOffset = Quatd::Identity();///< Rotation offset for the camera via telescope angulation double m_angulationTranslationOffset = 0.1; + std::shared_ptr<VRPNArduinoDeviceClient> arduinoClient; + bool arduinoActive = false; + double m_rollOffset = 0; }; } // imstk