From d3f671646c4143d8a7c54a789cb8bfae21ed87e2 Mon Sep 17 00:00:00 2001
From: martha-johnston <martha.johnston@viam.com>
Date: Fri, 6 Dec 2024 15:07:29 -0500
Subject: [PATCH 1/3] add mpu9250 model, no magnetometer

---
 main.go                     |   5 +
 mpu9250/mpu9250.go          | 417 ++++++++++++++++++++++++++++++++++++
 mpu9250/mpu9250_nonlinux.go |   2 +
 mpu9250/mpu9250_test.go     | 262 ++++++++++++++++++++++
 4 files changed, 686 insertions(+)
 create mode 100644 mpu9250/mpu9250.go
 create mode 100644 mpu9250/mpu9250_nonlinux.go
 create mode 100644 mpu9250/mpu9250_test.go

diff --git a/main.go b/main.go
index aa82a00..2716836 100644
--- a/main.go
+++ b/main.go
@@ -5,6 +5,7 @@ import (
 	"context"
 
 	"tdk-invensense/mpu6050"
+	"tdk-invensense/mpu9250"
 
 	"go.viam.com/rdk/components/movementsensor"
 	"go.viam.com/rdk/logging"
@@ -26,6 +27,10 @@ func mainWithArgs(ctx context.Context, args []string, logger logging.Logger) err
 		return err
 	}
 
