https://nxttime.wordpress.com/2011/11/22/imu-sensor-software/

By request I will publish the Lejos software for the dIMU sensor.

The software contains of drivers for the Dexter Industries IMU sensor, programs to calibrate the sensor and the IMU filter that I wrote about on this blog before. The filter can be used for other IMU sensors as well. Also included is the sample program that utilizes this filter. It is the very same program I used in the video’s published in a previous post. There is no formal support and no warranty. If you are going to use it you will have to rely on your own wit, the javadoc and on this blog.
Download and install

You can download the software from this link. After downloading you will have to extract it keeping the directory structure intact. Make sure yor compiler can find the code.
Using the IMU drivers

The dIMU consists of two sensors, a gyroscope and a accelerometer, each has it’s own driver. The gyroscope driver is called L3G4200D and the driver for the accelerometer is MMA7455L. (I use the technical names of the sensors a driver names.) On top of these drivers I made user interfaces that allowes you to examine, configure and calibrate the sensors. The user interfaces are called L3G4200D_E and MMA7455L_E respectively. You can use these programs as driver as well. It gives your programs access to the additional functionality but it needs the NXT screen and might result in larger programs. There is a sample program included that gives access to the user interfaces, it is called testDIMU.

This is how you enable the drivers in your code,

SensorPort.S4.i2cEnable(I2CPort.HIGH_SPEED);
MMA7455L accel = new MMA7455L(SensorPort.S4);
L3G4200D gyro = new L3G4200D(SensorPort.S4);

 

The first line instructs Lejos to use high speed I2C. The sensors support this.

This is how you get data (rate, acceleration and tilt) from the sensors.

float[] rate = new float[3];
gyro.fetchAllRate(rate);

float[] accel = new float[3];
accel.fetchAllAccel(accel);

float[] tilt = new float[3];
accel.fetchAllTilt(tilt);

As you can see the drivers return data from all three axis is one call. If you just need data from one axis you can get it from the arrays. The order is X, Y, Z. The gyro returns degrees per second by default. The accelerometer returns Milli-G for acceleration and Degrees for tilt, also by default. If you want to use other units in your programs you can change the default values like this.

gyro.setRateUnit(RateUnits.RPS);
accel.setAccelUnit(AccelUnits.MS2);
accel.setTiltUnit(TiltUnits.RADIANS);

This changes the default units to radians per second, meter per second^2 and radians respectively. Other units are available, check the javaDoc. The default unit can be overridden per data request by specifying the desired unit as second parameter in the FetchAll… methods.
Configuring the sensors

The gyro can be configured for dynamic range and sample rate using the setRange and setSampleRate methods. As a rule one should select the lowest value that your application allows for. This gives data of best possible quality.

The accelerometer cannot be configured. I found this of little use.
Calibrating the sensors

The gyro of my dIMU doesn’t really need calibrating. however there is the possibility to do so. Calibration is started b calling gyro.calculateOffset(). During calibration the sensor should remain motionless. Calibration settings of the gyro are not stored, so they are lost when your program terminates. (Storing calibration settings of gyro sensors is of no use as the calibration values depend on environmental factors and are not stable over time.)

The accelerometer needs calibration. The user interface driver provides functionality to calibrate the sensor and to store the calibration settings. The (base) driver looks for stored calibration settings upon initialization and loads these automatically when they are available. Calibration settings of the accelerometer are stable over time so you’ll need to do this just once. Each of the three axes has to be calibrated separately. To calibrate an axis one has to point it straight up first and hold still while the calibration routine collects data samples. Then the axis has to be pointed straight down and held still for some time. Follow the on screen instructions and do not forget to save the settings after calibration.
Using the IMU filter

The IMU filter can be used with any three-axis gyro and any three-axis accelerometer as long as their drivers implement the RataData and TiltData interfaces. This is how you initialise the filter

NLCFilter attitude = new NLCFilter(gyro, accel);
attitude.start();

The two parameters to the filter constructor are the gyro driver and accelerometer driver. One could leave out the accelerometer, the filter will work but values will drift over time. The second line of code starts the filter. The filter needs 2 to 5 seconds to settle at start up, therefore you need to keep the sensor motionless and more or less level for a few seconds. You can find out if the filter is ready settling with the Initializing() method.

The IMU filter keeps track of the attitude, or pose, of your sensor/robot. You can query the roll, pitch and yaw angles this way

attitude.setTiltUnit(TiltUnits.RADIANS);
float roll=attitude.getRoll();
float pitch=attitude.getPitch();
float yaw=attitude.getYaw();

or

float[] tilt = new float[3];
attitude.fetchAllTilt(tilt);

By default these methods return angles in radians. You can change this by setting a different unit with the setTiltUnit() method.

You can also use the filter to transform data from a body frame to a world frame. This is useful if another sensor returns data that needs to be corrected for the robots current attitude. the next example transforms a distance returned by a UltraSonic sensor to world coordinates. The example assumes the us and IMU sensors are aligned and that the US sensor measures along the X-axis.

Matrix usBody=new Matrix(1,3);
Matrix usWorld=null;
us = new UltrasonicSensor(SensorPort.S1);
usBody.set(0,0,us.getDistance());
usWorld=attitude.transformToWorld(usBody);

The matrix usWorld now contains the distance from sensor to the detected object over the X, Y and Z axis.
Configuring and tuning the IMU filter

By default the IMU filter updates as often as possible. It’s update frequency is over 100 Hertz. To free computer resources one can lower the update frequency by using the setFrequency() method. The filter will try to match the specified frequency. A parameter value of 0 will run the filter at maximum speed.

The filter can be tuned to increase the quality of its output. I advise you not to tune the filter until you are familiar with it and understand its inner workings. Tuning consists of altering the P and I values of it’s internal PI controller. The I value takes care of gyro drift cancellation and the P value controls how fast attitude is corrected by use of tilt data from the accelerometer. The values can be modified by using the setKp() and setKi() methods.

There are two ways the filter can assist you in tuning. It keeps track of the integral of absolute errors, or absI. This is a measure of the total error of the filter over time. The smaller the error (given a fixed period) the better the filter performs. /* The filter also allows you to send data over bluetooth to the pc for examination. For this one has to use the NXTChartingLogger on the pc that is part of the Lejos distribution. You instruct the filter to send its state to the pc by supplying a NXTDataLogger object with the setDataLogger() method. */
Running the demo program

The demo program is called testIMU. At startup of this program the sensor must be horizontal and motionless. The sensor is assumed to be aligned ith the NXT brick with the sensor plug facing to the same direction as the sensor ports. Once you see the wire frame you can start moving the sensor.The demo has four display modes:

Wire frame. Here it shows a wire frame of the sensor on the NXT screen
Rotation matrix. The screen will show the content of the rotation matrix. In this matrix the current attitude is stored. The matrix is also used to convert body coordinates to world coordinates by matrix multiplication..
Roll, Pitch, Yaw. The screen shows the Roll, Pitch, Yaw angles of the sensor.
Update speed. The screen shows the actual update speed of the filter.

You can browse through the display modes by using the arrow keys. The enter key resets the filter. The clip below shows the demo program in action in wire frame mode.