Accelerometer Intermittently Gives Incorrect Readings

Hi all,

Our team is experiencing some intermittent issues with reading an accelerometer/gyro sensor. We are using the GY-521 / MPU 6050 (link) to measure the angle of a controllable arm. Specifically, after a certain amount of time, we believe the raw accelerometer readings become offset by a constant amount, giving us an incorrect reading.

We tested this by leaving the robot disabled for some time. Note that we run the sensor updates in a separate thread so the same sensor code is running whether or not the robot is enabled. After some time (though not consistently at all), the angle read by the accelerometer is wrong. Although the accelerometer is still giving readings, when we post the raw accelerometer values, they are clearly wrong (e.g. y value giving greater than 1 g reading when accelerometer is still). When we move the accelerometer, the values change in response in the correct direction, and when we plot the value we receive vs the “expected” value based on the angle, it seems like the accelerometer values are off by a constant (although we only tested 3 points for both x and y). The wrong readings survive a redeploy of the robot code, in the sense that redeploying code does not cause the sensors to revert to giving correct readings. However, a power cycle has consistently restored the sensors ot the zero-offset state. Therefore, we suspect something is being written to the chips by accident.

Here is the class that manages the gyro:
Accel_GY521.java

package frc.robot.sensors.gy521;


import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Watchdog;
import edu.wpi.first.wpilibj.interfaces.Accelerometer;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import frc.robot.sensors.util.I2CUtils;

import java.nio.ByteBuffer;

import static frc.robot.sensors.gy521.GY521_Constants.*;
import static java.util.Objects.requireNonNull;

@SuppressWarnings({"Duplicates", "UnnecessaryLocalVariable", "SameParameterValue"})
public class Accel_GY521  implements Accelerometer, Gyro, Sendable {

    // MARK - complimentary filter initialization
    private static final double kGyroInfluence = 0.98;
    private double previousAngle = 0.0;
    private Timer filterTimer;

    private I2C i2c_conn;
    private Range currRange;
    private int deviceAddr;

    private double xAccelOffset = 0, yAccelOffset = 0;
    private double gyroOffset = 0;

    private static class Calibrator {
        private double[] vals;

    }

    // MARK - watchdog config
    private Watchdog watchdog;
    private static final double kSensorDisconnectTimeout = 3; // sec // todo better name?
    private boolean watchdogEnabled;

    // MARK  - sendable config
    private String sendableName;
    private String subsystemName;

    private boolean isSensorConnected = false;


    // TODO get link to manual


    public Accel_GY521(int address, boolean watchdogEnabled) {
        // device setup
        this.deviceAddr = address;
        this.watchdogEnabled = watchdogEnabled;
        i2c_conn = new I2C(I2C.Port.kOnboard, address);
        currRange = Range.k2G;
        this.resetDevice();
        this.setSleepMode(false);

        // complimentary filter setup
        this.filterTimer = new Timer();

        // watchdog setup
        this.watchdog = new Watchdog(kSensorDisconnectTimeout, this::reconnectDevice);
        this.initWatchdog();

        // sendable setup
        this.sendableName = this.toString(); // default name
    }

    // MARK - instance configuration methods

    @Override
    //sets the range for the accelerometer
    public void setRange(Range range) {
        //Ensures that the range is not null pointer
        requireNonNull(range, "The range that has been entered is invalid");

        ByteBuffer ctrl4Buffer = ByteBuffer.allocate(1);
        i2c_conn.read(ACCEL_RANGE, 1, ctrl4Buffer);

        byte ctrl4 = ctrl4Buffer.get();
        ctrl4 &= ~0x18; // this basically resets only the FS1 and FS0 bits
        ctrl4 |= getSettingFromRange(range) << 3; // this sets only the FS1 and FS0

        i2c_conn.write(ACCEL_RANGE, ctrl4);

        this.currRange = range;
    }

    //gets the resolution
    private int getCurrentResolution() {
        switch (currRange) {
            case k16G:
                return ACCEL_RESOLUTION_16G;
            case k8G:
                return ACCEL_RESOLUTION_8G;
            case k4G:
                return ACCEL_RESOLUTION_4G;
            case k2G:
                return ACCEL_RESOLUTION_2G;

            default:
                throw new IllegalArgumentException("Invalid range given");
        }
    }

    //gets the range requested
    private int getSettingFromRange(Range range) {
        switch (range) {
            case k2G:
                return ACCEL_SETTING_2G;
            case k4G:
                return ACCEL_SETTING_4G;
            case k8G:
                return ACCEL_SETTING_8G;
            case k16G:
                return ACCEL_SETTING_16G;
            default:
                throw new IllegalArgumentException("Invalid range given");
        }
    }


