diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino
index 13bd811..c7f7768 100644
--- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino
+++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/imu.ino
@@ -43,21 +43,30 @@
       data.gx = event.gyro.x;
       data.gy = event.gyro.y;
       data.gz = event.gyro.z;
-
+/*
       if (dof.accelGetOrientation(&accel_event, &orientation))
       {
-         /* 'orientation' should have valid .roll and .pitch fields */
         data.roll = orientation.roll;
         data.pitch = orientation.pitch;
       }
-      
+*/
+
       /* Calculate the heading using the magnetometer */
       mag.getEvent(&mag_event);
+
+/*
       if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation))
       {
-        /* 'orientation' should have valid .heading data now */
         data.mz = orientation.heading;
       }
+*/
+
+      if (dof.fusionGetOrientation(&accel_event, &mag_event, &orientation))
+      {
+        data.roll = orientation.roll;
+        data.pitch = orientation.pitch;
+        data.ch = orientation.heading;
+      }
       
       return data;
     }