From 2a7ea184890ff92ab07f97068f0553ac59e50217 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Davide=20Tom=C3=A9?= <davide.tome@iit.it>
Date: Tue, 3 Aug 2021 16:24:48 +0200
Subject: [PATCH] added support for publishing mtb4 IMU data (#286)

---
 .../hardware/inertials/mc4plusloco-IMU.xml    | 52 +++++++++++++++++++
 .../mc4plus-loco-project/main.xml             |  5 ++
 .../inertials/mc4plusloco-IMU-wrapper.xml     | 20 +++++++
 3 files changed, 77 insertions(+)
 create mode 100755 experimentalSetups/mc4plus-loco-project/hardware/inertials/mc4plusloco-IMU.xml
 create mode 100755 experimentalSetups/mc4plus-loco-project/wrappers/inertials/mc4plusloco-IMU-wrapper.xml

diff --git a/experimentalSetups/mc4plus-loco-project/hardware/inertials/mc4plusloco-IMU.xml b/experimentalSetups/mc4plus-loco-project/hardware/inertials/mc4plusloco-IMU.xml
new file mode 100755
index 0000000000..8e37cddc3a
--- /dev/null
+++ b/experimentalSetups/mc4plus-loco-project/hardware/inertials/mc4plusloco-IMU.xml
@@ -0,0 +1,52 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="mc4plusloco-imu" type="embObjIMU">
+
+        <xi:include href="../../general.xml" />
+
+        <xi:include href="../../hardware/electronics/devmc4plusloco-mc4plus01-eln.xml" />
+
+        <group name="SERVICE">
+
+            <param name="type"> eomn_serv_AS_inertials3 </param>
+
+            <group name="PROPERTIES">
+
+                <group name="CANBOARDS">
+                    <param name="type">                 mtb4           </param>
+
+                    <group name="PROTOCOL">
+                        <param name="major">            2                   </param>
+                        <param name="minor">            0                   </param>
+                    </group>
+                    <group name="FIRMWARE">
+                        <param name="major">            1                   </param>
+                        <param name="minor">            4                   </param>
+                        <param name="build">            6                   </param>
+                    </group>
+                </group>
+
+                <group name="SENSORS">
+                    <param name="id">       mc4plusloco_acc     mc4plusloco_gyro    mc4plusloco_eul    mc4plusloco_mag    mc4plusloco_stat                    </param>
+
+                    <param name="type">     eoas_imu_acc             eoas_imu_gyr              eoas_imu_eul             eoas_imu_mag             eoas_imu_status                    </param>
+
+                    <param name="boardType"> mtb4       mtb4      mtb4       mtb4        mtb4                    </param>
+
+                    <param name="location">  CAN1:14       CAN1:14      CAN1:14       CAN1:14        CAN1:14                    </param>
+                </group>
+
+            </group>
+
+            <group name="SETTINGS">
+                <param name="acquisitionRate">      50      </param>
+                <param name="enabledSensors">       mc4plusloco_acc    mc4plusloco_gyro    mc4plusloco_eul    mc4plusloco_mag    mc4plusloco_stat
+                </param>
+            </group>
+
+        </group>
+
+    </device>
\ No newline at end of file
diff --git a/experimentalSetups/mc4plus-loco-project/main.xml b/experimentalSetups/mc4plus-loco-project/main.xml
index 863c6704fc..5abd6bab5a 100644
--- a/experimentalSetups/mc4plus-loco-project/main.xml
+++ b/experimentalSetups/mc4plus-loco-project/main.xml
@@ -8,6 +8,11 @@
     <xi:include href="./hardware/motorControl/mc4plusloco_mc_hardware.xml" /> 
     <xi:include href="./wrappers/motorControl/mc4plusloco_mc_wrapper.xml" /> 
 
+    <!-- IMU-->
+    <xi:include href="hardware/inertials/mc4plusloco-IMU.xml" /> 
+    
+    <xi:include href="wrappers/inertials/mc4plusloco-IMU-wrapper.xml" /> 
+
     <!-- CALIBRATORS -->
     <xi:include href="./calibrators/mc4plusloco_calib.xml" />
     
diff --git a/experimentalSetups/mc4plus-loco-project/wrappers/inertials/mc4plusloco-IMU-wrapper.xml b/experimentalSetups/mc4plus-loco-project/wrappers/inertials/mc4plusloco-IMU-wrapper.xml
new file mode 100755
index 0000000000..ee06660cef
--- /dev/null
+++ b/experimentalSetups/mc4plus-loco-project/wrappers/inertials/mc4plusloco-IMU-wrapper.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="mc4plusloco-imu_wrapper" type="multipleanalogsensorsserver">
+        <param name="period">       10                  </param>
+        <param name="name">     /icub/mc4plusloco-imu/imu                </param>
+
+
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+                <!-- The name of the element can be any string (we use SetOfInertials to better express its nature).
+                     Its value must match the device name in the corresponding body_part-jx_y-inertials.xml file
+                     or in body_part-ebX-inertials.xml -->
+                <elem name="SetOfInertials">mc4plusloco-imu </elem>
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="5" type="detach" />
+    </device>
\ No newline at end of file