    // MARK - device config methods

    private void setSleepMode(boolean activate) {
        int setting = activate ?
                0b0100_0000  // sleep bit on
                : 0b0000_0000; // otherwise set everything to 0 and enable
        // ref pg 40
        i2c_conn.write(PWR_MGMT_1, setting);
    }

    private void resetDevice() {
        i2c_conn.write(RESET_ADDRESS, RESET_VAL);
    }

    private void setDataRate() {

    }
    // MARK - device misc methods

    /**
     * This method checks whether the sensor is connected
     * @return  whether or not the sensor is connected
     */
    private boolean isConnected() {
        return i2c_conn.verifySensor(WHO_AM_I, 1, WHO_AM_I_DEFAULT);
    }

    public boolean isSensorConnected() {
        return isSensorConnected;
    }


    // MARK - Sensor Robustness / Watchdog Methods
    private void reconnectDevice() {
        System.out.println("Resetting " + this.toString());
        this.resetDevice();
        this.watchdog.reset(); // assume that the sensor recovered
    }

    /**
     * This method explicitly writes our configuration after resetting.
     */
    public void emergencySensorReset() {
        System.out.println("Initializing emergency procedures for GY521 sensor");
        this.resetDevice();
        this.setSleepMode(false);
        i2c_conn.write(0x77, 0x00); // XA offst high low
        i2c_conn.write(0x78, 0x00);
        i2c_conn.write(0x79, 0x00);
        i2c_conn.write(0x80, 0x00);

        // z
        i2c_conn.write(0x81, 0x00);
        i2c_conn.write(0x81, 0x00);


    }


    private void updateWatchdog() {
        // only feed watchdog if the sensor is connected, meaning that it is operational.
        isSensorConnected = this.isConnected();
        if (this.watchdogEnabled) {
            if (isSensorConnected) {
                this.watchdog.reset();
            } else {
                System.err.println("WARNING: Accel GY521 has been disconnected/put into an error state. An attempt to reconnect will be made");
            }
        }
    }

    private void initWatchdog() {
        if (this.watchdogEnabled) {
            this.watchdog.enable();
        } else {
            this.watchdog.disable();
        }
    }


    // MARK - Accelerometer Methods

    // The reason we cast the word to short is because
    // the MPU6050 returns all values in two's complement.
    // Casting to short tells java to interpret the number as such,
    // giving us the actual value.
    @Override
    public double getX() {
        short rawX = (short) I2CUtils.readWord(i2c_conn, ACCEL_XOUT_H);
        double xVal =  (double) rawX / this.getCurrentResolution();
        return xVal;
    }

    @Override
    public double getY() {
        short rawY = (short) I2CUtils.readWord(i2c_conn, ACCEL_YOUT_H);
        double yVal = (double) rawY / this.getCurrentResolution();
        return yVal;
    }

    @Override
    public double getZ() {
        short rawZ = (short) I2CUtils.readWord(i2c_conn, ACCEL_ZOUT_H);
        double zVal = (double) rawZ / this.getCurrentResolution();
        return zVal;
    }

    @Override
    public void calibrate() {
        this.resetDevice();
    }

    @Override
    public void reset() {
        this.resetDevice();
    }


    // MARK - Complimentary filter methods

    @Override
    public double getAngle() {
        return previousAngle;
    }

    /**
     * This method is responsible for updating the sensor readings based on the complimentary filter,
     * as well as keeping the watchdog fed.
     */
    public void update() {
        this.updateWatchdog();
        double accelAngle = this.getAccelAngle();
        double gyroRate = this.getRate();
        previousAngle = (previousAngle + gyroRate * getElapsedTime()) * kGyroInfluence
                + (accelAngle) * (1 - kGyroInfluence);
        // Assumptions:
        // The apparatus that this device is mounted on will be at a steady-state (not shaking around)
        // at startup.

        // This is an implementation of a complementary filter
        // at a high level, it combines the gyro and accelerometer readings to get
        // an accurate reading of the angle.

        // The filter does this by using the accelerometer to get initial reading of the angle, then
        // since the accelerometer gives a more trust worthy absolute measure of angle
        // when it is not subject to acceleration. This is in contrast to the gyro, which gives
        // very accurate angular acceleration readings, but not good steady-state angle readings.
    }

    private double getElapsedTime() {
        double timeElapsed = filterTimer.get();
        filterTimer.reset();
        filterTimer.start();
        return timeElapsed;
    }


    public double getAccelAngle() {
        double y = this.getY(), x = this.getX();
        return Math.toDegrees(Math.atan2(y, x));
    }

