diff --git a/main.go b/main.go index aa82a00..1324513 100644 --- a/main.go +++ b/main.go @@ -4,7 +4,7 @@ package main import ( "context" - "tdk-invensense/mpu6050" + "tdk-invensense/mpu" "go.viam.com/rdk/components/movementsensor" "go.viam.com/rdk/logging" @@ -22,7 +22,11 @@ func mainWithArgs(ctx context.Context, args []string, logger logging.Logger) err return err } - if err = module.AddModelFromRegistry(ctx, movementsensor.API, mpu6050.Model); err != nil { + if err = module.AddModelFromRegistry(ctx, movementsensor.API, mpu.Model6050); err != nil { + return err + } + + if err = module.AddModelFromRegistry(ctx, movementsensor.API, mpu.Model9250); err != nil { return err } diff --git a/mpu6050/mpu6050.go b/mpu/mpu6050.go similarity index 85% rename from mpu6050/mpu6050.go rename to mpu/mpu6050.go index 4e4c1c0..f64b473 100644 --- a/mpu6050/mpu6050.go +++ b/mpu/mpu6050.go @@ -18,7 +18,7 @@ // // If you use the alternate address, your config file for this component must set its // "use_alternate_i2c_address" boolean to true. -package mpu6050 +package mpu import ( "context" @@ -39,7 +39,7 @@ import ( ) // Model for viam supported tdk-invensense mpu6050 movement sensor. -var Model = resource.NewModel("viam", "tdk-invensense", "mpu6050") +var Model6050 = resource.NewModel("viam", "tdk-invensense", "mpu6050") const ( defaultAddressRegister = 117 @@ -65,16 +65,17 @@ func (conf *Config) Validate(path string) ([]string, error) { } func init() { - resource.RegisterComponent(movementsensor.API, Model, resource.Registration[movementsensor.MovementSensor, *Config]{ + resource.RegisterComponent(movementsensor.API, Model6050, resource.Registration[movementsensor.MovementSensor, *Config]{ Constructor: newMpu6050, }) } -type mpu6050 struct { +type mpu struct { resource.Named resource.AlwaysRebuild bus buses.I2C i2cAddress byte + magAddress byte mu sync.Mutex // The 3 things we can measure: lock the mutex before reading or writing these. @@ -138,7 +139,7 @@ func makeMpu6050( } logger.CDebugf(ctx, "Using address %d for MPU6050 sensor", address) - sensor := &mpu6050{ + sensor := &mpu{ Named: conf.ResourceName().AsNamed(), bus: bus, i2cAddress: address, @@ -206,7 +207,7 @@ func makeMpu6050( return sensor, nil } -func (mpu *mpu6050) readByte(ctx context.Context, register byte) (byte, error) { +func (mpu *mpu) readByte(ctx context.Context, register byte) (byte, error) { result, err := mpu.readBlock(ctx, register, 1) if err != nil { return 0, err @@ -214,7 +215,7 @@ func (mpu *mpu6050) readByte(ctx context.Context, register byte) (byte, error) { return result[0], err } -func (mpu *mpu6050) readBlock(ctx context.Context, register byte, length uint8) ([]byte, error) { +func (mpu *mpu) readBlock(ctx context.Context, register byte, length uint8) ([]byte, error) { handle, err := mpu.bus.OpenHandle(mpu.i2cAddress) if err != nil { return nil, err @@ -230,7 +231,7 @@ func (mpu *mpu6050) readBlock(ctx context.Context, register byte, length uint8) return results, err } -func (mpu *mpu6050) writeByte(ctx context.Context, register, value byte) error { +func (mpu *mpu) writeByte(ctx context.Context, register, value byte) error { handle, err := mpu.bus.OpenHandle(mpu.i2cAddress) if err != nil { return err @@ -280,17 +281,17 @@ func toLinearAcceleration(data []byte) r3.Vector { } } -func (mpu *mpu6050) AngularVelocity(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { +func (mpu *mpu) AngularVelocity(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { mpu.mu.Lock() defer mpu.mu.Unlock() return mpu.angularVelocity, mpu.err.Get() } -func (mpu *mpu6050) LinearVelocity(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { +func (mpu *mpu) LinearVelocity(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { return r3.Vector{}, movementsensor.ErrMethodUnimplementedLinearVelocity } -func (mpu *mpu6050) LinearAcceleration(ctx context.Context, exta map[string]interface{}) (r3.Vector, error) { +func (mpu *mpu) LinearAcceleration(ctx context.Context, exta map[string]interface{}) (r3.Vector, error) { mpu.mu.Lock() defer mpu.mu.Unlock() @@ -301,23 +302,23 @@ func (mpu *mpu6050) LinearAcceleration(ctx context.Context, exta map[string]inte return mpu.linearAcceleration, nil } -func (mpu *mpu6050) Orientation(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { +func (mpu *mpu) Orientation(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { return spatialmath.NewOrientationVector(), movementsensor.ErrMethodUnimplementedOrientation } -func (mpu *mpu6050) CompassHeading(ctx context.Context, extra map[string]interface{}) (float64, error) { +func (mpu *mpu) CompassHeading(ctx context.Context, extra map[string]interface{}) (float64, error) { return 0, movementsensor.ErrMethodUnimplementedCompassHeading } -func (mpu *mpu6050) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { +func (mpu *mpu) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { return geo.NewPoint(0, 0), 0, movementsensor.ErrMethodUnimplementedPosition } -func (mpu *mpu6050) Accuracy(ctx context.Context, extra map[string]interface{}) (*movementsensor.Accuracy, error) { +func (mpu *mpu) Accuracy(ctx context.Context, extra map[string]interface{}) (*movementsensor.Accuracy, error) { return movementsensor.UnimplementedOptionalAccuracies(), nil } -func (mpu *mpu6050) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { +func (mpu *mpu) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { mpu.mu.Lock() defer mpu.mu.Unlock() @@ -329,14 +330,14 @@ func (mpu *mpu6050) Readings(ctx context.Context, extra map[string]interface{}) return readings, mpu.err.Get() } -func (mpu *mpu6050) Properties(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { +func (mpu *mpu) Properties(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { return &movementsensor.Properties{ AngularVelocitySupported: true, LinearAccelerationSupported: true, }, nil } -func (mpu *mpu6050) Close(ctx context.Context) error { +func (mpu *mpu) Close(ctx context.Context) error { mpu.workers.Stop() mpu.mu.Lock() diff --git a/mpu6050/mpu6050_test.go b/mpu/mpu6050_test.go similarity index 99% rename from mpu6050/mpu6050_test.go rename to mpu/mpu6050_test.go index 561d70a..a96166f 100644 --- a/mpu6050/mpu6050_test.go +++ b/mpu/mpu6050_test.go @@ -1,6 +1,6 @@ //go:build linux -package mpu6050 +package mpu import ( "context" diff --git a/mpu/mpu9250.go b/mpu/mpu9250.go new file mode 100644 index 0000000..38e4c06 --- /dev/null +++ b/mpu/mpu9250.go @@ -0,0 +1,307 @@ +//go:build linux + +// Package mpu implements the movementsensor interface for an MPU-9250 6-axis accelerometer. A +// datasheet for this chip is at +// https://invensense.tdk.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf and a +// description of the I2C registers is at +// https://invensense.tdk.com/wp-content/uploads/2015/02/RM-MPU-9250A-00-v1.6.pdf +// +// We support reading the accelerometer, gyroscope, and thermometer data off of the chip. We do not +// yet support reading the magnetometer +// +// The chip has two possible I2C addresses, which can be selected by wiring the AD0 pin to either +// hot or ground: +// - if AD0 is wired to ground, it uses the default I2C address of 0x71 +// - if AD0 is wired to hot, it uses the alternate I2C address of 0x69 +// +// If you use the alternate address, your config file for this component must set its +// "use_alternate_i2c_address" boolean to true. +package mpu + +import ( + "context" + "time" + + "github.com/golang/geo/r3" + "github.com/pkg/errors" + "go.viam.com/rdk/components/board/genericlinux/buses" + "go.viam.com/rdk/components/movementsensor" + "go.viam.com/rdk/logging" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/utils" + goutils "go.viam.com/utils" +) + +var ( + // Model for viam supported tdk-invensense mpu9250 movement sensor. + Model9250 = resource.NewModel("viam", "tdk-invensense", "mpu9250") + + // scales for various readings. + accelScale float64 + gyroScale float64 +) + +const ( + expectedConfigurationReadAddress = 0x71 + magnetometerAddress = 0x0C + magnetometerWhoAmI = 0x00 + magnetometerWhoAmIReturn = 0x48 +) + +func init() { + resource.RegisterComponent(movementsensor.API, Model9250, resource.Registration[movementsensor.MovementSensor, *Config]{ + Constructor: newMpu9250, + }) +} + +// newMpu9250 constructs a new Mpu9250 object. +func newMpu9250( + ctx context.Context, + deps resource.Dependencies, + conf resource.Config, + logger logging.Logger, +) (movementsensor.MovementSensor, error) { + newConf, err := resource.NativeConfig[*Config](conf) + if err != nil { + return nil, err + } + + bus, err := buses.NewI2cBus(newConf.I2cBus) + if err != nil { + return nil, err + } + return makeMpu9250(ctx, deps, conf, logger, bus) +} + +// This function is separated from NewMpu9250 solely so you can inject a mock I2C bus in tests. +func makeMpu9250( + ctx context.Context, + _ resource.Dependencies, + conf resource.Config, + logger logging.Logger, + bus buses.I2C, +) (movementsensor.MovementSensor, error) { + newConf, err := resource.NativeConfig[*Config](conf) + if err != nil { + return nil, err + } + + var address byte + if newConf.UseAlternateI2CAddress { + address = alternateAddress + } else { + address = expectedDefaultAddress + } + logger.CDebugf(ctx, "Using address %d for MPU9250 sensor", address) + + sensor := &mpu{ + Named: conf.ResourceName().AsNamed(), + bus: bus, + i2cAddress: address, + magAddress: magnetometerAddress, + logger: logger, + // On overloaded boards, the I2C bus can become flaky. Only report errors if at least 5 of + // the last 10 attempts to talk to the device have failed. + err: movementsensor.NewLastError(10, 5), + } + + // To check that we're able to talk to the chip, we should be able to read register 117 and get + // back the device's expected configuration read address (0x71). + defaultAddress, err := sensor.readByte(ctx, defaultAddressRegister) + if err != nil { + return nil, addressReadError(err, address, newConf.I2cBus) + } + if defaultAddress != expectedConfigurationReadAddress { + return nil, unexpectedDeviceError(address, defaultAddress) + } + + // The chip starts out in standby mode (the Sleep bit in the power management register defaults + // to 1). Set it to measurement mode (by turning off the Sleep bit) so we can get data from it. + // To do this, we set register 107 to 0. + err = sensor.writeByte(ctx, 107, 0) + if err != nil { + return nil, errors.Errorf("Unable to wake up MPU9250: '%s'", err.Error()) + } + + // enable passthrough + err = sensor.writeByte(ctx, 37, 0x22) + if err != nil { + return nil, errors.Errorf("Unable to enable passthrough: '%s'", err.Error()) + } + logger.Error("enabled passthrough successfully") + + err = sensor.writeByte(ctx, 38, 0x01) + if err != nil { + return nil, errors.Errorf("Unable to enable passthrough: '%s'", err.Error()) + } + logger.Error("enabled passthrough successfully 2") + + // // read pass through status + // passthroughStatus, err := sensor.readByte(ctx, defaultAddressRegister) + // if err != nil { + // return nil, errors.Errorf("Unable to read passthrough status: '%s'", err.Error()) + // } + // logger.Errorf("PASSTHROUGH STATUS = %v", passthroughStatus>>7) + + // read who am i magnetometer + defaultMagAddress, err := sensor.readMagByte(ctx, magnetometerWhoAmI) + if defaultMagAddress != magnetometerWhoAmIReturn { + logger.Errorf("mag address wrong. expected %v, got %v", magnetometerWhoAmIReturn, defaultMagAddress) + } + + // set measurement scales + gyroScale, accelScale, err = sensor.getReadingScales(ctx) + if err != nil { + return nil, err + } + + // Now, turn on the background goroutine that constantly reads from the chip and stores data in + // the object we created. + sensor.workers = goutils.NewBackgroundStoppableWorkers(func(cancelCtx context.Context) { + // Reading data a thousand times per second is probably fast enough. + timer := time.NewTicker(time.Millisecond) + defer timer.Stop() + + for { + select { + case <-timer.C: + rawData, err := sensor.readBlock(cancelCtx, 59, 14) + // Record `err` no matter what: even if it's nil, that's useful information. + sensor.err.Set(err) + if err != nil { + sensor.logger.CErrorf(ctx, "error reading MPU9250 sensor: '%s'", err) + continue + } + + linearAcceleration := toLinearAcceleration(rawData[0:6]) + temperature := float64(utils.Int16FromBytesBE(rawData[6:8]))/333.87 + 21.0 + angularVelocity := toAngularVelocity(rawData[8:14]) + + // Lock the mutex before modifying the state within the object. By keeping the mutex + // unlocked for everything else, we maximize the time when another thread can read the + // values. + sensor.mu.Lock() + sensor.linearAcceleration = linearAcceleration + sensor.temperature = temperature + sensor.angularVelocity = angularVelocity + sensor.mu.Unlock() + case <-cancelCtx.Done(): + return + } + } + }) + + return sensor, nil +} + +func (mpu *mpu) readMagByte(ctx context.Context, register byte) (byte, error) { + result, err := mpu.readMagBlock(ctx, register, 1) + if err != nil { + return 0, err + } + return result[0], err +} + +func (mpu *mpu) readMagBlock(ctx context.Context, register byte, length uint8) ([]byte, error) { + handle, err := mpu.bus.OpenHandle(mpu.magAddress) + if err != nil { + return nil, err + } + defer func() { + err := handle.Close() + if err != nil { + mpu.logger.CError(ctx, err) + } + }() + + results, err := handle.ReadBlockData(ctx, register, length) + return results, err +} + +func (mpu *mpu) writeMagByte(ctx context.Context, register, value byte) error { + handle, err := mpu.bus.OpenHandle(mpu.magAddress) + if err != nil { + return err + } + defer func() { + err := handle.Close() + if err != nil { + mpu.logger.CError(ctx, err) + } + }() + + return handle.WriteByteData(ctx, register, value) +} + +func (mpu *mpu) getReadingScales(ctx context.Context) (float64, float64, error) { + var gyroScale, accelScale float64 + // get gyroscope scale + result, err := mpu.readByte(ctx, 27) + if err != nil { + return 0, 0, err + } + switch result { + case 0o0: + gyroScale = 250.0 / 32768.0 + case 0o1: + gyroScale = 500.0 / 32768.0 + case 10: + gyroScale = 1000.0 / 32768.0 + case 11: + gyroScale = 2000.0 / 32768.0 + default: + } + + // get accelerometer scale + result, err = mpu.readByte(ctx, 28) + if err != nil { + return 0, 0, err + } + switch result { + case 0o0: + accelScale = 2.0 / 32768.0 + case 0o1: + accelScale = 4.0 / 32768.0 + case 10: + accelScale = 8.0 / 32768.0 + case 11: + accelScale = 16.0 / 32768.0 + default: + } + return gyroScale, accelScale, nil +} + +// Given a value, scales it so that the range of int16s becomes the range of +/- maxValue. +func setScale9250(value int, maxValue float64) float64 { + return float64(value) * maxValue +} + +// A helper function to abstract out shared code: takes 6 bytes and gives back AngularVelocity, in +// radians per second. +func toAngularVelocity9250(data []byte) spatialmath.AngularVelocity { + gx := int(utils.Int16FromBytesBE(data[0:2])) + gy := int(utils.Int16FromBytesBE(data[2:4])) + gz := int(utils.Int16FromBytesBE(data[4:6])) + + // gyroScale is the maximum degrees per second measurable + return spatialmath.AngularVelocity{ + X: setScale9250(gx, gyroScale), + Y: setScale9250(gy, gyroScale), + Z: setScale9250(gz, gyroScale), + } +} + +// A helper function that takes 6 bytes and gives back linear acceleration. +func toLinearAcceleration9250(data []byte) r3.Vector { + x := int(utils.Int16FromBytesBE(data[0:2])) + y := int(utils.Int16FromBytesBE(data[2:4])) + z := int(utils.Int16FromBytesBE(data[4:6])) + + // The scale is +/- X Gs based on the calculated accelScale, but our units should be m/sec/sec. + return r3.Vector{ + X: setScale9250(x, accelScale) * 9.81, + Y: setScale9250(y, accelScale) * 9.81, + Z: setScale9250(z, accelScale) * 9.81, + } +} diff --git a/mpu/mpu_nonlinux.go b/mpu/mpu_nonlinux.go new file mode 100644 index 0000000..c4a4750 --- /dev/null +++ b/mpu/mpu_nonlinux.go @@ -0,0 +1,2 @@ +// Package mpu is only implemented for Linux systems. +package mpu diff --git a/mpu6050/mpu6050_nonlinux.go b/mpu6050/mpu6050_nonlinux.go deleted file mode 100644 index 9087d5a..0000000 --- a/mpu6050/mpu6050_nonlinux.go +++ /dev/null @@ -1,2 +0,0 @@ -// Package mpu6050 is only implemented for Linux systems. -package mpu6050 diff --git a/mpu9250/mpu9250_nonlinux.go b/mpu9250/mpu9250_nonlinux.go new file mode 100644 index 0000000..11c4006 --- /dev/null +++ b/mpu9250/mpu9250_nonlinux.go @@ -0,0 +1,2 @@ +// Package mpu9250 is only implemented for Linux systems. +package mpu9250 diff --git a/mpu9250/mpu9250_test.go b/mpu9250/mpu9250_test.go new file mode 100644 index 0000000..c932f7f --- /dev/null +++ b/mpu9250/mpu9250_test.go @@ -0,0 +1,262 @@ +//go:build linux + +package mpu9250 + +import ( + "context" + "testing" + + "github.com/pkg/errors" + "go.viam.com/rdk/components/board/genericlinux/buses" + "go.viam.com/rdk/components/movementsensor" + "go.viam.com/rdk/logging" + "go.viam.com/rdk/resource" + "go.viam.com/rdk/testutils/inject" + "go.viam.com/test" + "go.viam.com/utils/testutils" +) + +func TestValidateConfig(t *testing.T) { + cfg := Config{} + deps, err := cfg.Validate("path") + expectedErr := resource.NewConfigValidationFieldRequiredError("path", "i2c_bus") + test.That(t, err, test.ShouldBeError, expectedErr) + test.That(t, deps, test.ShouldBeEmpty) +} + +func TestInitializationFailureOnChipCommunication(t *testing.T) { + logger := logging.NewTestLogger(t) + i2cName := "i2c" + + t.Run("fails on read error", func(t *testing.T) { + cfg := resource.Config{ + Name: "movementsensor", + Model: Model, + API: movementsensor.API, + ConvertedAttributes: &Config{ + I2cBus: i2cName, + }, + } + i2cHandle := &inject.I2CHandle{} + readErr := errors.New("read error") + i2cHandle.ReadBlockDataFunc = func(ctx context.Context, register byte, numBytes uint8) ([]byte, error) { + if register == defaultAddressRegister { + return nil, readErr + } + return []byte{}, nil + } + i2cHandle.CloseFunc = func() error { return nil } + i2c := &inject.I2C{} + i2c.OpenHandleFunc = func(addr byte) (buses.I2CHandle, error) { + return i2cHandle, nil + } + + deps := resource.Dependencies{} + sensor, err := makeMpu9250(context.Background(), deps, cfg, logger, i2c) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, addressReadError(readErr, expectedDefaultAddress, i2cName)) + test.That(t, sensor, test.ShouldBeNil) + }) + + t.Run("fails on unexpected address", func(t *testing.T) { + cfg := resource.Config{ + Name: "movementsensor", + Model: Model, + API: movementsensor.API, + ConvertedAttributes: &Config{ + I2cBus: i2cName, + UseAlternateI2CAddress: true, + }, + } + i2cHandle := &inject.I2CHandle{} + i2cHandle.ReadBlockDataFunc = func(ctx context.Context, register byte, numBytes uint8) ([]byte, error) { + if register == defaultAddressRegister { + return []byte{0x64}, nil + } + return nil, errors.New("unexpected register") + } + i2cHandle.CloseFunc = func() error { return nil } + i2c := &inject.I2C{} + i2c.OpenHandleFunc = func(addr byte) (buses.I2CHandle, error) { + return i2cHandle, nil + } + + deps := resource.Dependencies{} + sensor, err := makeMpu9250(context.Background(), deps, cfg, logger, i2c) + test.That(t, err, test.ShouldNotBeNil) + test.That(t, err, test.ShouldBeError, unexpectedDeviceError(alternateAddress, 0x64)) + test.That(t, sensor, test.ShouldBeNil) + }) +} + +func TestSuccessfulInitializationAndClose(t *testing.T) { + logger := logging.NewTestLogger(t) + i2cName := "i2c" + + cfg := resource.Config{ + Name: "movementsensor", + Model: Model, + API: movementsensor.API, + ConvertedAttributes: &Config{ + I2cBus: i2cName, + UseAlternateI2CAddress: true, + }, + } + i2cHandle := &inject.I2CHandle{} + i2cHandle.ReadBlockDataFunc = func(ctx context.Context, register byte, numBytes uint8) ([]byte, error) { + return []byte{expectedDefaultAddress}, nil + } + // the only write operations that the sensor implementation performs is + // the command to put it into either measurement mode or sleep mode, + // and measurement mode results from a write of 0, so if is closeWasCalled is toggled + // we know Close() was successfully called + closeWasCalled := false + i2cHandle.WriteByteDataFunc = func(ctx context.Context, register, data byte) error { + if data == 1<<6 { + closeWasCalled = true + } + return nil + } + i2cHandle.CloseFunc = func() error { return nil } + i2c := &inject.I2C{} + i2c.OpenHandleFunc = func(addr byte) (buses.I2CHandle, error) { + return i2cHandle, nil + } + + deps := resource.Dependencies{} + sensor, err := makeMpu9250(context.Background(), deps, cfg, logger, i2c) + test.That(t, err, test.ShouldBeNil) + err = sensor.Close(context.Background()) + test.That(t, err, test.ShouldBeNil) + test.That(t, closeWasCalled, test.ShouldBeTrue) +} + +func setupDependencies(mockData []byte) (resource.Config, buses.I2C) { + i2cName := "i2c" + + cfg := resource.Config{ + Name: "movementsensor", + Model: Model, + API: movementsensor.API, + ConvertedAttributes: &Config{ + I2cBus: i2cName, + UseAlternateI2CAddress: true, + }, + } + + i2cHandle := &inject.I2CHandle{} + i2cHandle.ReadBlockDataFunc = func(ctx context.Context, register byte, numBytes uint8) ([]byte, error) { + if register == defaultAddressRegister { + return []byte{expectedDefaultAddress}, nil + } + return mockData, nil + } + i2cHandle.WriteByteDataFunc = func(ctx context.Context, b1, b2 byte) error { + return nil + } + i2cHandle.CloseFunc = func() error { return nil } + i2c := &inject.I2C{} + i2c.OpenHandleFunc = func(addr byte) (buses.I2CHandle, error) { + return i2cHandle, nil + } + return cfg, i2c +} + +//nolint:dupl +func TestLinearAcceleration(t *testing.T) { + // linear acceleration, temperature, and angular velocity are all read + // sequentially from the same series of 16-bytes, so we need to fill in + // the mock data at the appropriate portion of the sequence + linearAccelMockData := make([]byte, 16) + // x-accel + linearAccelMockData[0] = 64 + linearAccelMockData[1] = 0 + expectedAccelX := 9.81 + // y-accel + linearAccelMockData[2] = 32 + linearAccelMockData[3] = 0 + expectedAccelY := 4.905 + // z-accel + linearAccelMockData[4] = 16 + linearAccelMockData[5] = 0 + expectedAccelZ := 2.4525 + + logger := logging.NewTestLogger(t) + deps := resource.Dependencies{} + cfg, i2c := setupDependencies(linearAccelMockData) + sensor, err := makeMpu9250(context.Background(), deps, cfg, logger, i2c) + test.That(t, err, test.ShouldBeNil) + defer sensor.Close(context.Background()) + testutils.WaitForAssertion(t, func(tb testing.TB) { + linAcc, err := sensor.LinearAcceleration(context.Background(), nil) + test.That(tb, err, test.ShouldBeNil) + test.That(tb, linAcc, test.ShouldNotBeZeroValue) + }) + accel, err := sensor.LinearAcceleration(context.Background(), nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, accel.X, test.ShouldEqual, expectedAccelX) + test.That(t, accel.Y, test.ShouldEqual, expectedAccelY) + test.That(t, accel.Z, test.ShouldEqual, expectedAccelZ) +} + +//nolint:dupl +func TestAngularVelocity(t *testing.T) { + // linear acceleration, temperature, and angular velocity are all read + // sequentially from the same series of 16-bytes, so we need to fill in + // the mock data at the appropriate portion of the sequence + angVelMockData := make([]byte, 16) + // x-vel + angVelMockData[8] = 64 + angVelMockData[9] = 0 + expectedAngVelX := 125.0 + // y-accel + angVelMockData[10] = 32 + angVelMockData[11] = 0 + expectedAngVelY := 62.5 + // z-accel + angVelMockData[12] = 16 + angVelMockData[13] = 0 + expectedAngVelZ := 31.25 + + logger := logging.NewTestLogger(t) + deps := resource.Dependencies{} + cfg, i2c := setupDependencies(angVelMockData) + sensor, err := makeMpu9250(context.Background(), deps, cfg, logger, i2c) + test.That(t, err, test.ShouldBeNil) + defer sensor.Close(context.Background()) + testutils.WaitForAssertion(t, func(tb testing.TB) { + angVel, err := sensor.AngularVelocity(context.Background(), nil) + test.That(tb, err, test.ShouldBeNil) + test.That(tb, angVel, test.ShouldNotBeZeroValue) + }) + angVel, err := sensor.AngularVelocity(context.Background(), nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, angVel.X, test.ShouldEqual, expectedAngVelX) + test.That(t, angVel.Y, test.ShouldEqual, expectedAngVelY) + test.That(t, angVel.Z, test.ShouldEqual, expectedAngVelZ) +} + +func TestTemperature(t *testing.T) { + // linear acceleration, temperature, and angular velocity are all read + // sequentially from the same series of 16-bytes, so we need to fill in + // the mock data at the appropriate portion of the sequence + temperatureMockData := make([]byte, 16) + temperatureMockData[6] = 231 + temperatureMockData[7] = 202 + expectedTemp := 18.3 + + logger := logging.NewTestLogger(t) + deps := resource.Dependencies{} + cfg, i2c := setupDependencies(temperatureMockData) + sensor, err := makeMpu9250(context.Background(), deps, cfg, logger, i2c) + test.That(t, err, test.ShouldBeNil) + defer sensor.Close(context.Background()) + testutils.WaitForAssertion(t, func(tb testing.TB) { + readings, err := sensor.Readings(context.Background(), nil) + test.That(tb, err, test.ShouldBeNil) + test.That(tb, readings["temperature_celsius"], test.ShouldNotBeZeroValue) + }) + readings, err := sensor.Readings(context.Background(), nil) + test.That(t, err, test.ShouldBeNil) + test.That(t, readings["temperature_celsius"], test.ShouldAlmostEqual, expectedTemp, 0.001) +}