BNO055IMU angular velocity axes do not match those of angular orientation
Lunerwalker2 opened this issue · 3 comments
Overview
When trying to solve issues recently noticed in this tuning opmode, I came across a potential issue with the getAngularVelocity() function of the BNO055IMU, where the X and Z angular velocity axes are flipped.
Description
The function of that opmode is to spin the robot in a circle for 4 seconds, and printing the maximum reported angular velocity. The original issue was that the angular velocity reported by the IMU was very low, in the 0-5 degree range.
My first step was eliminating possible user error, which I did by directly accessing the getAngularVelocity() method of the IMU instance being used in the same program's loop. I printed out the angular velocity values of each axis in kind, as shown below.
AngularVelocity ang = drive.imu.getAngularVelocity();
telemetry.addData("Current IMU Ang Velo Deg X", Math.toDegrees(ang.xRotationRate));
telemetry.addData("Current IMU Ang Velo Deg Y", Math.toDegrees(ang.yRotationRate));
telemetry.addData("Current IMU Ang Velo Deg Z", Math.toDegrees(ang.zRotationRate));
telemetry.addData("Current IMU Ang Velo Unit", ang.unit);
telemetry.update();My hubs are flat on the robot with the front logo facing directly upwards, meaning I use the Z Axis for orientation.
I have confirmed that this is the correct axis as I have been using it for a few months now with no issues. However, the results of the above test, taken about 2 seconds in, show that the Z axis is not in fact the one that is actually being rotated around.
(The "unit" value shown is not actually the unit of the numbers above it, which are in degrees, it's actually the unit of the AngularVelocity object. I just wanted to make sure there were no conversion errors in the user code.)
After seeing this, I made another opmode that simply printed out the orientation and angular velocity of the IMU repeatedly to telemetry.
After running this, I first rotated the robot as it would normally turn. As expected for orientation, the Z axis changed, however for angular velocity, the X axis was being changed instead.
Similarly, after pitching the robot forward, the X axis in orientation follows the movement, however in angular velocity, the Z axis is being effected.
When I tilted the robot to the left and right, both the Y orientation and the Y angular velocity were consistent.
How to Reproduce
Print out either the X or Z axes of the IMU's orientation, as well as the same axes in angular velocity. Then, rotate the imu around either the X or Z axes. The values of each orientation axis should be different from the axis being changed in the angular velocity. The Y axis for both however, as far as I can tell, is correct.
As for how this has only been noticed now, at least by me and the people I know, I would assume it is because the angular velocity function is little used by many people. The only real usage that I know of is through the same tuning opmode I was debugging to begin with, in the RoadRunner Quickstart. For the last few months, this tuner has been known to be broken as it returns incorrect values. Thus, whenever a question is asked regarding it, the general advice has been to simply ignore the tuner altogether and use an approximate value. Also, users who's hubs are mounted so that the Y axis is the value needed would not see any issue.
Control Hub OS and firmware on both hubs are both updated fully, ssing SDK v7.0.
Side note: As shown in the images, I have both an Expansion Hub and a Control Hub. The images here are all from the Expansion Hub's IMU, named "imu" (I confirmed this from the configuration). However, I repeated these steps with the Control Hub's IMU and got identical results.
I think the problem may be that the I2C driver treats the data as ZYX order:
@Override
public synchronized AngularVelocity getAngularVelocity(org.firstinspires.ftc.robotcore.external.navigation.AngleUnit unit)
{
VectorData vector = getVector(VECTOR.GYROSCOPE, getAngularScale());
float zRotationRate = -vector.next();
float yRotationRate = vector.next();
float xRotationRate = vector.next();
return new AngularVelocity(parameters.angleUnit.toAngleUnit(),
xRotationRate, yRotationRate, zRotationRate,
vector.data.nanoTime)
.toAngleUnit(unit);
}When the datasheet says the order is XYZ (starting from register 0x14 which is the starting register used in the driver for the GYROSCOPE vector):
So, just for temporary reference, to get the correct angular velocities for Z, one should do -imu.getAngularVelocity().xRotationRate; and for X, it would be -imu.getAngularVelocity().zRotationRate;?
Can this issue be closed with the release of SDK v8.0?