    @Override
    public double getRate() {
        short val = (short) I2CUtils.readWord(i2c_conn, GYRO_ZOUT_H);
        return -1 * (double) val / GYRO_SCALE_MODIFIER_250DEG;
    }

    // TODO impelment gyro and accel config modification
    public int getGyroConfig() {
        return I2CUtils.readWord(i2c_conn, GYRO_CONFIG); // GYRO_CFNGI
    }

    public int getAccelConfig() {
        return I2CUtils.readWord(i2c_conn, ACCEL_CONFIG);
    }

    
}
 // some non-crucial parts, such as initSendable() and toString() have been elided.

GY521_Constants.java

package frc.robot.sensors.gy521;

@SuppressWarnings("WeakerAccess")
public class GY521_Constants {
    public static final int WHO_AM_I = 0x75;
    // https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf
    public static final byte[] WHO_AM_I_DEFAULT = new byte[] {
//      0b01101000
      0x68 // page 45
    };
    public static final byte RESET_DEVICE = 0b00000111;
    public static final double STANDARD_GRAVITY = 9.806;

    public static final int RESET_ADDRESS = 0x68;
    public static final int RESET_VAL = 0x07;
    public static final int ACCEL_RANGE = 0x1C;


    public static final int ACCEL_SETTING_2G = 0b00,
                            ACCEL_SETTING_4G = 0b01,
                            ACCEL_SETTING_8G = 0b10,
                            ACCEL_SETTING_16G = 0b11;

    public static final int ACCEL_RESOLUTION_2G = 16384,
                            ACCEL_RESOLUTION_4G = 8192,
                            ACCEL_RESOLUTION_8G = 4096,
                            ACCEL_RESOLUTION_16G = 2048;

    public static final int GYRO_SETTING_250 = 0b00,
                            GYRO_SETTING_500 = 0b01,
                            GYRO_SETTING_1000 = 0b10,
                            GYRO_SETTING_2000 = 0b11;

    // MARK - accelerometer address values
    public static final int ACCEL_XOUT_H = 0x3B,
                            ACCEL_YOUT_H = 0x3D,
                            ACCEL_ZOUT_H = 0x3F;

    // MARK - gyroscope address values
    // TODO implement x and y address
    public static final int GYRO_ZOUT_H = 0x47;

    // MARK - Gyro Scale Modifiers
    public static final double GYRO_SCALE_MODIFIER_250DEG = 131.0,
                               GYRO_SCALE_MODIFIER_500DEG = 65.5,
                               GYRO_SCALE_MODIFIER_1000DEG = 32.8,
                               GYRO_SCALE_MODIFIER_2000DEG = 16.4;

    // MARK - Configuration addresses
    public static final int GYRO_CONFIG  = 0x1B,
                            ACCEL_CONFIG = 0x1C;


    public static final int PWR_MGMT_1 = 0x6B;
}

I2CUtils.java

package frc.robot.sensors.util;

import edu.wpi.first.wpilibj.I2C;

import java.nio.ByteOrder;

import static java.util.Objects.requireNonNull;

/**
 * This is a collection of pure static functions that help make dealing with I2C a little bit easier
 */
public final class I2CUtils {

    /**
     * Returns a 2 byte value (word) that starts at the specified address.
     * @param connection - The I2C connection of the desired device
     * @param address - The starting address of the word
     * @param order - The endianness of the bytes that will be read.
     * @return the 2 byte value that was requested at the specified address
     */
    public static int readWord(I2C connection, int address, ByteOrder order) {
        requireNonNull(connection);
        requireNonNull(order);

        // todo check endianness/order of reading
        // TODO Refactor this
//        byte[] highByte = new byte[1];
//        byte[] lowByte = new byte[1];
//
//        if (order.equals(ByteOrder.BIG_ENDIAN)) {
//            connection.read(address, 1, highByte);
//            connection.read(address + 1, 1, lowByte);
//        } else if (order.equals(ByteOrder.LITTLE_ENDIAN)){
//            connection.read(address, 1, lowByte);
//            connection.read(address + 1, 1, highByte);
//        }
        byte[] bts = new byte[2];
        connection.read(address, 2, bts);


        int high = bts[0] & 0xFF; // (<byte> & 0xFF) converts a signed byte into an unsigned byte
        int low = bts[1] & 0xFF;

        return (high << 8) + low; // move high bits to high bits position, and add in low bits
                                  // this essentially creates a 2 byte value (referred to as a word)

    }

    public static int readWord(I2C connection, int address) {
        return readWord(connection, address, ByteOrder.BIG_ENDIAN); // High -> Low
    }

    public static short asTwosComplement(int word) {
        return (short) word;
    }

}

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.