.Net Class for Invensnese ICM-20948

matthewflynn_ · 2024-10-10T08:37:33.173Z

I've written the attach class to get the accelerometer and gyro data from the ICM-20948 unit. Unfortunately the data fluctuates massively when I am trying to do a basic calibration. Can anyone help me? Am I getting the reading correctly or is it my calibration that is off. The unit is by itself flat in another room with not noise or anything there. My current usage is ``` var i2cSettings = new I2cConnectionSettings(105,1); var i2cDevice = I2cDevice.Create(i2cSettings); _icm20948 = new Icm20948(i2cDevice); // Set bandwidth for accelerometer and gyroscope var bandwidth = 50; _icm20948.SetBandwidth(bandwidth, bandwidth); ``` For completeness from the pi ``` sudo i2cdetect -y 1 # for I2C bus 1 0 1 2 3 4 5 6 7 8 9 a b c d e f 00: -- -- -- -- -- -- -- -- 10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 60: -- -- -- -- -- -- -- -- -- 69 -- -- -- -- -- -- 70: -- -- -- -- -- -- -- -- ```

M

matthewflynn_OP•10/10/24, 8:38 AM

Also here is a copy of the sample data I have.

sample_data_icm20948_-_1000.txt107.19KB

M

matthewflynn_OP•10/10/24, 11:13 AM

I have updated my class with the following tweaks to the calibration:

Median adjustments
Moving average adjustments
low pass filter adjustments

Icm20948.cs17.86KB

M

matthewflynn_OP•10/10/24, 11:13 AM

I have added in bandwidth testing as well, so when setting up I am effectively calling;

var i2cSettings = new I2cConnectionSettings(i2cBusId, i2cAddress);
var i2cDevice = I2cDevice.Create(i2cSettings);
_icm20948 = new Icm20948(i2cDevice);  // Initialize the ICM20948

// Set bandwidth for accelerometer and gyroscope
var result = _icm20948.FindOptimalBandwidthAsync().Result;

_icm20948.SetBandwidth(result.bestBandwidth, result.bestBandwidth);

var i2cSettings = new I2cConnectionSettings(i2cBusId, i2cAddress);
var i2cDevice = I2cDevice.Create(i2cSettings);
_icm20948 = new Icm20948(i2cDevice);  // Initialize the ICM20948

// Set bandwidth for accelerometer and gyroscope
var result = _icm20948.FindOptimalBandwidthAsync().Result;

_icm20948.SetBandwidth(result.bestBandwidth, result.bestBandwidth);

var i2cSettings = new I2cConnectionSettings(i2cBusId, i2cAddress);
var i2cDevice = I2cDevice.Create(i2cSettings);
_icm20948 = new Icm20948(i2cDevice);  // Initialize the ICM20948

// Set bandwidth for accelerometer and gyroscope
var result = _icm20948.FindOptimalBandwidthAsync().Result;

_icm20948.SetBandwidth(result.bestBandwidth, result.bestBandwidth);

var i2cSettings = new I2cConnectionSettings(i2cBusId, i2cAddress);
var i2cDevice = I2cDevice.Create(i2cSettings);
_icm20948 = new Icm20948(i2cDevice);  // Initialize the ICM20948

// Set bandwidth for accelerometer and gyroscope
var result = _icm20948.FindOptimalBandwidthAsync().Result;

_icm20948.SetBandwidth(result.bestBandwidth, result.bestBandwidth);

M

matthewflynn_OP•10/10/24, 11:13 AM

Also when getting the data i now get a sample of 10 values