+	if err = module.AddModelFromRegistry(ctx, movementsensor.API, mpu9250.Model); err != nil {
+		return err
+	}
+
 	err = module.Start(ctx)
 	defer module.Close(ctx)
 	if err != nil {
diff --git a/mpu9250/mpu9250.go b/mpu9250/mpu9250.go
new file mode 100644
index 0000000..a0eb67b
--- /dev/null
+++ b/mpu9250/mpu9250.go
@@ -0,0 +1,417 @@
+//go:build linux
+
+// Package mpu9250 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 mpu9250
+
+import (
+	"context"
+	"fmt"
+	"sync"
+	"time"
+
+	"github.com/golang/geo/r3"
+	geo "github.com/kellydunn/golang-geo"
+	"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.
+	Model = resource.NewModel("viam", "tdk-invensense", "mpu9250")
+
+	// scales for various readings
+	accelScale float64
+	gyroScale  float64
+)
+
+const (
+	defaultAddressRegister           = 117
+	expectedDefaultAddress           = 0x68
+	expectedConfigurationReadAddress = 0x71
+	alternateAddress                 = 0x69
+)
+
+// Config is used to configure the attributes of the chip.
+type Config struct {
+	I2cBus                 string `json:"i2c_bus"`
+	UseAlternateI2CAddress bool   `json:"use_alt_i2c_address,omitempty"`
+}
+
+// Validate ensures all parts of the config are valid, and then returns the list of things we
+// depend on.
+func (conf *Config) Validate(path string) ([]string, error) {
+	if conf.I2cBus == "" {
+		return nil, resource.NewConfigValidationFieldRequiredError(path, "i2c_bus")
+	}
+
+	var deps []string
+	return deps, nil
+}
+
+func init() {
+	resource.RegisterComponent(movementsensor.API, Model, resource.Registration[movementsensor.MovementSensor, *Config]{
+		Constructor: newMpu9250,
+	})
+}
+
+type mpu9250 struct {
+	resource.Named
+	resource.AlwaysRebuild
+	bus        buses.I2C
+	i2cAddress byte
+	mu         sync.Mutex
+
+	// The 3 things we can measure: lock the mutex before reading or writing these.
+	angularVelocity    spatialmath.AngularVelocity
+	temperature        float64
+	linearAcceleration r3.Vector
+	// Stores the most recent error from the background goroutine
+	err movementsensor.LastError
+
+	workers *goutils.StoppableWorkers
+	logger  logging.Logger
+}
+
+func addressReadError(err error, address byte, bus string) error {
+	msg := fmt.Sprintf("can't read from I2C address %d on bus %s", address, bus)
+	return errors.Wrap(err, msg)
+}
+
+func unexpectedDeviceError(address, defaultAddress byte) error {
+	return errors.Errorf("unexpected non-MPU9250 device at address %d: response '%d'",
+		address, defaultAddress)
+}
+
+// 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 := &mpu9250{
+		Named:      conf.ResourceName().AsNamed(),
+		bus:        bus,
+		i2cAddress: address,
+		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())
+	}
+
+	// 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 *mpu9250) 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 00:
+		gyroScale = 250.0 / 32768.0
+	case 01:
+		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 00:
+		accelScale = 2.0 / 32768.0
+	case 01:
+		accelScale = 4.0 / 32768.0
+	case 10:
+		accelScale = 8.0 / 32768.0
+	case 11:
+		accelScale = 16.0 / 32768.0
+	default:
+	}
+	return gyroScale, accelScale, nil
+}
+
+func (mpu *mpu9250) getAccelScale(ctx context.Context) (float64, error) {
+	result, err := mpu.readByte(ctx, 28)
+	if err != nil {
+		return 0, err
+	}
+	switch result {
+	case 00:
+		return 2.0 / 32768.0, nil
+	case 01:
+		return 4.0 / 32768.0, nil
+	case 10:
+		return 8.0 / 32768.0, nil
+	case 11:
+		return 16.0 / 32768.0, nil
+	default:
+	}
+	return 0, nil
+}
+
+func (mpu *mpu9250) readByte(ctx context.Context, register byte) (byte, error) {
+	result, err := mpu.readBlock(ctx, register, 1)
+	if err != nil {
+		return 0, err
+	}
+	return result[0], err
+}
+
+func (mpu *mpu9250) readBlock(ctx context.Context, register byte, length uint8) ([]byte, error) {
+	handle, err := mpu.bus.OpenHandle(mpu.i2cAddress)
+	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 *mpu9250) writeByte(ctx context.Context, register, value byte) error {
+	handle, err := mpu.bus.OpenHandle(mpu.i2cAddress)
+	if err != nil {
+		return err
+	}
+	defer func() {
+		err := handle.Close()
+		if err != nil {
+			mpu.logger.CError(ctx, err)
+		}
+	}()
+
+	return handle.WriteByteData(ctx, register, value)
+}
+
+// Given a value, scales it so that the range of int16s becomes the range of +/- maxValue.
+func setScale(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 toAngularVelocity(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]))
+
+	// maxRotation := 250.0 // Maximum degrees per second measurable in the default configuration
+	return spatialmath.AngularVelocity{
+		X: setScale(gx, gyroScale),
+		Y: setScale(gy, gyroScale),
+		Z: setScale(gz, gyroScale),
+	}
+}
+
+// A helper function that takes 6 bytes and gives back linear acceleration.
+func toLinearAcceleration(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 +/- 2G's, but our units should be m/sec/sec.
+	// maxAcceleration := 9.81 /* m/sec/sec */
+	return r3.Vector{
+		X: setScale(x, accelScale) * 9.81,
+		Y: setScale(y, accelScale) * 9.81,
+		Z: setScale(z, accelScale) * 9.81,
+	}
+}
+
+func (mpu *mpu9250) 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 *mpu9250) LinearVelocity(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) {
+	return r3.Vector{}, movementsensor.ErrMethodUnimplementedLinearVelocity
+}
+
+func (mpu *mpu9250) LinearAcceleration(ctx context.Context, exta map[string]interface{}) (r3.Vector, error) {
+	mpu.mu.Lock()
+	defer mpu.mu.Unlock()
+
+	lastError := mpu.err.Get()
+	if lastError != nil {
+		return r3.Vector{}, lastError
+	}
+	return mpu.linearAcceleration, nil
+}
+
+func (mpu *mpu9250) Orientation(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) {
+	return spatialmath.NewOrientationVector(), movementsensor.ErrMethodUnimplementedOrientation
+}
+
+func (mpu *mpu9250) CompassHeading(ctx context.Context, extra map[string]interface{}) (float64, error) {
+	return 0, movementsensor.ErrMethodUnimplementedCompassHeading
+}
+
+func (mpu *mpu9250) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) {
+	return geo.NewPoint(0, 0), 0, movementsensor.ErrMethodUnimplementedPosition
+}
+
+func (mpu *mpu9250) Accuracy(ctx context.Context, extra map[string]interface{}) (*movementsensor.Accuracy, error) {
+	return movementsensor.UnimplementedOptionalAccuracies(), nil
+}
+
+func (mpu *mpu9250) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) {
+	mpu.mu.Lock()
+	defer mpu.mu.Unlock()
+
+	readings := make(map[string]interface{})
+	readings["linear_acceleration"] = mpu.linearAcceleration
+	readings["temperature_celsius"] = mpu.temperature
+	readings["angular_velocity"] = mpu.angularVelocity
+
+	return readings, mpu.err.Get()
+}
+
+func (mpu *mpu9250) Properties(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) {
+	return &movementsensor.Properties{
+		AngularVelocitySupported:    true,
+		LinearAccelerationSupported: true,
+	}, nil
+}
+
+func (mpu *mpu9250) Close(ctx context.Context) error {
+	mpu.workers.Stop()
+
+	mpu.mu.Lock()
+	defer mpu.mu.Unlock()
+	// Set the Sleep bit (bit 6) in the power control register (register 107).
+	err := mpu.writeByte(ctx, 107, 1<<6)
+	if err != nil {
+		mpu.logger.CError(ctx, err)
+	}
+	return err
+}
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)
+}

