Additional detail for those interested …
Reads Bosch BNO055 Inertial Measurement Unit (IMU) sensors: Rate Gyro, Accelerometer, and Magnetometer. Numerically
integrates Gyro rates to accumulate degree displacement. Reads data fusion outputs.
These IMU subVIs are designed to be used with the AdaFruit 9-DoF Absolute Orientation IMU Breakout - containing a high
speed ARM Cortex-M0 based 32-bit processor running a Bosch sensor fusion algorithm to blend the integrated 3-axis
Accelerometer, Rate Gyroscope, and Magnetometer readings (Product ID: 2472). The fusion algorithm, although
unspecified, is likely a Kalman filter variant.
The RoboBees IMU ORIENT Series subVIs are:
OPEN - Performs IMU initialization, setup, and calibration of each axis of Rate Gyroscope, Accelerometer, and Magnetometer.
The device can be configured to a number of different fusion or non-fusion operating modes.
IMU RefNum Registry Set / Get - These are standard WPI-type functional globals used to pass IMU configuration data.
READ - Reads IMU sensors. Gyro output provides XYZ rotation rates as well as integrating these rates for degree displacements.
Accelerometer output provides XYZ raw acceleration, as well as separating acceleration due to movement (linear)
from acceleration due to gravity. Magnetometer output provides XYZ magnetic field strength and anomaly detection.
Orientation outputs available in both Roll / Pitch / Yaw (Euler degrees) and in quaternions (WXYZ).
Calibration of each sensor is performed, or re-established, by the IMU Open vi. The essential design of this vi is to provide
read operations for a set of indication functions. Each function can also be selectively enabled/disabled by settings passed
in the Read Enable cluster, to allow the user to optimize read cycle times (enabling only what is used). These functions
provide real-time calibration status, orientation in Euler angles and quaternions, rotation rate and angular displacement,
multiple modes of acceleration, and magnetic field strength. The various read functions access the appropriate IMU data
registers over the I2C bus, then data bytes are assembled, and finally scaled by specification sheet sensitivities to put the
values into the correct units. Gyro rates are numerically integrated for rotation displacement, with an available
The on-board fusion algorithm will continue to update the calibration as the sensor is read. If enabled, the Cal Status is updated
in real-time, providing Gyro, Accel, Mag, and System status. One can observe the status change as the sensor is exposed to
movement or non-Earth magnetic fields. Calibration status is provided in a color-coded Boolean Cal Status cluster, where red
indicates a status level of 0 (uncalibrated) and green indicates status level 3 (fully calibrated).
Each sensor (G/A/M) shares the basic design of storing their measurements across two one-byte registers, and the native data
register sequence for each sensor is Little Endian - review this term and its relationship to Gulliver’s Travels by Jonathan Swift
for insight on reference (software engineers have a great sense of humor). Orientation in degrees for Yaw, Roll, and Pitch is
provided in an Orient Y/R/P output cluster. Rotation rates and Degrees displacements are provided in a Gyro X,Y,Z output cluster.
Three modes of acceleration can be alternately indicated, selectable via the Read Enable, Accel Mode input control. Selecting
Raw Data yields 3-axis acceleration measurements that combine acceleration due to movement with reacting forces due to
gravity. (Review Einstein’s Principle of Equivalence, which asserts there is “complete physical equivalence of a gravitational
field and a corresponding acceleration of the reference system”, which is why an accelerometer will always report a composite
of both ‘accelerations’). Selecting Linear as the control value modifies the indication to reflect only acceleration due to
movement, as separated by the fusion algorithm. Similarly, selecting Gravity modifies the indication to reflect the fusion
algorithm’s estimate of only the reacting forces due to gravity. Acceleration values are provided in a Acc X,Y,Z output cluster.
Orientation expressed in quaternions is provided in the Quaternions output cluster for values in W, X, Y, and Z. Not to be
confused with axis-angle representations, the quaternion is an extension of complex numbers, from one of the less mainstream
branches of mathematics. (The quaternion represents a point on the surface of a four-dimensional hypersphere.) These are
unit quaternions, where the root-sum-square of all parameters is one. Quaternions are useful in avoiding gimbal lock and for
smoothly interpolating rotations. The algorithm reportedly performs all internal sensor fusion calculations in quaternions.
Magnetic field strength measurements in all three axes is provided in a Mag Field X,Y,Z cluster. The vector magnitude is
calculated and compared to the expected Earth field for magnetic anomaly detection. One can use the average value displayed
on this vi’s Front Panel (magnitude), which should be consistent with data available at the National Geophysical Data Center :
… input USA zip code or city/country, select Get Lat/Long then Calculate, and note “Total Field” value, expressing this in
microTeslas (not nanoTeslas as NOAA displays). A Mag Field Tolerance value is used as tolerance for the comparison.
I2CDEVREF - Device Reference containing IMU configuration data.
READ ENABLE - Cluster containing switches for each read function, and a control to choose the mode of acceleration data.
GYRO (DISPLACEMENT) RESET - Boolean control to reset gyro 3-axis displacement values to zero. Reset occurs on Rising-Edge.
EARTH FIELD (uT) - Cluster for expected total 3D Earth magnetic field vector magnitude, and tolerance used for anomaly detect.
CAL STATUS - Cluster indicating cal status of accel, rate gyro, mag and system. Status is a ‘real-time’ report of the fusion
algorithm’s belief of the degree of IMU calibration. Level 0 indicates uncalibrated, level 3 indicates fully calibrated.
ORIENT Y, R, P - Cluster containing rotation orientation for Yaw, Roll, and Pitch, in degrees.
GYRO X,Y,Z - Cluster containing rotation rate and displacements for each axis, in degrees/sec and degrees.
ACC X,Y,Z - Cluster containing, for each axis, calibrated acceleration measurements in the selected mode, in mGs.
QUATERNIONS - Cluster containing quaternion parameterization (W,X,Y,Z) of orientation, unitless.
MAG FIELD X,Y,Z - Cluster containing, for each axis, calibrated magnetic field strength, in microTeslas, and an anomaly indicator.
Execution Reentrancy is set to Non-Reentrant Execution
While either of the two roboRIO I2C interfaces can be designated, it is recommended that this software be used only with the
MXP I2C interface (the Rev Robotics ‘More Board’ provides a convenient expansion of the MXP interface). Although the software
is and runs the same for either I2C port, the On-Board I2C interface performs differently, and can cause frequent Lo-byte / Hi-Byte
measurement pairing failures during read operations.
Despite the sensor calibration in IMU Open, neither the Mag cal status, nor the Mag anomaly detection, nor the correspondence
of 0 degrees of Yaw to magnetic North, has been observed to be highly trustworthy during an initial operating period, until the
background calibration process has been allowed to experience some 3-D movement. This does not appear to greatly affect the
quality of the relative orientation readings (Euler Yaw/Roll/Pitch or quaternion).
This design performs numerical integration off-FPGA for the Digital Gyro (unlike the Analog Gyro subVIs, which typically utilize
the FPGA to perform accumulation for superior speed resulting in less integration error). Testing indicates this off-FPGA
approach may be adequate for FRC application, however, it is important to ensure the code runs without any timing issues.
This is why a Timed Loop is recommended for the IMU Read within Periodic Tasks (executes at a higher priority). Non-uniformity
in read iteration is mitigated by design, as each dt is uniquely calculated by time stamping each read event. This means that while
some variation in timing can be tolerated (the occasional late iteration), excessively long durations (periods) between successive
reads will fail to capture the actual rate profile as it changes resulting in integration error.
In short, use a Timed Loop structure to iterate IMU READs, and set the loop period for a relatively fast iteration (<75 ms). Shorter
periods provide improved accuracy of degree displacements, however setting this too short can impact the execution of your
remaining code (especially when using Timed Loops). One way of determining an optimized period is to set it while monitoring
the “Finished Late?” Loop parameter, ensuring this value stays False. It helps if the CPU Utilization remains low (<75%), which
can be monitored with the RT Get CPU Loads subVI (Real Time menu). Study LabVIEW determinism for additional insight.
IMU Orientation Read benchmarks, with roboRIO.
Time to execute IMU Orient Read in milliseconds (ms):
Device … Read (ms)
Cal Status 1
C + O 1-3
O + G 1-3
C + O + G 2-5
Ensure loop time calling this subVI is significantly above listed read execution time (say, >*10) to minimize load on CPU utilization.
----- The RoboBees FRC Team 836! ----- You too can conquer inertia!! … orient on a career in STEM!