public async Task<AccelerometerData> GetDataAsync()
{
    if (_icm20948 == null)
    {
        throw new InvalidOperationException("ICM20948 is not initialized.");
    }

    int numReadings = 10;

    Vector3 accelTotal = new Vector3(0, 0, 0);
    Vector3 gyroTotal = new Vector3(0, 0, 0);

    for (int i = 0; i < numReadings; i++)
    {
        var acceleration = _icm20948.GetAccelerometer();
        var angularVelocity = _icm20948.GetGyroscope();

        accelTotal += acceleration;
        gyroTotal += angularVelocity;

        await Task.Delay(10); // Small delay between readings
    }

    Vector3 accelAverage = accelTotal / numReadings;
    Vector3 gyroAverage = gyroTotal / numReadings;

    var pitch = CalculatePitch(accelAverage);
    var roll = CalculateRoll(accelAverage);

    var currentTime = DateTime.Now;
    var deltaTime = (currentTime - _lastUpdate).TotalSeconds;
    _lastUpdate = currentTime;

    _previousYaw += gyroAverage.Z * deltaTime;
    var yaw = _previousYaw;

    _logger.LogInformation($"Data from ICM20948 is Pitch:{pitch}, Roll:{roll}, Yaw:{yaw}.");

    return new AccelerometerData
    {
        Pitch = pitch,
        Roll = roll,
        Yaw = yaw
    };
}

public async Task<AccelerometerData> GetDataAsync()
{
    if (_icm20948 == null)
    {
        throw new InvalidOperationException("ICM20948 is not initialized.");
    }

    int numReadings = 10;

    Vector3 accelTotal = new Vector3(0, 0, 0);
    Vector3 gyroTotal = new Vector3(0, 0, 0);

    for (int i = 0; i < numReadings; i++)
    {
        var acceleration = _icm20948.GetAccelerometer();
        var angularVelocity = _icm20948.GetGyroscope();

        accelTotal += acceleration;
        gyroTotal += angularVelocity;

        await Task.Delay(10); // Small delay between readings
    }

    Vector3 accelAverage = accelTotal / numReadings;
    Vector3 gyroAverage = gyroTotal / numReadings;

    var pitch = CalculatePitch(accelAverage);
    var roll = CalculateRoll(accelAverage);

    var currentTime = DateTime.Now;
    var deltaTime = (currentTime - _lastUpdate).TotalSeconds;
    _lastUpdate = currentTime;

    _previousYaw += gyroAverage.Z * deltaTime;
    var yaw = _previousYaw;

    _logger.LogInformation($"Data from ICM20948 is Pitch:{pitch}, Roll:{roll}, Yaw:{yaw}.");

    return new AccelerometerData
    {
        Pitch = pitch,
        Roll = roll,
        Yaw = yaw
    };
}

public async Task<AccelerometerData> GetDataAsync()
{
    if (_icm20948 == null)
    {
        throw new InvalidOperationException("ICM20948 is not initialized.");
    }

    int numReadings = 10;

    Vector3 accelTotal = new Vector3(0, 0, 0);
    Vector3 gyroTotal = new Vector3(0, 0, 0);

    for (int i = 0; i < numReadings; i++)
    {
        var acceleration = _icm20948.GetAccelerometer();
        var angularVelocity = _icm20948.GetGyroscope();

        accelTotal += acceleration;
        gyroTotal += angularVelocity;

        await Task.Delay(10); // Small delay between readings
    }

    Vector3 accelAverage = accelTotal / numReadings;
    Vector3 gyroAverage = gyroTotal / numReadings;

    var pitch = CalculatePitch(accelAverage);
    var roll = CalculateRoll(accelAverage);

    var currentTime = DateTime.Now;
    var deltaTime = (currentTime - _lastUpdate).TotalSeconds;
    _lastUpdate = currentTime;

    _previousYaw += gyroAverage.Z * deltaTime;
    var yaw = _previousYaw;

    _logger.LogInformation($"Data from ICM20948 is Pitch:{pitch}, Roll:{roll}, Yaw:{yaw}.");

    return new AccelerometerData
    {
        Pitch = pitch,
        Roll = roll,
        Yaw = yaw
    };
}

