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