diff --git a/scenarios/tests/CMakeLists.txt b/scenarios/tests/CMakeLists.txt
index 8aec5dbf1ec6f311c170e0720c834d0f0a145101..6fe1633926efe66a8f149f774359d54e3e2917c2 100644
--- a/scenarios/tests/CMakeLists.txt
+++ b/scenarios/tests/CMakeLists.txt
@@ -1,6 +1,3 @@
-
add_subdirectory(statecharttestscenario)
add_subdirectory(WeissHapticSensorsUnitTest)
-
-
-
+add_subdirectory(InertialMeasurementUnit)
diff --git a/scenarios/tests/InertialMeasurementUnit/CMakeLists.txt b/scenarios/tests/InertialMeasurementUnit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f26b47b2fe9f95208f79d43c5eeee62eeaa6411d
--- /dev/null
+++ b/scenarios/tests/InertialMeasurementUnit/CMakeLists.txt
@@ -0,0 +1,20 @@
+# Add your components below as shown in the following example:
+#
+# set(SCENARIO_COMPONENTS
+# ConditionHandler
+# Observer
+# PickAndPlaceComponent)
+
+set(SCENARIO_COMPONENTS
+ XsensIMUApp
+ InertialMeasurementUnitObserverApp
+ )
+
+# optional 3rd parameter: "path/to/global/config.cfg"
+armarx_scenario("InertialMeasurementUnit" "${SCENARIO_COMPONENTS}")
+
+#set(SCENARIO_CONFIGS
+# config/ComponentName.optionalString.cfg
+# )
+# optional 3rd parameter: "path/to/global/config.cfg"
+#armarx_scenario_from_configs("InertialMeasurementUnit" "${SCENARIO_CONFIGS}")
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index 4419a2a7548c3ba2fffa13e5a86fc31cd7eafefa..3599c80085f66dc1186af4df6147d23b01894722 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -22,3 +22,6 @@ add_subdirectory(ForceTorqueUnitSimulation)
add_subdirectory(MMMPlayer)
+
+add_subdirectory(XsensIMU)
+add_subdirectory(InertialMeasurementUnitObserver)
diff --git a/source/RobotAPI/applications/InertialMeasurementUnitObserver/CMakeLists.txt b/source/RobotAPI/applications/InertialMeasurementUnitObserver/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9fd6cf01ff46b757947cacb71591b6edba6b9d7b
--- /dev/null
+++ b/source/RobotAPI/applications/InertialMeasurementUnitObserver/CMakeLists.txt
@@ -0,0 +1,16 @@
+armarx_component_set_name("InertialMeasurementUnitObserverApp")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+#
+# all include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+# include_directories(${MyLib_INCLUDE_DIRS})
+#endif()
+
+set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore ArmarXCoreObservers RobotAPIUnits)
+
+set(EXE_SOURCE InertialMeasurementUnitObserverApp.h main.cpp)
+
+armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/InertialMeasurementUnitObserver/InertialMeasurementUnitObserverApp.h b/source/RobotAPI/applications/InertialMeasurementUnitObserver/InertialMeasurementUnitObserverApp.h
new file mode 100644
index 0000000000000000000000000000000000000000..eb1acbd082b3e17a9ba4a8fea2694c4b25ab0b1c
--- /dev/null
+++ b/source/RobotAPI/applications/InertialMeasurementUnitObserver/InertialMeasurementUnitObserverApp.h
@@ -0,0 +1,56 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::application::InertialMeasurementUnitObserver
+ * @author Markus Grotz ( markus-grotz at web dot de )
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_APPLICATION_RobotAPI_InertialMeasurementUnitObserver_H
+#define _ARMARX_APPLICATION_RobotAPI_InertialMeasurementUnitObserver_H
+
+
+#include <RobotAPI/components/units/InertialMeasurementUnitObserver.h>
+
+#include <Core/core/application/Application.h>
+#include <Core/core/Component.h>
+
+
+namespace armarx
+{
+ /**
+ * @class InertialMeasurementUnitObserverApp
+ * @brief A brief description
+ *
+ * Detailed Description
+ */
+ class InertialMeasurementUnitObserverApp :
+ virtual public armarx::Application
+ {
+ /**
+ * @see armarx::Application::setup()
+ */
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry,
+ Ice::PropertiesPtr properties)
+ {
+ registry->addObject( Component::create<InertialMeasurementUnitObserver>(properties) );
+ }
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/applications/InertialMeasurementUnitObserver/main.cpp b/source/RobotAPI/applications/InertialMeasurementUnitObserver/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..661d5706f88f4630aad7da1be2749629c69f9cd6
--- /dev/null
+++ b/source/RobotAPI/applications/InertialMeasurementUnitObserver/main.cpp
@@ -0,0 +1,33 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::application::InertialMeasurementUnitObserver
+ * @author Markus Grotz ( markus-grotz at web dot de )
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "InertialMeasurementUnitObserverApp.h"
+#include <Core/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ armarx::ApplicationPtr app = armarx::Application::createInstance < armarx::InertialMeasurementUnitObserverApp > ();
+ app->setName("InertialMeasurementUnitObserver");
+
+ return app->main(argc, argv);
+}
diff --git a/source/RobotAPI/applications/XsensIMU/CMakeLists.txt b/source/RobotAPI/applications/XsensIMU/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..73778cdf5323c15503526ab8e9461e503dd2e3e4
--- /dev/null
+++ b/source/RobotAPI/applications/XsensIMU/CMakeLists.txt
@@ -0,0 +1,16 @@
+armarx_component_set_name("XsensIMUApp")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+#
+# all include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+# include_directories(${MyLib_INCLUDE_DIRS})
+#endif()
+
+set(COMPONENT_LIBS ArmarXInterfaces ArmarXCore XsensIMU)
+
+set(EXE_SOURCE XsensIMUApp.h main.cpp)
+
+armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/XsensIMU/XsensIMUApp.h b/source/RobotAPI/applications/XsensIMU/XsensIMUApp.h
new file mode 100644
index 0000000000000000000000000000000000000000..92c9f0be0193fb1d8a30794a43443b229c332547
--- /dev/null
+++ b/source/RobotAPI/applications/XsensIMU/XsensIMUApp.h
@@ -0,0 +1,56 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::application::XsensIMU
+ * @author Markus Grotz ( markus-grotz at web dot de )
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_APPLICATION_RobotAPI_XsensIMU_H
+#define _ARMARX_APPLICATION_RobotAPI_XsensIMU_H
+
+
+#include <RobotAPI/libraries/drivers/XsensIMU/XsensIMU.h>
+
+#include <Core/core/application/Application.h>
+#include <Core/core/Component.h>
+
+
+namespace armarx
+{
+ /**
+ * @class XsensIMUApp
+ * @brief A brief description
+ *
+ * Detailed Description
+ */
+ class XsensIMUApp :
+ virtual public armarx::Application
+ {
+ /**
+ * @see armarx::Application::setup()
+ */
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry,
+ Ice::PropertiesPtr properties)
+ {
+ registry->addObject( Component::create<XsensIMU>(properties) );
+ }
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/applications/XsensIMU/main.cpp b/source/RobotAPI/applications/XsensIMU/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e58d9dd795abaf00f1e9eb2d7af93c47c413f6cf
--- /dev/null
+++ b/source/RobotAPI/applications/XsensIMU/main.cpp
@@ -0,0 +1,33 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::application::XsensIMU
+ * @author Markus Grotz ( markus-grotz at web dot de )
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "XsensIMUApp.h"
+#include <Core/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ armarx::ApplicationPtr app = armarx::Application::createInstance < armarx::XsensIMUApp > ();
+ app->setName("XsensIMU");
+
+ return app->main(argc, argv);
+}
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index a8496a5c973569acac67a62b97b874beaa56a9e5..e7f220a189dde78cdea4764011711728a850df76 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -31,6 +31,8 @@ set(LIB_HEADERS
HeadIKUnit.h
HapticUnit.h
HapticObserver.h
+ InertialMeasurementUnit.h
+ InertialMeasurementUnitObserver.h
TCPControlUnit.h
TCPControlUnitObserver.h
@@ -54,6 +56,8 @@ set(LIB_FILES
HeadIKUnit.cpp
HapticUnit.cpp
HapticObserver.cpp
+ InertialMeasurementUnit.cpp
+ InertialMeasurementUnitObserver.cpp
TCPControlUnit.cpp
TCPControlUnitObserver.cpp
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnit.cpp b/source/RobotAPI/components/units/InertialMeasurementUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..672f856d35c99fa1c63b1e63c1981a98bf8c3154
--- /dev/null
+++ b/source/RobotAPI/components/units/InertialMeasurementUnit.cpp
@@ -0,0 +1,59 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::InertialMeasurementUnit
+ * @author Markus Grotz ( markus-grotz at web dot de )
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "InertialMeasurementUnit.h"
+
+
+using namespace armarx;
+
+
+void InertialMeasurementUnit::onInitComponent()
+{
+ offeringTopic(getProperty<std::string>("IMUTopicName").getValue());
+ onInitIMU();
+}
+
+
+void InertialMeasurementUnit::onConnectComponent()
+{
+ IMUTopicPrx = getTopic<InertialMeasurementUnitListenerPrx>(getProperty<std::string>("IMUTopicName").getValue());
+ onStartIMU();
+}
+
+
+void InertialMeasurementUnit::onDisconnectComponent()
+{
+}
+
+
+void InertialMeasurementUnit::onExitComponent()
+{
+ onExitIMU();
+}
+
+PropertyDefinitionsPtr InertialMeasurementUnit::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new InertialMeasurementUnitPropertyDefinitions(
+ getConfigIdentifier()));
+}
+
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnit.h b/source/RobotAPI/components/units/InertialMeasurementUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..b624d203c464c263ecdf6423e86639e137f2ff41
--- /dev/null
+++ b/source/RobotAPI/components/units/InertialMeasurementUnit.h
@@ -0,0 +1,107 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::InertialMeasurementUnit
+ * @author Markus Grotz ( markus-grotz at web dot de )
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_COMPONENT_RobotAPI_InertialMeasurementUnit_H
+#define _ARMARX_COMPONENT_RobotAPI_InertialMeasurementUnit_H
+
+
+#include "SensorActorUnit.h"
+
+#include <Core/core/Component.h>
+#include <RobotAPI/interface/units/InertialMeasurementUnit.h>
+
+namespace armarx
+{
+ /**
+ * @class InertialMeasurementUnitPropertyDefinitions
+ * @brief
+ * @ingroup Components
+ */
+ class InertialMeasurementUnitPropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ InertialMeasurementUnitPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>("IMUTopicName", "IMUValues", "Name of the IMU Topic.");
+ }
+ };
+
+ /**
+ * @class InertialMeasurementUnit
+ * @brief A brief description
+ *
+ * Detailed Description
+ */
+ class InertialMeasurementUnit :
+ virtual public armarx::InertialMeasurementUnitInterface,
+ virtual public armarx::SensorActorUnit
+ {
+ public:
+ /**
+ * @see armarx::ManagedIceObject::getDefaultName()
+ */
+ virtual std::string getDefaultName() const
+ {
+ return "InertialMeasurementUnit";
+ }
+
+
+ protected:
+ /**
+ * @see armarx::ManagedIceObject::onInitComponent()
+ */
+ virtual void onInitComponent();
+
+ /**
+ * @see armarx::ManagedIceObject::onConnectComponent()
+ */
+ virtual void onConnectComponent();
+
+ /**
+ * @see armarx::ManagedIceObject::onDisconnectComponent()
+ */
+ virtual void onDisconnectComponent();
+
+ /**
+ * @see armarx::ManagedIceObject::onExitComponent()
+ */
+ virtual void onExitComponent();
+
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+
+ virtual void onInitIMU() = 0;
+ virtual void onStartIMU() = 0;
+ virtual void onExitIMU() = 0;
+
+ InertialMeasurementUnitListenerPrx IMUTopicPrx;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f499853938ae00bab86d150d96d76127bf4576ba
--- /dev/null
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
@@ -0,0 +1,81 @@
+#include "InertialMeasurementUnitObserver.h"
+
+
+#include <Core/observers/checks/ConditionCheckEquals.h>
+#include <Core/observers/checks/ConditionCheckLarger.h>
+#include <Core/observers/checks/ConditionCheckSmaller.h>
+#include <Core/observers/checks/ConditionCheckUpdated.h>
+
+#include <RobotAPI/libraries/robotstate/remote/ArmarPose.h>
+
+#include <Core/observers/variant/TimestampVariant.h>
+
+
+
+using namespace armarx;
+
+
+void InertialMeasurementUnitObserver::onInitObserver()
+{
+ usingTopic(getProperty<std::string>("IMUTopicName").getValue());
+
+
+ offerConditionCheck("updated", new ConditionCheckUpdated());
+ offerConditionCheck("larger", new ConditionCheckLarger());
+ offerConditionCheck("equals", new ConditionCheckEquals());
+ offerConditionCheck("smaller", new ConditionCheckSmaller());
+}
+
+
+
+void InertialMeasurementUnitObserver::onConnectObserver()
+{
+}
+
+
+void InertialMeasurementUnitObserver::onExitObserver()
+{
+
+}
+
+void InertialMeasurementUnitObserver::reportSensorValues(const std::string &device, const std::string &name, const IMUData &values, const TimestampBasePtr ×tamp, const Ice::Current &c)
+{
+ ScopedLock lock(dataMutex);
+
+ TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
+
+ Vector3BasePtr acceleration = new Vector3(values.acceleration.at(0), values.acceleration.at(1), values.acceleration.at(2));
+ Vector3BasePtr gyroscopeRotation = new Vector3(values.gyroscopeRotation.at(0), values.gyroscopeRotation.at(1), values.gyroscopeRotation.at(2));
+ Vector3BasePtr magneticRotation = new Vector3(values.magneticRotation.at(0), values.magneticRotation.at(1), values.magneticRotation.at(2));
+ QuaternionBasePtr orientationQuaternion = new Quaternion(values.orientationQuaternion.at(0), values.orientationQuaternion.at(1), values.orientationQuaternion.at(2), values.orientationQuaternion.at(3));
+
+ if(!existsChannel(device))
+ {
+ offerChannel(device, "IMU data");\
+ //todo remove
+ offerDataFieldWithDefault(device, "name", Variant(name), "Name of the IMU sensor");
+ offerDataFieldWithDefault(device, "acceleration", acceleration, "acceleration values");
+ offerDataFieldWithDefault(device, "gyroscopeRotation", gyroscopeRotation, "gyroscope rotation values");
+ offerDataFieldWithDefault(device, "magneticRotation", magneticRotation, "magnetic rotation values");
+ offerDataFieldWithDefault(device, "orientationQuaternion", orientationQuaternion, "orientation quaternion values");
+ offerDataFieldWithDefault(device, "timestamp", timestampPtr, "Timestamp");
+ }
+ else
+ {
+ setDataField(device, "name", Variant(name));
+ setDataField(device, "acceleration", acceleration);
+ setDataField(device, "gyroscopeRotation", gyroscopeRotation);
+ setDataField(device, "magneticRotation", magneticRotation);
+ setDataField(device, "orientationQuaternion", orientationQuaternion);
+ setDataField(device, "timestamp", timestampPtr);
+ }
+ updateChannel(device);
+}
+
+
+PropertyDefinitionsPtr InertialMeasurementUnitObserver::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new InertialMeasurementUnitObserverPropertyDefinitions(getConfigIdentifier()));
+}
+
+
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..6ce41f2bf336cb98ca4288f77887e021d6ec0315
--- /dev/null
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
@@ -0,0 +1,73 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::units
+ * @author David Schiebener <schiebener at kit dot edu>
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_IMU_OBSERVER_H
+#define _ARMARX_ROBOTAPI_IMU_OBSERVER_H
+
+#include <RobotAPI/interface/units/InertialMeasurementUnit.h>
+#include <Core/observers/Observer.h>
+
+
+namespace armarx
+{
+
+ class InertialMeasurementUnitObserverPropertyDefinitions:
+ public ComponentPropertyDefinitions
+ {
+ public:
+ InertialMeasurementUnitObserverPropertyDefinitions(std::string prefix):
+ ComponentPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>("IMUTopicName", "IMUValues", "Name of the IMU Topic.");
+ }
+ };
+
+
+ class InertialMeasurementUnitObserver :
+ virtual public Observer,
+ virtual public InertialMeasurementUnitObserverInterface
+ {
+ public:
+ InertialMeasurementUnitObserver(){}
+
+ virtual std::string getDefaultName() const { return "InertialMeasurementUnitObserver"; }
+ virtual void onInitObserver();
+ virtual void onConnectObserver();
+ virtual void onExitObserver();
+
+ void reportSensorValues(const std::string &device, const std::string &name, const IMUData &values, const TimestampBasePtr ×tamp, const Ice::Current &c = ::Ice::Current());
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+
+ private:
+ Mutex dataMutex;
+
+
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index bfa1ac0f38c8dc318d93306f4ff7f276a46f79da..f65a1b039f60ef03b8f9d64e860abf95248221b8 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -18,6 +18,7 @@ set(SLICE_FILES
selflocalisation/SelfLocalisationProcess.ice
units/ForceTorqueUnit.ice
+ units/InertialMeasurementUnit.ice
units/HandUnitInterface.ice
units/HapticUnit.ice
units/WeissHapticUnit.ice
diff --git a/source/RobotAPI/interface/units/InertialMeasurementUnit.ice b/source/RobotAPI/interface/units/InertialMeasurementUnit.ice
new file mode 100644
index 0000000000000000000000000000000000000000..0231a8ff11dbd4e56066fe70962c27adec2a33cf
--- /dev/null
+++ b/source/RobotAPI/interface/units/InertialMeasurementUnit.ice
@@ -0,0 +1,64 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI
+ * @author Markus Grotz <markus dot grotz at kit dot edu>
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_UNITS_INERTIALMEASUREMENTUNIT_SLICE_
+#define _ARMARX_ROBOTAPI_UNITS_INERTIALMEASUREMENTUNIT_SLICE_
+
+
+#include <RobotAPI/interface/units/UnitInterface.ice>
+#include <RobotAPI/interface/robotstate/RobotState.ice>
+
+#include <Core/interface/core/UserException.ice>
+#include <Core/interface/core/BasicTypes.ice>
+#include <Core/interface/observers/VariantBase.ice>
+#include <Core/interface/observers/Matrix.ice>
+#include <Core/interface/observers/Timestamp.ice>
+#include <Core/interface/observers/ObserverInterface.ice>
+
+
+
+module armarx
+{
+ struct IMUData {
+ FloatSequence orientationQuaternion;
+ FloatSequence magneticRotation;
+ FloatSequence gyroscopeRotation;
+ FloatSequence acceleration;
+ };
+
+ interface InertialMeasurementUnitInterface extends armarx::SensorActorUnitInterface
+ {
+ };
+
+ interface InertialMeasurementUnitListener
+ {
+ void reportSensorValues(string device, string name, IMUData values, TimestampBase timestamp);
+ };
+
+ interface InertialMeasurementUnitObserverInterface extends ObserverInterface, InertialMeasurementUnitListener
+ {
+ };
+
+};
+
+#endif
diff --git a/source/RobotAPI/interface/units/InertialMeasurementUnitInterface.ice b/source/RobotAPI/interface/units/InertialMeasurementUnitInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..0231a8ff11dbd4e56066fe70962c27adec2a33cf
--- /dev/null
+++ b/source/RobotAPI/interface/units/InertialMeasurementUnitInterface.ice
@@ -0,0 +1,64 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI
+ * @author Markus Grotz <markus dot grotz at kit dot edu>
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_UNITS_INERTIALMEASUREMENTUNIT_SLICE_
+#define _ARMARX_ROBOTAPI_UNITS_INERTIALMEASUREMENTUNIT_SLICE_
+
+
+#include <RobotAPI/interface/units/UnitInterface.ice>
+#include <RobotAPI/interface/robotstate/RobotState.ice>
+
+#include <Core/interface/core/UserException.ice>
+#include <Core/interface/core/BasicTypes.ice>
+#include <Core/interface/observers/VariantBase.ice>
+#include <Core/interface/observers/Matrix.ice>
+#include <Core/interface/observers/Timestamp.ice>
+#include <Core/interface/observers/ObserverInterface.ice>
+
+
+
+module armarx
+{
+ struct IMUData {
+ FloatSequence orientationQuaternion;
+ FloatSequence magneticRotation;
+ FloatSequence gyroscopeRotation;
+ FloatSequence acceleration;
+ };
+
+ interface InertialMeasurementUnitInterface extends armarx::SensorActorUnitInterface
+ {
+ };
+
+ interface InertialMeasurementUnitListener
+ {
+ void reportSensorValues(string device, string name, IMUData values, TimestampBase timestamp);
+ };
+
+ interface InertialMeasurementUnitObserverInterface extends ObserverInterface, InertialMeasurementUnitListener
+ {
+ };
+
+};
+
+#endif
diff --git a/source/RobotAPI/libraries/drivers/CMakeLists.txt b/source/RobotAPI/libraries/drivers/CMakeLists.txt
index 802a6b533e8945574afb9e92d9d86a1d0e41fe19..ce2834d8731aa2a82f549313634a7d28b2277084 100644
--- a/source/RobotAPI/libraries/drivers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/drivers/CMakeLists.txt
@@ -1 +1,2 @@
add_subdirectory(WeissHapticSensor)
+add_subdirectory(XsensIMU)
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/CMakeLists.txt b/source/RobotAPI/libraries/drivers/XsensIMU/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..23e7c251619ef7e2dbfd53e18ff40d0b77662294
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/CMakeLists.txt
@@ -0,0 +1,17 @@
+armarx_component_set_name("XsensIMU")
+
+set(COMPONENT_LIBS ArmarXCore RobotAPIUnits ArmarXCoreObservers LibIMU)
+
+set(SOURCES
+./XsensIMU.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(HEADERS
+./XsensIMU.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+
+add_subdirectory(IMU)
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/CMakeLists.txt b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b3623f33a11debc6b718bc5214226718f53dd308
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/CMakeLists.txt
@@ -0,0 +1,32 @@
+armarx_set_target("LibIMU")
+
+set(LIB_NAME LibIMU)
+set(LIB_VERSION 0.0.1)
+set(LIB_SOVERSION 1)
+set(LIBS pthread)
+
+set(LIB_FILES
+./Xsens/XsensMTiModule.cpp
+./IIMUEventDispatcher.cpp
+./IMUDeducedReckoning.cpp
+./IMUDevice.cpp
+./IMUHelpers.cpp
+./IMUEvent.cpp
+./IMUState.cpp
+)
+
+set(LIB_HEADERS
+./Xsens/Xsens.h
+./Xsens/XsensMTiModule.h
+./IIMUEventDispatcher.h
+./IMUDeducedReckoning.h
+./IMUDevice.h
+./IMUEvent.h
+./IMUHelpers.h
+./IMU.h
+./IMUState.h
+./Includes.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IIMUEventDispatcher.cpp b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IIMUEventDispatcher.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..92deccbc7b25866cc117d01d996814634078e701
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IIMUEventDispatcher.cpp
@@ -0,0 +1,277 @@
+/*
+ * IIMUEventDispatcher.cpp
+ *
+ * Created on: Mar 16, 2014
+ * Author: Dr.-Ing. David Israel González Aguirre
+ * Mail: david.gonzalez@kit.edu
+ */
+
+#include "IIMUEventDispatcher.h"
+#include "IMUDevice.h"
+
+namespace IMU
+{
+ IIMUEventDispatcher::IIMUEventDispatcher(CIMUDevice* pIMUDevice) :
+ m_DispatchingMode(eCoupled), m_MaximalPendingEvents(2), m_EventFlags(0XFFFF), m_pIMUDevice(pIMUDevice), m_LastStartTimeStamp(CTimeStamp::s_Zero), m_LastStopTimeStamp(CTimeStamp::s_Zero), m_LastCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastFusedCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastIntegratedStateReferenceTimeStamp(CTimeStamp::s_Zero), m_LastCustomEventReferenceTimeStamp(CTimeStamp::s_Zero)
+ {
+ pthread_mutex_init(&m_DispatchingModeMutex,NULL);
+ pthread_mutex_init(&m_MaximalPendingEventsMutex,NULL);
+ pthread_mutex_init(&m_EventFlagsMutex,NULL);
+ pthread_mutex_init(&m_IMUDeviceMutex,NULL);
+ pthread_mutex_init(&m_EventsQueueMutex,NULL);
+ pthread_mutex_init(&m_LastStartTimeStampMutex,NULL);
+ pthread_mutex_init(&m_LastStopTimeStampMutex,NULL);
+ pthread_mutex_init(&m_LastCycleReferenceTimeStampMutex,NULL);
+ pthread_mutex_init(&m_LastFusedCycleReferenceTimeStampMutex,NULL);
+ pthread_mutex_init(&m_LastIntegratedStateReferenceTimeStampMutex,NULL);
+ pthread_mutex_init(&m_LastCustomEventReferenceTimeStampMutex,NULL);
+ }
+
+ IIMUEventDispatcher::IIMUEventDispatcher() :
+ m_DispatchingMode(eCoupled), m_MaximalPendingEvents(2), m_EventFlags(0XFFFF), m_pIMUDevice(NULL), m_EventsQueue(), m_LastStartTimeStamp(CTimeStamp::s_Zero), m_LastStopTimeStamp(CTimeStamp::s_Zero), m_LastCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastFusedCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastIntegratedStateReferenceTimeStamp(CTimeStamp::s_Zero), m_LastCustomEventReferenceTimeStamp(CTimeStamp::s_Zero)
+ {
+ pthread_mutex_init(&m_DispatchingModeMutex,NULL);
+ pthread_mutex_init(&m_MaximalPendingEventsMutex,NULL);
+ pthread_mutex_init(&m_EventFlagsMutex,NULL);
+ pthread_mutex_init(&m_IMUDeviceMutex,NULL);
+ pthread_mutex_init(&m_EventsQueueMutex,NULL);
+ pthread_mutex_init(&m_LastStartTimeStampMutex,NULL);
+ pthread_mutex_init(&m_LastStopTimeStampMutex,NULL);
+ pthread_mutex_init(&m_LastCycleReferenceTimeStampMutex,NULL);
+ pthread_mutex_init(&m_LastFusedCycleReferenceTimeStampMutex,NULL);
+ pthread_mutex_init(&m_LastIntegratedStateReferenceTimeStampMutex,NULL);
+ pthread_mutex_init(&m_LastCustomEventReferenceTimeStampMutex,NULL);
+ }
+
+ IIMUEventDispatcher::~IIMUEventDispatcher()
+ {
+ if (m_pIMUDevice)
+ m_pIMUDevice->UnregisterEventDispatcher(this);
+ }
+
+ void IIMUEventDispatcher::SetIMU(CIMUDevice* pIMUDevice)
+ {
+ _MINIMAL___LOCK(m_IMUDeviceMutex)
+ m_pIMUDevice = pIMUDevice;
+ _MINIMAL_UNLOCK(m_IMUDeviceMutex)
+ }
+
+ uint32_t IIMUEventDispatcher::GetEventFlags()
+ {
+ _MINIMAL___LOCK(m_EventFlagsMutex)
+ const uint32_t EventFlagsCurrentState = m_EventFlags;
+ _MINIMAL_UNLOCK(m_EventFlagsMutex)
+ return EventFlagsCurrentState;
+ }
+
+ void IIMUEventDispatcher::SetDispatchingMode(const DispatchingMode Mode)
+ {
+ _MINIMAL___LOCK(m_DispatchingModeMutex)
+ m_DispatchingMode = Mode;
+ _MINIMAL_UNLOCK(m_DispatchingModeMutex)
+ }
+
+ IIMUEventDispatcher::DispatchingMode IIMUEventDispatcher::GetDispatchingMode()
+ {
+ _MINIMAL___LOCK(m_DispatchingModeMutex)
+ const DispatchingMode DispatchingModeCurrentState = m_DispatchingMode;
+ _MINIMAL_UNLOCK(m_DispatchingModeMutex)
+ return DispatchingModeCurrentState;
+ }
+
+ void IIMUEventDispatcher::SetMaximalPendingEvents(const uint32_t MaximalPendingEvents)
+ {
+ if ((MaximalPendingEvents > 1) && (MaximalPendingEvents != m_MaximalPendingEvents))
+ {
+ _MINIMAL___LOCK(m_MaximalPendingEventsMutex)
+ m_MaximalPendingEvents = MaximalPendingEvents;
+ _MINIMAL_UNLOCK(m_MaximalPendingEventsMutex)
+ if (m_DispatchingMode == eDecoupled)
+ PurgeEvents();
+ }
+ }
+
+ uint32_t IIMUEventDispatcher::GetMaximalPendingEvents()
+ {
+ _MINIMAL___LOCK(m_MaximalPendingEventsMutex)
+ const uint32_t MaximalPendingEventsCurrentState = m_MaximalPendingEvents;
+ _MINIMAL_UNLOCK(m_MaximalPendingEventsMutex)
+ return MaximalPendingEventsCurrentState;
+ }
+
+ void IIMUEventDispatcher::SetEventHandling(const CIMUEvent::EventType Type, const bool Enabled)
+ {
+ _MINIMAL___LOCK(m_EventFlagsMutex)
+ m_EventFlags = Enabled ? (m_EventFlags | Type) : (m_EventFlags & (~Type));
+ _MINIMAL_UNLOCK(m_EventFlagsMutex)
+ }
+
+ uint32_t IIMUEventDispatcher::GetEventHandlingFlags()
+ {
+ _MINIMAL___LOCK(m_EventFlagsMutex);
+ const uint32_t EventHandlingFlagsCurrentState = m_EventFlags;
+ _MINIMAL_UNLOCK(m_EventFlagsMutex);
+ return EventHandlingFlagsCurrentState;
+ }
+
+ void IIMUEventDispatcher::ReceiveEvent(const CIMUEvent& Event)
+ {
+ _MINIMAL___LOCK(m_EventFlagsMutex)
+ const bool HandelEvent = Event.GetEventType() & m_EventFlags;
+ _MINIMAL_UNLOCK(m_EventFlagsMutex)
+
+ if (HandelEvent)
+ {
+ if (m_DispatchingMode == eDecoupled)
+ {
+ _MINIMAL___LOCK(m_EventsQueueMutex)
+ if (m_EventsQueue.size() == m_MaximalPendingEvents)
+ m_EventsQueue.pop_front();
+ m_EventsQueue.push_back(Event);
+ _MINIMAL_UNLOCK(m_EventsQueueMutex)
+
+ switch(Event.GetEventType())
+ {
+ case CIMUEvent::eOnIMUCycle:
+ _MINIMAL___LOCK(m_LastCycleReferenceTimeStampMutex)
+ gettimeofday(&m_LastCycleReferenceTimeStamp,NULL);
+ _MINIMAL_UNLOCK(m_LastCycleReferenceTimeStampMutex)
+ return;
+ case CIMUEvent::eOnIMUFusedCycle:
+ _MINIMAL___LOCK(m_LastFusedCycleReferenceTimeStampMutex)
+ gettimeofday(&m_LastFusedCycleReferenceTimeStamp,NULL);
+ _MINIMAL_UNLOCK(m_LastFusedCycleReferenceTimeStampMutex)
+ return;
+ case CIMUEvent::eOnIMUIntegratedState:
+ _MINIMAL___LOCK(m_LastIntegratedStateReferenceTimeStampMutex)
+ gettimeofday(&m_LastIntegratedStateReferenceTimeStamp,NULL);
+ _MINIMAL_UNLOCK(m_LastIntegratedStateReferenceTimeStampMutex)
+ return;
+ case CIMUEvent::eOnIMUCustomEvent:
+ _MINIMAL___LOCK(m_LastCustomEventReferenceTimeStampMutex)
+ gettimeofday(&m_LastCustomEventReferenceTimeStamp,NULL);
+ _MINIMAL_UNLOCK(m_LastCustomEventReferenceTimeStampMutex)
+ return;
+ case CIMUEvent::eOnIMUStart:
+ _MINIMAL___LOCK(m_LastStartTimeStampMutex)
+ gettimeofday(&m_LastStartTimeStamp,NULL);
+ _MINIMAL_UNLOCK(m_LastStartTimeStampMutex)
+ return;
+ case CIMUEvent::eOnIMUStop:
+ _MINIMAL___LOCK(m_LastStopTimeStampMutex)
+ gettimeofday(&m_LastStopTimeStamp,NULL);
+ _MINIMAL_UNLOCK(m_LastStopTimeStampMutex)
+ return;
+ }
+ }
+ else
+ OnIMUEvent(Event);
+ }
+ }
+
+ bool IIMUEventDispatcher::ProcessPendingEvent()
+ {
+ _MINIMAL___LOCK(m_EventsQueueMutex)
+ if (m_EventsQueue.size())
+ {
+ OnIMUEvent(m_EventsQueue.front());
+ m_EventsQueue.pop_front();
+ _MINIMAL_UNLOCK(m_EventsQueueMutex)
+ return true;
+ }
+ else
+ {
+ _MINIMAL_UNLOCK(m_EventsQueueMutex)
+ return false;
+ }
+ }
+
+ void IIMUEventDispatcher::SetReferenceTimeStamps(const timeval& Reference)
+ {
+ _MINIMAL___LOCK(m_LastStartTimeStampMutex)
+ m_LastStartTimeStamp = Reference;
+ _MINIMAL_UNLOCK(m_LastStartTimeStampMutex)
+
+ _MINIMAL___LOCK(m_LastStopTimeStampMutex)
+ m_LastStopTimeStamp = Reference;
+ _MINIMAL_UNLOCK(m_LastStopTimeStampMutex)
+
+ _MINIMAL___LOCK(m_LastCycleReferenceTimeStampMutex)
+ m_LastCycleReferenceTimeStamp = Reference;
+ _MINIMAL_UNLOCK(m_LastCycleReferenceTimeStampMutex)
+
+ _MINIMAL___LOCK(m_LastFusedCycleReferenceTimeStampMutex)
+ m_LastFusedCycleReferenceTimeStamp = Reference;
+ _MINIMAL_UNLOCK(m_LastFusedCycleReferenceTimeStampMutex)
+
+ _MINIMAL___LOCK(m_LastIntegratedStateReferenceTimeStampMutex)
+ m_LastIntegratedStateReferenceTimeStamp = Reference;
+ _MINIMAL_UNLOCK(m_LastIntegratedStateReferenceTimeStampMutex)
+
+ _MINIMAL___LOCK(m_LastCustomEventReferenceTimeStampMutex)
+ m_LastCustomEventReferenceTimeStamp = Reference;
+ _MINIMAL_UNLOCK(m_LastCustomEventReferenceTimeStampMutex)
+ }
+
+ timeval IIMUEventDispatcher::GetLastStartTimeStamp()
+ {
+ _MINIMAL___LOCK(m_LastStartTimeStampMutex)
+ timeval TimeStampCurrentState = m_LastStartTimeStamp;
+ _MINIMAL_UNLOCK(m_LastStartTimeStampMutex)
+ return TimeStampCurrentState;
+ }
+
+ timeval IIMUEventDispatcher::GetLastStopTimeStamp()
+ {
+ _MINIMAL___LOCK(m_LastStopTimeStampMutex)
+ timeval TimeStampCurrentState = m_LastStopTimeStamp;
+ _MINIMAL_UNLOCK(m_LastStopTimeStampMutex)
+ return TimeStampCurrentState;
+ }
+
+ timeval IIMUEventDispatcher::GetLastCycleReferenceTimeStamp()
+ {
+ _MINIMAL___LOCK(m_LastCycleReferenceTimeStampMutex)
+ timeval TimeStampCurrentState = m_LastCycleReferenceTimeStamp;
+ _MINIMAL_UNLOCK(m_LastCycleReferenceTimeStampMutex)
+ return TimeStampCurrentState;
+ }
+
+ timeval IIMUEventDispatcher::GetLastFusedCycleReferenceTimeStamp()
+ {
+ _MINIMAL___LOCK(m_LastFusedCycleReferenceTimeStampMutex)
+ timeval TimeStampCurrentState = m_LastFusedCycleReferenceTimeStamp;
+ _MINIMAL_UNLOCK(m_LastFusedCycleReferenceTimeStampMutex)
+ return TimeStampCurrentState;
+ }
+
+ timeval IIMUEventDispatcher::GetLastIntegratedStateReferenceTimeStamp()
+ {
+ _MINIMAL___LOCK(m_LastIntegratedStateReferenceTimeStampMutex)
+ timeval TimeStampCurrentState = m_LastIntegratedStateReferenceTimeStamp;
+ _MINIMAL_UNLOCK(m_LastIntegratedStateReferenceTimeStampMutex)
+ return TimeStampCurrentState;
+ }
+
+ timeval IIMUEventDispatcher::GetLastCustomEventReferenceTimeStamp()
+ {
+ _MINIMAL___LOCK(m_LastCustomEventReferenceTimeStampMutex)
+ timeval TimeStampCurrentState = m_LastCustomEventReferenceTimeStamp;
+ _MINIMAL_UNLOCK(m_LastCustomEventReferenceTimeStampMutex)
+ return TimeStampCurrentState;
+ }
+
+ void IIMUEventDispatcher::PurgeEvents()
+ {
+ _MINIMAL___LOCK(m_EventsQueueMutex)
+ if (m_EventsQueue.size() >= m_MaximalPendingEvents)
+ {
+ const uint32_t TotalEventsToRemove = (uint32_t(m_EventsQueue.size()) - m_MaximalPendingEvents) + 1;
+ for(uint32_t i = 0 ; i < TotalEventsToRemove ; ++i)
+ m_EventsQueue.pop_front();
+ }
+ _MINIMAL_UNLOCK(m_EventsQueueMutex)
+ }
+
+}
+
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IIMUEventDispatcher.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IIMUEventDispatcher.h
new file mode 100755
index 0000000000000000000000000000000000000000..2693fca7afa038a4043c4b90bd74ab25854f6c4f
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IIMUEventDispatcher.h
@@ -0,0 +1,100 @@
+/*
+ * IIMUEventDispatcher.h
+ *
+ * Created on: Mar 16, 2014
+ * Author: Dr.-Ing. David Israel González Aguirre
+ * Mail: david.gonzalez@kit.edu
+ */
+
+#ifndef IIMUEVENTDISPATCHER_H_
+#define IIMUEVENTDISPATCHER_H_
+
+#include "Includes.h"
+#include "IMUEvent.h"
+#include "IMUHelpers.h"
+
+namespace IMU
+{
+ class CIMUDevice;
+ class IIMUEventDispatcher
+ {
+ public:
+
+ enum DispatchingMode
+ {
+ eCoupled, eDecoupled
+ };
+
+ IIMUEventDispatcher(CIMUDevice* pIMUDevice);
+ IIMUEventDispatcher();
+
+ virtual ~IIMUEventDispatcher();
+
+ void SetIMU(CIMUDevice* pIMUDevice);
+
+ uint32_t GetEventFlags();
+
+ void SetDispatchingMode(const DispatchingMode Mode);
+ DispatchingMode GetDispatchingMode();
+
+ void SetMaximalPendingEvents(const uint32_t MaximalPendingEvents);
+ uint32_t GetMaximalPendingEvents();
+
+ void SetEventHandling(const CIMUEvent::EventType Type, const bool Enabled);
+ uint32_t GetEventHandlingFlags();
+
+ void ReceiveEvent(const CIMUEvent& Event);
+
+ inline uint32_t GetTotalPendingEvents()
+ {
+ return uint32_t(m_EventsQueue.size());
+ }
+
+ inline bool HasPendingEvents()
+ {
+ return m_EventsQueue.size();
+ }
+
+ bool ProcessPendingEvent();
+
+ void SetReferenceTimeStamps(const timeval& Reference);
+
+ timeval GetLastStartTimeStamp();
+ timeval GetLastStopTimeStamp();
+ timeval GetLastCycleReferenceTimeStamp();
+ timeval GetLastFusedCycleReferenceTimeStamp();
+ timeval GetLastIntegratedStateReferenceTimeStamp();
+ timeval GetLastCustomEventReferenceTimeStamp();
+
+ virtual void OnIMUEvent(const CIMUEvent& Event) = 0;
+
+ private:
+
+ void PurgeEvents();
+
+ DispatchingMode m_DispatchingMode;
+ pthread_mutex_t m_DispatchingModeMutex;
+ uint32_t m_MaximalPendingEvents;
+ pthread_mutex_t m_MaximalPendingEventsMutex;
+ uint32_t m_EventFlags;
+ pthread_mutex_t m_EventFlagsMutex;
+ CIMUDevice* m_pIMUDevice;
+ pthread_mutex_t m_IMUDeviceMutex;
+ std::list<CIMUEvent> m_EventsQueue;
+ pthread_mutex_t m_EventsQueueMutex;
+ timeval m_LastStartTimeStamp;
+ pthread_mutex_t m_LastStartTimeStampMutex;
+ timeval m_LastStopTimeStamp;
+ pthread_mutex_t m_LastStopTimeStampMutex;
+ timeval m_LastCycleReferenceTimeStamp;
+ pthread_mutex_t m_LastCycleReferenceTimeStampMutex;
+ timeval m_LastFusedCycleReferenceTimeStamp;
+ pthread_mutex_t m_LastFusedCycleReferenceTimeStampMutex;
+ timeval m_LastIntegratedStateReferenceTimeStamp;
+ pthread_mutex_t m_LastIntegratedStateReferenceTimeStampMutex;
+ timeval m_LastCustomEventReferenceTimeStamp;
+ pthread_mutex_t m_LastCustomEventReferenceTimeStampMutex;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMU.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMU.h
new file mode 100755
index 0000000000000000000000000000000000000000..205cc8b596aec2f0247e3e9f81224735c87eca76
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMU.h
@@ -0,0 +1,17 @@
+/*
+ * IMU.h
+ *
+ * Created on: Mar 17, 2014
+ * Author: Dr.-Ing. David Israel González Aguirre
+ * Mail: david.gonzalez@kit.edu
+ */
+
+#ifndef IMU_H_
+#define IMU_H_
+
+#include "IMUDeducedReckoning.h"
+#include "IMUDevice.h"
+#include "IMUEvent.h"
+#include "IMUState.h"
+
+#endif /* IMU_H_ */
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDeducedReckoning.cpp b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDeducedReckoning.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..c386a4fdb8c2ab8ece76ce0f212743da9ba50480
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDeducedReckoning.cpp
@@ -0,0 +1,171 @@
+/*
+ * IMUDeducedReckoning.cpp
+ *
+ * Created on: Mar 16, 2014
+ * Author: Dr.-Ing. David Israel González Aguirre
+ * Mail: david.gonzalez@kit.edu
+ */
+
+#include "IMUDeducedReckoning.h"
+
+namespace IMU
+{
+ CIMUDeducedReckoning::CIMUDeducedReckoning(const bool IsVerbose) :
+ IIMUEventDispatcher(), m_IsVerbose(IsVerbose), m_G(IMU::CGeolocationInformation::GetGravitationalAcceleration())
+ {
+ SetEventHandling(CIMUEvent::eOnIMUStart,true);
+ SetEventHandling(CIMUEvent::eOnIMUStop,true);
+ SetEventHandling(CIMUEvent::eOnIMUCycle,false);
+ SetEventHandling(CIMUEvent::eOnIMUFusedCycle,true);
+ SetEventHandling(CIMUEvent::eOnIMUIntegratedState,false);
+ SetEventHandling(CIMUEvent::eOnIMUCustomEvent,true);
+ }
+
+ CIMUDeducedReckoning::~CIMUDeducedReckoning()
+ {
+ }
+
+ void CIMUDeducedReckoning::OnIMUEvent(const CIMUEvent& Event)
+ {
+ switch(Event.GetEventType())
+ {
+ case CIMUEvent::eOnIMUCycle:
+ OnIMUCycle(Event.GetTimeStamp(),Event.GetIMU());
+ break;
+ case CIMUEvent::eOnIMUFusedCycle:
+ OnIMUFusedCycle(Event.GetTimeStamp(),Event.GetIMU());
+ break;
+ case CIMUEvent::eOnIMUIntegratedState:
+ OnIMUIntegratedState(Event.GetTimeStamp(),Event.GetIMU());
+ break;
+ case CIMUEvent::eOnIMUCustomEvent:
+ OnIMUCustomEvent(Event);
+ break;
+ case CIMUEvent::eOnIMUStart:
+ OnIMUStart(Event.GetTimeStamp(),Event.GetIMU());
+ break;
+ case CIMUEvent::eOnIMUStop:
+ OnIMUStop(Event.GetTimeStamp(),Event.GetIMU());
+ break;
+ }
+ }
+
+ void CIMUDeducedReckoning::OnIMUStart(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
+ {
+ if (m_IsVerbose)
+ {
+ std::cout << "OnIMUStart(IMU Device ID = " << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << ")" << std::endl;
+ std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp,pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
+ std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp,GetLastStartTimeStamp()) << "]" << std::endl;
+ if (GetDispatchingMode() == eDecoupled)
+ {
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ }
+ }
+ }
+
+ void CIMUDeducedReckoning::OnIMUStop(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
+ {
+ if (m_IsVerbose)
+ {
+ std::cout << "OnIMUStop(IMU Device ID = " << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << ")" << std::endl;
+ std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp,pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
+ std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp,GetLastStopTimeStamp()) << "]" << std::endl;
+ if (GetDispatchingMode() == eDecoupled)
+ {
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ }
+ }
+ }
+
+ void CIMUDeducedReckoning::OnIMUCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
+ {
+ const IMUState CurrentState = pIMUDevice->GetIMUState();
+ if (m_IsVerbose)
+ {
+ std::cout << "OnIMUCycle(IMU Device ID = 0x" << std::hex << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
+ std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp,pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
+ std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp,GetLastCycleReferenceTimeStamp()) << " µs]" << std::endl;
+ if (GetDispatchingMode() == eDecoupled)
+ {
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ }
+ std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]" << std::endl;
+ std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0] << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << "," << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
+ std::cout << "\t[ Acceleration Magnitude = [" << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) * 1000.0f << " mm/s^2]" << std::endl;
+ std::cout << "\t[ Gyroscope Rotation = [" << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Magnetic Rotation = [" << CurrentState.m_PhysicalData.m_MagneticRotation[0] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[1] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Quaternion Rotation = [" << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
+ }
+ memcpy(m_OrientationQuaternion,CurrentState.m_PhysicalData.m_QuaternionRotation,sizeof(float) * 4);
+ memcpy(m_MagneticRotation,CurrentState.m_PhysicalData.m_MagneticRotation,sizeof(float) * 3);
+ memcpy(m_GyroscopeRotation,CurrentState.m_PhysicalData.m_GyroscopeRotation,sizeof(float) * 3);
+ memcpy(m_Accelaration,CurrentState.m_PhysicalData.m_Acceleration,sizeof(float) * 3);
+ }
+
+ void CIMUDeducedReckoning::OnIMUFusedCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
+ {
+ const IMUState CurrentState = pIMUDevice->GetIMUState();
+ if (m_IsVerbose)
+ {
+ std::cout << "OnIMUFusedCycle(IMU Device ID = 0x" << std::hex << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
+ std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp,pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
+ std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp,GetLastFusedCycleReferenceTimeStamp()) << " µs]" << std::endl;
+ if (GetDispatchingMode() == eDecoupled)
+ {
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ }
+ std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]" << std::endl;
+ std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0] << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << "," << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
+ std::cout << "\t[ Acceleration Magnitude = [" << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) * 1000.0f << " mm/s^2]" << std::endl;
+ std::cout << "\t[ Gyroscope Rotation = [" << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Magnetic Rotation = [" << CurrentState.m_PhysicalData.m_MagneticRotation[0] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[1] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Quaternion Rotation = [" << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
+ }
+ memcpy(m_OrientationQuaternion,CurrentState.m_PhysicalData.m_QuaternionRotation,sizeof(float) * 4);
+ memcpy(m_MagneticRotation,CurrentState.m_PhysicalData.m_MagneticRotation,sizeof(float) * 3);
+ memcpy(m_GyroscopeRotation,CurrentState.m_PhysicalData.m_GyroscopeRotation,sizeof(float) * 3);
+ memcpy(m_Accelaration,CurrentState.m_PhysicalData.m_Acceleration,sizeof(float) * 3);
+ }
+
+ void CIMUDeducedReckoning::OnIMUIntegratedState(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
+ {
+ const IMUState CurrentState = pIMUDevice->GetIMUState();
+ if (m_IsVerbose)
+ {
+ std::cout << "OnIMUStateUpdate(IMU Device ID = 0x" << std::hex << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
+ std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp,pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
+ std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp,GetLastIntegratedStateReferenceTimeStamp()) << " µs]" << std::endl;
+ if (GetDispatchingMode() == eDecoupled)
+ {
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ }
+ std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]" << std::endl;
+ std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0] << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << "," << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
+ std::cout << "\t[ Acceleration Magnitude = [" << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) * 1000.0f << " mm/s^2]" << std::endl;
+ std::cout << "\t[ Gyroscope Rotation = [" << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Magnetic Rotation = [" << CurrentState.m_PhysicalData.m_MagneticRotation[0] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[1] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Quaternion Rotation = [" << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
+ }
+ }
+
+ void CIMUDeducedReckoning::OnIMUCustomEvent(const CIMUEvent& CustomEvent)
+ {
+ if (m_IsVerbose)
+ {
+ std::cout << "OnIMUCustomEvent(IMU Device ID = 0x" << std::hex << CustomEvent.GetIMU()->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
+ std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(CustomEvent.GetTimeStamp(),CustomEvent.GetIMU()->GetReferenceTimeStamp()) << " µs]" << std::endl;
+ std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(CustomEvent.GetTimeStamp(),GetLastCustomEventReferenceTimeStamp()) << " µs]" << std::endl;
+ if (GetDispatchingMode() == eDecoupled)
+ {
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ }
+ }
+ }
+}
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDeducedReckoning.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDeducedReckoning.h
new file mode 100755
index 0000000000000000000000000000000000000000..d2617867402ba6e95a3369190de635114737bf9b
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDeducedReckoning.h
@@ -0,0 +1,66 @@
+/*
+ * IMUDeducedReckoning.h
+ *
+ * Created on: Mar 16, 2014
+ * Author: Dr.-Ing. David Israel González Aguirre
+ * Mail: david.gonzalez@kit.edu
+ */
+
+#ifndef IMUDEDUCEDRECKONING_H_
+#define IMUDEDUCEDRECKONING_H_
+
+#include "IIMUEventDispatcher.h"
+#include "IMUState.h"
+#include "IMUDevice.h"
+
+namespace IMU
+{
+ class CIMUDeducedReckoning : public IIMUEventDispatcher
+ {
+ public:
+
+ CIMUDeducedReckoning(const bool IsVerbose);
+ virtual ~CIMUDeducedReckoning();
+
+ inline const float* GetOrientationQuaternion() const
+ {
+ return m_OrientationQuaternion;
+ }
+
+ inline const float* GetMagneticRotation() const
+ {
+ return m_MagneticRotation;
+ }
+
+ inline const float* GetGyroscopeRotation() const
+ {
+ return m_GyroscopeRotation;
+ }
+
+ inline const float* GetAccelaration() const
+ {
+ return m_Accelaration;
+ }
+
+ protected:
+
+ virtual void OnIMUEvent(const CIMUEvent& Event);
+
+ void OnIMUStart(const timeval& TimeStamp, const CIMUDevice* pIMUDevice);
+ void OnIMUStop(const timeval& TimeStamp, const CIMUDevice* pIMUDevice);
+ void OnIMUFusedCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice);
+ void OnIMUCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice);
+ void OnIMUIntegratedState(const timeval& TimeStamp, const CIMUDevice* pIMUDevice);
+ void OnIMUCustomEvent(const CIMUEvent& CustomEvent);
+
+ const bool m_IsVerbose;
+ const float m_G;
+
+ float m_OrientationQuaternion[4];
+ float m_MagneticRotation[3];
+ float m_GyroscopeRotation[3];
+ float m_Accelaration[3];
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDevice.cpp b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDevice.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..77336df645c8aac6b4b561ae4700b92b39a8a8d6
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDevice.cpp
@@ -0,0 +1,569 @@
+/*
+ * IMU.cpp
+ *
+ * Created on: Mar 16, 2014
+ * Author: Dr.-Ing. David Israel González Aguirre
+ * Mail: david.gonzalez@kit.edu
+ */
+
+#include "IMUDevice.h"
+#include "IIMUEventDispatcher.h"
+
+namespace IMU
+{
+ CIMUDevice::CIMUDevice() :
+ m_DeviceId(0), m_SamplingFrequency(SamplingFrequency(0)), m_PeriodMicroSeconds(0), m_FusionStrategy(eNoFusion), m_SamplesPerFusion(0), m_CollectedFusionSamples(0), m_IsActive(false), m_IsDispatching(false), m_IsInitialized(false), m_pInternalThreadHandel(0), m_IMUEventDispatchers(), m_ReferenceTimeStamp(CTimeStamp::s_Zero), m_LastFrameTimeStamp(CTimeStamp::s_Zero)
+
+#ifdef _IMU_USE_XSENS_DEVICE_
+
+ , m_pXsensMTiModule(NULL)
+#endif
+
+ {
+ pthread_mutex_init(&m_IsActiveMutex,NULL);
+ pthread_mutex_init(&m_IsDispatchingMutex,NULL);
+ pthread_mutex_init(&m_EventDispatchersMutex,NULL);
+ pthread_mutex_init(&m_DeviceMutex,NULL);
+ }
+
+ CIMUDevice::~CIMUDevice()
+ {
+ FinalizeModuleDevice();
+ }
+
+ uint64_t CIMUDevice::GetDeviceId() const
+ {
+ return m_DeviceId;
+ }
+
+ bool CIMUDevice::Connect(const std::string& PortName, const SamplingFrequency Frequency)
+ {
+ if (m_IsInitialized)
+ return true;
+
+ if (!PortName.length())
+ {
+ std::cerr << "[IMU Error: Cannot connect to empty port name!]\n\t[Operation result: (PortName.length()==0)]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ return false;
+ }
+
+ m_IsInitialized = InitializeDevice(PortName,Frequency);
+
+ return m_IsInitialized;
+ }
+
+ bool CIMUDevice::Start(const bool Blocking)
+ {
+ if (m_IsInitialized && (!m_IsActive))
+ {
+ const int Result = pthread_create(&m_pInternalThreadHandel,NULL,CIMUDevice::ThreadLoop,(void*) this);
+ if (Result == 0)
+ {
+ while(Blocking && !m_IsActive)
+ pthread_yield();
+ return true;
+ }
+ }
+ return false;
+ }
+
+ void CIMUDevice::Stop(const bool Blocking)
+ {
+ if (m_IsActive)
+ {
+ _MINIMAL___LOCK(m_IsActiveMutex)
+ m_IsActive = false;
+ _MINIMAL_UNLOCK(m_IsActiveMutex)
+ pthread_join(m_pInternalThreadHandel,NULL);
+ while(Blocking && m_IsDispatching)
+ pthread_yield();
+ }
+ }
+
+ bool CIMUDevice::SetFusion(const FusionStrategy Strategy, const ushort SamplesPerFusion)
+ {
+ if (SamplesPerFusion > 1)
+ {
+ if ((m_FusionStrategy != Strategy) || (m_SamplesPerFusion != SamplesPerFusion))
+ {
+ m_FusionStrategy = Strategy;
+ m_SamplesPerFusion = SamplesPerFusion;
+ m_CollectedFusionSamples = 0;
+ }
+ return true;
+ }
+ else
+ {
+ std::cerr << "[IMU Device error: Cannot set fusion with less than 2 samples per fusion!]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ return false;
+ }
+ }
+
+ bool CIMUDevice::IsActive() const
+ {
+ return m_IsActive;
+ }
+
+ bool CIMUDevice::RegisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher)
+ {
+ if (pIMUEventDispatcher)
+ {
+ _MINIMAL___LOCK(m_EventDispatchersMutex)
+ if (m_IMUEventDispatchers.find(pIMUEventDispatcher) == m_IMUEventDispatchers.end())
+ {
+ pIMUEventDispatcher->SetIMU(this);
+ pIMUEventDispatcher->SetReferenceTimeStamps(m_ReferenceTimeStamp);
+ std::pair<std::set<IIMUEventDispatcher*>::iterator, bool> Result = m_IMUEventDispatchers.insert(pIMUEventDispatcher);
+ _MINIMAL_UNLOCK(m_EventDispatchersMutex)
+ return Result.second;
+ }
+ _MINIMAL_UNLOCK(m_EventDispatchersMutex)
+ }
+ return false;
+ }
+
+ bool CIMUDevice::UnregisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher)
+ {
+ if (pIMUEventDispatcher)
+ {
+ _MINIMAL___LOCK(m_EventDispatchersMutex)
+ std::set<IIMUEventDispatcher*>::iterator ppElement = m_IMUEventDispatchers.find(pIMUEventDispatcher);
+ if (ppElement != m_IMUEventDispatchers.end())
+ {
+ pIMUEventDispatcher->SetIMU(NULL);
+ m_IMUEventDispatchers.erase(ppElement);
+ _MINIMAL_UNLOCK(m_EventDispatchersMutex)
+ return true;
+ }
+ _MINIMAL_UNLOCK(m_EventDispatchersMutex)
+ }
+ return false;
+ }
+
+ void CIMUDevice::UnregisterEventDispatchers()
+ {
+ if (m_IMUEventDispatchers.size())
+ {
+ _MINIMAL___LOCK(m_EventDispatchersMutex)
+ for(std::set<IIMUEventDispatcher*>::iterator ppElement = m_IMUEventDispatchers.begin() ; ppElement != m_IMUEventDispatchers.end() ; ++ppElement)
+ (*ppElement)->SetIMU(NULL);
+ m_IMUEventDispatchers.clear();
+ _MINIMAL_UNLOCK(m_EventDispatchersMutex)
+ }
+ }
+
+#ifdef _IMU_USE_XSENS_DEVICE_
+
+ bool CIMUDevice::InitializeXsensDevice(const std::string& PortName, const SamplingFrequency Frequency)
+ {
+ if (m_IsInitialized)
+ return true;
+
+ m_pXsensMTiModule = new Xsens::CXsensMTiModule();
+
+ if (m_pXsensMTiModule->openPort(PortName.c_str()) != MTRV_OK)
+ {
+ std::cerr << "[IMU Device error: Cannot open port!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ DestroyXsensModuleDevice();
+ return false;
+ }
+
+ if (m_pXsensMTiModule->writeMessage(MID_GOTOCONFIG) != MTRV_OK)
+ {
+ std::cerr << "[IMU Device error: Cannot set configuration state!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ DestroyXsensModuleDevice();
+ return false;
+ }
+
+ if (m_pXsensMTiModule->setDeviceMode(OUTPUTMODE_CALIB | OUTPUTMODE_ORIENT,OUTPUTSETTINGS_ORIENTMODE_QUATERNION | OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) != MTRV_OK)
+ {
+ std::cerr << "[IMU Device error: Cannot set output mode!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ DestroyXsensModuleDevice();
+ return false;
+ }
+
+ if (m_pXsensMTiModule->setSetting(MID_SETPERIOD,Frequency,LEN_PERIOD) != MTRV_OK)
+ {
+ std::cerr << "[IMU Device error: Cannot set sampling period!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ DestroyXsensModuleDevice();
+ return false;
+ }
+
+ unsigned long DeviceId;
+ if (m_pXsensMTiModule->reqSetting(MID_REQDID,DeviceId) != MTRV_OK)
+ {
+ std::cerr << "[IMU Device error: Cannot get device ID!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ DestroyXsensModuleDevice();
+ return false;
+ }
+ m_DeviceId = DeviceId;
+
+ if (m_pXsensMTiModule->writeMessage(MID_GOTOMEASUREMENT) != MTRV_OK)
+ {
+ std::cerr << "[IMU Device error: Cannot enter measurement state!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ DestroyXsensModuleDevice();
+ return false;
+ }
+
+ return true;
+ }
+
+ void CIMUDevice::FinalizeXsensModuleDevice()
+ {
+ if (m_IsInitialized)
+ {
+ while(m_IsActive || m_IsDispatching)
+ pthread_yield();
+
+ _MINIMAL___LOCK(m_DeviceMutex)
+ DestroyXsensModuleDevice();
+ _MINIMAL_UNLOCK(m_DeviceMutex)
+ }
+ }
+
+ void CIMUDevice::DestroyXsensModuleDevice()
+ {
+ if (m_pXsensMTiModule)
+ {
+ if (m_pXsensMTiModule->isPortOpen())
+ m_pXsensMTiModule->close();
+ delete m_pXsensMTiModule;
+ m_pXsensMTiModule = NULL;
+ m_DeviceId = 0;
+ }
+ }
+
+#endif
+
+ bool CIMUDevice::LoadCurrentState()
+ {
+
+ _MINIMAL___LOCK(m_DeviceMutex)
+
+#ifdef _IMU_USE_XSENS_DEVICE_
+
+ if (m_pXsensMTiModule->readDataMessage(m_XsensMTiFrame.m_Data,m_XsensMTiFrame.m_DataLength) == MTRV_OK)
+ if (m_pXsensMTiModule->getValue(VALUE_CALIB_ACC,m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration,m_XsensMTiFrame.m_Data) == MTRV_OK)
+ if (m_pXsensMTiModule->getValue(VALUE_CALIB_GYR,m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_GyroscopeRotation,m_XsensMTiFrame.m_Data) == MTRV_OK)
+ if (m_pXsensMTiModule->getValue(VALUE_CALIB_MAG,m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_MagneticRotation,m_XsensMTiFrame.m_Data) == MTRV_OK)
+ if (m_pXsensMTiModule->getValue(VALUE_ORIENT_QUAT,m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_QuaternionRotation,m_XsensMTiFrame.m_Data) == MTRV_OK)
+ if (m_pXsensMTiModule->getValue(VALUE_SAMPLECNT,m_XsensMTiFrame.m_IMUState.m_ControlData.m_CurrentSampleCount,m_XsensMTiFrame.m_Data) == MTRV_OK)
+ {
+ if (m_XsensMTiFrame.m_IMUState.m_ControlData.m_PreviousSampleCount != -1)
+ m_XsensMTiFrame.m_IMUState.m_ControlData.m_IsConsecutive = ((m_XsensMTiFrame.m_IMUState.m_ControlData.m_PreviousSampleCount + 1) % 65536) == m_XsensMTiFrame.m_IMUState.m_ControlData.m_CurrentSampleCount;
+ m_XsensMTiFrame.m_IMUState.m_ControlData.m_PreviousSampleCount = m_XsensMTiFrame.m_IMUState.m_ControlData.m_CurrentSampleCount;
+ m_XsensMTiFrame.m_IMUState.m_ControlData.m_MessageCounter++;
+ gettimeofday(&m_XsensMTiFrame.m_IMUState.m_ControlData.m_TimeStamp,NULL);
+ m_LastFrameTimeStamp = m_XsensMTiFrame.m_IMUState.m_ControlData.m_TimeStamp;
+
+ m_XsensMTiFrame.m_IMUState.m_PhysicalData.UpdateAccelerationMagnitud();
+
+ _MINIMAL_UNLOCK(m_DeviceMutex)
+ return true;
+ }
+ else
+ std::cerr << "[IMU Device error: Fail to get sample count!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ else
+ std::cerr << "[IMU Device error: Fail to get quaternion rotation!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ else
+ std::cerr << "[IMU Device error: Fail to get magnetic rotation!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ else
+ std::cerr << "[IMU Device error: Fail to get gyroscope rotation!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ else
+ std::cerr << "[IMU Device error: Fail to get acceleration vector!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ else
+ std::cerr << "[IMU Device error: Fail to read message!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+
+#endif
+
+ _MINIMAL_UNLOCK(m_DeviceMutex)
+
+ return false;
+ }
+
+ bool CIMUDevice::MeanFuseCurrentState()
+ {
+ IncorporateCurrentStateMeanFusion();
+ if (m_CollectedFusionSamples == m_SamplesPerFusion)
+ {
+ MeanFusion();
+ return true;
+ }
+ return false;
+ }
+
+ void CIMUDevice::IncorporateCurrentStateMeanFusion()
+ {
+ m_FusedPhysicalData.m_Acceleration[0] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[0];
+ m_FusedPhysicalData.m_Acceleration[1] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[1];
+ m_FusedPhysicalData.m_Acceleration[2] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[2];
+ ++m_CollectedFusionSamples;
+ }
+
+ void CIMUDevice::MeanFusion()
+ {
+ //Execution the fusion
+ const float NormalizationFactor = 1.0f / float(m_CollectedFusionSamples);
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[0] = m_FusedPhysicalData.m_Acceleration[0] * NormalizationFactor;
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[1] = m_FusedPhysicalData.m_Acceleration[1] * NormalizationFactor;
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[2] = m_FusedPhysicalData.m_Acceleration[2] * NormalizationFactor;
+
+ //Derivated from fusion
+ m_FusedIMUState.m_PhysicalData.UpdateAccelerationMagnitud();
+
+ //Reset counters and accumulators
+ memset(m_FusedPhysicalData.m_Acceleration,0,sizeof(float) * 3);
+ m_CollectedFusionSamples = 0;
+ }
+
+ bool CIMUDevice::GaussianFuseCurrentState()
+ {
+ IncorporateCurrentStateGaussianFusion();
+ if (m_CollectedFusionSamples == m_SamplesPerFusion)
+ {
+ GaussianFusion();
+ return true;
+ }
+ return false;
+ }
+
+ void CIMUDevice::IncorporateCurrentStateGaussianFusion()
+ {
+ m_FusedPhysicalData.m_Acceleration[0] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[0];
+ m_FusedPhysicalData.m_Acceleration[1] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[1];
+ m_FusedPhysicalData.m_Acceleration[2] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[2];
+
+ ++m_CollectedFusionSamples;
+ }
+
+ void CIMUDevice::GaussianFusion()
+ {
+ //Execution the fusion
+ const float NormalizationFactor = 1.0f / float(m_CollectedFusionSamples);
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[0] = m_FusedPhysicalData.m_Acceleration[0] * NormalizationFactor;
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[1] = m_FusedPhysicalData.m_Acceleration[1] * NormalizationFactor;
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[2] = m_FusedPhysicalData.m_Acceleration[2] * NormalizationFactor;
+
+ //Derivated from fusion
+ m_FusedIMUState.m_PhysicalData.UpdateAccelerationMagnitud();
+
+ //Reset counters and accumulators
+ memset(m_FusedPhysicalData.m_Acceleration,0,sizeof(float) * 3);
+ m_CollectedFusionSamples = 0;
+ }
+
+ bool CIMUDevice::IntegrateCurrentState()
+ {
+ if (m_FusionStrategy == eNoFusion)
+ {
+ return IntegrateWithOutFusion();
+ }
+ else
+ {
+ return IntegrateWithFusion();
+ }
+ }
+
+ bool CIMUDevice::IntegrateWithOutFusion()
+ {
+ return true;
+ }
+
+ bool CIMUDevice::IntegrateWithFusion()
+ {
+ return true;
+ }
+
+ bool CIMUDevice::InitializeDevice(const std::string& PortName, const SamplingFrequency Frequency)
+ {
+
+ _MINIMAL___LOCK(m_DeviceMutex)
+
+#ifdef _IMU_USE_XSENS_DEVICE_
+
+ if (InitializeXsensDevice(PortName,Frequency))
+ {
+ m_SamplingFrequency = Frequency;
+ const int Cylces = 0X1C200 / int(m_SamplingFrequency);
+ m_PeriodMicroSeconds = int(round((1000000.0f * _IMU_DEVICE_DEFAUL_CHECK_PERIOD_FACTOR_) / float(Cylces)));
+ _MINIMAL_UNLOCK(m_DeviceMutex)
+ return true;
+ }
+ else
+ {
+ _MINIMAL_UNLOCK(m_DeviceMutex)
+ return false;
+ }
+
+#endif
+
+ }
+
+ void CIMUDevice::FinalizeModuleDevice()
+ {
+ Stop(true);
+
+#ifdef _IMU_USE_XSENS_DEVICE_
+
+ FinalizeXsensModuleDevice();
+
+#endif
+
+ UnregisterEventDispatchers();
+
+ }
+
+ void CIMUDevice::ShouldYield()
+ {
+ const long int RemainingTime = m_PeriodMicroSeconds - CTimeStamp::GetElapsedMicroseconds(m_LastFrameTimeStamp);
+ if (RemainingTime > 0)
+ usleep(__useconds_t(RemainingTime));
+ }
+
+ bool CIMUDevice::DispatchCylcle()
+ {
+ if (LoadCurrentState())
+ {
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp,CIMUEvent::eOnIMUCycle,this));
+ switch(m_FusionStrategy)
+ {
+ case eMeanFusion:
+ if (MeanFuseCurrentState())
+ {
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp,CIMUEvent::eOnIMUFusedCycle,this,m_FusedIMUState));
+ if (IntegrateCurrentState())
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp,CIMUEvent::eOnIMUIntegratedState,this,m_IntegratedIMUState));
+ }
+ break;
+ case eGaussianFusion:
+ if (GaussianFuseCurrentState())
+ {
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp,CIMUEvent::eOnIMUIntegratedState,this,m_FusedIMUState));
+ if (IntegrateCurrentState())
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp,CIMUEvent::eOnIMUIntegratedState,this,m_IntegratedIMUState));
+ }
+ break;
+ case eNoFusion:
+ if (IntegrateCurrentState())
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp,CIMUEvent::eOnIMUIntegratedState,this,m_IntegratedIMUState));
+ break;
+ }
+ return true;
+ }
+ return false;
+ }
+
+ void CIMUDevice::SendEvent(const CIMUEvent& Event)
+ {
+ _MINIMAL___LOCK(m_EventDispatchersMutex)
+ const unsigned long int TotalDispatchers = m_IMUEventDispatchers.size();
+ if (TotalDispatchers)
+ {
+ if (TotalDispatchers == 1)
+ (*m_IMUEventDispatchers.begin())->ReceiveEvent(Event);
+ else
+ for(std::set<IIMUEventDispatcher*>::iterator ppIIMUEventDispatcher = m_IMUEventDispatchers.begin() ; ppIIMUEventDispatcher != m_IMUEventDispatchers.end() ; ++ppIIMUEventDispatcher)
+ (*ppIIMUEventDispatcher)->ReceiveEvent(Event);
+ }
+ _MINIMAL_UNLOCK(m_EventDispatchersMutex)
+ }
+
+ void CIMUDevice::SetReferenceTimeStamps()
+ {
+ _MINIMAL___LOCK(m_EventDispatchersMutex)
+ gettimeofday(&m_ReferenceTimeStamp,NULL);
+ gettimeofday(&m_LastFrameTimeStamp,NULL);
+ if (m_IMUEventDispatchers.size())
+ for(std::set<IIMUEventDispatcher*>::iterator ppIIMUEventDispatcher = m_IMUEventDispatchers.begin() ; ppIIMUEventDispatcher != m_IMUEventDispatchers.end() ; ++ppIIMUEventDispatcher)
+ (*ppIIMUEventDispatcher)->SetReferenceTimeStamps(m_ReferenceTimeStamp);
+ _MINIMAL_UNLOCK(m_EventDispatchersMutex)
+ }
+
+ bool CIMUDevice::SetThreadRunnigMode(const ThreadPolicyType ThreadPolicy, const float NormalizedPriority)
+ {
+ if (m_IsActive)
+ {
+ int Policy = -1;
+ struct sched_param SchedulingParameters;
+ if (pthread_getschedparam(m_pInternalThreadHandel,&Policy,&SchedulingParameters) == 0)
+ {
+ const int MaximalPriority = sched_get_priority_max(ThreadPolicy);
+ const int MinimalPriority = sched_get_priority_min(ThreadPolicy);
+ const int PriorityRange = MaximalPriority - MinimalPriority;
+ const int Priority = int(round(float(PriorityRange) * NormalizedPriority + float(MinimalPriority)));
+ SchedulingParameters.sched_priority = Priority;
+ const int Result = pthread_setschedparam(m_pInternalThreadHandel,ThreadPolicy,&SchedulingParameters);
+ switch(Result)
+ {
+ case 0:
+ return true;
+ break;
+ case EINVAL:
+ std::cerr << "[IMU Device error: SetThreadRunnigMode() returns EINVAL!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ break;
+ case ENOTSUP:
+ std::cerr << "[IMU Device error: SetThreadRunnigMode() returns ENOTSUP!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ break;
+ case EPERM:
+ std::cerr << "[IMU Device error: SetThreadRunnigMode() returns EPERM!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ break;
+ case ESRCH:
+ std::cerr << "[IMU Device error: SetThreadRunnigMode() returns ESRCH!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ break;
+ }
+ }
+ return false;
+ }
+ else
+ {
+ std::cerr << "[IMU Device error: SetThreadRunnigMode() cannot set running mode while thread is not active!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ return false;
+ }
+ }
+
+ void* CIMUDevice::ThreadLoop(void* pData)
+ {
+ if (pData)
+ {
+
+ CIMUDevice* pIMUDevice = (CIMUDevice*) pData;
+
+ _MINIMAL___LOCK(pIMUDevice->m_IsActiveMutex)
+ pIMUDevice->m_IsActive = true;
+ _MINIMAL_UNLOCK(pIMUDevice->m_IsActiveMutex)
+
+ pIMUDevice->SetReferenceTimeStamps();
+
+ pIMUDevice->SendEvent(CIMUEvent(CIMUEvent::eOnIMUStart,pIMUDevice));
+
+ while(pIMUDevice->m_IsActive)
+ {
+ pIMUDevice->ShouldYield();
+
+ _MINIMAL___LOCK(pIMUDevice->m_IsDispatchingMutex)
+ pIMUDevice->m_IsDispatching = true;
+
+ const bool DispatchingResult = pIMUDevice->DispatchCylcle();
+
+ pIMUDevice->m_IsDispatching = false;
+ _MINIMAL_UNLOCK(pIMUDevice->m_IsDispatchingMutex)
+
+ if (!DispatchingResult)
+ {
+ std::cerr << "[IMU Device error: DispatchCylcle() returns false!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ break;
+ }
+
+ }
+ if (pIMUDevice->m_IsActive)
+ {
+ _MINIMAL___LOCK(pIMUDevice->m_IsActiveMutex)
+ pIMUDevice->m_IsActive = false;
+ _MINIMAL_UNLOCK(pIMUDevice->m_IsActiveMutex)
+ }
+
+ pIMUDevice->SendEvent(CIMUEvent(CIMUEvent::eOnIMUStop,pIMUDevice));
+ }
+ return NULL;
+ }
+}
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDevice.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDevice.h
new file mode 100755
index 0000000000000000000000000000000000000000..ff37a7d3d9de28038ca765cf83eebf03b5ef9be8
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUDevice.h
@@ -0,0 +1,219 @@
+/*
+ * IMU.h
+ *
+ * Created on: Mar 16, 2014
+ * Author: Dr.-Ing. David Israel González Aguirre
+ * Mail: david.gonzalez@kit.edu
+ */
+
+#ifndef IMUDEVICE_H_
+#define IMUDEVICE_H_
+
+#include "Includes.h"
+#include "IMUHelpers.h"
+#include "IMUEvent.h"
+#include "IMUState.h"
+
+#ifdef _IMU_USE_XSENS_DEVICE_
+
+#include "Xsens/Xsens.h"
+#include "Xsens/XsensMTiModule.h"
+
+#endif
+
+#define _IMU_DEVICE_DEFAUL_CONNECTION_ std::string("/dev/ttyUSB0")
+#define _IMU_DEVICE_DEFAUL_FREQUENCY_ IMU::CIMUDevice::eSamplingFrequency_120HZ
+#define _IMU_DEVICE_DEFAUL_CHECK_PERIOD_FACTOR_ 0.5f
+#define _IMU_DEVICE_DEFAUL_NORMALIZED_THREAD_PRIORITY_ 0.125f
+
+namespace IMU
+{
+ //Forward definition
+ class IIMUEventDispatcher;
+
+ /*!
+ \class CIMUDevice
+ \ingroup IMU
+ \brief This class contains the the devices module and the thread for read the measurements.
+
+ CIMUDevice encapsulates the device details for the rest of the library and applications.
+ This also includes a thread which is in charge of generate the IMU events
+ */
+ class CIMUDevice
+ {
+ public:
+
+ /*!
+ \brief Enum specifying the running thread policy.
+ */
+ enum ThreadPolicyType
+ {
+ eRealTime = SCHED_FIFO, eRoundRobinPriorityBased = SCHED_RR, eBatch = SCHED_BATCH, eIdle = SCHED_IDLE
+ };
+
+ /*!
+ \brief Enum specifying the supported sampling frequencies.
+ */
+ enum SamplingFrequency
+ {
+ eSamplingFrequency_10HZ = 0x2D00,
+ eSamplingFrequency_12HZ = 0x2580,
+ eSamplingFrequency_15HZ = 0x1E00,
+ eSamplingFrequency_16HZ = 0x1C20,
+ eSamplingFrequency_18HZ = 0x1900,
+ eSamplingFrequency_20HZ = 0x1680,
+ eSamplingFrequency_24HZ = 0x12C0,
+ eSamplingFrequency_25HZ = 0x1200,
+ eSamplingFrequency_30HZ = 0x0F00,
+ eSamplingFrequency_32HZ = 0x0E10,
+ eSamplingFrequency_36HZ = 0x0C80,
+ eSamplingFrequency_40HZ = 0x0B40,
+ eSamplingFrequency_45HZ = 0x0A00,
+ eSamplingFrequency_48HZ = 0x0960,
+ eSamplingFrequency_50HZ = 0x0900,
+ eSamplingFrequency_60HZ = 0x0780,
+ eSamplingFrequency_64HZ = 0x0708,
+ eSamplingFrequency_72HZ = 0x0640,
+ eSamplingFrequency_75HZ = 0x0600,
+ eSamplingFrequency_80HZ = 0x05A0,
+ eSamplingFrequency_90HZ = 0x0500,
+ eSamplingFrequency_96HZ = 0x04B0,
+ eSamplingFrequency_100HZ = 0x0480,
+ eSamplingFrequency_120HZ = 0x03C0,
+ eSamplingFrequency_128HZ = 0x0384,
+ eSamplingFrequency_144HZ = 0x0320,
+ eSamplingFrequency_150HZ = 0x0300,
+ eSamplingFrequency_160HZ = 0x02D0,
+ eSamplingFrequency_180HZ = 0x0280,
+ eSamplingFrequency_192HZ = 0x0258,
+ eSamplingFrequency_200HZ = 0x0240,
+ eSamplingFrequency_225HZ = 0x0200,
+ eSamplingFrequency_240HZ = 0x01E0,
+ eSamplingFrequency_256HZ = 0x01C2,
+ eSamplingFrequency_288HZ = 0x0190,
+ eSamplingFrequency_300HZ = 0x0180,
+ eSamplingFrequency_320HZ = 0x0168,
+ eSamplingFrequency_360HZ = 0x0140,
+ eSamplingFrequency_384HZ = 0x012C,
+ eSamplingFrequency_400HZ = 0x0120,
+ eSamplingFrequency_450HZ = 0x0100,
+ eSamplingFrequency_480HZ = 0x00F0,
+ eSamplingFrequency_512HZ = 0x00E1
+ };
+
+ enum FusionStrategy
+ {
+ eNoFusion, eMeanFusion, eGaussianFusion
+ };
+
+ /*!
+ \brief The default constructor.
+ The default constructor sets all member variables to zero, i.e. after construction no valid device nor thread are represented.
+ */
+ CIMUDevice();
+
+ /*!
+ \brief The destructor.
+ */
+ virtual ~CIMUDevice();
+
+ uint64_t GetDeviceId() const;
+
+ inline IMUState GetIMUState() const
+ {
+
+#ifdef _IMU_USE_XSENS_DEVICE_
+
+ return m_XsensMTiFrame.m_IMUState;
+
+#endif
+
+ }
+
+ bool Connect(const std::string& PortName, const SamplingFrequency Frequency);
+
+ bool Start(const bool Blocking = true);
+
+ bool SetThreadRunnigMode(const ThreadPolicyType ThreadPolicy, const float NormalizedPriority);
+
+ void Stop(const bool Blocking = true);
+
+ bool SetFusion(const FusionStrategy Strategy, const ushort SamplesPerFusion);
+
+ bool IsActive() const;
+
+ bool RegisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher);
+
+ bool UnregisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher);
+
+ inline const timeval& GetReferenceTimeStamp() const
+ {
+ return m_ReferenceTimeStamp;
+ }
+
+ protected:
+
+ bool LoadCurrentState();
+
+ bool MeanFuseCurrentState();
+ void IncorporateCurrentStateMeanFusion();
+ void MeanFusion();
+
+ bool GaussianFuseCurrentState();
+ void IncorporateCurrentStateGaussianFusion();
+ void GaussianFusion();
+
+ bool IntegrateCurrentState();
+ bool IntegrateWithOutFusion();
+ bool IntegrateWithFusion();
+
+ private:
+
+ void UnregisterEventDispatchers();
+
+ bool InitializeDevice(const std::string& PortName, const SamplingFrequency Frequency);
+ void FinalizeModuleDevice();
+ void ShouldYield();
+ bool DispatchCylcle();
+ void SendEvent(const CIMUEvent& Event);
+ void SetReferenceTimeStamps();
+
+ static void* ThreadLoop(void* pData);
+
+ uint64_t m_DeviceId;
+ SamplingFrequency m_SamplingFrequency;
+ int m_PeriodMicroSeconds;
+ FusionStrategy m_FusionStrategy;
+ int m_SamplesPerFusion;
+ int m_CollectedFusionSamples;
+ volatile bool m_IsActive;
+ volatile bool m_IsDispatching;
+ bool m_IsInitialized;
+ pthread_t m_pInternalThreadHandel;
+ pthread_mutex_t m_IsActiveMutex;
+ pthread_mutex_t m_IsDispatchingMutex;
+ pthread_mutex_t m_EventDispatchersMutex;
+ pthread_mutex_t m_DeviceMutex;
+ std::set<IIMUEventDispatcher*> m_IMUEventDispatchers;
+ timeval m_ReferenceTimeStamp;
+ timeval m_LastFrameTimeStamp;
+
+ IMUState::PhysicalData m_FusedPhysicalData;
+ IMUState m_FusedIMUState;
+ IMUState m_IntegratedIMUState;
+
+#ifdef _IMU_USE_XSENS_DEVICE_
+
+ bool InitializeXsensDevice(const std::string& PortName, const SamplingFrequency Frequency);
+ void FinalizeXsensModuleDevice();
+ void DestroyXsensModuleDevice();
+
+ Xsens::CXsensMTiModule* m_pXsensMTiModule;
+ Xsens::XsensMTiFrame m_XsensMTiFrame;
+
+#endif
+
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUEvent.cpp b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUEvent.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..5a2aaef79521e8e7b64fd464b73287cf8b6f8a2d
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUEvent.cpp
@@ -0,0 +1,54 @@
+/*
+ * IMUEvent.cpp
+ *
+ * Created on: Mar 16, 2014
+ * Author: Dr.-Ing. David Israel González Aguirre
+ * Mail: david.gonzalez@kit.edu
+ */
+
+#include "IMUEvent.h"
+#include "IMUDevice.h"
+
+namespace IMU
+{
+ uint32_t CIMUEvent::s_IdCounter = 0;
+ pthread_mutex_t CIMUEvent::s_IdCounterMutex = PTHREAD_MUTEX_INITIALIZER;
+
+ CIMUEvent::CIMUEvent(const timeval& TimeStamp, const EventType EventType, const CIMUDevice* pIMUDevice, const IMUState& EventState) :
+ m_Id(CreatId()) ,
+m_TimeStamp (TimeStamp), m_EventType(EventType), m_pIMUDevice(pIMUDevice), m_IMUState(EventState)
+ {
+ }
+
+ CIMUEvent::CIMUEvent(const timeval& TimeStamp, const EventType EventType, const CIMUDevice* pIMUDevice) :
+ m_Id(CreatId()), m_TimeStamp(TimeStamp), m_EventType(EventType), m_pIMUDevice(pIMUDevice), m_IMUState(pIMUDevice->GetIMUState())
+ {
+ }
+
+ CIMUEvent::CIMUEvent(const EventType EventType, const CIMUDevice* pIMUDevice) :
+ m_Id(CreatId()), m_TimeStamp(CTimeStamp::GetCurrentTimeStamp()), m_EventType(EventType), m_pIMUDevice(pIMUDevice), m_IMUState(pIMUDevice->GetIMUState())
+ {
+ }
+
+ CIMUEvent::CIMUEvent(const CIMUEvent& Event) :
+ m_Id(CreatId()), m_TimeStamp(Event.m_TimeStamp), m_EventType(Event.m_EventType), m_pIMUDevice(Event.m_pIMUDevice), m_IMUState(Event.m_IMUState)
+ {
+ }
+
+ CIMUEvent::CIMUEvent() :
+ m_Id(CreatId()), m_TimeStamp(CTimeStamp::s_Zero), m_EventType(EventType(0x0)), m_pIMUDevice(NULL), m_IMUState()
+ {
+ }
+
+ CIMUEvent::~CIMUEvent()
+ {
+ }
+
+ uint32_t CIMUEvent::CreatId()
+ {
+ pthread_mutex_lock(&s_IdCounterMutex);
+ uint32_t Id = CIMUEvent::s_IdCounter++;
+ pthread_mutex_unlock(&s_IdCounterMutex);
+ return Id;
+ }
+}
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUEvent.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUEvent.h
new file mode 100755
index 0000000000000000000000000000000000000000..f10f4755e27fd2a4c51ad2f38cf5241491877cd2
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUEvent.h
@@ -0,0 +1,71 @@
+/*
+ * IMUEvent.h
+ *
+ * Created on: Mar 16, 2014
+ * Author: Dr.-Ing. David Israel González Aguirre
+ * Mail: david.gonzalez@kit.edu
+ */
+
+#ifndef IMUEVENT_H_
+#define IMUEVENT_H_
+
+#include "IMUHelpers.h"
+#include "IMUState.h"
+
+namespace IMU
+{
+ class CIMUDevice;
+ class CIMUEvent
+ {
+ public:
+
+ enum EventType
+ {
+ eOnIMUStart = 0X0001, eOnIMUStop = 0X0002, eOnIMUCycle = 0X0004, eOnIMUFusedCycle = 0X0008, eOnIMUIntegratedState = 0X0010, eOnIMUCustomEvent = 0X8000
+ };
+
+ CIMUEvent(const timeval& TimeStamp, const EventType EventType, const CIMUDevice* pIMUDevice, const IMUState& EventState);
+ CIMUEvent(const timeval& TimeStamp, const EventType EventType, const CIMUDevice* pIMUDevice);
+ CIMUEvent(const EventType EventType, const CIMUDevice* pIMUDevice);
+ CIMUEvent(const CIMUEvent& Event);
+ CIMUEvent();
+
+ virtual ~CIMUEvent();
+
+ inline uint32_t GetId() const
+ {
+ return m_Id;
+ }
+
+ inline EventType GetEventType() const
+ {
+ return m_EventType;
+ }
+
+ inline const CIMUDevice* GetIMU() const
+ {
+ return m_pIMUDevice;
+ }
+
+ inline const timeval& GetTimeStamp() const
+ {
+ return m_TimeStamp;
+ }
+
+ protected:
+
+ uint32_t m_Id;
+ const timeval m_TimeStamp;
+ const EventType m_EventType;
+ const CIMUDevice* m_pIMUDevice;
+ const IMUState m_IMUState;
+
+ private:
+
+ static uint32_t CreatId();
+ static uint32_t s_IdCounter;
+ static pthread_mutex_t s_IdCounterMutex;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUHelpers.cpp b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUHelpers.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..1ceeff59f0115005639a78973fd904217fe2fa6e
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUHelpers.cpp
@@ -0,0 +1,18 @@
+/*
+ * IMUHelpers.cpp
+ *
+ * Created on: Mar 16, 2014
+ * Author: Dr.-Ing. David Israel González Aguirre
+ * Mail: david.gonzalez@kit.edu
+ */
+
+#include "IMUHelpers.h"
+
+namespace IMU
+{
+ const timeval CTimeStamp::s_Zero = { 0 , 0 };
+
+ const float CGeolocationInformation::s_G_LPoles = 9.832f;
+ const float CGeolocationInformation::s_G_L45 = 9.806f;
+ const float CGeolocationInformation::s_G_LEquator = 9.780f;
+}
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUHelpers.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUHelpers.h
new file mode 100755
index 0000000000000000000000000000000000000000..82632ec54165d23e5ba09d15a1a7afa718e0dcf6
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUHelpers.h
@@ -0,0 +1,77 @@
+/*
+ * IMUHelpers.h
+ *
+ * Created on: Mar 17, 2014
+ * Author: Dr.-Ing. David Israel González Aguirre
+ * Mail: david.gonzalez@kit.edu
+ */
+
+#ifndef IMUHELPERS_H_
+#define IMUHELPERS_H_
+
+#include "Includes.h"
+
+#define _MINIMAL___LOCK(MUTEX) pthread_mutex_lock(&MUTEX);
+#define _MINIMAL_UNLOCK(MUTEX) pthread_mutex_unlock(&MUTEX);
+
+namespace IMU
+{
+ class CTimeStamp
+ {
+ public:
+
+ inline static timeval GetCurrentTimeStamp()
+ {
+ timeval TimeStamp;
+ gettimeofday(&TimeStamp,NULL);
+ return TimeStamp;
+ }
+
+ inline static float GetElapsedSeconds(const timeval& Post, const timeval& Pre)
+ {
+ return float(double(GetElapsedMicroseconds(Post,Pre)) / 1000000.0);
+ }
+
+ inline static float GetElapsedMilliseconds(const timeval& Post, const timeval& Pre)
+ {
+ return float(double(GetElapsedMicroseconds(Post,Pre)) / 1000.0);
+ }
+
+ inline static long GetElapsedMicroseconds(const timeval& Post, const timeval& Pre)
+ {
+ return ((Post.tv_sec - Pre.tv_sec) * 1000000) + (Post.tv_usec - Pre.tv_usec);
+ }
+
+ inline static long GetElapsedMicroseconds(const timeval& Pre)
+ {
+ timeval Post;
+ gettimeofday(&Post,NULL);
+ return ((Post.tv_sec - Pre.tv_sec) * 1000000) + (Post.tv_usec - Pre.tv_usec);
+ }
+
+ static const timeval s_Zero;
+ };
+
+ class CGeolocationInformation
+ {
+ public:
+
+ static const float s_G_LPoles;
+ static const float s_G_L45;
+ static const float s_G_LEquator;
+
+ //LatitudeInDegrees of your location:
+ //http://www.mapsofworld.com/lat_long/germany-lat-long.html
+ //49.0167 Karlsruhe, Germany
+
+ static float GetGravitationalAcceleration(const float LatitudeInDegrees = 49.0167f)
+ {
+ return s_G_L45 - (s_G_LPoles - s_G_LEquator) * std::cos((float(M_PI) / 90.0f) * LatitudeInDegrees);
+ }
+ };
+
+
+
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUState.cpp b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUState.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..cde825c3c17e262d80f8721531370f053dde7756
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUState.cpp
@@ -0,0 +1,14 @@
+/*
+ * IMUState.cpp
+ *
+ * Created on: Mar 16, 2014
+ * Author: Dr.-Ing. David Israel González Aguirre
+ * Mail: david.gonzalez@kit.edu
+ */
+
+#include "IMUState.h"
+
+namespace IMU
+{
+
+}
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUState.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUState.h
new file mode 100755
index 0000000000000000000000000000000000000000..7f8fe38c447973993230c921c48176059bc70fe0
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUState.h
@@ -0,0 +1,66 @@
+/*
+ * IMUState.h
+ *
+ * Created on: Mar 16, 2014
+ * Author: Dr.-Ing. David Israel González Aguirre
+ * Mail: david.gonzalez@kit.edu
+ */
+
+#ifndef IMUSTATE_H_
+#define IMUSTATE_H_
+
+#include "Includes.h"
+
+namespace IMU
+{
+ struct IMUState
+ {
+ inline IMUState() :
+ m_ControlData(), m_PhysicalData()
+ {
+ }
+
+ struct ControlData
+ {
+ ControlData() :
+ m_PreviousSampleCount(-1), m_CurrentSampleCount(0), m_MessageCounter(0), m_IsConsecutive(false)
+ {
+ memset(&m_TimeStamp,0,sizeof(timeval));
+ }
+
+ long m_PreviousSampleCount;
+ unsigned short m_CurrentSampleCount;
+ long m_MessageCounter;
+ bool m_IsConsecutive;
+ timeval m_TimeStamp;
+ };
+
+ struct PhysicalData
+ {
+ PhysicalData() :
+ m_AccelerationMagnitud(0.0f)
+ {
+ memset(m_Acceleration,0,sizeof(float) * 3);
+ memset(m_GyroscopeRotation,0,sizeof(float) * 3);
+ memset(m_MagneticRotation,0,sizeof(float) * 3);
+ memset(m_QuaternionRotation,0,sizeof(float) * 4);
+ }
+
+ float m_Acceleration[3];
+ float m_AccelerationMagnitud;
+ float m_GyroscopeRotation[3];
+ float m_MagneticRotation[3];
+ float m_QuaternionRotation[4];
+
+ inline void UpdateAccelerationMagnitud()
+ {
+ m_AccelerationMagnitud = std::sqrt(m_Acceleration[0] * m_Acceleration[0] + m_Acceleration[1] * m_Acceleration[1] + m_Acceleration[2] * m_Acceleration[2]);
+ }
+ };
+
+ ControlData m_ControlData;
+ PhysicalData m_PhysicalData;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Includes.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Includes.h
new file mode 100755
index 0000000000000000000000000000000000000000..958dea63913562559ba9b810d6febc65e88be338
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Includes.h
@@ -0,0 +1,55 @@
+/*
+ * Includes.h
+ *
+ * Created on: Mar 17, 2014
+ * Author: Dr.-Ing. David Israel González Aguirre
+ * Mail: david.gonzalez@kit.edu
+ */
+
+#ifndef INCLUDES_H_
+#define INCLUDES_H_
+
+/////////////////////////////////////////////////////////////////////////////
+//GENERAL INFORMATION
+/////////////////////////////////////////////////////////////////////////////
+#define _LIBRARY_ACRONYM_ "IMU"
+#define _LIBRARY_NAME_ "Robot Vision Library"
+#define _MAJOR_VERSION_ 0
+#define _MINOR_VERSION_ 1
+#define _REVISION_NUMBER_ 0
+#define _BUILD_NUMBER_ 0
+#define _AUTHOR_ "Dr.-Ing. David Israel González Aguirre"
+#define _AUTHOR_MAIL_ "david.gonzalez@kit.edu"
+#define _CONTRIBUTOR_0_ ""
+#define _SUPPORTER_0_ "EU-Project Koroibot"
+#define _SUPPORTER_1_ "EU-Project Walk-Man"
+#define _SUPPORTER_2_ "ARMAR X"
+/////////////////////////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////////////////////////
+//GENERAL INCLUDES
+/////////////////////////////////////////////////////////////////////////////
+#include <stdint.h>
+#include <pthread.h>
+#include <string.h>
+#include <set>
+#include <list>
+#include <iostream>
+#include <cmath>
+/////////////////////////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////////////////////////
+//LINUX INCLUDES
+/////////////////////////////////////////////////////////////////////////////
+#include <sys/time.h>
+#include <sched.h>
+#include <errno.h>
+/////////////////////////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////////////////////////
+//GENERAL SWITCHES
+/////////////////////////////////////////////////////////////////////////////
+#define _IMU_USE_XSENS_DEVICE_
+/////////////////////////////////////////////////////////////////////////////
+
+#endif /* INCLUDES_H_ */
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/Xsens.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/Xsens.h
new file mode 100755
index 0000000000000000000000000000000000000000..f52c895894940c56b40fdf50c5d4d70a34bbc9ca
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/Xsens.h
@@ -0,0 +1,39 @@
+/*
+ * Xsens.h
+ *
+ * Created on: Mar 17, 2014
+ * Author: gonzalez
+ */
+
+#ifndef XSENS_H_
+#define XSENS_H_
+
+#include "../Includes.h"
+#include "../IMUState.h"
+
+#ifdef _IMU_USE_XSENS_DEVICE_
+
+#include "XsensMTiModule.h"
+
+namespace IMU
+{
+ namespace Xsens
+ {
+ struct XsensMTiFrame
+ {
+ XsensMTiFrame() :
+ m_DataLength(0)
+ {
+ memset(m_Data,0,sizeof(unsigned char) * MAXMSGLEN);
+ }
+
+ short m_DataLength;
+ unsigned char m_Data[MAXMSGLEN ];
+ IMUState m_IMUState;
+ };
+ }
+}
+
+#endif
+
+#endif
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.cpp b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..389219d7aa4fd18cf340e366e42e32359b5a4275
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.cpp
@@ -0,0 +1,2949 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2006 Radu Bogdan Rusu (rusu@cs.tum.edu)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+/*
+ Desc: Driver for XSens MTx/MTi IMU. CMTComm class borrowed from XSens under GPL.
+ Author: Radu Bogdan Rusu
+ Date: 1 Aug 2006
+ */
+// MTComm.cpp: implementation of the CMTComm class.
+//
+// Version 1.2.0
+// Public release
+//
+// v1.2.0
+// 27-02-2006 - Renamed Xbus class to Motion Tracker C++ Communication class, short MTComm
+// - Defines XBRV_* accordingly renamed to MTRV_*
+// - Fixed device length not correct for bid 0 when using Xbus Master and setDeviceMode function
+//
+// v1.1.7
+// 15-02-2006 - Added fixed point signed 12.20 dataformat support
+// Added selective calibrated data output per sensor type support
+// Added outputmode temperature support
+// Fixed warning C4244: '=' : conversion from '' to '', possible loss of data
+// v1.1.6
+// 25-01-2006 - Added escape function for CLRDTR, CLRRTS, SETDTR, SETRTS, SETXOFF, SETXON, SETBREAK, CLRBREAK
+//
+// v1.1.5
+// 14-11-2005 - Made swapEndian a static member function, Job Mulder
+//
+// v1.1.4
+// 08-11-2005 - Changed practically all uses of m_timeOut into uses of the new m_clkEnd
+// - Changed COM timeout in win32 to return immediately if data is available,
+// but wait 1ms otherwise
+//
+// v1.1.3
+// 18-10-2005 - Added MID_REQPRODUCTCODE, MID_REQ/SETTRANSMITDELAY
+// - Added MTRV_TIMEOUTNODATA indicating timeout occurred due to no data read
+//
+// v1.1.2
+// 16-09-2005 - Added eMTS version 0.1->1.0 changes (EMTS_FACTORYMODE)
+// - Added factory output mode defines
+//
+// v1.1.1
+// 01-09-2005 - Added defines for Extended output mode
+// - Added reqSetting (byte array in + out & no param variant)
+//
+// v1.1
+// 08-08-2005 - Added file read and write support
+// - Added functions for data retrieval (getValue etc)
+// for easy data retrieval of acc, gyr, mag etc
+// - ReadMessageRaw:
+// - added a temporary buffer for unprocessed bytes
+// - check for invalid length messages
+// - Changed BID_MT into 1 and added BID_MASTER (=0xFF)
+// - Changed various ....SerialPort functions to .....Port
+// - Changed mtSendMessage functions to mtWriteMessage
+// - Added numerous defines
+// - Deleted obsolete functions
+// - Changed function getLastErrorCode into getLastDeviceError
+// - Changed OpenPort function for compatiblity with Bluetooth ports
+// - Added workaround for lockup of USB driver (close function)
+// - Added workaround for clock() problem with read function of USB driver
+//
+// v1.0.2
+// 29-06-2005 - Inserted initSerialPort with devicename input
+// - Changed return value defines names from X_ to MTRV_
+// - Removed unneeded includes for linux
+//
+// v1.0.1
+// 22-06-2005 - Fixed ReqSetting functions (byte array & param variant)
+// mtSendRawString had wrong length input
+//
+// v1.0.0
+// 20-06-2005 - Initial release
+//
+// ----------------------------------------------------------------------------
+// This file is an Xsens Code Examples.
+//
+// Copyright (C) Xsens Technologies B.V., 2005.
+//
+// This source code is intended only as a example of the implementation
+// of the Xsens MT Communication protocol.
+// It was written for cross platform capabilities.
+//
+// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
+// KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
+// PARTICULAR PURPOSE.
+//////////////////////////////////////////////////////////////////////
+#include "XsensMTiModule.h"
+
+#ifdef _IMU_USE_XSENS_DEVICE_
+
+#ifdef _DEBUG
+#undef THIS_FILE
+static char THIS_FILE[]=__FILE__;
+#define new DEBUG_NEW
+#endif
+
+#ifndef CRTSCTS
+#ifdef IHFLOW
+#ifdef OHFLOW
+#define CRTSCTS ((IHFLOW) | (OHFLOW))
+#endif
+#endif
+#endif
+
+namespace IMU
+{
+ namespace Xsens
+ {
+ //////////////////////////////////////////////////////////////////////
+ // Construction/Destruction
+ //////////////////////////////////////////////////////////////////////
+ //
+ CXsensMTiModule::CXsensMTiModule()
+ {
+ m_handle = -1;
+ m_portOpen = false;
+ m_fileOpen = false;
+ m_deviceError = 0; // No error
+ m_retVal = MTRV_OK;
+ m_timeOut = TO_DEFAULT;
+ m_nTempBufferLen = 0;
+ m_clkEnd = 0;
+ for(int i = 0 ; i < MAXDEVICES + 1 ; i++)
+ {
+ m_storedOutputMode[i] = INVALIDSETTINGVALUE;
+ m_storedOutputSettings[i] = INVALIDSETTINGVALUE;
+ m_storedDataLength[i] = 0;
+ }
+ }
+
+ CXsensMTiModule::~CXsensMTiModule()
+ {
+ close();
+ }
+
+////////////////////////////////////////////////////////////////////
+// clockms
+//
+// Calculates the processor time used by the calling process.
+// For linux use gettimeofday instead of clock() function
+// When using read from FTDI serial port in a loop the
+// clock() function very often keeps returning 0.
+//
+// Output
+// returns processor time in milliseconds
+//
+ clock_t CXsensMTiModule::clockms()
+ {
+ clock_t clk;
+#ifdef WIN32
+ clk = clock(); // Get current processor time
+#if (CLOCKS_PER_SEC != 1000)
+ clk /= (CLOCKS_PER_SEC / 1000);
+ // clk = (clk * 1000) / CLOCKS_PER_SEC;
+#endif
+#else
+ struct timeval tv;
+ struct timezone tz;
+ gettimeofday(&tv,&tz);
+ clk = tv.tv_sec * 1000 + (tv.tv_usec / 1000);
+#endif
+ return clk;
+ }
+
+////////////////////////////////////////////////////////////////////
+// openPort (serial port number as input parameter)
+//
+// Opens a 'live' connection to a MT or XM
+//
+// Input
+// portNumber : number of serial port to open (1-99)
+// baudrate : baudrate value (One of the PBR_* defines), default = PBR_115K2
+// inqueueSize : size of input queue, default = 4096
+// outqueueSize: size of output queue, default = 1024
+//
+// Output
+// returns MTRV_OK if serial port is successfully opened, else MTRV_INPUTCANNOTBEOPENED
+//
+ short CXsensMTiModule::openPort(const int portNumber, const unsigned long baudrate, const unsigned long /*inqueueSize*/, const unsigned long /*outqueueSize*/)
+ {
+ m_clkEnd = 0;
+
+ if (m_fileOpen || m_portOpen)
+ {
+ return (m_retVal = MTRV_ANINPUTALREADYOPEN);
+ }
+#ifdef WIN32
+ char pchFileName[10];
+
+ sprintf(pchFileName,"\\\\.\\COM%d",portNumber); // Create file name
+
+ m_handle = CreateFile(pchFileName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
+ if (m_handle == INVALID_HANDLE_VALUE)
+ {
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
+
+ // Once here, port is open
+ m_portOpen = true;
+
+ //Get the current state & then change it
+ DCB dcb;
+
+ GetCommState(m_handle, &dcb);// Get current state
+
+ dcb.BaudRate = baudrate;// Setup the baud rate
+ dcb.Parity = NOPARITY;// Setup the Parity
+ dcb.ByteSize = 8;// Setup the data bits
+ dcb.StopBits = TWOSTOPBITS;// Setup the stop bits
+ dcb.fDsrSensitivity = FALSE;// Setup the flow control
+ dcb.fOutxCtsFlow = FALSE;// NoFlowControl:
+ dcb.fOutxDsrFlow = FALSE;
+ dcb.fOutX = FALSE;
+ dcb.fInX = FALSE;
+ if (!SetCommState(m_handle, (LPDCB)&dcb))
+ { // Set new state
+ // Bluetooth ports cannot always be opened with 2 stopbits
+ // Now try to open port with 1 stopbit.
+ dcb.StopBits = ONESTOPBIT;
+ if (!SetCommState(m_handle, (LPDCB)&dcb))
+ {
+ CloseHandle(m_handle);
+ m_handle = INVALID_HANDLE_VALUE;
+ m_portOpen = false;
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
+ }
+
+ // Set COM timeouts
+ COMMTIMEOUTS CommTimeouts;
+
+ GetCommTimeouts(m_handle,&CommTimeouts);// Fill CommTimeouts structure
+
+ // immediate return if data is available, wait 1ms otherwise
+ CommTimeouts.ReadTotalTimeoutConstant = 1;
+ CommTimeouts.ReadIntervalTimeout = MAXDWORD;
+ CommTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
+
+ // immediate return whether data is available or not
+// CommTimeouts.ReadTotalTimeoutConstant = 0;
+// CommTimeouts.ReadIntervalTimeout = MAXDWORD;
+// CommTimeouts.ReadTotalTimeoutMultiplier = 0;
+
+ SetCommTimeouts(m_handle, &CommTimeouts);// Set CommTimeouts structure
+
+ // Other initialization functions
+ EscapeCommFunction(m_handle, SETRTS);// Enable RTS (for Xbus Master use)
+ SetupComm(m_handle,inqueueSize,outqueueSize);// Set queue size
+
+ // Remove any 'old' data in buffer
+ PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
+
+ return (m_retVal = MTRV_OK);
+#else
+ char chPort[15];
+ struct termios options;
+
+ /* Open port */
+ sprintf(chPort,"/dev/ttyS%d",(portNumber - 1));
+ m_handle = open(chPort,O_RDWR | O_NOCTTY);
+
+ // O_RDWR: Read+Write
+ // O_NOCTTY: Raw input, no "controlling terminal"
+ // O_NDELAY: Don't care about DCD signal
+
+ if (m_handle < 0)
+ {
+ // Port not open
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
+
+ // Once here, port is open
+ m_portOpen = true;
+
+ /* Start configuring of port for non-canonical transfer mode */
+ // Get current options for the port
+ tcgetattr(m_handle,&options);
+
+ // Set baudrate.
+ cfsetispeed(&options,baudrate);
+ cfsetospeed(&options,baudrate);
+
+ // Enable the receiver and set local mode
+ options.c_cflag |= (CLOCAL | CREAD);
+ // Set character size to data bits and set no parity Mask the characte size bits
+ options.c_cflag &= ~(CSIZE | PARENB);
+ options.c_cflag |= CS8; // Select 8 data bits
+ options.c_cflag |= CSTOPB; // send 2 stop bits
+ // Disable hardware flow control
+ options.c_cflag &= ~CRTSCTS;
+ options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
+ // Disable software flow control
+ options.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
+ // Set Raw output
+ options.c_oflag &= ~OPOST;
+ // Timeout 0.005 sec for first byte, read minimum of 0 bytes
+ options.c_cc[VMIN] = 0;
+ options.c_cc[VTIME] = 5;
+
+ // Set the new options for the port
+ tcsetattr(m_handle,TCSANOW,&options);
+
+ tcflush(m_handle,TCIOFLUSH);
+
+ return (m_retVal = MTRV_OK);
+#endif
+ }
+
+////////////////////////////////////////////////////////////////////
+// openPort (string as input parameter)
+//
+// Opens a 'live' connection to a MT or XM
+//
+// Input
+// portName : device name of serial port ("/dev/ttyUSB0" or "\\\\.\\COM1")
+// baudrate : baudrate value (One of the PBR_* defines), default = PBR_115K2
+// inqueueSize : size of input queue, default = 4096
+// outqueueSize: size of output queue, default = 1024
+//
+// Output
+// returns MTRV_OK if serial port is successfully opened, else MTRV_INPUTCANNOTBEOPENED
+//
+ short CXsensMTiModule::openPort(const char *portName, const unsigned long baudrate, const unsigned long /*inqueueSize*/, const unsigned long /*outqueueSize*/)
+ {
+ m_clkEnd = 0;
+
+ if (m_fileOpen || m_portOpen)
+ {
+ return (m_retVal = MTRV_ANINPUTALREADYOPEN);
+ }
+#ifdef WIN32
+ m_handle = CreateFile(portName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
+ if (m_handle == INVALID_HANDLE_VALUE)
+ {
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
+
+ // Once here, port is open
+ m_portOpen = true;
+
+ //Get the current state & then change it
+ DCB dcb;
+
+ GetCommState(m_handle, &dcb);// Get current state
+
+ dcb.BaudRate = baudrate;// Setup the baud rate
+ dcb.Parity = NOPARITY;// Setup the Parity
+ dcb.ByteSize = 8;// Setup the data bits
+ dcb.StopBits = TWOSTOPBITS;// Setup the stop bits
+ dcb.fDsrSensitivity = FALSE;// Setup the flow control
+ dcb.fOutxCtsFlow = FALSE;// NoFlowControl:
+ dcb.fOutxDsrFlow = FALSE;
+ dcb.fOutX = FALSE;
+ dcb.fInX = FALSE;
+ if (!SetCommState(m_handle, (LPDCB)&dcb))
+ { // Set new state
+ // Bluetooth ports cannot always be opened with 2 stopbits
+ // Now try to open port with 1 stopbit.
+ dcb.StopBits = ONESTOPBIT;
+ if (!SetCommState(m_handle, (LPDCB)&dcb))
+ {
+ CloseHandle(m_handle);
+ m_handle = INVALID_HANDLE_VALUE;
+ m_portOpen = false;
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
+ }
+
+ // Set COM timeouts
+ COMMTIMEOUTS CommTimeouts;
+
+ GetCommTimeouts(m_handle,&CommTimeouts);// Fill CommTimeouts structure
+
+ // immediate return if data is available, wait 1ms otherwise
+ CommTimeouts.ReadTotalTimeoutConstant = 1;
+ CommTimeouts.ReadIntervalTimeout = MAXDWORD;
+ CommTimeouts.ReadTotalTimeoutMultiplier = MAXDWORD;
+
+ // immediate return whether data is available or not
+// CommTimeouts.ReadTotalTimeoutConstant = 0;
+// CommTimeouts.ReadIntervalTimeout = MAXDWORD;
+// CommTimeouts.ReadTotalTimeoutMultiplier = 0;
+ SetCommTimeouts(m_handle, &CommTimeouts);// Set CommTimeouts structure
+
+ // Other initialization functions
+ EscapeCommFunction(m_handle, SETRTS);// Enable RTS (for Xbus Master use)
+ SetupComm(m_handle,inqueueSize,outqueueSize);// Set queue size
+
+ // Remove any 'old' data in buffer
+ PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
+
+ return (m_retVal = MTRV_OK);
+#else
+ struct termios options;
+
+ /* Open port */
+
+ m_handle = open(portName,O_RDWR | O_NOCTTY);
+
+ // O_RDWR: Read+Write
+ // O_NOCTTY: Raw input, no "controlling terminal"
+ // O_NDELAY: Don't care about DCD signal
+
+ if (m_handle < 0)
+ {
+ // Port not open
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
+
+ // Once here, port is open
+ m_portOpen = true;
+
+ /* Start configuring of port for non-canonical transfer mode */
+ // Get current options for the port
+ tcgetattr(m_handle,&options);
+
+ // Set baudrate.
+ cfsetispeed(&options,baudrate);
+ cfsetospeed(&options,baudrate);
+
+ // Enable the receiver and set local mode
+ options.c_cflag |= (CLOCAL | CREAD);
+ // Set character size to data bits and set no parity Mask the characte size bits
+ options.c_cflag &= ~(CSIZE | PARENB);
+ options.c_cflag |= CS8; // Select 8 data bits
+ options.c_cflag |= CSTOPB; // send 2 stop bits
+ // Disable hardware flow control
+ options.c_cflag &= ~CRTSCTS;
+ options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
+ // Disable software flow control
+ options.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
+ // Set Raw output
+ options.c_oflag &= ~OPOST;
+ // Timeout 0.005 sec for first byte, read minimum of 0 bytes
+ options.c_cc[VMIN] = 0;
+ options.c_cc[VTIME] = 5;
+
+ // Set the new options for the port
+ tcsetattr(m_handle,TCSANOW,&options);
+
+ tcflush(m_handle,TCIOFLUSH);
+
+ return (m_retVal = MTRV_OK);
+#endif
+ }
+
+////////////////////////////////////////////////////////////////////
+// openFile
+//
+// Open file for reading and writing
+// Filepos is at start of file
+//
+// Input
+// fileName : file name including path
+// createAlways: empties the log file, if existing
+//
+// Output
+// returns MTRV_OK if the file is opened
+// returns MTRV_INPUTCANNOTBEOPENED if the file can not be opened
+// returns MTRV_ANINPUTALREADYOPEN if a serial port / file is already opened
+//
+ short CXsensMTiModule::openFile(const char *fileName, bool createAlways)
+ {
+ m_clkEnd = 0;
+
+ if (m_portOpen || m_portOpen)
+ {
+ return (m_retVal = MTRV_ANINPUTALREADYOPEN);
+ }
+#ifdef WIN32
+ DWORD disposition = OPEN_ALWAYS;
+ if (createAlways == true)
+ {
+ disposition = CREATE_ALWAYS;
+ }
+ m_handle = CreateFile(fileName, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ, NULL, disposition, 0, NULL);
+ if (m_handle == INVALID_HANDLE_VALUE)
+ {
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
+#else
+ int openMode = O_RDWR | O_CREAT;
+ if (createAlways == true)
+ {
+ openMode |= O_TRUNC;
+ }
+ m_handle = open(fileName,openMode,S_IRWXU);
+ if (m_handle < 0)
+ {
+ return (m_retVal = MTRV_INPUTCANNOTBEOPENED);
+ }
+#endif
+ m_timeOut = 0; // No timeout when using file input
+ m_fileOpen = true;
+ return (m_retVal = MTRV_OK);
+
+ }
+
+////////////////////////////////////////////////////////////////////
+// isPortOpen
+//
+// Return if serial port is open or not
+//
+ bool CXsensMTiModule::isPortOpen()
+ {
+ return m_portOpen;
+ }
+
+////////////////////////////////////////////////////////////////////
+// isFileOpen
+//
+// Return if serial port is open or not
+//
+ bool CXsensMTiModule::isFileOpen()
+ {
+ return m_fileOpen;
+ }
+
+////////////////////////////////////////////////////////////////////
+// readData
+//
+// Reads bytes from serial port or file
+//
+// Input
+// msgBuffer : pointer to buffer in which next string will be stored
+// nBytesToRead : number of buffer bytes to read from serial port
+// retval : return value, either MTRV_OK, MTRV_ENDOFFILE or MTRV_NOINPUTINITIALIZED
+//
+// Output
+// number of bytes actually read
+ int CXsensMTiModule::readData(unsigned char* msgBuffer, const int nBytesToRead)
+ {
+ //if(!m_fileOpen && !m_portOpen)
+ if (!(m_portOpen || m_fileOpen))
+ {
+ m_retVal = MTRV_NOINPUTINITIALIZED;
+ return 0;
+ }
+#ifdef WIN32
+ DWORD nBytesRead;
+ BOOL retval = ReadFile(m_handle, msgBuffer, nBytesToRead, &nBytesRead, NULL);
+ if (retval && nBytesRead == 0 && m_fileOpen)
+ {
+ m_retVal = MTRV_ENDOFFILE;
+ }
+ else
+ m_retVal = MTRV_OK;
+ return nBytesRead;
+#else
+ const int nBytesRead = read(m_handle,msgBuffer,nBytesToRead);
+
+ m_retVal = MTRV_OK;
+ if (nBytesRead)
+ return nBytesRead;
+ else if (m_fileOpen)
+ m_retVal = MTRV_ENDOFFILE;
+ return nBytesRead;
+ /*
+ if (nBytesRead == 0 && m_fileOpen)
+ {
+ nBytesRead = 0;
+ m_retVal = MTRV_ENDOFFILE;
+ }
+ else
+ m_retVal = MTRV_OK;
+ return nBytesRead;*/
+
+ // In Linux it is sometimes better to read per byte instead of a block of bytes at once
+ // Use this block if experiencing problems with the above code
+ /*
+ int j = 0; // Index in buffer for read data
+ int k = 0; // Timeout factor
+ int nRead = 0; // Bytes read from port, return by read function
+
+ do {
+ nRead = read(m_handle, &msgBuffer[j], 1);
+ if (nRead == 1) { // Byte read
+ k = 0;
+ j++;
+ }
+ else {
+ k++;
+ }
+
+ if (k == 3) { // Timeout, too long no data read from port
+ return nRead;
+ }
+ }
+ while (j < nBytesToRead);
+
+ return j;
+ */
+#endif
+ }
+
+////////////////////////////////////////////////////////////////////
+// writeData
+//
+// Writes bytes to serial port (to do: file)
+//
+// Input
+// msgBuffer : a pointer to a char buffer containing
+// the characters to be written to serial port
+// nBytesToWrite : number of buffer bytes to write to serial port
+//
+// Output
+// number of bytes actually written
+//
+// The MTComm return value is != MTRV_OK if serial port is closed
+ int CXsensMTiModule::writeData(const unsigned char* msgBuffer, const int nBytesToWrite)
+ {
+ if (!m_fileOpen && !m_portOpen)
+ {
+ m_retVal = MTRV_NOINPUTINITIALIZED;
+ return 0;
+ }
+
+ m_retVal = MTRV_OK;
+#ifdef WIN32
+ DWORD nBytesWritten;
+ WriteFile(m_handle, msgBuffer, nBytesToWrite, &nBytesWritten, NULL);
+ return nBytesWritten;
+#else
+ return write(m_handle,msgBuffer,nBytesToWrite);
+#endif
+ }
+
+////////////////////////////////////////////////////////////////////
+// flush
+//
+// Remove any 'old' data in COM port buffer and flushes internal
+// temporary buffer
+//
+ void CXsensMTiModule::flush()
+ {
+ if (m_portOpen)
+ {
+#ifdef WIN32
+ // Remove any 'old' data in buffer
+ PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR);
+#else
+ tcflush(m_handle,TCIOFLUSH);
+#endif
+ }
+ m_nTempBufferLen = 0;
+ m_retVal = MTRV_OK;
+ }
+
+////////////////////////////////////////////////////////////////////
+// escape
+//
+// Directs a COM port to perform an extended function
+//
+// Input
+// function : Windows define. Can be one of following:
+// CLRDTR, CLRRTS, SETDTR, SETRTS, SETXOFF, SETXON, SETBREAK, CLRBREAK
+ void CXsensMTiModule::escape(unsigned long /*function*/)
+ {
+#ifdef WIN32
+ EscapeCommFunction(m_handle, function);
+#else
+#endif
+ }
+
+////////////////////////////////////////////////////////////////////
+// setPortQueueSize
+//
+// Set input and output buffer size of serial port
+//
+ void CXsensMTiModule::setPortQueueSize(const unsigned long /*inqueueSize = 4096 */, const unsigned long /*outqueueSize = 1024 */)
+ {
+ if (m_portOpen)
+ {
+#ifdef WIN32
+ SetupComm(m_handle,inqueueSize,outqueueSize); // Set queue size
+#else
+ // Not yet implemented
+#endif
+ }
+ m_retVal = MTRV_OK;
+ }
+
+////////////////////////////////////////////////////////////////////
+// setFilePos
+//
+// Sets the current position of the file pointer for file input
+//
+// Input
+// relPos : 32-bit value specifying the relative move in bytes
+// origin is specified in moveMethod
+// moveMethod : FILEPOS_BEGIN, FILEPOS_CURRENT or FILEPOS_END
+// Output
+//
+// returns MTRV_OK if file pointer is successfully set
+//
+ short CXsensMTiModule::setFilePos(long relPos, unsigned long moveMethod)
+ {
+#ifdef WIN32
+ if (m_fileOpen)
+ {
+ if(SetFilePointer(m_handle, relPos, NULL, moveMethod) != INVALID_SET_FILE_POINTER)
+ {
+ return (m_retVal = MTRV_OK);
+ }
+ }
+#else
+ if (m_fileOpen)
+ {
+ if (lseek(m_handle,relPos,moveMethod) != -1)
+ {
+ return (m_retVal = MTRV_OK);
+ }
+ }
+#endif
+ return (m_retVal = MTRV_NOTSUCCESSFUL);
+ }
+
+////////////////////////////////////////////////////////////////////
+// getFileSize
+//
+// Retrieves the file size of the opened file
+//
+// Input
+// Output
+// fileSize : Number of bytes of the current file
+//
+// returns MTRV_OK if successful
+//
+ short CXsensMTiModule::getFileSize(unsigned long &fileSize)
+ {
+#ifdef WIN32
+ if (m_fileOpen)
+ {
+ if ((fileSize = GetFileSize(m_handle, NULL)) != INVALID_FILE_SIZE)
+ {
+ return (m_retVal = MTRV_OK);
+ }
+ }
+#else
+ if (m_fileOpen)
+ {
+ struct stat buf;
+ if (fstat(m_handle,&buf) == 0)
+ {
+ fileSize = buf.st_size;
+ return (m_retVal = MTRV_OK);
+ }
+ }
+#endif
+ return (m_retVal = MTRV_NOTSUCCESSFUL);
+ }
+
+////////////////////////////////////////////////////////////////////
+// close
+//
+// Closes handle of file or serial port
+//
+ short CXsensMTiModule::close()
+ {
+ if (m_portOpen || m_fileOpen)
+ {
+#ifdef WIN32
+ if (m_portOpen)
+ { // Because of USB-serial driver bug
+ flush();
+ }
+ CloseHandle(m_handle);
+#else
+ ::close(m_handle);
+#endif
+ }
+ m_fileOpen = m_portOpen = false;
+ m_timeOut = TO_DEFAULT; // Restore timeout value (file input)
+ m_clkEnd = 0;
+ m_nTempBufferLen = 0;
+ m_deviceError = 0; // No error
+ for(int i = 0 ; i < MAXDEVICES + 1 ; i++)
+ {
+ m_storedOutputMode[i] = INVALIDSETTINGVALUE;
+ m_storedOutputSettings[i] = INVALIDSETTINGVALUE;
+ m_storedDataLength[i] = 0;
+ }
+ return (m_retVal = MTRV_OK);
+ }
+
+////////////////////////////////////////////////////////////////////
+// readMessage
+//
+// Reads the next message from serial port buffer or file.
+// To be read within current time out period
+//
+// Input
+// Output
+// mid : MessageID of message received
+// data : array pointer to data bytes (no header)
+// dataLen : number of data bytes read
+// bid : BID or address of message read (optional)
+//
+// returns MTRV_OK if a message has been read else <>MTRV_OK
+//
+// Remarks
+// allocate enough memory for message buffer
+// use setTimeOut for different timeout value
+ short CXsensMTiModule::readMessage(unsigned char &mid, unsigned char data[], short &dataLen, unsigned char *bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (!(m_fileOpen || m_portOpen))
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
+ {
+ // Message read
+ mid = buffer[IND_MID];
+ if (bid != NULL)
+ {
+ *bid = buffer[IND_BID];
+ }
+ if (buffer[IND_LEN] != EXTLENCODE)
+ {
+ dataLen = buffer[IND_LEN];
+ memcpy(data,&buffer[IND_DATA0],dataLen);
+ }
+ else
+ {
+ dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
+ memcpy(data,&buffer[IND_DATAEXT0],dataLen);
+ }
+ }
+ return m_retVal;
+ }
+
+////////////////////////////////////////////////////////////////////
+// readDataMessage
+//
+// Read a MTData or XMData message from serial port (using TimeOut val)
+//
+// Input
+// data : pointer to buffer in which the DATA field of MTData/XMData is stored
+// dataLen : number of bytes in buffer (= number bytes in DATA field)
+// Output
+// returns MTRV_OK if MTData / XMData message has been read else <>MTRV_OK
+//
+// Remarks
+// allocate enough memory for data buffer
+// use setTimeOut for different timeout value
+ short CXsensMTiModule::readDataMessage(unsigned char data[], short &dataLen)
+ {
+ if (!(m_portOpen || m_fileOpen))
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+
+ unsigned char buffer[MAXMSGLEN ];
+ short buflen;
+
+ if (readMessageRaw(buffer,&buflen) == MTRV_OK)
+ {
+ if (buffer[IND_MID] == MID_MTDATA)
+ { // MID_XMDATA is same
+ if (buffer[IND_LEN] != EXTLENCODE)
+ {
+ dataLen = buffer[IND_LEN];
+ memcpy(data,&buffer[IND_DATA0],dataLen);
+ }
+ else
+ {
+ dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
+ memcpy(data,&buffer[IND_DATAEXT0],dataLen);
+ }
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ }
+ return m_retVal;
+ }
+
+////////////////////////////////////////////////////////////////////
+// readMessageRaw
+//
+// Read a message from serial port for a certain period
+//
+// Input
+// msgBuffer : pointer to buffer in which next msg will be stored
+// msgBufferLength: integer to number of bytes written in buffer (header + data + chksum)
+// Output
+// returns MTRV_OK if a message has been read else <>MTRV_OK
+//
+// Remarks
+// allocate enough memory for message buffer
+// use setTimeOut for different timeout value
+ short CXsensMTiModule::readMessageRaw(unsigned char *msgBuffer, short *msgBufferLength)
+ {
+ int state = 0;
+ int nBytesToRead = 1;
+ int nBytesRead = 0;
+ int nOffset = 0;
+ int nMsgDataLen = 0;
+ int nMsgLen;
+ unsigned char chCheckSum;
+ bool Synced = false;
+
+ if (!(m_portOpen || m_fileOpen))
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+
+ // Copy previous read bytes if available
+ if (m_nTempBufferLen > 0)
+ {
+ memcpy(msgBuffer,m_tempBuffer,m_nTempBufferLen);
+ nBytesRead = m_nTempBufferLen;
+ m_nTempBufferLen = 0;
+ }
+ clock_t clkStart = clockms(); // Get current processor time
+ clock_t clkEnd = m_clkEnd; // check if the end timer is already set
+ if (clkEnd == 0)
+ clkEnd = clkStart + m_timeOut;
+
+ while(true)
+ {
+ do
+ {
+ // First check if we already have some bytes read
+ if (nBytesRead > 0 && nBytesToRead > 0)
+ {
+ if (nBytesToRead > nBytesRead)
+ {
+ nOffset += nBytesRead;
+ nBytesToRead -= nBytesRead;
+ nBytesRead = 0;
+ }
+ else
+ {
+ nOffset += nBytesToRead;
+ nBytesRead -= nBytesToRead;
+ nBytesToRead = 0;
+ }
+ }
+
+ // Check if serial port buffer must be read
+ if (nBytesToRead > 0)
+ {
+ nBytesRead = readData(msgBuffer + nOffset,nBytesToRead);
+ if (m_retVal == MTRV_ENDOFFILE)
+ return (m_retVal = MTRV_ENDOFFILE);
+ nOffset += nBytesRead;
+ nBytesToRead -= nBytesRead;
+ nBytesRead = 0;
+ }
+
+ if (!nBytesToRead)
+ {
+ switch(state)
+ {
+ case 0: // Check preamble
+ if (msgBuffer[IND_PREAMBLE] == PREAMBLE)
+ {
+ state++;
+ nBytesToRead = 3;
+ }
+ else
+ {
+ nOffset = 0;
+ nBytesToRead = 1;
+ }
+ break;
+ case 1: // Check ADDR, MID, LEN
+ if (msgBuffer[IND_LEN] != 0xFF)
+ {
+ state = 3;
+ nBytesToRead = (nMsgDataLen = msgBuffer[IND_LEN]) + 1; // Read LEN + 1 (chksum)
+ }
+ else
+ {
+ state = 2;
+ nBytesToRead = 2; // Read extended length
+ }
+ break;
+ case 2:
+ state = 3;
+ nBytesToRead = (nMsgDataLen = msgBuffer[IND_LENEXTH] * 256 + msgBuffer[IND_LENEXTL]) + 1; // Read LENEXT + CS
+ if (nMsgDataLen > MAXMSGLEN - LEN_MSGEXTHEADERCS)
+ {
+ // Not synced - check for preamble in the bytes read
+ for(int i = 1 ; i < LEN_MSGEXTHEADER ; i++)
+ if (msgBuffer[i] == PREAMBLE)
+ {
+ // Found a maybe preamble - 'fill buffer'
+ nBytesRead = LEN_MSGEXTHEADER - i;
+ memmove(msgBuffer,msgBuffer + i,nBytesRead);
+ break;
+ }
+ Synced = false;
+ nOffset = 0;
+ state = 0;
+ nBytesToRead = 1; // Start looking for preamble
+ }
+ break;
+ case 3: // Check msg
+ chCheckSum = 0;
+ nMsgLen = nMsgDataLen + 5 + (msgBuffer[IND_LEN] == 0xFF ? 2 : 0);
+
+ for(int i = 1 ; i < nMsgLen ; i++)
+ chCheckSum += msgBuffer[i];
+
+ if (chCheckSum == 0)
+ { // Checksum ok?
+ if (nBytesRead > 0)
+ { // Store bytes if read too much
+ memcpy(m_tempBuffer,msgBuffer + nMsgLen,nBytesRead);
+ m_nTempBufferLen = nBytesRead;
+ }
+ *msgBufferLength = nMsgLen;
+ Synced = true;
+ return (m_retVal = MTRV_OK);
+ }
+ else
+ {
+ if (!Synced)
+ for(int i = 1 ; i < nMsgLen ; i++) // Not synced - maybe recheck for preamble in this incorrect message
+ if (msgBuffer[i] == PREAMBLE)
+ {
+ // Found a maybe preamble - 'fill buffer'
+ nBytesRead = nMsgLen - i;
+ memmove(msgBuffer,msgBuffer + i,nBytesRead);
+ break;
+ }
+ Synced = false;
+ nOffset = 0;
+ state = 0;
+ nBytesToRead = 1; // Start looking for preamble
+ }
+ break;
+ }
+ }
+ }
+ while((clkEnd >= clockms()) || m_timeOut == 0 || nBytesRead != 0);
+
+ // Check if pending message has a valid message
+ if (state > 0)
+ {
+ int i;
+ // Search for preamble
+ for(i = 1; i < nOffset ; i++)
+ if (msgBuffer[i] == PREAMBLE)
+ {
+ // Found a maybe preamble - 'fill buffer'
+ nBytesRead = nOffset - i - 1; // no preamble
+ memmove(msgBuffer + 1,msgBuffer + i + 1,nBytesRead);
+ break;
+ }
+ if (i < nOffset)
+ {
+ // Found preamble in message - recheck
+ nOffset = 1;
+ state = 1;
+ nBytesToRead = 3; // Start looking for preamble
+ continue;
+ }
+ }
+ break;
+ }
+
+ return (m_retVal = MTRV_TIMEOUT);
+ }
+
+////////////////////////////////////////////////////////////////////
+// writeMessage (optional: integer value)
+//
+// Sends a message and in case of an serial port interface it checks
+// if the return message (ack, error or timeout). See return value
+// In case an file is opened the functions returns directly after
+// 'sending' the message
+//
+// Use this function for GotoConfig, Reset, ResetOrientation etc
+//
+// Input
+// mid : MessageID of message to send
+// dataValue : A integer value that will be included into the data message field
+// can be a 1,2 or 4 bytes values
+// dataValueLen : Size of dataValue in bytes
+// bid : BID or address to use in message to send (default = 0xFF)
+//
+// Return value
+// = MTRV_OK if an Ack message received / or data successfully written to file
+// = MTRV_RECVERRORMSG if an error message received
+// = MTRV_TIMEOUT if timeout occurred
+// = MTRV_NOINPUTINITIALIZED
+//
+ short CXsensMTiModule::writeMessage(const unsigned char mid, const unsigned long dataValue, const unsigned char dataValueLen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (!(m_fileOpen || m_portOpen))
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = dataValueLen;
+ if (dataValueLen)
+ swapEndian((const unsigned char *) &dataValue,&buffer[IND_DATA0],dataValueLen);
+ calcChecksum(buffer,LEN_MSGHEADER + dataValueLen);
+
+ // Send message
+ writeData(buffer,LEN_MSGHEADERCS + dataValueLen);
+
+ // Return if file opened
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_OK);
+ }
+
+ // Keep reading until an Ack or Error message is received (or timeout)
+ clock_t clkStart , clkOld;
+ bool msgRead = false;
+
+ clkStart = clockms(); // Get current processor time
+ clkOld = m_clkEnd;
+ if (clkOld == 0)
+ m_clkEnd = m_timeOut + clkStart;
+
+ while(m_clkEnd >= clockms() || (m_timeOut == 0))
+ {
+ if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
+ {
+ // Message received
+ msgRead = true;
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_OK); // Acknowledge received
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ }
+ }
+
+ m_clkEnd = clkOld;
+ if (msgRead)
+ return (m_retVal = MTRV_TIMEOUT);
+ else
+ return (m_retVal = MTRV_TIMEOUTNODATA);
+ }
+
+////////////////////////////////////////////////////////////////////
+// writeMessage (data array)
+//
+// Sends a message and in case of an serial port interface it checks
+// if the return message (ack, error or timeout). See return value
+// In case an file is opened the functions returns directly after
+// 'sending' the message
+//
+// Input
+// mid : MessageID of message to send
+// data : array pointer to data bytes
+// dataLen : number of bytes to include in message
+// bid : BID or address to use in message to send (default = 0xFF)
+//
+// Output
+// = MTRV_OK if an Ack message received
+// = MTRV_RECVERRORMSG if an error message received
+// = MTRV_TIMEOUT if timeout occurred
+// = MTRV_NOINPUTINITIALIZED
+//
+ short CXsensMTiModule::writeMessage(const unsigned char mid, const unsigned char data[], const unsigned short &dataLen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+ short headerLength;
+
+ if (!(m_fileOpen || m_portOpen))
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ // Build message to send
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+
+ if (dataLen < EXTLENCODE)
+ {
+ buffer[IND_LEN] = (unsigned char) dataLen;
+ headerLength = LEN_MSGHEADER;
+ }
+ else
+ {
+ buffer[IND_LEN] = EXTLENCODE;
+ buffer[IND_LENEXTH] = (unsigned char) (dataLen >> 8);
+ buffer[IND_LENEXTL] = (unsigned char) (dataLen & 0x00FF);
+ headerLength = LEN_MSGEXTHEADER;
+ }
+ memcpy(&buffer[headerLength],data,dataLen);
+ calcChecksum(buffer,headerLength + dataLen);
+
+ // Send message
+ writeData(buffer,headerLength + dataLen + LEN_CHECKSUM);
+
+ // Return if file opened
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_OK);
+ }
+
+ // Keep reading until an Ack or Error message is received (or timeout)
+ bool msgRead = false;
+ clock_t clkStart , clkOld;
+
+ clkStart = clockms(); // Get current processor time
+ clkOld = m_clkEnd;
+ if (clkOld == 0)
+ m_clkEnd = m_timeOut + clkStart;
+
+ while(m_clkEnd >= clockms() || (m_timeOut == 0))
+ {
+ if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
+ {
+ // Message received
+ msgRead = true;
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_OK); // Acknowledge received
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ }
+ }
+
+ m_clkEnd = clkOld;
+ if (msgRead)
+ return (m_retVal = MTRV_TIMEOUT);
+ else
+ return (m_retVal = MTRV_TIMEOUTNODATA);
+ }
+
+////////////////////////////////////////////////////////////////////
+// waitForMessage
+//
+// Read messages from serial port or file using the current timeout period
+// until the received message is equal to a specific message identifier
+// By default the timeout period by file input is set to infinity (= until
+// end of file is reached)
+//
+// Input/Output
+// mid : message identifier of message that should be returned
+// data : pointer to buffer in which the data of the requested msg will be stored
+// dataLen : integer to number of data bytes
+// bid : optional, pointer which holds the bid of the returned message
+// Output
+// returns MTRV_OK if the message has been read else != MTRV_OK
+//
+// Remarks
+// allocate enough memory for data message buffer
+// use setTimeOut for different timeout value
+ short CXsensMTiModule::waitForMessage(const unsigned char mid, unsigned char data[], short *dataLen, unsigned char *bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short buflen;
+
+ clock_t clkStart , clkOld;
+
+ if (!(m_fileOpen || m_portOpen))
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+ clkStart = clockms(); // Get current processor time
+ clkOld = m_clkEnd;
+ if (clkOld == 0)
+ m_clkEnd = m_timeOut + clkStart;
+
+ while(m_clkEnd >= clockms() || (m_timeOut == 0))
+ {
+ if (readMessageRaw(buffer,&buflen) == MTRV_OK)
+ {
+ if (buffer[IND_MID] == mid)
+ {
+ if (bid != NULL)
+ {
+ *bid = buffer[IND_BID];
+ }
+ if (data != NULL && dataLen != NULL)
+ {
+ if (buffer[IND_LEN] != EXTLENCODE)
+ {
+ *dataLen = buffer[IND_LEN];
+ memcpy(data,&buffer[IND_DATA0],*dataLen);
+ }
+ else
+ {
+ *dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
+ memcpy(data,&buffer[IND_DATAEXT0],*dataLen);
+ }
+ }
+ else if (dataLen != NULL)
+ dataLen = 0;
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_OK);
+ }
+ }
+ else if (m_retVal == MTRV_ENDOFFILE)
+ {
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_ENDOFFILE);
+ }
+ }
+
+ m_clkEnd = clkOld;
+ return (m_retVal = MTRV_TIMEOUT);
+ }
+
+////////////////////////////////////////////////////////////////////
+// reqSetting (integer & no param variant)
+//
+// Request a integer setting from the device. This setting
+// can be an unsigned 1,2 or 4 bytes setting. Only valid
+// for serial port connections
+//
+// Input
+// mid : Message ID of message to send
+// bid : Bus ID of message to send (def 0xFF)
+//
+// Output
+// = MTRV_OK if an Ack message is received
+// = MTRV_RECVERRORMSG if an error message is received
+// = MTRV_TIMEOUT if timeout occurred
+//
+// value contains the integer value of the data field of the ack message
+//
+ short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned long &value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = 0;
+ calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next message or else timeout
+ if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ // Acknowledge received
+ value = 0;
+ swapEndian(&buffer[IND_DATA0],(unsigned char *) &value,buffer[IND_LEN]);
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ else
+ {
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ }
+ }
+ return m_retVal;
+ }
+
+////////////////////////////////////////////////////////////////////
+// reqSetting (integer & param variant)
+//
+// Request a integer setting from the device. This setting
+// can be an unsigned 1,2 or 4 bytes setting. Only valid
+// for serial port connections.
+//
+// Input
+// mid : Message ID of message to send
+// param : For messages that need a parameter
+// bid : Bus ID of message to send (def 0xFF)
+//
+// Output
+// = MTRV_OK if an Ack message is received
+// = MTRV_RECVERRORMSG if an error message is received
+// = MTRV_TIMEOUT if timeout occurred
+//
+// value contains the integer value of the data field of the ack message
+//
+ short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, unsigned long &value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ if (param != 0xFF)
+ {
+ buffer[IND_LEN] = 1;
+ buffer[IND_DATA0] = param;
+ }
+ else
+ {
+ buffer[IND_LEN] = 0;
+ }
+ calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next message or else timeout
+ if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ // Acknowledge received
+ value = 0;
+ if (param == 0xFF)
+ {
+ swapEndian(&buffer[IND_DATA0],(unsigned char *) &value,buffer[IND_LEN]);
+ }
+ else
+ {
+ swapEndian(&buffer[IND_DATA0] + 1,(unsigned char *) &value,buffer[IND_LEN] - 1);
+ }
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ else
+ {
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ }
+ }
+ return m_retVal;
+ }
+
+////////////////////////////////////////////////////////////////////
+// reqSetting (float & no param variant)
+//
+// Request a float setting from the device. Only valid
+// for serial port connections.
+//
+// Input
+// mid : Message ID of message to send
+// bid : Bus ID of message to send (def 0xFF)
+//
+// Output
+// = MTRV_OK if an Ack message is received
+// = MTRV_RECVERRORMSG if an error message is received
+// = MTRV_TIMEOUT if timeout occurred
+//
+// value contains the float value of the acknowledge data field
+//
+ short CXsensMTiModule::reqSetting(const unsigned char mid, float &value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = 0;
+ calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next message or else timeout
+ if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ // Acknowledge received
+ value = 0;
+ swapEndian(&buffer[IND_DATA0],(unsigned char *) &value,buffer[IND_LEN]);
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ else
+ {
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ }
+ }
+ return m_retVal;
+ }
+
+////////////////////////////////////////////////////////////////////
+// reqSetting (float & param variant)
+//
+// Request a float setting from the device. Only valid
+// for serial port connections.
+//
+// Input
+// mid : Message ID of message to send
+// param : For messages that need a parameter (optional)
+// bid : Bus ID of message to send (def 0xFF)
+//
+// Output
+// = MTRV_OK if an Ack message is received
+// = MTRV_RECVERRORMSG if an error message is received
+// = MTRV_TIMEOUT if timeout occurred
+//
+// value contains the float value of the acknowledge data field
+//
+ short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, float &value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ if (param != 0xFF)
+ {
+ buffer[IND_LEN] = 1;
+ buffer[IND_DATA0] = param;
+ }
+ else
+ {
+ buffer[IND_LEN] = 0;
+ }
+ calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next message or else timeout
+ if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ // Acknowledge received
+ value = 0;
+ if (param == 0xFF)
+ {
+ swapEndian(&buffer[IND_DATA0],(unsigned char *) &value,buffer[IND_LEN]);
+ }
+ else
+ {
+ swapEndian(&buffer[IND_DATA0] + 1,(unsigned char *) &value,buffer[IND_LEN] - 1);
+ }
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ else
+ {
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ }
+ }
+ return m_retVal;
+ }
+
+////////////////////////////////////////////////////////////////////
+// reqSetting (byte array & no param variant)
+//
+// Send a message to the device and the data of acknowledge message
+// will be returned. Only valid for serial port connections
+//
+// Input
+// mid : Message ID of message to send
+// bid : Bus ID of message to send (def 0xFF)
+//
+// Output
+// = MTRV_OK if an Ack message is received
+// = MTRV_RECVERRORMSG if an error message is received
+// = MTRV_TIMEOUT if timeout occurred
+//
+// data[] contains the data of the acknowledge message
+// dataLen contains the number bytes returned
+//
+ short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned char data[], short &dataLen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = 0;
+ calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ dataLen = 0;
+ // Read next message or else timeout
+ if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ // Acknowledge received
+ if (buffer[IND_LEN] != EXTLENCODE)
+ {
+ dataLen = buffer[IND_LEN];
+ memcpy(data,&buffer[IND_DATA0],dataLen);
+ }
+ else
+ {
+ dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
+ memcpy(data,&buffer[IND_DATAEXT0],dataLen);
+ }
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ else
+ {
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ }
+ }
+ return m_retVal;
+ }
+
+////////////////////////////////////////////////////////////////////
+// reqSetting (byte array in + out & no param variant)
+//
+// Send a message to the device and the data of acknowledge message
+// will be returned. Only valid for serial port connections
+//
+// Input
+// mid : Message ID of message to send
+// bid : Bus ID of message to send (def 0xFF)
+// dataIn : Data to be included in request
+// dataInLen : Number of bytes in dataIn
+//
+// Output
+// = MTRV_OK if an Ack message is received
+// = MTRV_RECVERRORMSG if an error message is received
+// = MTRV_TIMEOUT if timeout occurred
+//
+// dataOut[] contains the data of the acknowledge message
+// dataOutLen contains the number bytes returned
+//
+ short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned char dataIn[], short dataInLen, unsigned char dataOut[], short &dataOutLen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short headerLength;
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ if (dataInLen < EXTLENCODE)
+ {
+ buffer[IND_LEN] = (unsigned char) dataInLen;
+ headerLength = LEN_MSGHEADER;
+ }
+ else
+ {
+ buffer[IND_LEN] = EXTLENCODE;
+ buffer[IND_LENEXTH] = (unsigned char) (dataInLen >> 8);
+ buffer[IND_LENEXTL] = (unsigned char) (dataInLen & 0x00FF);
+ headerLength = LEN_MSGEXTHEADER;
+ }
+ memcpy(&buffer[headerLength],dataIn,dataInLen);
+ calcChecksum(buffer,headerLength + dataInLen);
+
+ // Send message
+ writeData(buffer,headerLength + dataInLen + LEN_CHECKSUM);
+
+ dataOutLen = 0;
+ // Read next message or else timeout
+ if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ // Acknowledge received
+ if (buffer[IND_LEN] != EXTLENCODE)
+ {
+ dataOutLen = buffer[IND_LEN];
+ memcpy(dataOut,&buffer[IND_DATA0],dataOutLen);
+ }
+ else
+ {
+ dataOutLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
+ memcpy(dataOut,&buffer[IND_DATAEXT0],dataOutLen);
+ }
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ else
+ {
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ }
+ }
+ return m_retVal;
+ }
+
+////////////////////////////////////////////////////////////////////
+// reqSetting (byte array & param variant)
+//
+// Send a message to the device and the data of acknowledge message
+// will be returned. Only valid for serial port connections
+//
+// Input
+// mid : Message ID of message to send
+// param : For messages that need a parameter (optional)
+// bid : Bus ID of message to send (def 0xFF)
+//
+// Output
+// = MTRV_OK if an Ack message is received
+// = MTRV_RECVERRORMSG if an error message is received
+// = MTRV_TIMEOUT if timeout occurred
+//
+// data[] contains the data of the acknowledge message (including param!!)
+// dataLen contains the number bytes returned
+//
+ short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, unsigned char data[], short &dataLen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ if (param != 0xFF)
+ {
+ buffer[IND_LEN] = 1;
+ buffer[IND_DATA0] = param;
+ }
+ else
+ {
+ buffer[IND_LEN] = 0;
+ }
+ calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ dataLen = 0;
+ // Read next message or else timeout
+ if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ // Acknowledge received
+ if (buffer[IND_LEN] != EXTLENCODE)
+ {
+ dataLen = buffer[IND_LEN];
+ memcpy(data,&buffer[IND_DATA0],dataLen);
+ }
+ else
+ {
+ dataLen = buffer[IND_LENEXTH] * 256 + buffer[IND_LENEXTL];
+ memcpy(data,&buffer[IND_DATAEXT0],dataLen);
+ }
+ return (m_retVal = MTRV_OK);
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ else
+ {
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ }
+ }
+ return m_retVal;
+ }
+
+////////////////////////////////////////////////////////////////////
+// setSetting (integer & no param variant)
+//
+// Sets a integer setting of the device. This setting
+// can be an unsigned 1,2 or 4 bytes setting. Only valid
+// for serial port connections.
+//
+// Input
+// mid : Message ID of message to send
+// bid : Bus ID of message to send (def 0xFF)
+// value : Contains the integer value to be used
+// valuelen : Length in bytes of the value
+//
+// Output
+// = MTRV_OK if an Ack message is received
+// = MTRV_RECVERRORMSG if an error message is received
+// = MTRV_TIMEOUT if timeout occurred
+//
+//
+ short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned long value, const unsigned short valuelen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+ msgLen = LEN_MSGHEADER;
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = (unsigned char) valuelen;
+ swapEndian((unsigned char *) &value,&buffer[msgLen],valuelen);
+ calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next received message
+ if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ return (m_retVal = MTRV_OK); // Acknowledge received
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ }
+ return m_retVal;
+ }
+
+////////////////////////////////////////////////////////////////////
+// setSetting (integer & param variant)
+//
+// Sets a integer setting of the device. This setting
+// can be an unsigned 1,2 or 4 bytes setting. Only valid
+// for serial port connections.
+//
+// Input
+// mid : Message ID of message to send
+// param : For messages that need a parameter (optional)
+// bid : Bus ID of message to send (def 0xFF)
+// value : Contains the integer value to be used
+// valuelen : Length in bytes of the value
+//
+// Output
+// = MTRV_OK if an Ack message is received
+// = MTRV_RECVERRORMSG if an error message is received
+// = MTRV_TIMEOUT if timeout occurred
+//
+//
+ short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const unsigned long value, const unsigned short valuelen, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+ msgLen = LEN_MSGHEADER;
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ if (param != 0xFF)
+ {
+ msgLen++;
+ buffer[IND_LEN] = valuelen + 1;
+ buffer[IND_DATA0] = param;
+ }
+ else
+ {
+ buffer[IND_LEN] = (unsigned char) valuelen;
+ }
+ swapEndian((unsigned char *) &value,&buffer[msgLen],valuelen);
+ calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next received message
+ if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ return (m_retVal = MTRV_OK); // Acknowledge received
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ }
+ return m_retVal;
+ }
+
+////////////////////////////////////////////////////////////////////
+// setSetting (float & no param variant)
+//
+// Sets a float setting of the device. Only valid
+// for serial port connections.
+//
+// Input
+// mid : Message ID of message to send
+// bid : Bus ID of message to send (def 0xFF)
+// value : Contains the float value to be used
+//
+// Output
+// = MTRV_OK if an Ack message is received
+// = MTRV_RECVERRORMSG if an error message is received
+// = MTRV_TIMEOUT if timeout occurred
+//
+ short CXsensMTiModule::setSetting(const unsigned char mid, const float value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+ msgLen = LEN_MSGHEADER;
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ buffer[IND_LEN] = LEN_FLOAT;
+ swapEndian((unsigned char *) &value,&buffer[msgLen],LEN_FLOAT);
+ calcChecksum(buffer,LEN_MSGHEADER + LEN_FLOAT);
+
+ // Send message
+ writeData(buffer,LEN_MSGHEADERCS + LEN_FLOAT);
+
+ // Read next received message
+ if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ return (m_retVal = MTRV_OK); // Acknowledge received
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ }
+ return m_retVal;
+ }
+
+////////////////////////////////////////////////////////////////////
+// setSetting (float & param variant)
+//
+// Sets a float setting of the device. Only valid
+// for serial port connections.
+//
+// Input
+// mid : Message ID of message to send
+// param : For messages that need a parameter (optional)
+// bid : Bus ID of message to send (def 0xFF)
+// value : Contains the float value to be used
+//
+// Output
+// = MTRV_OK if an Ack message is received
+// = MTRV_RECVERRORMSG if an error message is received
+// = MTRV_TIMEOUT if timeout occurred
+//
+//
+ short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const float value, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+ msgLen = LEN_MSGHEADER;
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ if (param != 0xFF)
+ {
+ msgLen++;
+ buffer[IND_LEN] = LEN_FLOAT + 1;
+ buffer[IND_DATA0] = param;
+ }
+ else
+ {
+ buffer[IND_LEN] = LEN_FLOAT;
+ }
+ swapEndian((unsigned char *) &value,&buffer[msgLen],LEN_FLOAT);
+ calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next received message
+ if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ return (m_retVal = MTRV_OK); // Acknowledge received
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ }
+ return m_retVal;
+ }
+
+////////////////////////////////////////////////////////////////////
+// setSetting (float & param & store variant)
+//
+// Sets a float setting of the device and with the Store field.
+// Only valid for serial port connections
+//
+// Input
+// mid : Message ID of message to send
+// param : For messages that need a parameter (optional)
+// value : Contains the float value to be used
+// store ; Store in non-volatile memory (1) or not (0)
+// bid : Bus ID of message to send (def 0xFF)
+//
+// Output
+// = MTRV_OK if an Ack message is received
+// = MTRV_RECVERRORMSG if an error message is received
+// = MTRV_TIMEOUT if timeout occurred
+//
+//
+ short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const float value, const bool store, const unsigned char bid)
+ {
+ unsigned char buffer[MAXMSGLEN ];
+ short msgLen;
+
+ if (m_fileOpen)
+ {
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+ if (!m_portOpen)
+ {
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+ msgLen = LEN_MSGHEADER;
+ buffer[IND_PREAMBLE] = PREAMBLE;
+ buffer[IND_BID] = bid;
+ buffer[IND_MID] = mid;
+ if (param != 0xFF)
+ {
+ msgLen++;
+ buffer[IND_LEN] = LEN_FLOAT + 2;
+ buffer[IND_DATA0] = param;
+ }
+ else
+ {
+ buffer[IND_LEN] = LEN_FLOAT + 1;
+ }
+ swapEndian((unsigned char *) &value,&buffer[msgLen],LEN_FLOAT);
+ buffer[msgLen + LEN_FLOAT ] = store;
+ calcChecksum(buffer,LEN_MSGHEADER + buffer[IND_LEN]);
+
+ // Send message
+ writeData(buffer,LEN_MSGHEADERCS + buffer[IND_LEN]);
+
+ // Read next received message
+ if (readMessageRaw(buffer,&msgLen) == MTRV_OK)
+ {
+ // Message received
+ if (buffer[IND_MID] == (mid + 1))
+ {
+ return (m_retVal = MTRV_OK); // Acknowledge received
+ }
+ else if (buffer[IND_MID] == MID_ERROR)
+ {
+ m_deviceError = buffer[IND_DATA0];
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ }
+ }
+ return m_retVal;
+ }
+
+////////////////////////////////////////////////////////////////////
+// getDeviceMode
+//
+// Requests the current output mode/setting of input (file or serialport)
+// the Outputmode, Outputsettings, DataLength & number of devices
+// are stored in member variables of the MTComm class. These values
+// are needed for the GetValue functions.
+// The function optionally returns the number of devices
+//
+// File: expects the Configuration message at the start of the file
+// which holds the OutputMode & OutputSettings. File position
+// is after the first message
+//
+// Input
+// Output
+// numDevices : [optional] number of devices connected to port or
+// found in configuration file
+//
+// returns MTRV_OK if the mode & settings are read
+//
+ short CXsensMTiModule::getDeviceMode(unsigned short *numDevices)
+ {
+ unsigned char mid = 0 , data[MAXMSGLEN ];
+ short datalen;
+
+ if (numDevices != NULL)
+ {
+ *numDevices = 0;
+ }
+
+ // In case serial port is used (live device / XM or MT)
+ if (m_portOpen)
+ {
+ if (reqSetting(MID_INITBUS,data,datalen) != MTRV_OK)
+ {
+ return m_retVal;
+ }
+
+ // Retrieve outputmode + outputsettings
+ for(int i = 0 ; i < datalen / LEN_DEVICEID ; i++)
+ {
+ if (reqSetting(MID_REQOUTPUTMODE,m_storedOutputMode[BID_MT + i],BID_MT + i) != MTRV_OK)
+ {
+ return m_retVal;
+ }
+
+ if (reqSetting(MID_REQOUTPUTSETTINGS,m_storedOutputSettings[BID_MT + i],BID_MT + i) != MTRV_OK)
+ {
+ return m_retVal;
+ }
+
+ if (reqSetting(MID_REQDATALENGTH,m_storedDataLength[BID_MT + i],BID_MT + i) != MTRV_OK)
+ {
+ return m_retVal;
+ }
+ }
+
+ if (numDevices != NULL)
+ {
+ *numDevices = datalen / LEN_DEVICEID;
+ }
+
+ unsigned char masterDID[4];
+ short DIDlen;
+
+ if (reqSetting(MID_REQDID,masterDID,DIDlen) != MTRV_OK)
+ {
+ return m_retVal;
+ }
+
+ if (memcmp(masterDID,data,LEN_DEVICEID) != 0)
+ {
+ // Using an XbusMaster
+ m_storedOutputMode[0] = OUTPUTMODE_XM;
+ m_storedOutputSettings[0] = OUTPUTSETTINGS_XM;
+ m_storedDataLength[0] = LEN_SAMPLECNT;
+ }
+ else
+ {
+ m_storedOutputMode[0] = m_storedOutputMode[BID_MT ];
+ m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ];
+ m_storedDataLength[0] = m_storedDataLength[BID_MT ];
+ }
+ return (m_retVal = MTRV_OK);
+ }
+ else if (m_fileOpen)
+ {
+ // Configuration message should be the first message in the file
+ setFilePos(0);
+ if (readMessage(mid,data,datalen) == MTRV_OK)
+ {
+ if (mid == MID_CONFIGURATION)
+ {
+ unsigned short _numDevices = 0;
+ swapEndian(data + CONF_NUMDEVICES,(unsigned char *) &_numDevices,CONF_NUMDEVICESLEN);
+ for(unsigned int i = 0 ; i < _numDevices ; i++)
+ {
+ m_storedOutputMode[BID_MT + i] = 0;
+ swapEndian(data + CONF_OUTPUTMODE + i * CONF_BLOCKLEN,(unsigned char *) (m_storedOutputMode + BID_MT + i),
+ CONF_OUTPUTMODELEN);
+ m_storedOutputSettings[BID_MT + i] = 0;
+ swapEndian(data + CONF_OUTPUTSETTINGS + i * CONF_BLOCKLEN,(unsigned char *) (m_storedOutputSettings + BID_MT + i),
+ CONF_OUTPUTSETTINGSLEN);
+ m_storedDataLength[BID_MT + i] = 0;
+ swapEndian(data + CONF_DATALENGTH + i * CONF_BLOCKLEN,(unsigned char *) (m_storedDataLength + BID_MT + i),
+ CONF_DATALENGTHLEN);
+ }
+ if (numDevices != NULL)
+ {
+ *numDevices = _numDevices;
+ }
+ if (memcmp(data + CONF_MASTERDID,data + CONF_DID,LEN_DEVICEID) != 0)
+ {
+ // Using an XbusMaster
+ m_storedOutputMode[0] = OUTPUTMODE_XM;
+ m_storedOutputSettings[0] = OUTPUTSETTINGS_XM;
+ m_storedDataLength[0] = LEN_SAMPLECNT;
+ }
+ else
+ {
+ m_storedOutputMode[0] = m_storedOutputMode[BID_MT ];
+ m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ];
+ m_storedDataLength[0] = m_storedDataLength[BID_MT ];
+ }
+ return (m_retVal = MTRV_OK);
+ }
+ }
+ return (m_retVal = MTRV_NOTSUCCESSFUL);
+ }
+ return (m_retVal = MTRV_NOINPUTINITIALIZED);
+ }
+
+////////////////////////////////////////////////////////////////////
+// setDeviceMode
+//
+// Sets the current output mode/setting of input (not for file-based
+// inputs)
+//
+// Input
+// OutputMode : OutputMode to be set in device & stored in MTComm
+// class member variable
+// OutputSettings : OutputSettings to be set in device & stored in
+// MTComm class member variable
+// Output
+//
+// returns MTRV_OK if the mode & settings are read
+//
+ short CXsensMTiModule::setDeviceMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid)
+ {
+ // In case serial port is used (live XM / MT)
+ if (m_portOpen)
+ {
+ // Set OutputMode
+ if (setSetting(MID_SETOUTPUTMODE,OutputMode,LEN_OUTPUTMODE,bid) != MTRV_OK)
+ {
+ return m_retVal;
+ }
+ if (bid == BID_MASTER || (bid == BID_MT && m_storedOutputMode[0] != OUTPUTMODE_XM))
+ {
+ m_storedOutputMode[0] = m_storedOutputMode[BID_MT ] = OutputMode;
+ }
+ else
+ {
+ m_storedOutputMode[bid] = OutputMode;
+ }
+ // Set OutputSettings
+ if (setSetting(MID_SETOUTPUTSETTINGS,OutputSettings,LEN_OUTPUTSETTINGS,bid) != MTRV_OK)
+ {
+ return m_retVal;
+ }
+ if (bid == BID_MASTER || (bid == BID_MT && m_storedOutputMode[0] != OUTPUTMODE_XM))
+ {
+ m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ] = OutputSettings;
+ }
+ else
+ {
+ m_storedOutputSettings[bid] = OutputSettings;
+ }
+ // Get DataLength from device
+ if (OutputMode != OUTPUTMODE_XM)
+ {
+ unsigned long value;
+ if (reqSetting(MID_REQDATALENGTH,value,bid) == MTRV_OK)
+ {
+ if ((bid == BID_MASTER ) || ((bid == BID_MT ) && (m_storedOutputMode[0] != OUTPUTMODE_XM)))
+ {
+ m_storedDataLength[0] = m_storedDataLength[BID_MT ] = value;
+ }
+ else
+ {
+ m_storedDataLength[bid] = value;
+ }
+ }
+ }
+ else
+ {
+ m_storedDataLength[0] = LEN_SAMPLECNT;
+ }
+ return (m_retVal = MTRV_OK);
+ }
+ return (m_retVal = MTRV_INVALIDFORFILEINPUT);
+ }
+
+////////////////////////////////////////////////////////////////////
+// getMode
+//
+// Gets the output mode/setting used in MTComm class and the corresponding
+// datalength. These variables are set by the functions GetDeviceMode,
+// SetDeviceMode or SetMode
+//
+// Input
+// Output
+// OutputMode : OutputMode stored in MTComm class member variable
+// OutputSettings : OutputSettings stored in MTComm class member variable
+//
+// returns always MTRV_OK
+//
+ short CXsensMTiModule::getMode(unsigned long &OutputMode, unsigned long &OutputSettings, unsigned short &dataLength, const unsigned char bid)
+ {
+ unsigned char nbid = (bid == BID_MASTER ) ? 0 : bid;
+ OutputMode = m_storedOutputMode[nbid];
+ OutputSettings = m_storedOutputSettings[nbid];
+ dataLength = (unsigned short) m_storedDataLength[nbid];
+ return (m_retVal = MTRV_OK);
+ }
+
+////////////////////////////////////////////////////////////////////
+// setMode
+//
+// Sets the output mode/setting used in MTComm class. Use the function
+// GetDeviceMode to retrieve the current values of file/device.
+// This function will also calculate the data length field
+//
+// Input
+// OutputMode : OutputMode to be stored in MTComm class member variable
+// OutputSettings : OutputSettings to be stored in MTComm class member variable
+// Output
+//
+// returns always MTRV_OK
+//
+ short CXsensMTiModule::setMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid)
+ {
+ unsigned char nbid = bid;
+
+ if (nbid == BID_MASTER)
+ {
+ nbid = 0;
+ }
+ m_storedOutputMode[nbid] = OutputMode;
+ m_storedOutputSettings[nbid] = OutputSettings;
+ if (OutputMode == INVALIDSETTINGVALUE || OutputSettings == INVALIDSETTINGVALUE)
+ {
+ m_storedDataLength[nbid] = 0;
+ }
+ else
+ {
+ unsigned short dataLength = 0;
+ if (OutputMode & OUTPUTMODE_MT9)
+ {
+ dataLength = ((OutputSettings & OUTPUTSETTINGS_TIMESTAMP_MASK) == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0 + LEN_RAWDATA;
+ }
+ else if (OutputMode == OUTPUTMODE_XM)
+ {
+ // XbusMaster concatenates sample counter
+ dataLength = LEN_SAMPLECNT;
+ }
+ else
+ {
+ if (OutputMode & OUTPUTMODE_RAW)
+ {
+ dataLength = LEN_RAWDATA;
+ }
+ else
+ {
+ if (OutputMode & OUTPUTMODE_CALIB)
+ {
+ dataLength = LEN_CALIBDATA;
+ }
+ if (OutputMode & OUTPUTMODE_ORIENT)
+ {
+ switch(OutputSettings & OUTPUTSETTINGS_ORIENTMODE_MASK)
+ {
+ case OUTPUTSETTINGS_ORIENTMODE_QUATERNION:
+ dataLength += LEN_ORIENT_QUATDATA;
+ break;
+ case OUTPUTSETTINGS_ORIENTMODE_EULER:
+ dataLength += LEN_ORIENT_EULERDATA;
+ break;
+ case OUTPUTSETTINGS_ORIENTMODE_MATRIX:
+ dataLength += LEN_ORIENT_MATRIXDATA;
+ break;
+ default:
+ break;
+ }
+ }
+ }
+ switch(OutputSettings & OUTPUTSETTINGS_TIMESTAMP_MASK)
+ {
+ case OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT:
+ dataLength += LEN_SAMPLECNT;
+ break;
+ default:
+ break;
+ }
+ }
+ m_storedDataLength[nbid] = dataLength;
+ }
+ // If not XbusMaster store also in BID_MT
+ if (bid == BID_MASTER && OutputMode != OUTPUTMODE_XM)
+ {
+ m_storedOutputMode[BID_MT ] = m_storedOutputMode[0];
+ m_storedOutputSettings[BID_MT ] = m_storedOutputSettings[0];
+ m_storedDataLength[BID_MT ] = m_storedDataLength[0];
+ }
+ return (m_retVal = MTRV_OK);
+ }
+
+////////////////////////////////////////////////////////////////////
+// getValue (unsigned short variant)
+//
+// Retrieves a unsigned short value from the data input parameter
+// This function is valid for the following value specifiers:
+// VALUE_RAW_TEMP
+// VALUE_SAMPLECNT
+//
+// Use getDeviceMode or setMode to initialize the Outputmode
+// and Outputsettings member variables used to retrieve the correct
+// value
+//
+// Input
+// valueSpec : Specifier of the value to be retrieved
+// data[] : Data field of a MTData / BusData message
+// bid : bus identifier of the device of which the
+// value should be returned (default = BID_MT)
+// Output
+// value : reference to unsigned short in which the retrieved
+// value will be returned
+//
+// Return value
+// MTRV_OK : value is successfully retrieved
+// != MTRV_OK : not successful
+//
+ short CXsensMTiModule::getValue(const unsigned long valueSpec, unsigned short &value, const unsigned char data[], const unsigned char bid)
+ {
+ short offset = 0;
+ unsigned char nbid = bid;
+
+ if (nbid == BID_MASTER)
+ {
+ nbid = 0;
+ }
+
+ // Check for invalid mode/settings
+ if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
+ {
+ return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
+ }
+
+ // Calculate offset for XM input
+ if (m_storedOutputMode[0] == OUTPUTMODE_XM)
+ {
+ int i = 0;
+ while(i < nbid)
+ {
+ offset += (short) m_storedDataLength[i++];
+ }
+ }
+
+ // Check if data is unsigned short & available in data
+ m_retVal = MTRV_INVALIDVALUESPEC;
+ if (valueSpec == VALUE_RAW_TEMP)
+ {
+ if (m_storedOutputMode[nbid] & (OUTPUTMODE_RAW | OUTPUTMODE_MT9))
+ {
+ offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 && m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0;
+ swapEndian(data + offset + valueSpec * LEN_UNSIGSHORT * 3,(unsigned char *) &value,LEN_RAW_TEMP);
+ m_retVal = MTRV_OK;
+ }
+ }
+ else if (valueSpec == VALUE_SAMPLECNT)
+ {
+ if ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_TIMESTAMP_MASK) == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT)
+ {
+ if (!(m_storedOutputMode[nbid] == OUTPUTMODE_MT9))
+ {
+ offset += (short) m_storedDataLength[nbid] - LEN_SAMPLECNT;
+ }
+ swapEndian(data + offset,(unsigned char *) &value,LEN_SAMPLECNT);
+ m_retVal = MTRV_OK;
+ }
+ }
+ return m_retVal;
+ }
+
+////////////////////////////////////////////////////////////////////
+// getValue (array of unsigned short variant)
+//
+// Retrieves an array of unsigned short values from the data input
+// parameter. This function is valid for the following value specifiers:
+// VALUE_RAW_ACC
+// VALUE_RAW_GYR
+// VALUE_RAW_MAG
+//
+// Use getDeviceMode or setMode to initialize the Outputmode
+// and Outputsettings member variables used to retrieve the correct
+// value
+//
+// Input
+// valueSpec : Specifier of the value to be retrieved
+// data[] : Data field of a MTData / BusData message
+// bid : bus identifier of the device of which the
+// value should be returned (default = BID_MT)
+// Output
+// value[] : pointer to array of unsigned shorts in which the
+// retrieved values will be returned
+//
+// Return value
+// MTRV_OK : value is successfully retrieved
+// != MTRV_OK : not successful
+//
+ short CXsensMTiModule::getValue(const unsigned long valueSpec, unsigned short value[], const unsigned char data[], const unsigned char bid)
+ {
+ short offset = 0;
+ unsigned char nbid = bid;
+
+ if (nbid == BID_MASTER)
+ {
+ nbid = 0;
+ }
+ // Check for invalid mode/settings
+ if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
+ {
+ return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
+ }
+
+ // Calculate offset for XM input
+ if (m_storedOutputMode[0] == OUTPUTMODE_XM)
+ {
+ int i = 0;
+ while(i < nbid)
+ {
+ offset += (short) m_storedDataLength[i++];
+ }
+ }
+
+ // Check if data is unsigned short, available in data & retrieve data
+ m_retVal = MTRV_INVALIDVALUESPEC;
+ //if (valueSpec >= VALUE_RAW_ACC && valueSpec <= VALUE_RAW_MAG)
+ if (valueSpec <= VALUE_RAW_MAG)
+ {
+ if (m_storedOutputMode[nbid] & (OUTPUTMODE_RAW | OUTPUTMODE_MT9))
+ {
+ offset += (short) (valueSpec * LEN_UNSIGSHORT * 3);
+ offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 && m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0;
+ for(int i = 0 ; i < 3 ; i++)
+ {
+ swapEndian(data + offset + i * LEN_UNSIGSHORT,(unsigned char *) value + i * LEN_UNSIGSHORT,LEN_UNSIGSHORT);
+ }
+ m_retVal = MTRV_OK;
+ }
+ }
+ return m_retVal;
+ }
+
+////////////////////////////////////////////////////////////////////
+// getValue (array of floats variant)
+//
+// Retrieves an array of float values from the data input parameter.
+// This function is valid for the following value specifiers:
+// VALUE_TEMP
+// VALUE_CALIB_ACC
+// VALUE_CALIB_GYR
+// VALUE_CALIB_MAG
+// VALUE_ORIENT_QUAT
+// VALUE_ORIENT_EULER
+// VALUE_ORIENT_MATRIX
+//
+// Use getDeviceMode or setMode to initialize the Outputmode
+// and Outputsettings member variables used to retrieve the correct
+// value
+//
+// Input
+// valueSpec : Specifier of the value to be retrieved
+// data[] : Data field of a MTData / BusData message
+// bid : bus identifier of the device of which the
+// value should be returned (default = BID_MT)
+// Output
+// value[] : pointer to array of floats in which the
+// retrieved values will be returned
+//
+// Return value
+// MTRV_OK : value is successfully retrieved
+// != MTRV_OK : not successful
+//
+ short CXsensMTiModule::getValue(const unsigned long valueSpec, float value[], const unsigned char data[], const unsigned char bid)
+ {
+ short offset = 0;
+ int nElements = 0;
+ unsigned char nbid = bid;
+
+ if (nbid == BID_MASTER)
+ {
+ nbid = 0;
+ }
+
+ // Check for invalid mode/settings
+ if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
+ {
+ return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
+ }
+
+ // Calculate offset for XM input
+ if (m_storedOutputMode[0] == OUTPUTMODE_XM)
+ {
+ int i = 0;
+ while(i < nbid)
+ {
+ offset += (short) m_storedDataLength[i++];
+ }
+ }
+
+ // Check if data is float & available in data
+ m_retVal = MTRV_INVALIDVALUESPEC;
+ if (valueSpec == VALUE_TEMP)
+ {
+ if (m_storedOutputMode[nbid] & OUTPUTMODE_TEMP)
+ {
+ nElements = LEN_TEMPDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
+ }
+ }
+ else if (valueSpec == VALUE_CALIB_ACC)
+ {
+ offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
+ if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0)
+ {
+ nElements = LEN_CALIB_ACCDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
+ }
+ }
+ else if (valueSpec == VALUE_CALIB_GYR)
+ {
+ offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
+ if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0)
+ {
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
+ nElements = LEN_CALIB_GYRDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
+ }
+ }
+ else if (valueSpec == VALUE_CALIB_MAG)
+ {
+ offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
+ if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0)
+ {
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0) ? LEN_CALIB_GYRX * 3 : 0;
+ nElements = LEN_CALIB_MAGDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
+ }
+ }
+ else if (valueSpec >= VALUE_ORIENT_QUAT && valueSpec <= VALUE_ORIENT_MATRIX)
+ {
+ offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
+ if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB))
+ {
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0) ? LEN_CALIB_GYRX * 3 : 0;
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0) ? LEN_CALIB_MAGX * 3 : 0;
+ }
+ if (m_storedOutputMode[nbid] & OUTPUTMODE_ORIENT)
+ {
+ unsigned long orientmode = m_storedOutputSettings[nbid] & OUTPUTSETTINGS_ORIENTMODE_MASK;
+ switch(valueSpec)
+ {
+ case VALUE_ORIENT_QUAT:
+ if (orientmode == OUTPUTSETTINGS_ORIENTMODE_QUATERNION)
+ {
+ nElements = LEN_ORIENT_QUATDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
+ }
+ break;
+ case VALUE_ORIENT_EULER:
+ if (orientmode == OUTPUTSETTINGS_ORIENTMODE_EULER)
+ {
+ nElements = LEN_ORIENT_EULERDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
+ }
+ break;
+ case VALUE_ORIENT_MATRIX:
+ if (orientmode == OUTPUTSETTINGS_ORIENTMODE_MATRIX)
+ {
+ nElements = LEN_ORIENT_MATRIXDATA / LEN_FLOAT;
+ m_retVal = MTRV_OK;
+ }
+ break;
+ default:
+ break;
+ }
+ }
+ }
+ if (m_retVal == MTRV_OK)
+ {
+ if ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_DATAFORMAT_F1220) == 0)
+ {
+ for(int i = 0 ; i < nElements ; i++)
+ {
+ swapEndian(data + offset + i * LEN_FLOAT,(unsigned char *) value + i * LEN_FLOAT,LEN_FLOAT);
+ }
+ }
+ else
+ {
+ int temp;
+ for(int i = 0 ; i < nElements ; i++)
+ {
+ swapEndian(data + offset + i * LEN_FLOAT,(unsigned char*) &temp,4);
+ value[i] = (float) temp / 1048576;
+ }
+ }
+ }
+ return m_retVal;
+ }
+
+//////////////////////////////////////////////////////////////////////
+// getLastDeviceError
+//
+// Returns the last reported device error of the latest received Error
+// message
+//
+// Output
+// Error code
+ short CXsensMTiModule::getLastDeviceError()
+ {
+ return m_deviceError;
+ }
+
+//////////////////////////////////////////////////////////////////////
+// getLastRetVal
+//
+// Returns the returned value of the last called function
+//
+// Output
+// Return value
+ short CXsensMTiModule::getLastRetVal()
+ {
+ return m_retVal;
+ }
+
+//////////////////////////////////////////////////////////////////////
+// setTimeOut
+//
+// Sets the time out value in milliseconds used by the functions
+// Use 0 for infinite timeout
+//
+// Output
+// MTRV_OK is set, MTRV_INVALIDTIMEOUT if time value < 0
+ short CXsensMTiModule::setTimeOut(short timeOutMs)
+ {
+ if (timeOutMs >= 0)
+ {
+ m_timeOut = timeOutMs;
+ return (m_retVal = MTRV_OK);
+ }
+ else
+ return (m_retVal = MTRV_INVALIDTIMEOUT);
+ }
+
+//////////////////////////////////////////////////////////////////////
+// swapEndian
+//
+// Convert 2 or 4 bytes data from little to big endian or back
+//
+// Input
+// input : Pointer to data to be converted
+// output : Pointer where converted data is stored
+// length : Length of setting (0,2 & 4)
+//
+// Remarks:
+// Allocate enough bytes for output buffer
+
+ void CXsensMTiModule::swapEndian(const unsigned char input[], unsigned char output[], const int length)
+ {
+ switch(length)
+ {
+ case 4:
+ output[0] = input[3];
+ output[1] = input[2];
+ output[2] = input[1];
+ output[3] = input[0];
+ break;
+ case 2:
+ output[0] = input[1];
+ output[1] = input[0];
+ break;
+ case 1:
+ output[0] = input[0];
+ break;
+ default:
+ for(int i = 0 , j = length - 1 ; i < length ; i++ , j--)
+ output[j] = input[i];
+ break;
+ }
+ }
+
+//////////////////////////////////////////////////////////////////////
+// calcChecksum
+//
+// Calculate and append checksum to msgBuffer
+//
+ void CXsensMTiModule::calcChecksum(unsigned char *msgBuffer, const int msgBufferLength)
+ {
+ unsigned char checkSum = 0;
+ int i;
+
+ for(i = 1; i < msgBufferLength ; i++)
+ checkSum += msgBuffer[i];
+
+ msgBuffer[msgBufferLength] = -checkSum; // Store chksum
+ }
+
+//////////////////////////////////////////////////////////////////////
+// checkChecksum
+//
+// Checks if message checksum is valid
+//
+// Output
+// returns true checksum is OK
+ bool CXsensMTiModule::checkChecksum(const unsigned char *msgBuffer, const int msgBufferLength)
+ {
+ unsigned char checkSum = 0;
+ int i;
+
+ for(i = 1; i < msgBufferLength ; i++)
+ checkSum += msgBuffer[i];
+
+ if (checkSum == 0)
+ {
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+ }
+ }
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.h b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.h
new file mode 100755
index 0000000000000000000000000000000000000000..8c4d0d0d0a17c06bf409c228934a978c8c1c3144
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.h
@@ -0,0 +1,1018 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2006 Radu Bogdan Rusu (rusu@cs.tum.edu)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+/*
+ Desc: Driver for XSens MTx/MTi IMU. CMTComm class borrowed from XSens under GPL.
+ Author: Radu Bogdan Rusu
+ Date: 1 Aug 2006
+ */
+// MTComm.h: interface for the CMTComm class.
+//
+// Version 1.2.0
+// Public release
+//
+// v1.2.0
+// 27-02-2006 - Renamed Xbus class to Motion Tracker C++ Communication class, short MTComm
+// - Defines XBRV_* accordingly renamed to MTRV_*
+// - Fixed device length not correct for bid 0 when using Xbus Master and setDeviceMode function
+//
+// v1.1.7
+// 15-02-2006 - Added fixed point signed 12.20 dataformat support
+// Added selective calibrated data output per sensor type support
+// Added outputmode temperature support
+// Fixed warning C4244: '=' : conversion from '' to '', possible loss of data
+// v1.1.6
+// 25-01-2006 - Added escape function for CLRDTR, CLRRTS, SETDTR, SETRTS, SETXOFF, SETXON, SETBREAK, CLRBREAK
+//
+// v1.1.5
+// 14-11-2005 - Made swapEndian a static member function, Job Mulder
+//
+// v1.1.4
+// 08-11-2005 - Changed practically all uses of m_timeOut into uses of the new m_clkEnd
+// - Changed COM timeout in win32 to return immediately if data is available,
+// but wait 1ms otherwise
+//
+// v1.1.3
+// 18-10-2005 - Added MID_REQPRODUCTCODE, MID_REQ/SETTRANSMITDELAY
+// - Added XBRV_TIMEOUTNODATA indicating timeout occurred due to no data read
+//
+// v1.1.2
+// 16-09-2005 - Added eMTS version 0.1->1.0 changes (EMTS_FACTORYMODE)
+// - Added factory output mode defines
+//
+// v1.1.1
+// 01-09-2005 - Added defines for Extended output mode
+// - Added reqSetting (byte array in + out & no param variant)
+//
+// v1.1
+// 08-08-2005 - Added file read and write support
+// - Added functions for data retrieval (getValue etc)
+// for easy data retrieval of acc, gyr, mag etc
+// - ReadMessageRaw:
+// - added a temporary buffer for unprocessed bytes
+// - check for invalid length messages
+// - Changed BID_MT into 1 and added BID_MASTER (=0xFF)
+// - Changed various ....SerialPort functions to .....Port
+// - Changed mtSendMessage functions to mtWriteMessage
+// - Added numerous defines
+// - Deleted obsolete functions
+// - Changed function getLastErrorCode into getLastDeviceError
+// - Changed OpenPort function for compatiblity with Bluetooth ports
+// - Added workaround for lockup of USB driver (close function)
+// - Added workaround for clock() problem with read function of USB driver
+//
+// v1.0.2
+// 29-06-2005 - Inserted initSerialPort with devicename input
+// - Changed return value defines names from X_ to XBRV_
+// - Removed unneeded includes for linux
+//
+// v1.0.1
+// 22-06-2005 - Fixed ReqSetting functions (byte array & param variant)
+// mtSendRawString had wrong length input
+//
+// v1.0.0
+// 20-06-2005 - Initial release
+//
+// ----------------------------------------------------------------------------
+// This file is an Xsens Code Example.
+//
+// Copyright (C) Xsens Technologies B.V., 2005.
+//
+// This source code is intended only as a example of the implementation
+// of the Xsens MT Communication protocol.
+// It was written for cross platform capabilities.
+//
+// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
+// KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
+// PARTICULAR PURPOSE.
+//////////////////////////////////////////////////////////////////////
+#if !defined(AFX_XBUS_H__F4580A3F_2CF2_4ED2_A747_B4B015A0328E__INCLUDED_)
+#define AFX_XBUS_H__F4580A3F_2CF2_4ED2_A747_B4B015A0328E__INCLUDED_
+
+#if _MSC_VER > 1000
+#pragma once
+#endif // _MSC_VER > 1000
+
+#include <string.h>
+#include <stdio.h>
+#ifdef WIN32
+#include <windows.h>
+#include <conio.h>
+#include <time.h>
+#else
+#include <fcntl.h> /* POSIX Standard: 6.5 File Control Operations */
+#include <termios.h> /* terminal i/o system, talks to /dev/tty* ports */
+#include <unistd.h> /* Read function */
+#include <sys/time.h> /* gettimeofday function */
+#include <sys/stat.h> /* stat calls and structures*/
+#endif
+
+#include "../Includes.h"
+
+#ifdef _IMU_USE_XSENS_DEVICE_
+
+namespace IMU
+{
+ namespace Xsens
+ {
+
+#ifndef INVALID_SET_FILE_POINTER
+#define INVALID_SET_FILE_POINTER ((DWORD)(-1))
+#endif
+
+// Field message indexes
+#define IND_PREAMBLE 0
+#define IND_BID 1
+#define IND_MID 2
+#define IND_LEN 3
+#define IND_DATA0 4
+#define IND_LENEXTH 4
+#define IND_LENEXTL 5
+#define IND_DATAEXT0 6
+
+// Maximum number of sensors supported
+#define MAXDEVICES 20
+
+#define PREAMBLE (const unsigned char)0xFA
+#define BID_MASTER (const unsigned char)0xFF
+#define BID_MT (const unsigned char)0x01
+#define EXTLENCODE 0xFF
+
+#define LEN_MSGHEADER (const unsigned short)4
+#define LEN_MSGEXTHEADER (const unsigned short)6
+#define LEN_MSGHEADERCS (const unsigned short)5
+#define LEN_MSGEXTHEADERCS (const unsigned short)7
+#define LEN_CHECKSUM (const unsigned short)1
+#define LEN_UNSIGSHORT (const unsigned short)2
+#define LEN_UNSIGINT (const unsigned short)4
+#define LEN_FLOAT (const unsigned short)4
+
+// Maximum message/data length
+#define MAXDATALEN (const unsigned short)2048
+#define MAXSHORTDATALEN (const unsigned short)254
+#define MAXMSGLEN (const unsigned short)(MAXDATALEN+7)
+#define MAXSHORTMSGLEN (const unsigned short)(MAXSHORTDATALEN+5)
+
+// DID Type (high nibble)
+#define DID_TYPEH_MASK (const unsigned long)0x00F00000
+#define DID_TYPEH_MT (const unsigned long)0x00000000
+#define DID_TYPEH_XM (const unsigned long)0x00100000
+#define DID_TYPEH_MTI_MTX (const unsigned long)0x00300000
+
+// All Message identifiers
+// WakeUp state messages
+#define MID_WAKEUP (const unsigned char)0x3E
+#define MID_WAKEUPACK (const unsigned char)0x3F
+
+// Config state messages
+#define MID_REQDID (const unsigned char)0x00
+#define MID_DEVICEID (const unsigned char)0x01
+#define LEN_DEVICEID (const unsigned short)4
+#define MID_INITBUS (const unsigned char)0x02
+#define MID_INITBUSRESULTS (const unsigned char)0x03
+#define LEN_INITBUSRESULTS (const unsigned short)4
+#define MID_REQPERIOD (const unsigned char)0x04
+#define MID_REQPERIODACK (const unsigned char)0x05
+#define LEN_PERIOD (const unsigned short)2
+#define MID_SETPERIOD (const unsigned char)0x04
+#define MID_SETPERIODACK (const unsigned char)0x05
+// XbusMaster
+#define MID_SETBID (const unsigned char)0x06
+#define MID_SETBIDACK (const unsigned char)0x07
+#define MID_AUTOSTART (const unsigned char)0x06
+#define MID_AUTOSTARTACK (const unsigned char)0x07
+#define MID_BUSPWROFF (const unsigned char)0x08
+#define MID_BUSPWROFFACK (const unsigned char)0x09
+// End XbusMaster
+#define MID_REQDATALENGTH (const unsigned char)0x0A
+#define MID_DATALENGTH (const unsigned char)0x0B
+#define LEN_DATALENGTH (const unsigned short)2
+#define MID_REQCONFIGURATION (const unsigned char)0x0C
+#define MID_CONFIGURATION (const unsigned char)0x0D
+#define LEN_CONFIGURATION (const unsigned short)118
+#define MID_RESTOREFACTORYDEF (const unsigned char)0x0E
+#define MID_RESTOREFACTORYDEFACK (const unsigned char)0x0F
+
+#define MID_GOTOMEASUREMENT (const unsigned char)0x10
+#define MID_GOTOMEASUREMENTACK (const unsigned char)0x11
+#define MID_REQFWREV (const unsigned char)0x12
+#define MID_FIRMWAREREV (const unsigned char)0x13
+#define LEN_FIRMWAREREV (const unsigned short)3
+// XbusMaster
+#define MID_REQBTDISABLE (const unsigned char)0x14
+#define MID_REQBTDISABLEACK (const unsigned char)0x15
+#define MID_DISABLEBT (const unsigned char)0x14
+#define MID_DISABLEBTACK (const unsigned char)0x15
+#define MID_REQOPMODE (const unsigned char)0x16
+#define MID_REQOPMODEACK (const unsigned char)0x17
+#define MID_SETOPMODE (const unsigned char)0x16
+#define MID_SETOPMODEACK (const unsigned char)0x17
+// End XbusMaster
+#define MID_REQBAUDRATE (const unsigned char)0x18
+#define MID_REQBAUDRATEACK (const unsigned char)0x19
+#define LEN_BAUDRATE (const unsigned short)1
+#define MID_SETBAUDRATE (const unsigned char)0x18
+#define MID_SETBAUDRATEACK (const unsigned char)0x19
+// XbusMaster
+#define MID_REQSYNCMODE (const unsigned char)0x1A
+#define MID_REQSYNCMODEACK (const unsigned char)0x1B
+#define MID_SETSYNCMODE (const unsigned char)0x1A
+#define MID_SETSYNCMODEACK (const unsigned char)0x1B
+// End XbusMaster
+#define MID_REQPRODUCTCODE (const unsigned char)0x1C
+#define MID_PRODUCTCODE (const unsigned char)0x1D
+
+#define MID_REQOUTPUTMODE (const unsigned char)0xD0
+#define MID_REQOUTPUTMODEACK (const unsigned char)0xD1
+#define LEN_OUTPUTMODE (const unsigned short)2
+#define MID_SETOUTPUTMODE (const unsigned char)0xD0
+#define MID_SETOUTPUTMODEACK (const unsigned char)0xD1
+
+#define MID_REQOUTPUTSETTINGS (const unsigned char)0xD2
+#define MID_REQOUTPUTSETTINGSACK (const unsigned char)0xD3
+#define LEN_OUTPUTSETTINGS (const unsigned short)4
+#define MID_SETOUTPUTSETTINGS (const unsigned char)0xD2
+#define MID_SETOUTPUTSETTINGSACK (const unsigned char)0xD3
+
+#define MID_REQOUTPUTSKIPFACTOR (const unsigned char)0xD4
+#define MID_REQOUTPUTSKIPFACTORACK (const unsigned char)0xD5
+#define LEN_OUTPUTSKIPFACTOR (const unsigned short)2
+#define MID_SETOUTPUTSKIPFACTOR (const unsigned char)0xD4
+#define MID_SETOUTPUTSKIPFACTORACK (const unsigned char)0xD5
+
+#define MID_REQSYNCINSETTINGS (const unsigned char)0xD6
+#define MID_REQSYNCINSETTINGSACK (const unsigned char)0xD7
+#define LEN_SYNCINMODE (const unsigned short)2
+#define LEN_SYNCINSKIPFACTOR (const unsigned short)2
+#define LEN_SYNCINOFFSET (const unsigned short)4
+#define MID_SETSYNCINSETTINGS (const unsigned char)0xD6
+#define MID_SETSYNCINSETTINGSACK (const unsigned char)0xD7
+
+#define MID_REQSYNCOUTSETTINGS (const unsigned char)0xD8
+#define MID_REQSYNCOUTSETTINGSACK (const unsigned char)0xD9
+#define LEN_SYNCOUTMODE (const unsigned short)2
+#define LEN_SYNCOUTSKIPFACTOR (const unsigned short)2
+#define LEN_SYNCOUTOFFSET (const unsigned short)4
+#define LEN_SYNCOUTPULSEWIDTH (const unsigned short)4
+#define MID_SETSYNCOUTSETTINGS (const unsigned char)0xD8
+#define MID_SETSYNCOUTSETTINGSACK (const unsigned char)0xD9
+
+#define MID_REQERRORMODE (const unsigned char)0xDA
+#define MID_REQERRORMODEACK (const unsigned char)0xDB
+#define LEN_ERRORMODE (const unsigned short)2
+#define MID_SETERRORMODE (const unsigned char)0xDA
+#define MID_SETERRORMODEACK (const unsigned char)0xDB
+
+#define MID_REQTRANSMITDELAY (const unsigned char)0xDC
+#define MID_REQTRANSMITDELAYACK (const unsigned char)0xDD
+#define LEN_TRANSMITDELAY (const unsigned short)2
+#define MID_SETTRANSMITDELAY (const unsigned char)0xDC
+#define MID_SETTRANSMITDELAYACK (const unsigned char)0xDD
+
+// Xbus Master
+#define MID_REQXMERRORMODE (const unsigned char)0x82
+#define MID_REQXMERRORMODEACK (const unsigned char)0x83
+#define LEN_XMERRORMODE (const unsigned short)2
+#define MID_SETXMERRORMODE (const unsigned char)0x82
+#define MID_SETXMERRORMODEACK (const unsigned char)0x83
+
+#define MID_REQBUFFERSIZE (const unsigned char)0x84
+#define MID_REQBUFFERSIZEACK (const unsigned char)0x85
+#define LEN_BUFFERSIZE (const unsigned short)2
+#define MID_SETBUFFERSIZE (const unsigned char)0x84
+#define MID_SETBUFFERSIZEACK (const unsigned char)0x85
+// End Xbus Master
+
+#define MID_REQHEADING (const unsigned char)0x82
+#define MID_REQHEADINGACK (const unsigned char)0x83
+#define LEN_HEADING (const unsigned short)4
+#define MID_SETHEADING (const unsigned char)0x82
+#define MID_SETHEADINGACK (const unsigned char)0x83
+
+#define MID_REQLOCATIONID (const unsigned char)0x84
+#define MID_REQLOCATIONIDACK (const unsigned char)0x85
+#define LEN_LOCATIONID (const unsigned short)2
+#define MID_SETLOCATIONID (const unsigned char)0x84
+#define MID_SETLOCATIONIDACK (const unsigned char)0x85
+
+#define MID_REQEXTOUTPUTMODE (const unsigned char)0x86
+#define MID_REQEXTOUTPUTMODEACK (const unsigned char)0x87
+#define LEN_EXTOUTPUTMODE (const unsigned short)2
+#define MID_SETEXTOUTPUTMODE (const unsigned char)0x86
+#define MID_SETEXTOUTPUTMODEACK (const unsigned char)0x87
+
+// XbusMaster
+#define MID_REQBATLEVEL (const unsigned char)0x88
+#define MID_BATLEVEL (const unsigned char)0x89
+// End XbusMaster
+
+#define MID_REQINITTRACKMODE (const unsigned char)0x88
+#define MID_REQINITTRACKMODEACK (const unsigned char)0x89
+#define LEN_INITTRACKMODE (const unsigned short)2
+#define MID_SETINITTRACKMODE (const unsigned char)0x88
+#define MID_SETINITTRACKMODEACK (const unsigned char)0x89
+
+#define MID_STOREFILTERSTATE (const unsigned char)0x8A
+#define MID_STOREFILTERSTATEACK (const unsigned char)0x8B
+
+// Measurement state
+#define MID_GOTOCONFIG (const unsigned char)0x30
+#define MID_GOTOCONFIGACK (const unsigned char)0x31
+#define MID_BUSDATA (const unsigned char)0x32
+#define MID_MTDATA (const unsigned char)0x32
+
+// Manual
+#define MID_PREPAREDATA (const unsigned char)0x32
+#define MID_REQDATA (const unsigned char)0x34
+#define MID_REQDATAACK (const unsigned char)0x35
+
+// MTData defines
+// Length of data blocks in bytes
+#define LEN_RAWDATA (const unsigned short)20
+#define LEN_CALIBDATA (const unsigned short)36
+#define LEN_CALIB_ACCDATA (const unsigned short)12
+#define LEN_CALIB_GYRDATA (const unsigned short)12
+#define LEN_CALIB_MAGDATA (const unsigned short)12
+#define LEN_ORIENT_QUATDATA (const unsigned short)16
+#define LEN_ORIENT_EULERDATA (const unsigned short)12
+#define LEN_ORIENT_MATRIXDATA (const unsigned short)36
+#define LEN_SAMPLECNT (const unsigned short)2
+#define LEN_TEMPDATA (const unsigned short)4
+
+// Length of data blocks in floats
+#define LEN_CALIBDATA_FLT (const unsigned short)9
+#define LEN_ORIENT_QUATDATA_FLT (const unsigned short)4
+#define LEN_ORIENT_EULERDATA_FLT (const unsigned short)3
+#define LEN_ORIENT_MATRIXDATA_FLT (const unsigned short)9
+
+// Indices of fields in DATA field of MTData message in bytes
+// use in combination with LEN_CALIB etc
+// Un-calibrated raw data
+#define IND_RAW_ACCX 0
+#define IND_RAW_ACCY 2
+#define IND_RAW_ACCZ 4
+#define IND_RAW_GYRX 6
+#define IND_RAW_GYRY 8
+#define IND_RAW_GYRZ 10
+#define IND_RAW_MAGX 12
+#define IND_RAW_MAGY 14
+#define IND_RAW_MAGZ 16
+#define IND_RAW_TEMP 18
+// Calibrated data
+#define IND_CALIB_ACCX 0
+#define IND_CALIB_ACCY 4
+#define IND_CALIB_ACCZ 8
+#define IND_CALIB_GYRX 12
+#define IND_CALIB_GYRY 16
+#define IND_CALIB_GYRZ 20
+#define IND_CALIB_MAGX 24
+#define IND_CALIB_MAGY 28
+#define IND_CALIB_MAGZ 32
+// Orientation data - quat
+#define IND_ORIENT_Q0 0
+#define IND_ORIENT_Q1 4
+#define IND_ORIENT_Q2 8
+#define IND_ORIENT_Q3 12
+// Orientation data - euler
+#define IND_ORIENT_ROLL 0
+#define IND_ORIENT_PITCH 4
+#define IND_ORIENT_YAW 8
+// Orientation data - matrix
+#define IND_ORIENT_A 0
+#define IND_ORIENT_B 4
+#define IND_ORIENT_C 8
+#define IND_ORIENT_D 12
+#define IND_ORIENT_E 16
+#define IND_ORIENT_F 20
+#define IND_ORIENT_G 24
+#define IND_ORIENT_H 28
+#define IND_ORIENT_I 32
+// Orientation data - euler
+#define IND_SAMPLECNTH 0
+#define IND_SAMPLECNTL 1
+
+// Indices of fields in DATA field of MTData message
+// Un-calibrated raw data
+#define FLDNUM_RAW_ACCX 0
+#define FLDNUM_RAW_ACCY 1
+#define FLDNUM_RAW_ACCZ 2
+#define FLDNUM_RAW_GYRX 3
+#define FLDNUM_RAW_GYRY 4
+#define FLDNUM_RAW_GYRZ 5
+#define FLDNUM_RAW_MAGX 6
+#define FLDNUM_RAW_MAGY 7
+#define FLDNUM_RAW_MAGZ 8
+#define FLDNUM_RAW_TEMP 9
+// Calibrated data
+#define FLDNUM_CALIB_ACCX 0
+#define FLDNUM_CALIB_ACCY 1
+#define FLDNUM_CALIB_ACCZ 2
+#define FLDNUM_CALIB_GYRX 3
+#define FLDNUM_CALIB_GYRY 4
+#define FLDNUM_CALIB_GYRZ 5
+#define FLDNUM_CALIB_MAGX 6
+#define FLDNUM_CALIB_MAGY 7
+#define FLDNUM_CALIB_MAGZ 8
+// Orientation data - quat
+#define FLDNUM_ORIENT_Q0 0
+#define FLDNUM_ORIENT_Q1 1
+#define FLDNUM_ORIENT_Q2 2
+#define FLDNUM_ORIENT_Q3 3
+// Orientation data - euler
+#define FLDNUM_ORIENT_ROLL 0
+#define FLDNUM_ORIENT_PITCH 1
+#define FLDNUM_ORIENT_YAW 2
+// Orientation data - matrix
+#define FLDNUM_ORIENT_A 0
+#define FLDNUM_ORIENT_B 1
+#define FLDNUM_ORIENT_C 2
+#define FLDNUM_ORIENT_D 3
+#define FLDNUM_ORIENT_E 4
+#define FLDNUM_ORIENT_F 5
+#define FLDNUM_ORIENT_G 6
+#define FLDNUM_ORIENT_H 7
+#define FLDNUM_ORIENT_I 8
+// Length
+// Uncalibrated raw data
+#define LEN_RAW_ACCX 2
+#define LEN_RAW_ACCY 2
+#define LEN_RAW_ACCZ 2
+#define LEN_RAW_GYRX 2
+#define LEN_RAW_GYRY 2
+#define LEN_RAW_GYRZ 2
+#define LEN_RAW_MAGX 2
+#define LEN_RAW_MAGY 2
+#define LEN_RAW_MAGZ 2
+#define LEN_RAW_TEMP 2
+// Calibrated data
+#define LEN_CALIB_ACCX 4
+#define LEN_CALIB_ACCY 4
+#define LEN_CALIB_ACCZ 4
+#define LEN_CALIB_GYRX 4
+#define LEN_CALIB_GYRY 4
+#define LEN_CALIB_GYRZ 4
+#define LEN_CALIB_MAGX 4
+#define LEN_CALIB_MAGY 4
+#define LEN_CALIB_MAGZ 4
+// Orientation data - quat
+#define LEN_ORIENT_Q0 4
+#define LEN_ORIENT_Q1 4
+#define LEN_ORIENT_Q2 4
+#define LEN_ORIENT_Q3 4
+// Orientation data - euler
+#define LEN_ORIENT_ROLL 4
+#define LEN_ORIENT_PITCH 4
+#define LEN_ORIENT_YAW 4
+// Orientation data - matrix
+#define LEN_ORIENT_A 4
+#define LEN_ORIENT_B 4
+#define LEN_ORIENT_C 4
+#define LEN_ORIENT_D 4
+#define LEN_ORIENT_E 4
+#define LEN_ORIENT_F 4
+#define LEN_ORIENT_G 4
+#define LEN_ORIENT_H 4
+#define LEN_ORIENT_I 4
+
+// Defines for getDataValue
+#define VALUE_RAW_ACC 0
+#define VALUE_RAW_GYR 1
+#define VALUE_RAW_MAG 2
+#define VALUE_RAW_TEMP 3
+#define VALUE_CALIB_ACC 4
+#define VALUE_CALIB_GYR 5
+#define VALUE_CALIB_MAG 6
+#define VALUE_ORIENT_QUAT 7
+#define VALUE_ORIENT_EULER 8
+#define VALUE_ORIENT_MATRIX 9
+#define VALUE_SAMPLECNT 10
+#define VALUE_TEMP 11
+
+#define INVALIDSETTINGVALUE 0xFFFFFFFF
+
+// Valid in all states
+#define MID_RESET (const unsigned char)0x40
+#define MID_RESETACK (const unsigned char)0x41
+#define MID_ERROR (const unsigned char)0x42
+#define LEN_ERROR (const unsigned short)1
+// XbusMaster
+#define MID_XMPWROFF (const unsigned char)0x44
+// End XbusMaster
+
+#define MID_REQFILTERSETTINGS (const unsigned char)0xA0
+#define MID_REQFILTERSETTINGSACK (const unsigned char)0xA1
+#define LEN_FILTERSETTINGS (const unsigned short)4
+#define MID_SETFILTERSETTINGS (const unsigned char)0xA0
+#define MID_SETFILTERSETTINGSACK (const unsigned char)0xA1
+#define MID_REQAMD (const unsigned char)0xA2
+#define MID_REQAMDACK (const unsigned char)0xA3
+#define LEN_AMD (const unsigned short)2
+#define MID_SETAMD (const unsigned char)0xA2
+#define MID_SETAMDACK (const unsigned char)0xA3
+#define MID_RESETORIENTATION (const unsigned char)0xA4
+#define MID_RESETORIENTATIONACK (const unsigned char)0xA5
+#define LEN_RESETORIENTATION (const unsigned short)2
+
+// All Messages
+// WakeUp state messages
+#define MSG_WAKEUPLEN 5
+#define MSG_WAKEUPACK (const unsigned char *)"\xFA\xFF\x3F\x00"
+#define MSG_WAKEUPACKLEN 4
+// Config state messages
+#define MSG_REQDID (const unsigned char *)"\xFA\xFF\x00\x00"
+#define MSG_REQDIDLEN 4
+#define MSG_DEVICEIDLEN 9
+#define MSG_INITBUS (const unsigned char *)"\xFA\xFF\x02\x00"
+#define MSG_INITBUSLEN 4
+#define MSG_INITBUSRESMAXLEN (5 + 4 * MAXSENSORS)
+#define MSG_REQPERIOD (const unsigned char *)"\xFA\xFF\x04\x00"
+#define MSG_REQPERIODLEN 4
+#define MSG_REQPERIODACKLEN 7
+#define MSG_SETPERIOD (const unsigned char *)"\xFA\xFF\x04\x02"
+#define MSG_SETPERIODLEN 6
+#define MSG_SETPERIODACKLEN 5
+#define MSG_SETBID (const unsigned char *)"\xFA\xFF\x06\x05"
+#define MSG_SETBIDLEN 9
+#define MSG_SETBIDACKLEN 5
+#define MSG_AUTOSTART (const unsigned char *)"\xFA\xFF\x06\x00"
+#define MSG_AUTOSTARTLEN 4
+#define MSG_AUTOSTARTACKLEN 5
+#define MSG_BUSPWROFF (const unsigned char *)"\xFA\xFF\x08\x00"
+#define MSG_BUSPWROFFLEN 4
+#define MSG_BUSPWROFFACKLEN 5
+#define MSG_RESTOREFACTORYDEF (const unsigned char *)"\xFA\xFF\x0E\x00"
+#define MSG_RESTOREFACTORYDEFLEN 4
+#define MSG_RESTOREFACTORYDEFACKLEN 5
+#define MSG_REQDATALENGTH (const unsigned char *)"\xFA\xFF\x0A\x00"
+#define MSG_REQDATALENGTHLEN 4
+#define MSG_DATALENGTHLEN 7
+#define MSG_REQCONFIGURATION (const unsigned char *)"\xFA\xFF\x0C\x00"
+#define MSG_REQCONFIGURATIONLEN 4
+#define MSG_GOTOMEASUREMENT (const unsigned char *)"\xFA\xFF\x10\x00"
+#define MSG_GOTOMEASUREMENTLEN 4
+#define MSG_GOTOMEASMAN (const unsigned char *)"\xFA\x01\x10\x00"
+#define MSG_GOTOMEASMANLEN 4
+#define MSG_GOTOMEASACKLEN 5
+#define MSG_REQFWREV (const unsigned char *)"\xFA\xFF\x12\x00"
+#define MSG_REQFWREVLEN 4
+#define MSG_FIRMWAREREVLEN 8
+#define MSG_REQBTDISABLED (const unsigned char *)"\xFA\xFF\x14\x00"
+#define MSG_REQBTDISABLEDLEN 4
+#define MSG_REQBTDISABLEDACKLEN 6
+#define MSG_DISABLEBT (const unsigned char *)"\xFA\xFF\x14\x01"
+#define MSG_DISABLEBTLEN 5
+#define MSG_DISABLEBTACKLEN 5
+#define MSG_REQOPMODE (const unsigned char *)"\xFA\xFF\x16\x00"
+#define MSG_REQOPMODELEN 4
+#define MSG_REQOPMODEACKLEN 6
+#define MSG_SETOPMODE (const unsigned char *)"\xFA\xFF\x16\x01"
+#define MSG_SETOPMODELEN 5
+#define MSG_SETOPMODEACKLEN 5
+#define MSG_REQBAUDRATE (const unsigned char *)"\xFA\xFF\x18\x00"
+#define MSG_REQBAUDRATELEN 4
+#define MSG_REQBAUDRATEACKLEN 6
+#define MSG_SETBAUDRATE (const unsigned char *)"\xFA\xFF\x18\x01"
+#define MSG_SETBAUDRATELEN 5
+#define MSG_SETBAUDRATEACKLEN 5
+#define MSG_REQSYNCMODE (const unsigned char *)"\xFA\xFF\x1A\x00"
+#define MSG_REQSYNCMODELEN 4
+#define MSG_REQSYNCMODEACKLEN 6
+#define MSG_SETSYNCMODE (const unsigned char *)"\xFA\xFF\x1A\x01"
+#define MSG_SETSYNCMODELEN 5
+#define MSG_SETSYNCMODEACKLEN 6
+#define MSG_REQMTS (const unsigned char *)"\xFA\xFF\x90\x01"
+#define MSG_REQMTSLEN 5
+#define MSG_MTSDATA 61
+#define MSG_STORECUSMTS (const unsigned char *)"\xFA\xFF\x92\x58"
+#define MSG_STORECUSMTSLEN 92
+#define MSG_STORECUSMTSACKLEN 5
+#define MSG_REVTOORGMTS (const unsigned char *)"\xFA\xFF\x94\x00"
+#define MSG_REVTOORGMTSLEN 4
+#define MSG_REVTOORGMTSACKLEN 5
+#define MSG_STOREMTS (const unsigned char *)"\xFA\xFF\x96\x41"
+#define MSG_STOREMTSLEN 69
+#define MSG_STOREMTSACKLEN 5
+#define MSG_REQSYNCOUTMODE (const unsigned char *)"\xFA\xFF\xD8\x01\x00"
+#define MSG_REQSYNCOUTMODELEN 5
+#define MSG_REQSYNCOUTSKIPFACTOR (const unsigned char *)"\xFA\xFF\xD8\x01\x01"
+#define MSG_REQSYNCOUTSKIPFACTORLEN 5
+#define MSG_REQSYNCOUTOFFSET (const unsigned char *)"\xFA\xFF\xD8\x01\x02"
+#define MSG_REQSYNCOUTOFFSETLEN 5
+#define MSG_REQSYNCOUTPULSEWIDTH (const unsigned char *)"\xFA\xFF\xD8\x01\x03"
+#define MSG_REQSYNCOUTPULSEWIDTHLEN 5
+#define MSG_REQERRORMODE (const unsigned char *)"\xFA\xFF\xDA\x00"
+#define MSG_REQERRORMODELEN 4
+#define MSG_REQERRORMODEACKLEN 7
+// Measurement state - auto messages
+#define MSG_GOTOCONFIG (const unsigned char *)"\xFA\xFF\x30\x00"
+#define MSG_GOTOCONFIGLEN 4
+#define MSG_GOTOCONFIGACKLEN 5
+// Measurement state - manual messages (Use DID = 0x01)
+#define MSG_GOTOCONFIGM (const unsigned char *)"\xFA\x01\x30\x00"
+#define MSG_GOTOCONFIGMLEN 4
+#define MSG_GOTOCONFIGMACKLEN 5
+#define MSG_PREPAREDATA (const unsigned char *)"\xFA\x01\x32\x00"
+#define MSG_PREPAREDATALEN 4
+#define MSG_REQDATA (const unsigned char *)"\xFA\x01\x34\x00"
+#define MSG_REQDATALEN 4
+// Valid in all states
+#define MSG_RESET (const unsigned char *)"\xFA\xFF\x40\x00"
+#define MSG_RESETLEN 4
+#define MSG_RESETACKLEN 5
+#define MSG_XMPWROFF (const unsigned char *)"\xFA\xFF\x44\x00"
+#define MSG_XMPWROFFLEN 4
+#define MSG_XMPWROFFACKLEN 5
+
+// Baudrate defines for SetBaudrate message
+#define BAUDRATE_9K6 0x09
+#define BAUDRATE_14K4 0x08
+#define BAUDRATE_19K2 0x07
+#define BAUDRATE_28K8 0x06
+#define BAUDRATE_38K4 0x05
+#define BAUDRATE_57K6 0x04
+#define BAUDRATE_76K8 0x03
+#define BAUDRATE_115K2 0x02
+#define BAUDRATE_230K4 0x01
+#define BAUDRATE_460K8 0x00
+#define BAUDRATE_921K6 0x80
+
+// Xbus protocol error codes (Error)
+#define ERROR_NOBUSCOMM 0x01
+#define ERROR_BUSNOTREADY 0x02
+#define ERROR_PERIODINVALID 0x03
+#define ERROR_MESSAGEINVALID 0x04
+#define ERROR_INITOFBUSFAILED1 0x10
+#define ERROR_INITOFBUSFAILED2 0x11
+#define ERROR_INITOFBUSFAILED3 0x12
+#define ERROR_SETBIDPROCFAILED1 0x14
+#define ERROR_SETBIDPROCFAILED2 0x15
+#define ERROR_MEASUREMENTFAILED1 0x18
+#define ERROR_MEASUREMENTFAILED2 0x19
+#define ERROR_MEASUREMENTFAILED3 0x1A
+#define ERROR_MEASUREMENTFAILED4 0x1B
+#define ERROR_MEASUREMENTFAILED5 0x1C
+#define ERROR_MEASUREMENTFAILED6 0x1D
+#define ERROR_TIMEROVERFLOW 0x1E
+#define ERROR_BAUDRATEINVALID 0x20
+#define ERROR_PARAMETERINVALID 0x21
+#define ERROR_MEASUREMENTFAILED7 0x23
+
+// Error modes (SetErrorMode)
+#define ERRORMODE_IGNORE 0x0000
+#define ERRORMODE_INCSAMPLECNT 0x0001
+#define ERRORMODE_INCSAMPLECNT_SENDERROR 0x0002
+#define ERRORMODE_SENDERROR_GOTOCONFIG 0x0003
+
+// Configuration message defines
+#define CONF_MASTERDID 0
+#define CONF_PERIOD 4
+#define CONF_OUTPUTSKIPFACTOR 6
+#define CONF_SYNCIN_MODE 8
+#define CONF_SYNCIN_SKIPFACTOR 10
+#define CONF_SYNCIN_OFFSET 12
+#define CONF_DATE 16
+#define CONF_TIME 24
+#define CONF_NUMDEVICES 96
+// Configuration sensor block properties
+#define CONF_DID 98
+#define CONF_DATALENGTH 102
+#define CONF_OUTPUTMODE 104
+#define CONF_OUTPUTSETTINGS 106
+#define CONF_BLOCKLEN 20
+// To calculate the offset in data field for output mode of sensor #2 use
+// CONF_OUTPUTMODE + 1*CONF_BLOCKLEN
+#define CONF_MASTERDIDLEN 4
+#define CONF_PERIODLEN 2
+#define CONF_OUTPUTSKIPFACTORLEN 2
+#define CONF_SYNCIN_MODELEN 2
+#define CONF_SYNCIN_SKIPFACTORLEN 2
+#define CONF_SYNCIN_OFFSETLEN 4
+#define CONF_DATELEN 8
+#define CONF_TIMELEN 8
+#define CONF_RESERVED_CLIENTLEN 32
+#define CONF_RESERVED_HOSTLEN 32
+#define CONF_NUMDEVICESLEN 2
+// Configuration sensor block properties
+#define CONF_DIDLEN 4
+#define CONF_DATALENGTHLEN 2
+#define CONF_OUTPUTMODELEN 2
+#define CONF_OUTPUTSETTINGSLEN 4
+
+// Clock frequency for offset & pulse width
+#define SYNC_CLOCKFREQ 29.4912e6
+
+// SyncIn params
+#define PARAM_SYNCIN_MODE (const unsigned char)0x00
+#define PARAM_SYNCIN_SKIPFACTOR (const unsigned char)0x01
+#define PARAM_SYNCIN_OFFSET (const unsigned char)0x02
+
+// SyncIn mode
+#define SYNCIN_DISABLED 0x0000
+#define SYNCIN_EDGE_RISING 0x0001
+#define SYNCIN_EDGE_FALLING 0x0002
+#define SYNCIN_EDGE_BOTH 0x0003
+#define SYNCIN_TYPE_SENDLASTDATA 0x0004
+#define SYNCIN_TYPE_DOSAMPLING 0x0000
+#define SYNCIN_EDGE_MASK 0x0003
+#define SYNCIN_TYPE_MASK 0x000C
+
+// SyncOut params
+#define PARAM_SYNCOUT_MODE (const unsigned char)0x00
+#define PARAM_SYNCOUT_SKIPFACTOR (const unsigned char)0x01
+#define PARAM_SYNCOUT_OFFSET (const unsigned char)0x02
+#define PARAM_SYNCOUT_PULSEWIDTH (const unsigned char)0x03
+
+// SyncOut mode
+#define SYNCOUT_DISABLED 0x0000
+#define SYNCOUT_TYPE_TOGGLE 0x0001
+#define SYNCOUT_TYPE_PULSE 0x0002
+#define SYNCOUT_POL_NEG 0x0000
+#define SYNCOUT_POL_POS 0x0010
+#define SYNCOUT_TYPE_MASK 0x000F
+#define SYNCOUT_POL_MASK 0x0010
+
+// Sample frequencies (SetPeriod)
+#define PERIOD_10HZ 0x2D00
+#define PERIOD_12HZ 0x2580
+#define PERIOD_15HZ 0x1E00
+#define PERIOD_16HZ 0x1C20
+#define PERIOD_18HZ 0x1900
+#define PERIOD_20HZ 0x1680
+#define PERIOD_24HZ 0x12C0
+#define PERIOD_25HZ 0x1200
+#define PERIOD_30HZ 0x0F00
+#define PERIOD_32HZ 0x0E10
+#define PERIOD_36HZ 0x0C80
+#define PERIOD_40HZ 0x0B40
+#define PERIOD_45HZ 0x0A00
+#define PERIOD_48HZ 0x0960
+#define PERIOD_50HZ 0x0900
+#define PERIOD_60HZ 0x0780
+#define PERIOD_64HZ 0x0708
+#define PERIOD_72HZ 0x0640
+#define PERIOD_75HZ 0x0600
+#define PERIOD_80HZ 0x05A0
+#define PERIOD_90HZ 0x0500
+#define PERIOD_96HZ 0x04B0
+#define PERIOD_100HZ 0x0480
+#define PERIOD_120HZ 0x03C0
+#define PERIOD_128HZ 0x0384
+#define PERIOD_144HZ 0x0320
+#define PERIOD_150HZ 0x0300
+#define PERIOD_160HZ 0x02D0
+#define PERIOD_180HZ 0x0280
+#define PERIOD_192HZ 0x0258
+#define PERIOD_200HZ 0x0240
+#define PERIOD_225HZ 0x0200
+#define PERIOD_240HZ 0x01E0
+#define PERIOD_256HZ 0x01C2
+#define PERIOD_288HZ 0x0190
+#define PERIOD_300HZ 0x0180
+#define PERIOD_320HZ 0x0168
+#define PERIOD_360HZ 0x0140
+#define PERIOD_384HZ 0x012C
+#define PERIOD_400HZ 0x0120
+#define PERIOD_450HZ 0x0100
+#define PERIOD_480HZ 0x00F0
+#define PERIOD_512HZ 0x00E1
+
+// OutputModes
+#define OUTPUTMODE_MT9 0x8000
+#define OUTPUTMODE_XM 0x0000
+#define OUTPUTMODE_RAW 0x4000
+#define OUTPUTMODE_TEMP 0x0001
+#define OUTPUTMODE_CALIB 0x0002
+#define OUTPUTMODE_ORIENT 0x0004
+
+// OutputSettings
+#define OUTPUTSETTINGS_XM 0x00000001
+#define OUTPUTSETTINGS_TIMESTAMP_NONE 0x00000000
+#define OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT 0x00000001
+#define OUTPUTSETTINGS_ORIENTMODE_QUATERNION 0x00000000
+#define OUTPUTSETTINGS_ORIENTMODE_EULER 0x00000004
+#define OUTPUTSETTINGS_ORIENTMODE_MATRIX 0x00000008
+#define OUTPUTSETTINGS_CALIBMODE_ACCGYRMAG 0x00000000
+#define OUTPUTSETTINGS_CALIBMODE_ACC 0x00000060
+#define OUTPUTSETTINGS_CALIBMODE_ACCGYR 0x00000040
+#define OUTPUTSETTINGS_CALIBMODE_ACCMAG 0x00000020
+#define OUTPUTSETTINGS_CALIBMODE_GYR 0x00000050
+#define OUTPUTSETTINGS_CALIBMODE_GYRMAG 0x00000010
+#define OUTPUTSETTINGS_CALIBMODE_MAG 0x00000030
+#define OUTPUTSETTINGS_DATAFORMAT_FLOAT 0x00000000
+#define OUTPUTSETTINGS_DATAFORMAT_F1220 0x00000100
+#define OUTPUTSETTINGS_TIMESTAMP_MASK 0x00000003
+#define OUTPUTSETTINGS_ORIENTMODE_MASK 0x0000000C
+#define OUTPUTSETTINGS_CALIBMODE_ACC_MASK 0x00000010
+#define OUTPUTSETTINGS_CALIBMODE_GYR_MASK 0x00000020
+#define OUTPUTSETTINGS_CALIBMODE_MAG_MASK 0x00000040
+#define OUTPUTSETTINGS_CALIBMODE_MASK 0x00000070
+#define OUTPUTSETTINGS_DATAFORMAT_MASK 0x00000300
+
+// Extended Output Modes
+#define EXTOUTPUTMODE_DISABLED 0x0000
+#define EXTOUTPUTMODE_EULER 0x0001
+
+// Factory Output Mode
+#define FACTORYOUTPUTMODE_DISABLE 0x0000
+#define FACTORYOUTPUTMODE_DEFAULT 0x0001
+#define FACTORYOUTPUTMODE_CUSTOM 0x0002
+
+// Initial tracking mode (SetInitTrackMode)
+#define INITTRACKMODE_DISABLED 0x0000
+#define INITTRACKMODE_ENABLED 0x0001
+
+// Filter settings params
+#define PARAM_FILTER_GAIN (const unsigned char)0x00
+#define PARAM_FILTER_RHO (const unsigned char)0x01
+#define DONOTSTORE 0x00
+#define STORE 0x01
+
+// AMDSetting (SetAMD)
+#define AMDSETTING_DISABLED 0x0000
+#define AMDSETTING_ENABLED 0x0001
+
+// Reset orientation message type
+#define RESETORIENTATION_STORE 0
+#define RESETORIENTATION_HEADING 1
+#define RESETORIENTATION_GLOBAL 2
+#define RESETORIENTATION_OBJECT 3
+#define RESETORIENTATION_ALIGN 4
+
+// Send raw string mode
+#define SENDRAWSTRING_INIT 0
+#define SENDRAWSTRING_DEFAULT 1
+#define SENDRAWSTRING_SEND 2
+
+// Timeouts
+#define TO_DEFAULT 500
+#define TO_INIT 250
+#define TO_RETRY 50
+
+// openPort baudrates
+#ifdef WIN32
+#define PBR_9600 CBR_9600
+#define PBR_14K4 CBR_14400
+#define PBR_19K2 CBR_19200
+#define PBR_28K8 28800
+#define PBR_38K4 CBR_38400
+#define PBR_57K6 CBR_57600
+#define PBR_76K8 76800
+#define PBR_115K2 CBR_115200
+#define PBR_230K4 230400
+#define PBR_460K8 460800
+#define PBR_921K6 921600
+#else
+#define PBR_9600 B9600
+#define PBR_14K4 B14400
+#define PBR_19K2 B19200
+#define PBR_28K8 B28800
+#define PBR_38K4 B38400
+#define PBR_57K6 B57600
+#define PBR_76K8 B76800
+#define PBR_115K2 B115200
+#define PBR_230K4 B230400
+#define PBR_460K8 B460800
+#define PBR_921K6 B921600
+#endif
+
+// setFilePos defines
+#ifdef WIN32
+#define FILEPOS_BEGIN FILE_BEGIN
+#define FILEPOS_CURRENT FILE_CURRENT
+#define FILEPOS_END FILE_END
+#else
+#define FILEPOS_BEGIN SEEK_SET
+#define FILEPOS_CURRENT SEEK_CUR
+#define FILEPOS_END SEEK_END
+#endif
+
+// Return values
+#define MTRV_OK 0 // Operation successful
+#define MTRV_NOTSUCCESSFUL 1 // General no success return value
+#define MTRV_TIMEOUT 2 // Operation aborted because of a timeout
+#define MTRV_TIMEOUTNODATA 3 // Operation aborted because of no data read
+#define MTRV_CHECKSUMFAULT 4 // Checksum fault occured
+#define MTRV_NODATA 5 // No data is received
+#define MTRV_RECVERRORMSG 6 // A error message is received
+#define MTRV_OUTOFMEMORY 7 // No internal memory available
+#define MTRV_UNKNOWDATA 8 // An invalid message is read
+#define MTRV_INVALIDTIMEOUT 9 // An invalid value is used to set the timeout
+#define MTRV_UNEXPECTEDMSG 10 // Unexpected message received (e.g. no acknowledge message received)
+#define MTRV_INPUTCANNOTBEOPENED 11 // The specified file / serial port can not be opened
+#define MTRV_ANINPUTALREADYOPEN 12 // File and serial port can not be opened at same time
+#define MTRV_ENDOFFILE 13 // End of file is reached
+#define MTRV_NOINPUTINITIALIZED 14 // No file or serial port opened for reading/writing
+#define MTRV_NOVALIDMODESPECIFIED 15 // No valid outputmode or outputsettings are specified (use
+// mtGetDeviceMode or mtSetMode)
+#define MTRV_INVALIDVALUESPEC 16 // Value specifier does not match value type or not available in data
+#define MTRV_INVALIDFORFILEINPUT 17 // Function is not valid for file based interfaces
+
+ class CXsensMTiModule
+ {
+ public:
+ CXsensMTiModule();
+ virtual ~CXsensMTiModule();
+
+ // Low level general functions
+ clock_t clockms();
+
+ // Low level COM port / file functions
+ short openPort(const int portNumber, const unsigned long baudrate = PBR_115K2, const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
+ short openPort(const char *portName, const unsigned long baudrate = PBR_115K2, const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
+ short openFile(const char *fileName, bool createAlways = false);
+ bool isPortOpen();
+ bool isFileOpen();
+ int readData(unsigned char* msgBuffer, const int nBytesToRead);
+ int writeData(const unsigned char* msgBuffer, const int nBytesToWrite);
+ void flush();
+ void escape(unsigned long function);
+ void setPortQueueSize(const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
+ short setFilePos(long relPos, unsigned long moveMethod = FILEPOS_BEGIN);
+ short getFileSize(unsigned long &fileSize);
+ short close();
+
+ // Read & write message functions
+ short readMessage(unsigned char &mid, unsigned char data[], short &dataLen, unsigned char *bid = NULL);
+ short readDataMessage(unsigned char data[], short &dataLen);
+ short readMessageRaw(unsigned char *msgBuffer, short *msgBufferLength);
+ short writeMessage(const unsigned char mid, const unsigned long dataValue = 0, const unsigned char dataValueLen = 0, const unsigned char bid = BID_MASTER);
+ short writeMessage(const unsigned char mid, const unsigned char data[], const unsigned short &dataLen, const unsigned char bid = BID_MASTER);
+ short waitForMessage(const unsigned char mid, unsigned char data[] = NULL, short *dataLen = NULL, unsigned char *bid = NULL);
+
+ // Request & set setting functions
+ short reqSetting(const unsigned char mid, unsigned long &value, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, const unsigned char param, unsigned long &value, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, float &value, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, const unsigned char param, float &value, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, unsigned char data[], short &dataLen, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, unsigned char dataIn[], short dataInLen, unsigned char dataOut[], short &dataOutLen, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid, const unsigned char param, unsigned char data[], short &dataLen, const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid, const unsigned long value, const unsigned short valuelen, const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid, const unsigned char param, const unsigned long value, const unsigned short valuelen, const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid, const float value, const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid, const unsigned char param, const float value, const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid, const unsigned char param, const float value, const bool store, const unsigned char bid = BID_MASTER);
+ // Data-related functions
+ short getDeviceMode(unsigned short *numDevices = NULL);
+ short setDeviceMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid = BID_MASTER);
+ short getMode(unsigned long &OutputMode, unsigned long &OutputSettings, unsigned short &dataLength, const unsigned char bid = BID_MASTER);
+ short setMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid = BID_MASTER);
+ short getValue(const unsigned long valueSpec, unsigned short &value, const unsigned char data[], const unsigned char bid = BID_MT);
+ short getValue(const unsigned long valueSpec, unsigned short value[], const unsigned char data[], const unsigned char bid = BID_MT);
+ short getValue(const unsigned long valueSpec, float value[], const unsigned char data[], const unsigned char bid = BID_MT);
+
+ // Generic MTComm functions
+ short getLastDeviceError();
+ short getLastRetVal();
+ short setTimeOut(short timeOutMs);
+ static void swapEndian(const unsigned char input[], unsigned char output[], const int length);
+ void calcChecksum(unsigned char *msgBuffer, const int msgBufferLength);
+ bool checkChecksum(const unsigned char *msgBuffer, const int msgBufferLength);
+ protected:
+ // member variables
+#ifdef WIN32
+ HANDLE m_handle;
+#else
+ int m_handle;
+#endif
+ bool m_portOpen;
+ bool m_fileOpen;
+ short m_deviceError;
+ short m_retVal;
+ short m_timeOut;
+ clock_t m_clkEnd;
+
+ // OutputMode, OutputSettings & DataLength for connected devices + 1 for master
+ unsigned long m_storedOutputMode[MAXDEVICES + 1];
+ unsigned long m_storedOutputSettings[MAXDEVICES + 1];
+ unsigned long m_storedDataLength[MAXDEVICES + 1];
+
+ // Temporary buffer for excess bytes read in ReadMessageRaw
+ unsigned char m_tempBuffer[MAXMSGLEN ];
+ int m_nTempBufferLen;
+
+ private:
+ };
+ }
+}
+
+#endif
+
+#endif // !defined(AFX_XBUS_H__F4580A3F_2CF2_4ED2_A747_B4B015A0328E__INCLUDED_)
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.cpp b/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..65116cadf08f69d76d71a00370981f6ac6a37969
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.cpp
@@ -0,0 +1,130 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::XsensIMU
+ * @author Markus Grotz ( markus-grotz at web dot de )
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "XsensIMU.h"
+
+
+
+using namespace armarx;
+using namespace IMU;
+
+
+PropertyDefinitionsPtr XsensIMU::createPropertyDefinitions()
+{
+ return PropertyDefinitionsPtr(new XsensIMUPropertyDefinitions(getConfigIdentifier()));
+}
+
+void XsensIMU::frameAcquisitionTaskLoop()
+{
+ while(!sensorTask->isStopped()) {
+
+ while(HasPendingEvents()) {
+
+ ProcessPendingEvent();
+
+ TimestampVariantPtr now = TimestampVariant::nowPtr();
+ IMUData data;
+
+ data.acceleration.push_back(m_Accelaration[0]);
+ data.acceleration.push_back(m_Accelaration[1]);
+ data.acceleration.push_back(m_Accelaration[2]);
+
+ data.gyroscopeRotation.push_back(m_GyroscopeRotation[0]);
+ data.gyroscopeRotation.push_back(m_GyroscopeRotation[1]);
+ data.gyroscopeRotation.push_back(m_GyroscopeRotation[2]);
+
+
+ data.magneticRotation.push_back(m_MagneticRotation[0]);
+ data.magneticRotation.push_back(m_MagneticRotation[1]);
+ data.magneticRotation.push_back(m_MagneticRotation[2]);
+
+
+ data.orientationQuaternion.push_back(m_OrientationQuaternion[0]);
+ data.orientationQuaternion.push_back(m_OrientationQuaternion[1]);
+ data.orientationQuaternion.push_back(m_OrientationQuaternion[2]);
+ data.orientationQuaternion.push_back(m_OrientationQuaternion[3]);
+
+ IMUTopicPrx->reportSensorValues("device", "name", data, now);
+
+ }
+ usleep(10000);
+ }
+}
+
+
+/*
+void XsensIMU::OnIMUCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
+{
+ const IMUState CurrentState = pIMUDevice->GetIMUState();
+
+ TimestampVariantPtr now = TimestampVariant::nowPtr();
+ IMUData data;
+
+ data.acceleration.push_back(CurrentState.m_PhysicalData.m_Acceleration[0]);
+ data.acceleration.push_back(CurrentState.m_PhysicalData.m_Acceleration[1]);
+ data.acceleration.push_back(CurrentState.m_PhysicalData.m_Acceleration[2]);
+
+ data.gyroscopeRotation.push_back(CurrentState.m_PhysicalData.m_GyroscopeRotation[0]);
+ data.gyroscopeRotation.push_back(CurrentState.m_PhysicalData.m_GyroscopeRotation[1]);
+ data.gyroscopeRotation.push_back(CurrentState.m_PhysicalData.m_GyroscopeRotation[2]);
+
+ data.magneticRotation.push_back(CurrentState.m_PhysicalData.m_MagneticRotation[0]);
+ data.magneticRotation.push_back(CurrentState.m_PhysicalData.m_MagneticRotation[1]);
+ data.magneticRotation.push_back(CurrentState.m_PhysicalData.m_MagneticRotation[2]);
+
+ data.orientationQuaternion.push_back(CurrentState.m_PhysicalData.m_OrientationQuaternion[0]);
+ data.orientationQuaternion.push_back(CurrentState.m_PhysicalData.m_OrientationQuaternion[1]);
+ data.orientationQuaternion.push_back(CurrentState.m_PhysicalData.m_OrientationQuaternion[2]);
+ data.orientationQuaternion.push_back(CurrentState.m_PhysicalData.m_OrientationQuaternion[3]);
+
+ IMUTopicPrx->reportSensorValues("device", pIMUDevice->GetDeviceId(), data, now);
+
+}
+
+*/
+
+void XsensIMU::onInitIMU()
+{
+ sensorTask = new RunningTask<XsensIMU>(this, &XsensIMU::frameAcquisitionTaskLoop);
+
+ SetDispatchingMode(IMU::IIMUEventDispatcher::eDecoupled);
+ SetMaximalPendingEvents(5);
+
+ IMUDevice.SetFusion(IMU::CIMUDevice::eMeanFusion,16);
+ IMUDevice.RegisterEventDispatcher(this);
+
+ IMUDevice.Connect(_IMU_DEVICE_DEFAUL_CONNECTION_, IMU::CIMUDevice::eSamplingFrequency_512HZ);
+}
+
+void XsensIMU::onStartIMU()
+{
+ IMUDevice.Start(false);
+ sensorTask->start();
+}
+
+void XsensIMU::onExitIMU()
+{
+ IMUDevice.Stop();
+ sensorTask->stop();
+}
+
diff --git a/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.h b/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.h
new file mode 100644
index 0000000000000000000000000000000000000000..1c3678bf3d613028519610a2d43e43e335b4573b
--- /dev/null
+++ b/source/RobotAPI/libraries/drivers/XsensIMU/XsensIMU.h
@@ -0,0 +1,107 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::XsensIMU
+ * @author Markus Grotz ( markus-grotz at web dot de )
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_COMPONENT_RobotAPI_XsensIMU_H
+#define _ARMARX_COMPONENT_RobotAPI_XsensIMU_H
+
+
+#include <Core/core/Component.h>
+#include <Core/core/services/tasks/RunningTask.h>
+#include <Core/observers/variant/TimestampVariant.h>
+
+
+#include <RobotAPI/components/units/InertialMeasurementUnit.h>
+
+#include "IMU/IMU.h"
+
+namespace armarx
+{
+ /**
+ * @class XsensIMUPropertyDefinitions
+ * @brief
+ * @ingroup Components
+ */
+ class XsensIMUPropertyDefinitions:
+ public InertialMeasurementUnitPropertyDefinitions
+ {
+ public:
+ XsensIMUPropertyDefinitions(std::string prefix):
+ InertialMeasurementUnitPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty("deviceConnection", "/dev/ttyUSB0", "");
+
+ defineOptionalProperty("frequency", "", "");
+ defineOptionalProperty("maxPendingEvents", "", "");
+ }
+ };
+
+ /**
+ * @class XsensIMU
+ * @brief A brief description
+ *
+ * Detailed Description
+ */
+ class XsensIMU :
+ virtual public InertialMeasurementUnit,
+ virtual public IMU::CIMUDeducedReckoning
+ {
+ public:
+
+
+ XsensIMU(): CIMUDeducedReckoning(false)
+ {
+
+ }
+
+ /**
+ * @see armarx::ManagedIceObject::getDefaultName()
+ */
+ virtual std::string getDefaultName() const
+ {
+ return "XsensIMU";
+ }
+
+ protected:
+
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+ void frameAcquisitionTaskLoop();
+
+ // InertialMeasurementUnit interface
+
+ void onInitIMU();
+ void onStartIMU();
+ void onExitIMU();
+
+
+ private:
+
+ RunningTask<XsensIMU>::pointer_type sensorTask;
+ IMU::CIMUDevice IMUDevice;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/robotstate/remote/ArmarPose.cpp b/source/RobotAPI/libraries/robotstate/remote/ArmarPose.cpp
index 5d493e0f30150d312bc5db9d55b6eefdc17bf712..4a86c75a5e347d0866b253f68b0da1db5b9c5d22 100644
--- a/source/RobotAPI/libraries/robotstate/remote/ArmarPose.cpp
+++ b/source/RobotAPI/libraries/robotstate/remote/ArmarPose.cpp
@@ -122,6 +122,14 @@ namespace armarx
this->init(q);
}
+ Quaternion::Quaternion(::Ice::Float qw, ::Ice::Float qx, ::Ice::Float qy, ::Ice::Float qz)
+ {
+ this->qw = qw;
+ this->qx = qx;
+ this->qy = qy;
+ this->qz = qz;
+ }
+
Matrix3f Quaternion::toEigen() const
{
Matrix3f rot;
diff --git a/source/RobotAPI/libraries/robotstate/remote/ArmarPose.h b/source/RobotAPI/libraries/robotstate/remote/ArmarPose.h
index 5bcd3ca671da407d2345b80a6cd3c0e362ecf91f..b7ecd25ede4dbad8b8658a74fcae724468fa7ae7 100644
--- a/source/RobotAPI/libraries/robotstate/remote/ArmarPose.h
+++ b/source/RobotAPI/libraries/robotstate/remote/ArmarPose.h
@@ -134,6 +134,7 @@ namespace armarx
Quaternion(const Eigen::Matrix4f &);
Quaternion(const Eigen::Matrix3f &);
Quaternion(const Eigen::Quaternionf &);
+ Quaternion(::Ice::Float qw, ::Ice::Float qx, ::Ice::Float qy, ::Ice::Float qz);
Eigen::Matrix3f toEigen() const;
Eigen::Matrix3f slerp(float, const Eigen::Matrix3f &);