public async Task<AccelerometerData> GetDataAsync()
{
    if (_icm20948 == null)
    {
        throw new InvalidOperationException("ICM20948 is not initialized.");
    }

    int numReadings = 10;

    Vector3 accelTotal = new Vector3(0, 0, 0);
    Vector3 gyroTotal = new Vector3(0, 0, 0);

    for (int i = 0; i < numReadings; i++)
    {
        var acceleration = _icm20948.GetAccelerometer();
        var angularVelocity = _icm20948.GetGyroscope();

        accelTotal += acceleration;
        gyroTotal += angularVelocity;

        await Task.Delay(10); // Small delay between readings
    }

    Vector3 accelAverage = accelTotal / numReadings;
    Vector3 gyroAverage = gyroTotal / numReadings;

    var pitch = CalculatePitch(accelAverage);
    var roll = CalculateRoll(accelAverage);

    var currentTime = DateTime.Now;
    var deltaTime = (currentTime - _lastUpdate).TotalSeconds;
    _lastUpdate = currentTime;

    _previousYaw += gyroAverage.Z * deltaTime;
    var yaw = _previousYaw;

    _logger.LogInformation($"Data from ICM20948 is Pitch:{pitch}, Roll:{roll}, Yaw:{yaw}.");

    return new AccelerometerData
    {
        Pitch = pitch,
        Roll = roll,
        Yaw = yaw
    };
}

M

matthewflynn_OP•10/10/24, 11:13 AM

where

    private double CalculatePitch(Vector3 acceleration) =>
     Math.Atan2(acceleration.X, Math.Sqrt(acceleration.Y * acceleration.Y + acceleration.Z * acceleration.Z)) * (180.0 / Math.PI);

    private double CalculateRoll(Vector3 acceleration) =>
        Math.Atan2(acceleration.Y, Math.Sqrt(acceleration.X * acceleration.X + acceleration.Z * acceleration.Z)) * (180.0 / Math.PI);

    private double CalculateYaw(Vector3 angularVelocity, double deltaTime, ref double previousYaw)
    {
        previousYaw += angularVelocity.Z * deltaTime;
        return previousYaw;
    }

    private double CalculatePitch(Vector3 acceleration) =>
     Math.Atan2(acceleration.X, Math.Sqrt(acceleration.Y * acceleration.Y + acceleration.Z * acceleration.Z)) * (180.0 / Math.PI);

    private double CalculateRoll(Vector3 acceleration) =>
        Math.Atan2(acceleration.Y, Math.Sqrt(acceleration.X * acceleration.X + acceleration.Z * acceleration.Z)) * (180.0 / Math.PI);

    private double CalculateYaw(Vector3 angularVelocity, double deltaTime, ref double previousYaw)
    {
        previousYaw += angularVelocity.Z * deltaTime;
        return previousYaw;
    }

    private double CalculatePitch(Vector3 acceleration) =>
     Math.Atan2(acceleration.X, Math.Sqrt(acceleration.Y * acceleration.Y + acceleration.Z * acceleration.Z)) * (180.0 / Math.PI);

    private double CalculateRoll(Vector3 acceleration) =>
        Math.Atan2(acceleration.Y, Math.Sqrt(acceleration.X * acceleration.X + acceleration.Z * acceleration.Z)) * (180.0 / Math.PI);

    private double CalculateYaw(Vector3 angularVelocity, double deltaTime, ref double previousYaw)
    {
        previousYaw += angularVelocity.Z * deltaTime;
        return previousYaw;
    }

    private double CalculatePitch(Vector3 acceleration) =>
     Math.Atan2(acceleration.X, Math.Sqrt(acceleration.Y * acceleration.Y + acceleration.Z * acceleration.Z)) * (180.0 / Math.PI);

    private double CalculateRoll(Vector3 acceleration) =>
        Math.Atan2(acceleration.Y, Math.Sqrt(acceleration.X * acceleration.X + acceleration.Z * acceleration.Z)) * (180.0 / Math.PI);

    private double CalculateYaw(Vector3 angularVelocity, double deltaTime, ref double previousYaw)
    {
        previousYaw += angularVelocity.Z * deltaTime;
        return previousYaw;
    }

Similar Threads

Similar Threads

Similar Threads