From 705ae4874d833bb968d343ab30d0169b01534674 Mon Sep 17 00:00:00 2001
From: martha-johnston <martha.johnston@viam.com>
Date: Fri, 6 Dec 2024 15:34:47 -0500
Subject: [PATCH 2/3] lint fix

---
 mpu9250/mpu9250.go | 29 +++++------------------------
 1 file changed, 5 insertions(+), 24 deletions(-)

diff --git a/mpu9250/mpu9250.go b/mpu9250/mpu9250.go
index a0eb67b..d6b2d7d 100644
--- a/mpu9250/mpu9250.go
+++ b/mpu9250/mpu9250.go
@@ -40,7 +40,7 @@ var (
 	// Model for viam supported tdk-invensense mpu9250 movement sensor.
 	Model = resource.NewModel("viam", "tdk-invensense", "mpu9250")
 
-	// scales for various readings
+	// scales for various readings.
 	accelScale float64
 	gyroScale  float64
 )
@@ -224,9 +224,9 @@ func (mpu *mpu9250) getReadingScales(ctx context.Context) (float64, float64, err
 		return 0, 0, err
 	}
 	switch result {
-	case 00:
+	case 0o0:
 		gyroScale = 250.0 / 32768.0
-	case 01:
+	case 0o1:
 		gyroScale = 500.0 / 32768.0
 	case 10:
 		gyroScale = 1000.0 / 32768.0
@@ -241,9 +241,9 @@ func (mpu *mpu9250) getReadingScales(ctx context.Context) (float64, float64, err
 		return 0, 0, err
 	}
 	switch result {
-	case 00:
+	case 0o0:
 		accelScale = 2.0 / 32768.0
-	case 01:
+	case 0o1:
 		accelScale = 4.0 / 32768.0
 	case 10:
 		accelScale = 8.0 / 32768.0
@@ -254,25 +254,6 @@ func (mpu *mpu9250) getReadingScales(ctx context.Context) (float64, float64, err
 	return gyroScale, accelScale, nil
 }
 
-func (mpu *mpu9250) getAccelScale(ctx context.Context) (float64, error) {
-	result, err := mpu.readByte(ctx, 28)
-	if err != nil {
-		return 0, err
-	}
-	switch result {
-	case 00:
-		return 2.0 / 32768.0, nil
-	case 01:
-		return 4.0 / 32768.0, nil
-	case 10:
-		return 8.0 / 32768.0, nil
-	case 11:
-		return 16.0 / 32768.0, nil
-	default:
-	}
-	return 0, nil
-}
-
 func (mpu *mpu9250) readByte(ctx context.Context, register byte) (byte, error) {
 	result, err := mpu.readBlock(ctx, register, 1)
 	if err != nil {

From 0b132f937f1f7e0edd20dca6a09e03f2c3ba20a5 Mon Sep 17 00:00:00 2001
From: martha-johnston <martha.johnston@viam.com>
Date: Mon, 9 Dec 2024 16:34:36 -0500
Subject: [PATCH 3/3] pause mpu9250 implementation, currently missing
 magnetometer support

---
 main.go                          |   7 +-
 {mpu6050 => mpu}/mpu6050.go      |  37 ++---
 {mpu6050 => mpu}/mpu6050_test.go |   2 +-
 {mpu9250 => mpu}/mpu9250.go      | 263 ++++++++++---------------------
 mpu/mpu_nonlinux.go              |   2 +
 mpu6050/mpu6050_nonlinux.go      |   2 -
 6 files changed, 111 insertions(+), 202 deletions(-)
 rename {mpu6050 => mpu}/mpu6050.go (85%)
 rename {mpu6050 => mpu}/mpu6050_test.go (99%)
 rename {mpu9250 => mpu}/mpu9250.go (56%)
 create mode 100644 mpu/mpu_nonlinux.go
 delete mode 100644 mpu6050/mpu6050_nonlinux.go

diff --git a/main.go b/main.go
index 2716836..1324513 100644
--- a/main.go
+++ b/main.go
@@ -4,8 +4,7 @@ package main
 import (
 	"context"
 
-	"tdk-invensense/mpu6050"
-	"tdk-invensense/mpu9250"
+	"tdk-invensense/mpu"
 
 	"go.viam.com/rdk/components/movementsensor"
 	"go.viam.com/rdk/logging"
@@ -23,11 +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, mpu9250.Model); err != nil {
+	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/mpu9250/mpu9250.go b/mpu/mpu9250.go
similarity index 56%
rename from mpu9250/mpu9250.go
rename to mpu/mpu9250.go
index d6b2d7d..38e4c06 100644
--- a/mpu9250/mpu9250.go
+++ b/mpu/mpu9250.go
@@ -1,6 +1,6 @@
 //go:build linux
 
-// Package mpu9250 implements the movementsensor interface for an MPU-9250 6-axis accelerometer. A
+// 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
@@ -16,16 +16,13 @@
 //
 // If you use the alternate address, your config file for this component must set its
 // "use_alternate_i2c_address" boolean to true.
-package mpu9250
+package mpu
 
 import (
 	"context"
-	"fmt"
-	"sync"
 	"time"
 
 	"github.com/golang/geo/r3"
-	geo "github.com/kellydunn/golang-geo"
 	"github.com/pkg/errors"
 	"go.viam.com/rdk/components/board/genericlinux/buses"
 	"go.viam.com/rdk/components/movementsensor"
@@ -38,7 +35,7 @@ import (
 
 var (
 	// Model for viam supported tdk-invensense mpu9250 movement sensor.
-	Model = resource.NewModel("viam", "tdk-invensense", "mpu9250")
+	Model9250 = resource.NewModel("viam", "tdk-invensense", "mpu9250")
 
 	// scales for various readings.
 	accelScale float64
@@ -46,63 +43,18 @@ var (
 )
 
 const (
-	defaultAddressRegister           = 117
-	expectedDefaultAddress           = 0x68
 	expectedConfigurationReadAddress = 0x71
-	alternateAddress                 = 0x69
+	magnetometerAddress              = 0x0C
+	magnetometerWhoAmI               = 0x00
+	magnetometerWhoAmIReturn         = 0x48
 )
 
-// Config is used to configure the attributes of the chip.
-type Config struct {
-	I2cBus                 string `json:"i2c_bus"`
-	UseAlternateI2CAddress bool   `json:"use_alt_i2c_address,omitempty"`
-}
-
-// Validate ensures all parts of the config are valid, and then returns the list of things we
-// depend on.
-func (conf *Config) Validate(path string) ([]string, error) {
-	if conf.I2cBus == "" {
-		return nil, resource.NewConfigValidationFieldRequiredError(path, "i2c_bus")
-	}
-
-	var deps []string
-	return deps, nil
-}
-
 func init() {
-	resource.RegisterComponent(movementsensor.API, Model, resource.Registration[movementsensor.MovementSensor, *Config]{
+	resource.RegisterComponent(movementsensor.API, Model9250, resource.Registration[movementsensor.MovementSensor, *Config]{
 		Constructor: newMpu9250,
 	})
 }
 
-type mpu9250 struct {
-	resource.Named
-	resource.AlwaysRebuild
-	bus        buses.I2C
-	i2cAddress byte
-	mu         sync.Mutex
-
-	// The 3 things we can measure: lock the mutex before reading or writing these.
-	angularVelocity    spatialmath.AngularVelocity
-	temperature        float64
-	linearAcceleration r3.Vector
-	// Stores the most recent error from the background goroutine
-	err movementsensor.LastError
-
-	workers *goutils.StoppableWorkers
-	logger  logging.Logger
-}
-
-func addressReadError(err error, address byte, bus string) error {
-	msg := fmt.Sprintf("can't read from I2C address %d on bus %s", address, bus)
-	return errors.Wrap(err, msg)
-}
-
-func unexpectedDeviceError(address, defaultAddress byte) error {
-	return errors.Errorf("unexpected non-MPU9250 device at address %d: response '%d'",
-		address, defaultAddress)
-}
-
 // newMpu9250 constructs a new Mpu9250 object.
 func newMpu9250(
 	ctx context.Context,
@@ -143,10 +95,11 @@ func makeMpu9250(
 	}
 	logger.CDebugf(ctx, "Using address %d for MPU9250 sensor", address)
 
-	sensor := &mpu9250{
+	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.
@@ -171,6 +124,32 @@ func makeMpu9250(
 		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 {
@@ -216,7 +195,46 @@ func makeMpu9250(
 	return sensor, nil
 }
 
-func (mpu *mpu9250) getReadingScales(ctx context.Context) (float64, float64, error) {
+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)
@@ -254,145 +272,36 @@ func (mpu *mpu9250) getReadingScales(ctx context.Context) (float64, float64, err
 	return gyroScale, accelScale, nil
 }
 
-func (mpu *mpu9250) readByte(ctx context.Context, register byte) (byte, error) {
-	result, err := mpu.readBlock(ctx, register, 1)
-	if err != nil {
-		return 0, err
-	}
-	return result[0], err
-}
-
-func (mpu *mpu9250) readBlock(ctx context.Context, register byte, length uint8) ([]byte, error) {
-	handle, err := mpu.bus.OpenHandle(mpu.i2cAddress)
-	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 *mpu9250) writeByte(ctx context.Context, register, value byte) error {
-	handle, err := mpu.bus.OpenHandle(mpu.i2cAddress)
-	if err != nil {
-		return err
-	}
-	defer func() {
-		err := handle.Close()
-		if err != nil {
-			mpu.logger.CError(ctx, err)
-		}
-	}()
-
-	return handle.WriteByteData(ctx, register, value)
-}
-
 // Given a value, scales it so that the range of int16s becomes the range of +/- maxValue.
-func setScale(value int, maxValue float64) float64 {
+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 toAngularVelocity(data []byte) spatialmath.AngularVelocity {
+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]))
 
-	// maxRotation := 250.0 // Maximum degrees per second measurable in the default configuration
+	// gyroScale is the maximum degrees per second measurable
 	return spatialmath.AngularVelocity{
-		X: setScale(gx, gyroScale),
-		Y: setScale(gy, gyroScale),
-		Z: setScale(gz, gyroScale),
+		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 toLinearAcceleration(data []byte) r3.Vector {
+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 +/- 2G's, but our units should be m/sec/sec.
-	// maxAcceleration := 9.81 /* m/sec/sec */
+	// The scale is +/- X Gs based on the calculated accelScale, but our units should be m/sec/sec.
 	return r3.Vector{
-		X: setScale(x, accelScale) * 9.81,
-		Y: setScale(y, accelScale) * 9.81,
-		Z: setScale(z, accelScale) * 9.81,
-	}
-}
-
-func (mpu *mpu9250) 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 *mpu9250) LinearVelocity(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) {
-	return r3.Vector{}, movementsensor.ErrMethodUnimplementedLinearVelocity
-}
-
-func (mpu *mpu9250) LinearAcceleration(ctx context.Context, exta map[string]interface{}) (r3.Vector, error) {
-	mpu.mu.Lock()
-	defer mpu.mu.Unlock()
-
-	lastError := mpu.err.Get()
-	if lastError != nil {
-		return r3.Vector{}, lastError
-	}
-	return mpu.linearAcceleration, nil
-}
-
-func (mpu *mpu9250) Orientation(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) {
-	return spatialmath.NewOrientationVector(), movementsensor.ErrMethodUnimplementedOrientation
-}
-
-func (mpu *mpu9250) CompassHeading(ctx context.Context, extra map[string]interface{}) (float64, error) {
-	return 0, movementsensor.ErrMethodUnimplementedCompassHeading
-}
-
-func (mpu *mpu9250) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) {
-	return geo.NewPoint(0, 0), 0, movementsensor.ErrMethodUnimplementedPosition
-}
-
-func (mpu *mpu9250) Accuracy(ctx context.Context, extra map[string]interface{}) (*movementsensor.Accuracy, error) {
-	return movementsensor.UnimplementedOptionalAccuracies(), nil
-}
-
-func (mpu *mpu9250) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) {
-	mpu.mu.Lock()
-	defer mpu.mu.Unlock()
-
-	readings := make(map[string]interface{})
-	readings["linear_acceleration"] = mpu.linearAcceleration
-	readings["temperature_celsius"] = mpu.temperature
-	readings["angular_velocity"] = mpu.angularVelocity
-
-	return readings, mpu.err.Get()
-}
-
-func (mpu *mpu9250) Properties(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) {
-	return &movementsensor.Properties{
-		AngularVelocitySupported:    true,
-		LinearAccelerationSupported: true,
-	}, nil
-}
-
-func (mpu *mpu9250) Close(ctx context.Context) error {
-	mpu.workers.Stop()
-
-	mpu.mu.Lock()
-	defer mpu.mu.Unlock()
-	// Set the Sleep bit (bit 6) in the power control register (register 107).
-	err := mpu.writeByte(ctx, 107, 1<<6)
-	if err != nil {
-		mpu.logger.CError(ctx, err)
+		X: setScale9250(x, accelScale) * 9.81,
+		Y: setScale9250(y, accelScale) * 9.81,
+		Z: setScale9250(z, accelScale) * 9.81,
 	}
-	return err
 }
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