From b8931b271d36487a7b2fb2504893b0c92e1b2f61 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Mon, 12 Sep 2016 05:40:22 -0700 Subject: [PATCH] Added fusionGetOrientation function to Adafruit 9-dof imu in imu.ino --- .../src/libraries/ROSArduinoBridge/imu.ino | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) 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; }