diff --git a/Base/Devices/imstkVRPNArduinoDeviceClient.cpp b/Base/Devices/imstkVRPNArduinoDeviceClient.cpp index 160477a2010a2a57468c38a3ee7547f47164c91e..cb72f5f143c6887d96f9aad9812f210ef3a47823 100644 --- a/Base/Devices/imstkVRPNArduinoDeviceClient.cpp +++ b/Base/Devices/imstkVRPNArduinoDeviceClient.cpp @@ -32,6 +32,7 @@ VRPNArduinoDeviceClient::initModule() m_vrpnAnalog = std::make_shared<vrpn_Analog_Remote>(fullDeviceIp); m_vrpnAnalog->register_change_handler(this, analogChangeHandler); + m_vrpnAnalog->shutup = true; } void @@ -43,9 +44,9 @@ VRPNArduinoDeviceClient::runModule() void VRPNArduinoDeviceClient::cleanUpModule() { - m_vrpnAnalog->unregister_change_handler(this, analogChangeHandler); - - m_vrpnAnalog.reset(); + //Module cleanup is causing TCP errors - seems to work fine without it + //m_vrpnAnalog->unregister_change_handler(this, analogChangeHandler); + //m_vrpnAnalog.reset(); } diff --git a/Base/Devices/imstkVRPNDeviceServer.cpp b/Base/Devices/imstkVRPNDeviceServer.cpp index 229fc68b2ff834ab53fc7b9307e5c49b412a3fe2..4362fe4bfab58839cb7631a6f4e45da8af0f491c 100644 --- a/Base/Devices/imstkVRPNDeviceServer.cpp +++ b/Base/Devices/imstkVRPNDeviceServer.cpp @@ -106,7 +106,7 @@ VRPNDeviceServer::initModule() { SerialInfo connectionSettings = m_SerialInfoMap[name]; //open with 6 channels (max needed for IMU, can use less) - m_deviceConnections->add(new vrpn_Streaming_Arduino(name.c_str(), m_serverConnection, connectionSettings.port, 6,connectionSettings.baudRate)); + m_deviceConnections->add(new vrpn_Streaming_Arduino(name.c_str(), m_serverConnection, connectionSettings.port, 6, connectionSettings.baudRate)); } break; default: { diff --git a/Base/SceneElements/Controllers/imstkFLSCameraController.cpp b/Base/SceneElements/Controllers/imstkFLSCameraController.cpp index 0d0870068dcc58c5f13ed944791ec2a4607ed162..144a716607b02e6b683106e09e7db43fe1922376 100644 --- a/Base/SceneElements/Controllers/imstkFLSCameraController.cpp +++ b/Base/SceneElements/Controllers/imstkFLSCameraController.cpp @@ -61,22 +61,43 @@ FLSCameraController::setArduinoDevice(std::shared_ptr<VRPNArduinoDeviceClient> a } void -FLSCameraController::runModule() +FLSCameraController::setAngleCalibration(const double angle) { - //Get head angle from Arduino, performing calibration if this is the first valid report - if (arduinoActive & calibrated) - { - this->setCameraHeadAngleOffset(arduinoClient->getRoll() - m_rollOffset); - } - else if (arduinoActive) + m_rollOffset = angle; +} + +double FLSCameraController::getAngleCalibration() +{ + return m_rollOffset; +} + +void +FLSCameraController::recalibrate() +{ + if (arduinoActive) { if (arduinoClient->getRoll() != 0) { - std::cout << "FLS Camera Controller: Calibration complete; Safe to move camera" << std::endl; m_rollOffset = arduinoClient->getRoll(); + std::cout << "FLS Camera Controller: Calibration complete" << std::endl; calibrated = true; } + else + { + std::cout << "FLS Camera Controller: Cannot calibrate - bad input from device" << std::endl; + } } +} + +void +FLSCameraController::runModule() +{ + //Get head angle from Arduino + if (arduinoActive) + { + this->setCameraHeadAngleOffset(arduinoClient->getRoll() - m_rollOffset); + } + auto roff = Quatd(Eigen::AngleAxisd(m_cameraHeadAngleOffset*PI / 180., Vec3d(0., 0., 1.))); this->setCameraRotationOffset(roff); diff --git a/Base/SceneElements/Controllers/imstkFLSCameraController.h b/Base/SceneElements/Controllers/imstkFLSCameraController.h index 8ed16e677c707518e129909531acf3859774ff88..c4f185d0c3cb82bb81fca5f53e3ab43f1ffd2d36 100644 --- a/Base/SceneElements/Controllers/imstkFLSCameraController.h +++ b/Base/SceneElements/Controllers/imstkFLSCameraController.h @@ -54,6 +54,12 @@ public: void setCameraHeadAngleOffset(const double angle); const double getCameraHeadAngleOffset() const; + /// + /// \brief Get/Set the cameraHead angle calibration offset + /// + void setAngleCalibration( const double angle); + double getAngleCalibration(); + /// /// \brief Get/Set the angulation /// @@ -62,6 +68,8 @@ public: void setArduinoDevice(std::shared_ptr<VRPNArduinoDeviceClient> aClient); + void recalibrate(); + protected: ///