diff --git a/components/movementsensor/adxl345/adxl345.go b/components/movementsensor/adxl345/adxl345.go deleted file mode 100644 index 30ad4b10b21..00000000000 --- a/components/movementsensor/adxl345/adxl345.go +++ /dev/null @@ -1,582 +0,0 @@ -//go:build linux - -// Package adxl345 implements the MovementSensor interface for the ADXL345 accelerometer. -package adxl345 - -/* - This package supports ADXL345 accelerometer attached to an I2C bus on the robot (the chip supports - communicating over SPI as well, but this package does not support that interface). - The datasheet for this chip is available at: - https://www.analog.com/media/en/technical-documentation/data-sheets/adxl345.pdf - - Because we only support I2C interaction, the CS pin must be wired to hot (which tells the chip - which communication interface to use). The chip has two possible I2C addresses, which can be - selected by wiring the SDO pin to either hot or ground: - - if SDO is wired to ground, it uses the default I2C address of 0x53 - - if SDO is wired to hot, it uses the alternate I2C address of 0x1D - - If you use the alternate address, your config file for this component must set its - "use_alternate_i2c_address" boolean to true. -*/ - -import ( - "context" - "fmt" - "sync" - "time" - - "github.com/golang/geo/r3" - geo "github.com/kellydunn/golang-geo" - "github.com/pkg/errors" - "go.viam.com/utils" - - "go.viam.com/rdk/components/board" - "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" - rutils "go.viam.com/rdk/utils" -) - -var model = resource.DefaultModelFamily.WithModel("accel-adxl345") - -const ( - defaultI2CAddress = 0x53 - alternateI2CAddress = 0x1D - deviceIDRegister = 0 - expectedDeviceID = 0xE5 - powerControlRegister = 0x2D -) - -// Config is a description of how to find an ADXL345 accelerometer on the robot. -type Config struct { - I2cBus string `json:"i2c_bus"` - UseAlternateI2CAddress bool `json:"use_alternate_i2c_address,omitempty"` - BoardName string `json:"board,omitempty"` - SingleTap *TapConfig `json:"tap,omitempty"` - FreeFall *FreeFallConfig `json:"free_fall,omitempty"` -} - -// TapConfig is a description of the configs for tap registers. -type TapConfig struct { - AccelerometerPin int `json:"accelerometer_pin"` - InterruptPin string `json:"interrupt_pin"` - ExcludeX bool `json:"exclude_x,omitempty"` - ExcludeY bool `json:"exclude_y,omitempty"` - ExcludeZ bool `json:"exclude_z,omitempty"` - Threshold float32 `json:"threshold,omitempty"` - Dur float32 `json:"dur_us,omitempty"` -} - -// FreeFallConfig is a description of the configs for free fall registers. -type FreeFallConfig struct { - AccelerometerPin int `json:"accelerometer_pin"` - InterruptPin string `json:"interrupt_pin"` - Threshold float32 `json:"threshold,omitempty"` - Time float32 `json:"time_ms,omitempty"` -} - -// validateTapConfigs validates the tap piece of the config. -func (tapCfg *TapConfig) validateTapConfigs() error { - if tapCfg.AccelerometerPin != 1 && tapCfg.AccelerometerPin != 2 { - return errors.New("Accelerometer pin on the ADXL345 must be 1 or 2") - } - if tapCfg.Threshold != 0 { - if tapCfg.Threshold < 0 || tapCfg.Threshold > (255*threshTapScaleFactor) { - return errors.New("Tap threshold on the ADXL345 must be 0 between and 15,937mg") - } - } - if tapCfg.Dur != 0 { - if tapCfg.Dur < 0 || tapCfg.Dur > (255*durScaleFactor) { - return errors.New("Tap dur on the ADXL345 must be between 0 and 160,000µs") - } - } - return nil -} - -// validateFreeFallConfigs validates the freefall piece of the config. -func (freefallCfg *FreeFallConfig) validateFreeFallConfigs() error { - if freefallCfg.AccelerometerPin != 1 && freefallCfg.AccelerometerPin != 2 { - return errors.New("Accelerometer pin on the ADXL345 must be 1 or 2") - } - if freefallCfg.Threshold != 0 { - if freefallCfg.Threshold < 0 || freefallCfg.Threshold > (255*threshFfScaleFactor) { - return errors.New("Accelerometer tap threshold on the ADXL345 must be 0 between and 15,937mg") - } - } - if freefallCfg.Time != 0 { - if freefallCfg.Time < 0 || freefallCfg.Time > (255*timeFfScaleFactor) { - return errors.New("Accelerometer tap time on the ADXL345 must be between 0 and 1,275ms") - } - } - return nil -} - -// Validate ensures all parts of the config are valid, and then returns the list of things we -// depend on. -func (cfg *Config) Validate(path string) ([]string, error) { - var deps []string - if cfg.BoardName == "" { - // The board name is only required for interrupt-related functionality. - if cfg.SingleTap != nil || cfg.FreeFall != nil { - return nil, resource.NewConfigValidationFieldRequiredError(path, "board") - } - } else { - if cfg.SingleTap != nil || cfg.FreeFall != nil { - // The board is actually used! Add it to the dependencies. - deps = append(deps, cfg.BoardName) - } - } - if cfg.I2cBus == "" { - return nil, resource.NewConfigValidationFieldRequiredError(path, "i2c_bus") - } - if cfg.SingleTap != nil { - if err := cfg.SingleTap.validateTapConfigs(); err != nil { - return nil, err - } - } - if cfg.FreeFall != nil { - if err := cfg.FreeFall.validateFreeFallConfigs(); err != nil { - return nil, err - } - } - return deps, nil -} - -func init() { - resource.RegisterComponent( - movementsensor.API, - model, - resource.Registration[movementsensor.MovementSensor, *Config]{ - Constructor: newAdxl345, - }) -} - -type adxl345 struct { - resource.Named - resource.AlwaysRebuild - - bus buses.I2C - i2cAddress byte - logger logging.Logger - interruptsEnabled byte - interruptsFound map[InterruptID]int - configuredRegisterValues map[byte]byte - - // Lock the mutex when you want to read or write either the acceleration or the last error. - mu sync.Mutex - linearAcceleration r3.Vector - err movementsensor.LastError - - workers *utils.StoppableWorkers -} - -// newAdxl345 is a constructor to create a new object representing an ADXL345 accelerometer. -func newAdxl345( - 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 { - msg := fmt.Sprintf("can't find I2C bus '%q' for ADXL345 sensor", newConf.I2cBus) - return nil, errors.Wrap(err, msg) - } - - // The rest of the constructor is separated out so that you can pass in a mock I2C bus during - // tests. - return makeAdxl345(ctx, deps, conf, logger, bus) -} - -// makeAdxl345 is split out solely to be used during unit tests: it constructs a new object -// representing an AXDL345 accelerometer, but with the I2C bus already created and passed in as an -// argument. This lets you inject a mock I2C bus during the tests. It should not be used directly -// in production code (instead, use NewAdxl345, above). -func makeAdxl345( - ctx context.Context, - deps 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 = alternateI2CAddress - } else { - address = defaultI2CAddress - } - - interruptConfigurations := getInterruptConfigurations(newConf) - configuredRegisterValues := getFreeFallRegisterValues(newConf.FreeFall) - for k, v := range getSingleTapRegisterValues(newConf.SingleTap, logger) { - configuredRegisterValues[k] = v - } - - sensor := &adxl345{ - Named: conf.ResourceName().AsNamed(), - bus: bus, - i2cAddress: address, - interruptsEnabled: interruptConfigurations[intEnableAddr], - logger: logger, - configuredRegisterValues: configuredRegisterValues, - interruptsFound: make(map[InterruptID]int), - - // On overloaded boards, sometimes the I2C bus can be flaky. Only report errors if at least - // 5 of the last 10 times we've tried interacting with the device have had problems. - err: movementsensor.NewLastError(10, 5), - } - - // To check that we're able to talk to the chip, we should be able to read register 0 and get - // back the device ID (0xE5). - deviceID, err := sensor.readByte(ctx, deviceIDRegister) - if err != nil { - return nil, movementsensor.AddressReadError(err, address, newConf.I2cBus) - } - if deviceID != expectedDeviceID { - return nil, movementsensor.UnexpectedDeviceError(address, deviceID, sensor.Name().Name) - } - - // The chip starts out in standby mode. Set it to measurement mode so we can get data from it. - // To do this, we set the Power Control register (0x2D) to turn on the 8's bit. - if err = sensor.writeByte(ctx, powerControlRegister, 0x08); err != nil { - return nil, errors.Wrap(err, "unable to put ADXL345 into measurement mode") - } - - // Now, turn on the background goroutine that constantly reads from the chip and stores data in - // the object we created. - sensor.workers = utils.NewBackgroundStoppableWorkers(func(cancelContext context.Context) { - // Reading data a thousand times per second is probably fast enough. - timer := time.NewTicker(time.Millisecond) - defer timer.Stop() - - for { - select { - case <-cancelContext.Done(): - return - default: - } - select { - case <-timer.C: - // The registers with data are 0x32 through 0x37: two bytes each for X, Y, and Z. - rawData, err := sensor.readBlock(cancelContext, 0x32, 6) - // Record the errors no matter what: if the error is nil, that's useful information - // that will prevent errors from being returned later. - sensor.err.Set(err) - if err != nil { - continue - } - - linearAcceleration := toLinearAcceleration(rawData) - // Only lock the mutex to write to the shared data, so other threads can read the - // data as often as they want. - sensor.mu.Lock() - sensor.linearAcceleration = linearAcceleration - sensor.mu.Unlock() - case <-cancelContext.Done(): - return - } - } - }) - - // Clear out the source register upon starting the component - if _, err := sensor.readByte(ctx, intSourceAddr); err != nil { - // shut down goroutine reading sensor in the background - sensor.workers.Stop() - return nil, err - } - - if err := sensor.configureInterruptRegisters(ctx, interruptConfigurations[intMapAddr]); err != nil { - // shut down goroutine reading sensor in the background - sensor.workers.Stop() - return nil, err - } - - interruptList := []string{} - if (newConf.SingleTap != nil) && (newConf.SingleTap.InterruptPin != "") { - interruptList = append(interruptList, newConf.SingleTap.InterruptPin) - } - - if (newConf.FreeFall != nil) && (newConf.FreeFall.InterruptPin != "") { - interruptList = append(interruptList, newConf.FreeFall.InterruptPin) - } - - if len(interruptList) > 0 { - b, err := board.FromDependencies(deps, newConf.BoardName) - if err != nil { - return nil, err - } - interrupts := []board.DigitalInterrupt{} - for _, name := range interruptList { - interrupt, err := b.DigitalInterruptByName(name) - if err != nil { - return nil, err - } - interrupts = append(interrupts, interrupt) - } - ticksChan := make(chan board.Tick) - err = b.StreamTicks(sensor.workers.Context(), interrupts, ticksChan, nil) - if err != nil { - return nil, err - } - sensor.startInterruptMonitoring(ticksChan) - } - - return sensor, nil -} - -func (adxl *adxl345) startInterruptMonitoring(ticksChan chan board.Tick) { - adxl.workers.Add(func(cancelContext context.Context) { - for { - select { - case <-cancelContext.Done(): - return - case tick := <-ticksChan: - if tick.High { - utils.UncheckedError(adxl.readInterrupts(cancelContext)) - } - } - } - }) -} - -// This returns a map from register addresses to data which should be written to that register to configure the interrupt pin. -func getInterruptConfigurations(cfg *Config) map[byte]byte { - var intEnabled byte - var intMap byte - - if cfg.FreeFall != nil { - intEnabled += interruptBitPosition[freeFall] - if cfg.FreeFall.AccelerometerPin == 2 { - intMap |= interruptBitPosition[freeFall] - } else { - // Clear the freefall bit in the map to send the signal to pin INT1. - intMap &^= interruptBitPosition[freeFall] - } - } - if cfg.SingleTap != nil { - intEnabled += interruptBitPosition[singleTap] - if cfg.SingleTap.AccelerometerPin == 2 { - intMap |= interruptBitPosition[singleTap] - } else { - // Clear the single tap bit in the map to send the signal to pin INT1. - intMap &^= interruptBitPosition[singleTap] - } - } - - return map[byte]byte{intEnableAddr: intEnabled, intMapAddr: intMap} -} - -// This returns a map from register addresses to data which should be written to that register to configure single tap. -func getSingleTapRegisterValues(singleTapConfigs *TapConfig, logger logging.Logger) map[byte]byte { - registerValues := map[byte]byte{} - if singleTapConfigs == nil { - return registerValues - } - - registerValues[tapAxesAddr] = getAxes(singleTapConfigs.ExcludeX, singleTapConfigs.ExcludeY, singleTapConfigs.ExcludeZ) - - if singleTapConfigs.Threshold != 0 { - registerValues[threshTapAddr] = byte((singleTapConfigs.Threshold / threshTapScaleFactor)) - } - if singleTapConfigs.Dur != 0 { - registerValues[durAddr] = byte((singleTapConfigs.Dur / durScaleFactor)) - } - - logger.Info("Consider experimenting with dur_us and threshold attributes to achieve best results with single tap") - return registerValues -} - -// This returns a map from register addresses to data which should be written to that register to configure freefall. -func getFreeFallRegisterValues(freeFallConfigs *FreeFallConfig) map[byte]byte { - registerValues := map[byte]byte{} - if freeFallConfigs == nil { - return registerValues - } - if freeFallConfigs.Threshold != 0 { - registerValues[threshFfAddr] = byte((freeFallConfigs.Threshold / threshFfScaleFactor)) - } - if freeFallConfigs.Time != 0 { - registerValues[timeFfAddr] = byte((freeFallConfigs.Time / timeFfScaleFactor)) - } - return registerValues -} - -func (adxl *adxl345) readByte(ctx context.Context, register byte) (byte, error) { - result, err := adxl.readBlock(ctx, register, 1) - if err != nil { - return 0, err - } - return result[0], err -} - -func (adxl *adxl345) readBlock(ctx context.Context, register byte, length uint8) ([]byte, error) { - handle, err := adxl.bus.OpenHandle(adxl.i2cAddress) - if err != nil { - return nil, err - } - defer func() { - err := handle.Close() - if err != nil { - adxl.logger.CError(ctx, err) - } - }() - - results, err := handle.ReadBlockData(ctx, register, length) - return results, err -} - -func (adxl *adxl345) writeByte(ctx context.Context, register, value byte) error { - handle, err := adxl.bus.OpenHandle(adxl.i2cAddress) - if err != nil { - return err - } - defer func() { - err := handle.Close() - if err != nil { - adxl.logger.CError(ctx, err) - } - }() - - return handle.WriteByteData(ctx, register, value) -} - -func (adxl *adxl345) configureInterruptRegisters(ctx context.Context, interruptBitMap byte) error { - if adxl.interruptsEnabled == 0 { - return nil - } - adxl.configuredRegisterValues[intEnableAddr] = adxl.interruptsEnabled - adxl.configuredRegisterValues[intMapAddr] = interruptBitMap - for key, value := range defaultRegisterValues { - if configuredVal, ok := adxl.configuredRegisterValues[key]; ok { - value = configuredVal - } - if err := adxl.writeByte(ctx, key, value); err != nil { - return err - } - } - return nil -} - -func (adxl *adxl345) readInterrupts(ctx context.Context) error { - adxl.mu.Lock() - defer adxl.mu.Unlock() - intSourceRegister, err := adxl.readByte(ctx, intSourceAddr) - if err != nil { - return err - } - - for key, val := range interruptBitPosition { - if intSourceRegister&val&adxl.interruptsEnabled != 0 { - adxl.interruptsFound[key]++ - } - } - return nil -} - -// Given a value, scales it so that the range of values read in becomes the range of +/- maxValue. -// The trick here is that although the values are stored in 16 bits, the sensor only has 10 bits of -// resolution. So, there are only (1 << 9) possible positive values, and a similar number of -// negative ones. -func setScale(value int, maxValue float64) float64 { - return float64(value) * maxValue / (1 << 9) -} - -func toLinearAcceleration(data []byte) r3.Vector { - // Vectors take ints, but we've got int16's, so we need to convert. - x := int(rutils.Int16FromBytesLE(data[0:2])) - y := int(rutils.Int16FromBytesLE(data[2:4])) - z := int(rutils.Int16FromBytesLE(data[4:6])) - - // The default scale is +/- 2G's, but our units should be m/sec/sec. - maxAcceleration := 2.0 * 9.81 /* m/sec/sec */ - return r3.Vector{ - X: setScale(x, maxAcceleration), - Y: setScale(y, maxAcceleration), - Z: setScale(z, maxAcceleration), - } -} - -func (adxl *adxl345) AngularVelocity(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { - return spatialmath.AngularVelocity{}, movementsensor.ErrMethodUnimplementedAngularVelocity -} - -func (adxl *adxl345) LinearVelocity(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { - return r3.Vector{}, movementsensor.ErrMethodUnimplementedLinearVelocity -} - -func (adxl *adxl345) LinearAcceleration(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { - adxl.mu.Lock() - defer adxl.mu.Unlock() - lastError := adxl.err.Get() - - if lastError != nil { - return r3.Vector{}, lastError - } - - return adxl.linearAcceleration, nil -} - -func (adxl *adxl345) Orientation(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { - return spatialmath.NewOrientationVector(), movementsensor.ErrMethodUnimplementedOrientation -} - -func (adxl *adxl345) CompassHeading(ctx context.Context, extra map[string]interface{}) (float64, error) { - return 0, movementsensor.ErrMethodUnimplementedCompassHeading -} - -func (adxl *adxl345) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - return geo.NewPoint(0, 0), 0, movementsensor.ErrMethodUnimplementedPosition -} - -func (adxl *adxl345) Accuracy(ctx context.Context, extra map[string]interface{}) (*movementsensor.Accuracy, error) { - // this driver is unable to provide positional or compass heading data - return movementsensor.UnimplementedOptionalAccuracies(), nil -} - -func (adxl *adxl345) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { - readings, err := movementsensor.DefaultAPIReadings(ctx, adxl, extra) - if err != nil { - return nil, err - } - - adxl.mu.Lock() - defer adxl.mu.Unlock() - - readings["single_tap_count"] = adxl.interruptsFound[singleTap] - readings["freefall_count"] = adxl.interruptsFound[freeFall] - - return readings, adxl.err.Get() -} - -func (adxl *adxl345) Properties(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { - return &movementsensor.Properties{ - LinearAccelerationSupported: true, - }, nil -} - -// Puts the chip into standby mode. -func (adxl *adxl345) Close(ctx context.Context) error { - adxl.workers.Stop() - - adxl.mu.Lock() - defer adxl.mu.Unlock() - - // Put the chip into standby mode by setting the Power Control register (0x2D) to 0. - err := adxl.writeByte(ctx, powerControlRegister, 0x00) - if err != nil { - adxl.logger.CErrorf(ctx, "unable to turn off ADXL345 accelerometer: '%s'", err) - } - return nil -} diff --git a/components/movementsensor/adxl345/adxl345_nonlinux.go b/components/movementsensor/adxl345/adxl345_nonlinux.go deleted file mode 100644 index cd6ae67ae7b..00000000000 --- a/components/movementsensor/adxl345/adxl345_nonlinux.go +++ /dev/null @@ -1,2 +0,0 @@ -// Package adxl345 is Linux-only. -package adxl345 diff --git a/components/movementsensor/adxl345/adxl345_test.go b/components/movementsensor/adxl345/adxl345_test.go deleted file mode 100644 index ce2c087017a..00000000000 --- a/components/movementsensor/adxl345/adxl345_test.go +++ /dev/null @@ -1,393 +0,0 @@ -//go:build linux - -package adxl345 - -import ( - "context" - "testing" - "time" - - "github.com/pkg/errors" - "go.viam.com/test" - "go.viam.com/utils/testutils" - - "go.viam.com/rdk/components/board" - "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" -) - -func nowNanosTest() uint64 { - return uint64(time.Now().UnixNano()) -} - -func setupDependencies(mockData []byte) (resource.Config, resource.Dependencies, buses.I2C) { - cfg := resource.Config{ - Name: "movementsensor", - Model: model, - API: movementsensor.API, - ConvertedAttributes: &Config{ - I2cBus: "2", - UseAlternateI2CAddress: true, - }, - } - - i2cHandle := &inject.I2CHandle{} - i2cHandle.ReadBlockDataFunc = func(ctx context.Context, register byte, numBytes uint8) ([]byte, error) { - if register == 0 { - return []byte{0xE5}, 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, resource.Dependencies{}, i2c -} - -func sendInterrupt(ctx context.Context, adxl movementsensor.MovementSensor, t *testing.T, interrupt board.DigitalInterrupt, key string) { - interrupt.(*inject.DigitalInterrupt).Tick(ctx, true, nowNanosTest()) - testutils.WaitForAssertion(t, func(tb testing.TB) { - readings, err := adxl.Readings(ctx, map[string]interface{}{}) - test.That(tb, err, test.ShouldBeNil) - test.That(tb, readings[key], test.ShouldNotBeZeroValue) - }) -} - -func TestValidateConfig(t *testing.T) { - boardName := "local" - t.Run("fails when interrupts are used without a supplied board", func(t *testing.T) { - tapCfg := TapConfig{ - AccelerometerPin: 1, - InterruptPin: "on_missing_board", - } - cfg := Config{ - I2cBus: "3", - SingleTap: &tapCfg, - } - deps, err := cfg.Validate("path") - expectedErr := resource.NewConfigValidationFieldRequiredError("path", "board") - test.That(t, err, test.ShouldBeError, expectedErr) - test.That(t, deps, test.ShouldBeEmpty) - }) - - t.Run("fails with no I2C bus", func(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) - }) - - t.Run("passes with no board supplied, no dependencies", func(t *testing.T) { - cfg := Config{ - I2cBus: "3", - } - deps, err := cfg.Validate("path") - test.That(t, err, test.ShouldBeNil) - test.That(t, len(deps), test.ShouldEqual, 0) - }) - - t.Run("adds board name to dependencies on success with interrupts", func(t *testing.T) { - tapCfg := TapConfig{ - AccelerometerPin: 1, - InterruptPin: "on_present_board", - } - cfg := Config{ - BoardName: boardName, - I2cBus: "2", - SingleTap: &tapCfg, - } - deps, err := cfg.Validate("path") - - test.That(t, err, test.ShouldBeNil) - test.That(t, len(deps), test.ShouldEqual, 1) - test.That(t, deps[0], test.ShouldResemble, boardName) - }) -} - -func TestInitializationFailureOnChipCommunication(t *testing.T) { - logger := logging.NewTestLogger(t) - - t.Run("fails on read error", func(t *testing.T) { - cfg := resource.Config{ - Name: "movementsensor", - Model: model, - API: movementsensor.API, - ConvertedAttributes: &Config{ - I2cBus: "2", - }, - } - - i2cHandle := &inject.I2CHandle{} - readErr := errors.New("read error") - i2cHandle.ReadBlockDataFunc = func(ctx context.Context, register byte, numBytes uint8) ([]byte, error) { - if register == deviceIDRegister { - 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 - } - - sensor, err := makeAdxl345(context.Background(), resource.Dependencies{}, cfg, logger, i2c) - test.That(t, err, test.ShouldNotBeNil) - test.That(t, sensor, test.ShouldBeNil) - }) -} - -func TestInterrupts(t *testing.T) { - ctx := context.Background() - callbacks := []chan board.Tick{} - - interrupt := &inject.DigitalInterrupt{} - - interrupt.TickFunc = func(ctx context.Context, high bool, nanoseconds uint64) error { - tick := board.Tick{High: high, TimestampNanosec: nanoseconds} - for _, cb := range callbacks { - cb <- tick - } - return nil - } - - mockBoard := &inject.Board{} - mockBoard.DigitalInterruptByNameFunc = func(name string) (board.DigitalInterrupt, error) { return interrupt, nil } - mockBoard.StreamTicksFunc = func(ctx context.Context, interrupts []board.DigitalInterrupt, ch chan board.Tick, - extra map[string]interface{}, - ) error { - callbacks = append(callbacks, ch) - return nil - } - - i2cHandle := &inject.I2CHandle{} - i2cHandle.CloseFunc = func() error { return nil } - i2cHandle.WriteByteDataFunc = func(context.Context, byte, byte) error { return nil } - // The data returned from the readByteFunction is intended to signify which interrupts have gone off - i2cHandle.ReadByteDataFunc = func(context.Context, byte) (byte, error) { return byte(1<<6 + 1<<2), nil } - // i2cHandle.ReadBlockDataFunc gets called multiple times. The first time we need the first byte to be 0xE5 and the next - // time we need 6 bytes. This return provides more data than necessary for the first call to the function but allows - // both calls to it to work properly. - i2cHandle.ReadBlockDataFunc = func(context.Context, byte, uint8) ([]byte, error) { - return []byte{byte(0xE5), byte(0x1), byte(0x2), byte(0x3), byte(0x4), byte(0x5), byte(0x6)}, nil - } - - i2c := &inject.I2C{} - i2c.OpenHandleFunc = func(addr byte) (buses.I2CHandle, error) { return i2cHandle, nil } - - logger := logging.NewTestLogger(t) - - deps := resource.Dependencies{ - resource.NewName(board.API, "board"): mockBoard, - } - - tap := &TapConfig{ - AccelerometerPin: 1, - InterruptPin: "int1", - } - - ff := &FreeFallConfig{ - AccelerometerPin: 1, - InterruptPin: "int1", - } - - cfg := resource.Config{ - Name: "movementsensor", - Model: model, - API: movementsensor.API, - ConvertedAttributes: &Config{ - BoardName: "board", - I2cBus: "3", - SingleTap: tap, - FreeFall: ff, - }, - } - - t.Run("new adxl has interrupt counts set to 0", func(t *testing.T) { - adxl, err := makeAdxl345(ctx, deps, cfg, logger, i2c) - test.That(t, err, test.ShouldBeNil) - - readings, err := adxl.Readings(ctx, map[string]interface{}{}) - test.That(t, err, test.ShouldBeNil) - test.That(t, readings["freefall_count"], test.ShouldEqual, 0) - test.That(t, readings["single_tap_count"], test.ShouldEqual, 0) - }) - - t.Run("interrupts have been found correctly when both are configured to the same pin", func(t *testing.T) { - adxl, err := makeAdxl345(ctx, deps, cfg, logger, i2c) - test.That(t, err, test.ShouldBeNil) - - sendInterrupt(ctx, adxl, t, interrupt, "freefall_count") - - readings, err := adxl.Readings(ctx, map[string]interface{}{}) - test.That(t, err, test.ShouldBeNil) - test.That(t, readings["freefall_count"], test.ShouldEqual, 1) - test.That(t, readings["single_tap_count"], test.ShouldEqual, 1) - }) - - t.Run("interrupts have been found correctly only tap has been configured", func(t *testing.T) { - cfg := resource.Config{ - Name: "movementsensor", - Model: model, - API: movementsensor.API, - ConvertedAttributes: &Config{ - BoardName: "board", - I2cBus: "3", - SingleTap: tap, - }, - } - - adxl, err := makeAdxl345(ctx, deps, cfg, logger, i2c) - test.That(t, err, test.ShouldBeNil) - - sendInterrupt(ctx, adxl, t, interrupt, "single_tap_count") - - readings, err := adxl.Readings(ctx, map[string]interface{}{}) - test.That(t, err, test.ShouldBeNil) - test.That(t, readings["freefall_count"], test.ShouldEqual, 0) - test.That(t, readings["single_tap_count"], test.ShouldEqual, 1) - }) - - t.Run("interrupts have been found correctly only freefall has been configured", func(t *testing.T) { - cfg = resource.Config{ - Name: "movementsensor", - Model: model, - API: movementsensor.API, - ConvertedAttributes: &Config{ - BoardName: "board", - I2cBus: "3", - FreeFall: ff, - }, - } - - adxl, err := makeAdxl345(ctx, deps, cfg, logger, i2c) - test.That(t, err, test.ShouldBeNil) - - sendInterrupt(ctx, adxl, t, interrupt, "freefall_count") - - readings, err := adxl.Readings(ctx, map[string]interface{}{}) - test.That(t, err, test.ShouldBeNil) - test.That(t, readings["freefall_count"], test.ShouldEqual, 1) - test.That(t, readings["single_tap_count"], test.ShouldEqual, 0) - }) -} - -func TestReadInterrupts(t *testing.T) { - cancelContext, cancelFunc := context.WithCancel(context.Background()) - defer cancelFunc() - - i2cHandle := &inject.I2CHandle{} - i2cHandle.CloseFunc = func() error { return nil } - i2c := &inject.I2C{} - i2c.OpenHandleFunc = func(addr byte) (buses.I2CHandle, error) { - return i2cHandle, nil - } - - t.Run("increments tap and freefall counts when both interrupts have gone off", func(t *testing.T) { - i2cHandle.ReadBlockDataFunc = func(context.Context, byte, uint8) ([]byte, error) { - intSourceRegister := byte(1<<6) + byte(1<<2) - return []byte{intSourceRegister}, nil - } - - sensor := &adxl345{ - bus: i2c, - interruptsFound: map[InterruptID]int{}, - interruptsEnabled: byte(1<<6 + 1<<2), - } - sensor.readInterrupts(cancelContext) - test.That(t, sensor.interruptsFound[singleTap], test.ShouldEqual, 1) - test.That(t, sensor.interruptsFound[freeFall], test.ShouldEqual, 1) - }) - - t.Run("increments freefall count only when freefall has gone off", func(t *testing.T) { - i2cHandle.ReadBlockDataFunc = func(context.Context, byte, uint8) ([]byte, error) { - intSourceRegister := byte(1 << 2) - return []byte{intSourceRegister}, nil - } - - sensor := &adxl345{ - bus: i2c, - interruptsFound: map[InterruptID]int{}, - interruptsEnabled: byte(1<<6 + 1<<2), - } - sensor.readInterrupts(cancelContext) - test.That(t, sensor.interruptsFound[singleTap], test.ShouldEqual, 0) - test.That(t, sensor.interruptsFound[freeFall], test.ShouldEqual, 1) - }) - - t.Run("increments tap count only when only tap has gone off", func(t *testing.T) { - i2cHandle.ReadBlockDataFunc = func(context.Context, byte, uint8) ([]byte, error) { - intSourceRegister := byte(1 << 6) - return []byte{intSourceRegister}, nil - } - - sensor := &adxl345{ - bus: i2c, - interruptsFound: map[InterruptID]int{}, - interruptsEnabled: byte(1<<6 + 1<<2), - } - sensor.readInterrupts(cancelContext) - test.That(t, sensor.interruptsFound[singleTap], test.ShouldEqual, 1) - test.That(t, sensor.interruptsFound[freeFall], test.ShouldEqual, 0) - }) - - t.Run("does not increment counts when neither interrupt has gone off", func(t *testing.T) { - i2cHandle.ReadBlockDataFunc = func(context.Context, byte, uint8) ([]byte, error) { - intSourceRegister := byte(0) - return []byte{intSourceRegister}, nil - } - - sensor := &adxl345{ - bus: i2c, - interruptsFound: map[InterruptID]int{}, - interruptsEnabled: byte(1<<6 + 1<<2), - } - sensor.readInterrupts(cancelContext) - test.That(t, sensor.interruptsFound[singleTap], test.ShouldEqual, 0) - test.That(t, sensor.interruptsFound[freeFall], test.ShouldEqual, 0) - }) -} - -func TestLinearAcceleration(t *testing.T) { - linearAccelMockData := make([]byte, 16) - // x-accel - linearAccelMockData[0] = 40 - linearAccelMockData[1] = 0 - expectedAccelX := 1.5328125000000001 - // y-accel - linearAccelMockData[2] = 50 - linearAccelMockData[3] = 0 - expectedAccelY := 1.916015625 - // z-accel - linearAccelMockData[4] = 80 - linearAccelMockData[5] = 0 - expectedAccelZ := 3.0656250000000003 - - logger := logging.NewTestLogger(t) - cfg, deps, i2c := setupDependencies(linearAccelMockData) - sensor, err := makeAdxl345(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) -} diff --git a/components/movementsensor/adxl345/interrupts.go b/components/movementsensor/adxl345/interrupts.go deleted file mode 100644 index 768b313c82c..00000000000 --- a/components/movementsensor/adxl345/interrupts.go +++ /dev/null @@ -1,99 +0,0 @@ -//go:build linux - -// Package adxl345 is for an ADXL345 accelerometer. This file is for the interrupt-based -// functionality on the chip. -package adxl345 - -// addresses relevant to interrupts. -const ( - // an unsigned time value representing the maximum time that an event must be above the - // THRESH_TAP threshold to qualify as a tap event [625 µs/LSB]. - durAddr byte = 0x21 - // info on which interrupts have been enabled. - intEnableAddr byte = 0x2E - // info on which interrupt pin to send each interrupt. - intMapAddr byte = 0x2F - // info on which interrupt has gone off since the last time this address has been read from. - intSourceAddr byte = 0x30 - // an unsigned time value representing the wait time from the detection of a tap event to the - // start of the time window [1.25 ms/LSB]. - latentAddr byte = 0x22 - // info on which axes have been turned on for taps (X, Y, Z are bits 2, 1, 0 respectively). - tapAxesAddr byte = 0x2A - // an unsigned threshold value for tap interrupts [62.5 mg/LSB ]. - threshTapAddr byte = 0x1D - // che threshold value, in unsigned format, for free-fall detection. - threshFfAddr byte = 0x28 - // the minimum time that the value of all axes must be less than THRESH_FF to generate a free-fall interrupt. - timeFfAddr byte = 0x29 -) - -// InterruptID is a type of interrupts available on ADXL345. -type InterruptID = uint8 - -const ( - // singleTap is a key value used to find various needs associated with this interrupt. - singleTap InterruptID = iota - // freeFall is a key value used to find various needs associated with this interrupt. - freeFall InterruptID = iota -) - -var interruptBitPosition = map[InterruptID]byte{ - singleTap: 1 << 6, - freeFall: 1 << 2, -} - -/* -From the data sheet: - - In general, a good starting point is to set the Dur register to a value greater - than 0x10 (10 ms), the Latent register to a value greater than 0x10 (20 ms), the - Window register to a value greater than 0x40 (80 ms), and the ThreshTap register - to a value greater than 0x30 (3 g). -*/ -var defaultRegisterValues = map[byte]byte{ - // Interrupt Enabled - intEnableAddr: 0x00, - intMapAddr: 0x00, - - // Single Tap & Double Tap - tapAxesAddr: 0x07, - threshTapAddr: 0x30, - durAddr: 0x10, - latentAddr: 0x10, - - // Free Fall - timeFfAddr: 0x20, // 0x14 - 0x46 are recommended - threshFfAddr: 0x07, // 0x05 - 0x09 are recommended -} - -const ( - // threshTapScaleFactor is the scale factor for THRESH_TAP register. - threshTapScaleFactor float32 = 62.5 - // durScaleFactor is the scale factor for DUR register. - durScaleFactor float32 = 625 - // timeFfScaleFactor is the scale factor for TIME_FF register. - timeFfScaleFactor float32 = .5 - // threshFfScaleFactor is the scale factor for THRESH_FF register. - threshFfScaleFactor float32 = 62.5 -) - -const ( - xBit byte = 1 << 0 - yBit = 1 << 1 - zBit = 1 << 2 -) - -func getAxes(excludeX, excludeY, excludeZ bool) byte { - var tapAxes byte - if !excludeX { - tapAxes |= xBit - } - if !excludeY { - tapAxes |= yBit - } - if !excludeZ { - tapAxes |= zBit - } - return tapAxes -} diff --git a/components/movementsensor/imuwit/imu.go b/components/movementsensor/imuwit/imu.go deleted file mode 100644 index f3655438bb0..00000000000 --- a/components/movementsensor/imuwit/imu.go +++ /dev/null @@ -1,412 +0,0 @@ -// Package imuwit implements wit imus. -package imuwit - -/* -Sensor Manufacturer: Wit-motion -Supported Sensor Models: HWT901B, BWT901, BWT61CL -Supported OS: Linux -Tested Sensor Models and User Manuals: - - BWT61CL: https://drive.google.com/file/d/1cUTginKXArkHvwPB4LdqojG-ixm7PXCQ/view - BWT901: https://drive.google.com/file/d/18bScCGO5vVZYcEeNKjXNtjnT8OVlrHGI/view - HWT901B TTL: https://drive.google.com/file/d/10HW4MhvhJs4RP0ko7w2nnzwmzsFCKPs6/view - -This driver will connect to the sensor using a usb connection given as a serial path -using a default baud rate of 115200. We allow baud rate values of: 9600, 115200 -The default baud rate can be used to connect to all models. Only the HWT901B's baud rate is changeable. -We ask the user to refer to the datasheet if any baud rate changes are required for their application. - -Other models that connect over serial may work, but we ask the user to refer to wit-motion's datasheet -in that case as well. As of Feb 2023, Wit-motion has 48 gyro/inclinometer/imu models with varied levels of -driver commonality. - -Note: Model HWT905-TTL is not supported under the model name "imu-wit". Use the model name "imu-wit-hwt905" -for HWT905-TTL. -*/ - -import ( - "bufio" - "context" - "fmt" - "io" - "math" - "sync" - - "github.com/golang/geo/r3" - slib "github.com/jacobsa/go-serial/serial" - geo "github.com/kellydunn/golang-geo" - "github.com/pkg/errors" - "go.viam.com/utils" - - "go.viam.com/rdk/components/movementsensor" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/resource" - "go.viam.com/rdk/spatialmath" - rutils "go.viam.com/rdk/utils" -) - -var ( - model = resource.DefaultModelFamily.WithModel("imu-wit") - - // This is the dynamic integral cumulative error. - // Data acquired from datasheets of supported models. Links above. - compassAccuracy = 0.5 -) - -var baudRateList = []uint{115200, 9600, 0} - -// max tilt to use tilt compensation is 45 degrees. -var maxTiltInRad = rutils.DegToRad(45) - -// Config is used for converting a witmotion IMU MovementSensor config attributes. -type Config struct { - Port string `json:"serial_path"` - BaudRate uint `json:"serial_baud_rate,omitempty"` -} - -// Validate ensures all parts of the config are valid. -func (cfg *Config) Validate(path string) ([]string, error) { - // Validating serial path - if cfg.Port == "" { - return nil, resource.NewConfigValidationFieldRequiredError(path, "serial_path") - } - - // Validating baud rate - if !rutils.ValidateBaudRate(baudRateList, int(cfg.BaudRate)) { - return nil, resource.NewConfigValidationError(path, errors.Errorf("Baud rate is not in %v", baudRateList)) - } - - return nil, nil -} - -func init() { - resource.RegisterComponent(movementsensor.API, model, resource.Registration[movementsensor.MovementSensor, *Config]{ - Constructor: newWit, - }) -} - -type wit struct { - resource.Named - resource.AlwaysRebuild - angularVelocity spatialmath.AngularVelocity - orientation spatialmath.EulerAngles - acceleration r3.Vector - magnetometer r3.Vector - compassheading float64 - numBadReadings uint32 - err movementsensor.LastError - hasMagnetometer bool - mu sync.Mutex - reconfigMu sync.Mutex - port io.ReadWriteCloser - workers *utils.StoppableWorkers - logger logging.Logger - baudRate uint - serialPath string -} - -func (imu *wit) Reconfigure(ctx context.Context, deps resource.Dependencies, conf resource.Config) error { - imu.reconfigMu.Lock() - defer imu.reconfigMu.Unlock() - - newConf, err := resource.NativeConfig[*Config](conf) - if err != nil { - return err - } - - imu.baudRate = newConf.BaudRate - imu.serialPath = newConf.Port - - return nil -} - -func (imu *wit) AngularVelocity(ctx context.Context, extra map[string]interface{}) (spatialmath.AngularVelocity, error) { - imu.mu.Lock() - defer imu.mu.Unlock() - return imu.angularVelocity, imu.err.Get() -} - -func (imu *wit) LinearVelocity(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { - imu.mu.Lock() - defer imu.mu.Unlock() - return r3.Vector{}, movementsensor.ErrMethodUnimplementedLinearVelocity -} - -func (imu *wit) Orientation(ctx context.Context, extra map[string]interface{}) (spatialmath.Orientation, error) { - imu.mu.Lock() - defer imu.mu.Unlock() - return &imu.orientation, imu.err.Get() -} - -// LinearAcceleration returns linear acceleration in mm_per_sec_per_sec. -func (imu *wit) LinearAcceleration(ctx context.Context, extra map[string]interface{}) (r3.Vector, error) { - imu.mu.Lock() - defer imu.mu.Unlock() - return imu.acceleration, imu.err.Get() -} - -// getMagnetometer returns magnetic field in gauss. -func (imu *wit) getMagnetometer() (r3.Vector, error) { - imu.mu.Lock() - defer imu.mu.Unlock() - return imu.magnetometer, imu.err.Get() -} - -func (imu *wit) CompassHeading(ctx context.Context, extra map[string]interface{}) (float64, error) { - var err error - - imu.mu.Lock() - defer imu.mu.Unlock() - - // this will compensate for a tilted IMU if the tilt is less than 45 degrees - // do not let the imu near permanent magnets or things that make a strong magnetic field - imu.compassheading = imu.calculateCompassHeading() - - return imu.compassheading, err -} - -// Helper function to calculate compass heading with tilt compensation. -func (imu *wit) calculateCompassHeading() float64 { - pitch := imu.orientation.Pitch - roll := imu.orientation.Roll - - var x, y float64 - - // Tilt compensation only works if the pitch and roll are between -45 and 45 degrees. - if math.Abs(roll) <= maxTiltInRad && math.Abs(pitch) <= maxTiltInRad { - x, y = imu.calculateTiltCompensation(roll, pitch) - } else { - x = imu.magnetometer.X - y = imu.magnetometer.Y - } - - // calculate -180 to 180 heading from radians - // North (y) is 0 so the π/2 - atan2(y, x) identity is used - // directly - rad := math.Atan2(y, x) // -180 to 180 heading - compass := rutils.RadToDeg(rad) - compass = math.Mod(compass, 360) - compass = math.Mod(compass+360, 360) // compass 0 to 360 - - return compass -} - -func (imu *wit) calculateTiltCompensation(roll, pitch float64) (float64, float64) { - // calculate adjusted magnetometer readings. These get less accurate as the tilt angle increases. - xComp := imu.magnetometer.X*math.Cos(pitch) + imu.magnetometer.Z*math.Sin(pitch) - yComp := imu.magnetometer.X*math.Sin(roll)*math.Sin(pitch) + - imu.magnetometer.Y*math.Cos(roll) - imu.magnetometer.Z*math.Sin(roll)*math.Cos(pitch) - - return xComp, yComp -} - -func (imu *wit) Position(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - return geo.NewPoint(0, 0), 0, movementsensor.ErrMethodUnimplementedPosition -} - -func (imu *wit) Accuracy(ctx context.Context, extra map[string]interface{}) (*movementsensor.Accuracy, error, -) { - // return the compass heading error from the datasheet (0.5) of the witIMU if - // the pitch angle is less than 45 degrees and the roll angle is near zero - // mag projects at angles over this threshold cannot be determined because of the larger contribution of other - // orientations to the true compass heading - // return NaN for compass accuracy otherwise. - imu.mu.Lock() - defer imu.mu.Unlock() - - roll := imu.orientation.Roll - pitch := imu.orientation.Pitch - - if math.Abs(roll) <= 1 && math.Abs(pitch) <= maxTiltInRad { - return &movementsensor.Accuracy{CompassDegreeError: float32(compassAccuracy)}, nil - } - - return &movementsensor.Accuracy{CompassDegreeError: float32(math.NaN())}, nil -} - -func (imu *wit) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { - readings, err := movementsensor.DefaultAPIReadings(ctx, imu, extra) - if err != nil { - return nil, err - } - - mag, err := imu.getMagnetometer() - if err != nil { - return nil, err - } - readings["magnetometer"] = mag - - return readings, err -} - -func (imu *wit) Properties(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { - imu.mu.Lock() - defer imu.mu.Unlock() - - return &movementsensor.Properties{ - AngularVelocitySupported: true, - OrientationSupported: true, - LinearAccelerationSupported: true, - CompassHeadingSupported: imu.hasMagnetometer, - }, nil -} - -// newWit creates a new Wit IMU. -func newWit( - 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 - } - - options := slib.OpenOptions{ - PortName: newConf.Port, - BaudRate: 115200, - DataBits: 8, - StopBits: 1, - MinimumReadSize: 1, - } - - if newConf.BaudRate > 0 { - options.BaudRate = newConf.BaudRate - } else { - logger.CWarnf(ctx, - "no valid serial_baud_rate set, setting to default of %d, baud rate of wit imus are: %v", options.BaudRate, baudRateList, - ) - } - - i := wit{ - Named: conf.ResourceName().AsNamed(), - logger: logger, - err: movementsensor.NewLastError(1, 1), - } - logger.CDebugf(ctx, "initializing wit serial connection with parameters: %+v", options) - i.port, err = slib.Open(options) - if err != nil { - return nil, err - } - - portReader := bufio.NewReader(i.port) - i.startUpdateLoop(portReader, logger) - - return &i, nil -} - -func (imu *wit) startUpdateLoop(portReader *bufio.Reader, logger logging.Logger) { - imu.hasMagnetometer = false - imu.workers = utils.NewBackgroundStoppableWorkers(func(ctx context.Context) { - defer utils.UncheckedErrorFunc(func() error { - if imu.port != nil { - if err := imu.port.Close(); err != nil { - imu.port = nil - return err - } - imu.port = nil - } - return nil - }) - - for { - if ctx.Err() != nil { - return - } - select { - case <-ctx.Done(): - return - default: - } - - line, err := portReader.ReadString('U') - - func() { - imu.mu.Lock() - defer imu.mu.Unlock() - - switch { - case err != nil: - imu.err.Set(err) - imu.numBadReadings++ - if imu.numBadReadings < 20 { - logger.CError(ctx, err, "Check if wit imu is disconnected from port") - } else { - logger.CDebug(ctx, err) - } - - case len(line) != 11: - imu.numBadReadings++ - return - default: - imu.err.Set(imu.parseWIT(line)) - } - }() - } - }) -} - -func scale(a, b byte, r float64) float64 { - x := float64(int(b)<<8|int(a)) / 32768.0 // 0 -> 2 - x *= r // 0 -> 2r - x += r - x = math.Mod(x, r*2) - x -= r - return x -} - -func convertMagByteToTesla(a, b byte) float64 { - x := float64(int(int8(b))<<8 | int(a)) // 0 -> 2 - return x -} - -func (imu *wit) parseWIT(line string) error { - if line[0] == 0x52 { - if len(line) < 7 { - return fmt.Errorf("line is wrong for imu angularVelocity %d %v", len(line), line) - } - imu.angularVelocity.X = scale(line[1], line[2], 2000) - imu.angularVelocity.Y = scale(line[3], line[4], 2000) - imu.angularVelocity.Z = scale(line[5], line[6], 2000) - } - - if line[0] == 0x53 { - if len(line) < 7 { - return fmt.Errorf("line is wrong for imu orientation %d %v", len(line), line) - } - - imu.orientation.Roll = rutils.DegToRad(scale(line[1], line[2], 180)) - imu.orientation.Pitch = rutils.DegToRad(scale(line[3], line[4], 180)) - imu.orientation.Yaw = rutils.DegToRad(scale(line[5], line[6], 180)) - } - - if line[0] == 0x51 { - if len(line) < 7 { - return fmt.Errorf("line is wrong for imu acceleration %d %v", len(line), line) - } - imu.acceleration.X = scale(line[1], line[2], 16) * 9.80665 // converts to m_per_sec_per_sec in NYC - imu.acceleration.Y = scale(line[3], line[4], 16) * 9.80665 - imu.acceleration.Z = scale(line[5], line[6], 16) * 9.80665 - } - - if line[0] == 0x54 { - imu.hasMagnetometer = true - if len(line) < 7 { - return fmt.Errorf("line is wrong for imu magnetometer %d %v", len(line), line) - } - imu.magnetometer.X = convertMagByteToTesla(line[1], line[2]) // converts uT (micro Tesla) - imu.magnetometer.Y = convertMagByteToTesla(line[3], line[4]) - imu.magnetometer.Z = convertMagByteToTesla(line[5], line[6]) - } - - return nil -} - -// Close shuts down wit and closes imu.port. -func (imu *wit) Close(ctx context.Context) error { - imu.logger.CDebug(ctx, "Closing wit motion imu") - imu.workers.Stop() - imu.logger.CDebug(ctx, "Closed wit motion imu") - return imu.err.Get() -} diff --git a/components/movementsensor/imuwit/imuhwt905.go b/components/movementsensor/imuwit/imuhwt905.go deleted file mode 100644 index 9044e467a68..00000000000 --- a/components/movementsensor/imuwit/imuhwt905.go +++ /dev/null @@ -1,132 +0,0 @@ -// Package imuwit implements wit imu -package imuwit - -/* -Sensor Manufacturer: Wit-motion -Supported Sensor Models: HWT905 -Supported OS: Linux -This driver only supports HWT905-TTL model of Wit imu. -Tested Sensor Models and User Manuals: - HWT905 TTL: https://drive.google.com/file/d/1RV7j8yzZjPsPmvQY--1UHr_FhBzc2YwO/view -*/ - -import ( - "bufio" - "context" - "errors" - "time" - - slib "github.com/jacobsa/go-serial/serial" - "go.viam.com/utils" - - "go.viam.com/rdk/components/movementsensor" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/resource" -) - -var model905 = resource.DefaultModelFamily.WithModel("imu-wit-hwt905") - -func init() { - resource.RegisterComponent(movementsensor.API, model905, resource.Registration[movementsensor.MovementSensor, *Config]{ - Constructor: newWit905, - }) -} - -// newWit creates a new Wit IMU. -func newWit905( - 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 - } - - i := wit{ - Named: conf.ResourceName().AsNamed(), - logger: logger, - err: movementsensor.NewLastError(1, 1), - baudRate: newConf.BaudRate, - serialPath: newConf.Port, - } - - options := slib.OpenOptions{ - PortName: i.serialPath, - BaudRate: i.baudRate, - DataBits: 8, - StopBits: 1, - MinimumReadSize: 1, - } - if err := i.Reconfigure(ctx, deps, conf); err != nil { - return nil, err - } - - logger.Debugf("initializing wit serial connection with parameters: %+v", options) - i.port, err = slib.Open(options) - if err != nil { - return nil, err - } - - portReader := bufio.NewReader(i.port) - i.start905UpdateLoop(portReader, logger) - - return &i, nil -} - -func (imu *wit) start905UpdateLoop(portReader *bufio.Reader, logger logging.Logger) { - imu.hasMagnetometer = false - imu.workers = utils.NewBackgroundStoppableWorkers(func(cancelCtx context.Context) { - for { - if cancelCtx.Err() != nil { - return - } - - select { - case <-cancelCtx.Done(): - return - case <-time.After(10 * time.Second): - logger.Warn("ReadString timeout exceeded") - return - default: - line, err := readWithTimeout(portReader, 'U') - if err != nil { - logger.Error(err) - continue - } - - switch { - case len(line) != 11: - imu.numBadReadings++ - default: - imu.err.Set(imu.parseWIT(line)) - } - } - } - }) -} - -// readWithTimeout tries to read from the buffer until the delimiter is found or timeout occurs. -func readWithTimeout(r *bufio.Reader, delim byte) (string, error) { - lineChan := make(chan string) - errChan := make(chan error) - - go func() { - line, err := r.ReadString(delim) - if err != nil { - errChan <- err - return - } - lineChan <- line - }() - - select { - case line := <-lineChan: - return line, nil - case err := <-errChan: - return "", err - case <-time.After(10 * time.Second): - return "", errors.New("timeout exceeded while reading from serial port") - } -} diff --git a/components/movementsensor/mpu6050/mpu6050.go b/components/movementsensor/mpu6050/mpu6050.go deleted file mode 100644 index 68068158e4b..00000000000 --- a/components/movementsensor/mpu6050/mpu6050.go +++ /dev/null @@ -1,350 +0,0 @@ -//go:build linux - -// Package mpu6050 implements the movementsensor interface for an MPU-6050 6-axis accelerometer. A -// datasheet for this chip is at -// https://components101.com/sites/default/files/component_datasheet/MPU6050-DataSheet.pdf and a -// description of the I2C registers is at -// https://download.datasheets.com/pdfs/2015/3/19/8/3/59/59/invse_/manual/5rm-mpu-6000a-00v4.2.pdf -// -// We support reading the accelerometer, gyroscope, and thermometer data off of the chip. We do not -// yet support using the digital interrupt pin to notify on events (freefall, collision, etc.), -// nor do we yet support using the secondary I2C connection to add an external clock or -// 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 0x68 -// - 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 mpu6050 - -import ( - "context" - "fmt" - "sync" - "time" - - "github.com/golang/geo/r3" - geo "github.com/kellydunn/golang-geo" - "github.com/pkg/errors" - goutils "go.viam.com/utils" - - "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" -) - -var model = resource.DefaultModelFamily.WithModel("gyro-mpu6050") - -const ( - defaultAddressRegister = 117 - expectedDefaultAddress = 0x68 - 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: newMpu6050, - }) -} - -type mpu6050 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-MPU6050 device at address %d: response '%d'", - address, defaultAddress) -} - -// newMpu6050 constructs a new Mpu6050 object. -func newMpu6050( - 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 makeMpu6050(ctx, deps, conf, logger, bus) -} - -// This function is separated from NewMpu6050 solely so you can inject a mock I2C bus in tests. -func makeMpu6050( - 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 MPU6050 sensor", address) - - sensor := &mpu6050{ - 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 non-alternative address (0x68) - defaultAddress, err := sensor.readByte(ctx, defaultAddressRegister) - if err != nil { - return nil, addressReadError(err, address, newConf.I2cBus) - } - if defaultAddress != expectedDefaultAddress { - 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 MPU6050: '%s'", err.Error()) - } - - // 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 MPU6050 sensor: '%s'", err) - continue - } - - linearAcceleration := toLinearAcceleration(rawData[0:6]) - // Taken straight from the MPU6050 register map. Yes, these are weird constants. - temperature := float64(utils.Int16FromBytesBE(rawData[6:8]))/340.0 + 36.53 - 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 *mpu6050) 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 *mpu6050) 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 *mpu6050) 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 / (1 << 15) -} - -// 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, maxRotation), - Y: setScale(gy, maxRotation), - Z: setScale(gz, maxRotation), - } -} - -// 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 := 2.0 * 9.81 /* m/sec/sec */ - return r3.Vector{ - X: setScale(x, maxAcceleration), - Y: setScale(y, maxAcceleration), - Z: setScale(z, maxAcceleration), - } -} - -func (mpu *mpu6050) 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) { - return r3.Vector{}, movementsensor.ErrMethodUnimplementedLinearVelocity -} - -func (mpu *mpu6050) 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 *mpu6050) 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) { - return 0, movementsensor.ErrMethodUnimplementedCompassHeading -} - -func (mpu *mpu6050) 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) { - return movementsensor.UnimplementedOptionalAccuracies(), nil -} - -func (mpu *mpu6050) 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 *mpu6050) 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 { - 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/components/movementsensor/mpu6050/mpu6050_nonlinux.go b/components/movementsensor/mpu6050/mpu6050_nonlinux.go deleted file mode 100644 index 9087d5a9c18..00000000000 --- a/components/movementsensor/mpu6050/mpu6050_nonlinux.go +++ /dev/null @@ -1,2 +0,0 @@ -// Package mpu6050 is only implemented for Linux systems. -package mpu6050 diff --git a/components/movementsensor/mpu6050/mpu6050_test.go b/components/movementsensor/mpu6050/mpu6050_test.go deleted file mode 100644 index 14452db59f3..00000000000 --- a/components/movementsensor/mpu6050/mpu6050_test.go +++ /dev/null @@ -1,263 +0,0 @@ -//go:build linux - -package mpu6050 - -import ( - "context" - "testing" - - "github.com/pkg/errors" - "go.viam.com/test" - "go.viam.com/utils/testutils" - - "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" -) - -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 := makeMpu6050(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 := makeMpu6050(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 := makeMpu6050(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 := makeMpu6050(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 := makeMpu6050(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 := makeMpu6050(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) -} diff --git a/components/movementsensor/register/register.go b/components/movementsensor/register/register.go index ae7c5d8f260..197d779e67a 100644 --- a/components/movementsensor/register/register.go +++ b/components/movementsensor/register/register.go @@ -3,15 +3,12 @@ package register import ( // Load all movementsensors. - _ "go.viam.com/rdk/components/movementsensor/adxl345" _ "go.viam.com/rdk/components/movementsensor/dualgps" _ "go.viam.com/rdk/components/movementsensor/fake" _ "go.viam.com/rdk/components/movementsensor/gpsnmea" _ "go.viam.com/rdk/components/movementsensor/gpsrtk" _ "go.viam.com/rdk/components/movementsensor/imuvectornav" - _ "go.viam.com/rdk/components/movementsensor/imuwit" _ "go.viam.com/rdk/components/movementsensor/merged" - _ "go.viam.com/rdk/components/movementsensor/mpu6050" _ "go.viam.com/rdk/components/movementsensor/replay" _ "go.viam.com/rdk/components/movementsensor/wheeledodometry" ) diff --git a/components/powersensor/ina/ina.go b/components/powersensor/ina/ina.go deleted file mode 100644 index ff3e3e34529..00000000000 --- a/components/powersensor/ina/ina.go +++ /dev/null @@ -1,355 +0,0 @@ -//go:build linux - -// Package ina implements ina power sensors to measure voltage, current, and power -// INA219 datasheet: https://www.ti.com/lit/ds/symlink/ina219.pdf -// Example repo: https://github.com/periph/devices/blob/main/ina219/ina219.go -// INA226 datasheet: https://www.ti.com/lit/ds/symlink/ina226.pdf - -// The voltage, current and power can be read as -// 16 bit big endian integers from their given registers. -// This value is multiplied by the register LSB to get the reading in nanounits. - -// Voltage LSB: 1.25 mV for INA226, 4 mV for INA219 -// Current LSB: maximum expected current of the system / (1 << 15) -// Power LSB: 25*CurrentLSB for INA226, 20*CurrentLSB for INA219 - -// The calibration register is programmed to measure current and power properly. -// The calibration register is set to: calibratescale / (currentLSB * senseResistor) - -package ina - -import ( - "context" - "errors" - "fmt" - "strconv" - "sync" - - "github.com/d2r2/go-i2c" - i2clog "github.com/d2r2/go-logger" - "go.viam.com/utils" - - "go.viam.com/rdk/components/powersensor" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/resource" -) - -const ( - modelName219 = "ina219" - modelName226 = "ina226" - defaultI2Caddr = 0x40 - configRegister = 0x00 - shuntVoltageRegister = 0x01 - busVoltageRegister = 0x02 - powerRegister = 0x03 - currentRegister = 0x04 - calibrationRegister = 0x05 -) - -// values for inas in nano units so need to convert. -var ( - senseResistor = toNano(0.1) // .1 ohm - maxCurrent219 = toNano(3.2) // 3.2 amp - maxCurrent226 = toNano(20) // 20 amp -) - -// need to scale, making sure to not overflow int64. -var ( - calibratescale219 = (toNano(1) * toNano(1) / 100000) << 12 // .04096 is internal fixed value for ina219 - calibrateScale226 = (toNano(1) * toNano(1) / 100000) << 9 // .00512 is internal fixed value for ina226 -) - -var inaModels = []string{modelName219, modelName226} - -// Config is used for converting config attributes. -type Config struct { - I2CBus string `json:"i2c_bus"` - I2cAddr int `json:"i2c_addr,omitempty"` - MaxCurrent float64 `json:"max_current_amps,omitempty"` - ShuntResistance float64 `json:"shunt_resistance,omitempty"` -} - -// Validate ensures all parts of the config are valid. -func (conf *Config) Validate(path string) ([]string, error) { - var deps []string - if conf.I2CBus == "" { - return nil, resource.NewConfigValidationFieldRequiredError(path, "i2c_bus") - } - // The bus should be numeric. We store it as a string for consistency with other components, - // but we convert it to an int later, so let's check on that now. - if _, err := strconv.Atoi(conf.I2CBus); err != nil { - return nil, fmt.Errorf("i2c_bus must be numeric, not '%s': %w", conf.I2CBus, err) - } - return deps, nil -} - -func init() { - for _, modelName := range inaModels { - localModelName := modelName - inaModel := resource.DefaultModelFamily.WithModel(modelName) - resource.RegisterComponent( - powersensor.API, - inaModel, - resource.Registration[powersensor.PowerSensor, *Config]{ - Constructor: func( - ctx context.Context, - deps resource.Dependencies, - conf resource.Config, - logger logging.Logger, - ) (powersensor.PowerSensor, error) { - newConf, err := resource.NativeConfig[*Config](conf) - if err != nil { - return nil, err - } - return newINA(conf.ResourceName(), newConf, logger, localModelName) - }, - }) - } -} - -func newINA( - name resource.Name, - conf *Config, - logger logging.Logger, - modelName string, -) (powersensor.PowerSensor, error) { - err := i2clog.ChangePackageLogLevel("i2c", i2clog.InfoLevel) - if err != nil { - return nil, err - } - - addr := conf.I2cAddr - if addr == 0 { - addr = defaultI2Caddr - logger.Infof("using i2c address : %d", defaultI2Caddr) - } - - maxCurrent := toNano(conf.MaxCurrent) - if maxCurrent == 0 { - switch modelName { - case modelName219: - maxCurrent = maxCurrent219 - logger.Info("using default max current 3.2A") - case modelName226: - maxCurrent = maxCurrent226 - logger.Info("using default max current 20A") - } - } - - resistance := toNano(conf.ShuntResistance) - if resistance == 0 { - resistance = senseResistor - logger.Info("using default resistor value 0.1 ohms") - } - - busNumber, err := strconv.Atoi(conf.I2CBus) - if err != nil { - return nil, fmt.Errorf("non-numeric I2C bus number '%s': %w", conf.I2CBus, err) - } - - s := &ina{ - Named: name.AsNamed(), - logger: logger, - model: modelName, - bus: busNumber, - addr: byte(addr), - maxCurrent: maxCurrent, - resistance: resistance, - } - - err = s.setCalibrationScale(modelName) - if err != nil { - return nil, err - } - - return s, nil -} - -// ina is a i2c sensor device that reports voltage, current and power. -type ina struct { - resource.Named - resource.AlwaysRebuild - resource.TriviallyCloseable - - // This mutex is subtly important. The I2C library we're using is not thread safe because - // reading from a register is not atomic! So, this mutex is used to ensure that reading from - // two different registers in two different goroutines won't have race conditions resulting in - // bad data. All the fields in this struct are immutable, but the mutex is still needed to make - // interactions with the I2C bus atomic. - mu sync.Mutex - - logger logging.Logger - model string - bus int - addr byte - currentLSB int64 - powerLSB int64 - cal uint16 - maxCurrent int64 - resistance int64 -} - -func (d *ina) setCalibrationScale(modelName string) error { - var calibratescale int64 - d.currentLSB = d.maxCurrent / (1 << 15) - switch modelName { - case modelName219: - calibratescale = calibratescale219 - d.powerLSB = (d.maxCurrent*20 + (1 << 14)) / (1 << 15) - case modelName226: - calibratescale = calibrateScale226 - d.powerLSB = 25 * d.currentLSB - default: - return errors.New("ina model not supported") - } - - // Calibration Register = calibration scale / (current LSB * Shunt Resistance) - // Where lsb is in Amps and resistance is in ohms. - // Calibration register is 16 bits. - cal := calibratescale / (d.currentLSB * d.resistance) - if cal >= (1 << 16) { - return fmt.Errorf("ina calibrate: calibration register value invalid %d", cal) - } - d.cal = uint16(cal) - - return nil -} - -func (d *ina) calibrate() error { - handle, err := i2c.NewI2C(d.addr, d.bus) - if err != nil { - d.logger.Errorf("can't open ina i2c: %s", err) - return err - } - defer utils.UncheckedErrorFunc(handle.Close) - - // use the calibration result to set the scaling factor - // of the current and power registers for the maximum resolution - err = handle.WriteRegU16BE(calibrationRegister, d.cal) - if err != nil { - return err - } - - // set the config register to all default values. - err = handle.WriteRegU16BE(configRegister, uint16(0x399F)) - if err != nil { - return err - } - return nil -} - -func (d *ina) Voltage(ctx context.Context, extra map[string]interface{}) (float64, bool, error) { - d.mu.Lock() - defer d.mu.Unlock() - handle, err := i2c.NewI2C(d.addr, d.bus) - if err != nil { - d.logger.CErrorf(ctx, "can't open ina i2c: %s", err) - return 0, false, err - } - defer utils.UncheckedErrorFunc(handle.Close) - - bus, err := handle.ReadRegS16BE(busVoltageRegister) - if err != nil { - return 0, false, err - } - - var voltage float64 - switch d.model { - case modelName226: - // voltage is 1.25 mV/bit for the ina226 - voltage = float64(bus) * 1.25e-3 - case modelName219: - // lsb is 4mV, must shift right 3 bits - voltage = float64(bus>>3) * 4 / 1000 - default: - } - - isAC := false - return voltage, isAC, nil -} - -func (d *ina) Current(ctx context.Context, extra map[string]interface{}) (float64, bool, error) { - d.mu.Lock() - defer d.mu.Unlock() - handle, err := i2c.NewI2C(d.addr, d.bus) - if err != nil { - d.logger.CErrorf(ctx, "can't open ina i2c: %s", err) - return 0, false, err - } - defer utils.UncheckedErrorFunc(handle.Close) - - // Calibrate each time the current value is read, so if anything else is also writing to these registers - // we have the correct value. - err = d.calibrate() - if err != nil { - return 0, false, err - } - - rawCur, err := handle.ReadRegS16BE(currentRegister) - if err != nil { - return 0, false, err - } - - current := fromNano(float64(int64(rawCur) * d.currentLSB)) - isAC := false - return current, isAC, nil -} - -func (d *ina) Power(ctx context.Context, extra map[string]interface{}) (float64, error) { - d.mu.Lock() - defer d.mu.Unlock() - handle, err := i2c.NewI2C(d.addr, d.bus) - if err != nil { - d.logger.CErrorf(ctx, "can't open ina i2c handle: %s", err) - return 0, err - } - defer utils.UncheckedErrorFunc(handle.Close) - - // Calibrate each time the power value is read, so if anything else is also writing to these registers - // we have the correct value. - err = d.calibrate() - if err != nil { - return 0, err - } - - pow, err := handle.ReadRegS16BE(powerRegister) - if err != nil { - return 0, err - } - power := fromNano(float64(int64(pow) * d.powerLSB)) - return power, nil -} - -// Readings returns a map with voltage, current, power and isAC. -func (d *ina) Readings(ctx context.Context, extra map[string]interface{}) (map[string]interface{}, error) { - volts, isAC, err := d.Voltage(ctx, nil) - if err != nil { - d.logger.CErrorf(ctx, "failed to get voltage reading: %s", err.Error()) - } - - amps, _, err := d.Current(ctx, nil) - if err != nil { - d.logger.CErrorf(ctx, "failed to get current reading: %s", err.Error()) - } - - watts, err := d.Power(ctx, nil) - if err != nil { - d.logger.CErrorf(ctx, "failed to get power reading: %s", err.Error()) - } - return map[string]interface{}{ - "volts": volts, - "amps": amps, - "is_ac": isAC, - "watts": watts, - }, nil -} - -func toNano(value float64) int64 { - nano := value * 1e9 - return int64(nano) -} - -func fromNano(value float64) float64 { - unit := value / 1e9 - return unit -} diff --git a/components/powersensor/ina/ina_nonlinux.go b/components/powersensor/ina/ina_nonlinux.go deleted file mode 100644 index 10d6ac02206..00000000000 --- a/components/powersensor/ina/ina_nonlinux.go +++ /dev/null @@ -1,2 +0,0 @@ -// Package ina this is blank for mac -package ina diff --git a/components/powersensor/register/register.go b/components/powersensor/register/register.go index 89abb2b1f37..11b563b32d5 100644 --- a/components/powersensor/register/register.go +++ b/components/powersensor/register/register.go @@ -4,6 +4,5 @@ package register import ( // register all powersensors. _ "go.viam.com/rdk/components/powersensor/fake" - _ "go.viam.com/rdk/components/powersensor/ina" _ "go.viam.com/rdk/components/powersensor/renogy" ) diff --git a/go.mod b/go.mod index bc82ad14c81..9d8e9015a07 100644 --- a/go.mod +++ b/go.mod @@ -19,8 +19,6 @@ require ( github.com/charmbracelet/huh v0.6.0 github.com/charmbracelet/huh/spinner v0.0.0-20240917123815-c9b2c9cdb7b6 github.com/creack/pty v1.1.19-0.20220421211855-0d412c9fbeb1 - github.com/d2r2/go-i2c v0.0.0-20191123181816-73a8a799d6bc - github.com/d2r2/go-logger v0.0.0-20210606094344-60e9d1233e22 github.com/de-bkg/gognss v0.0.0-20220601150219-24ccfdcdbb5d github.com/disintegration/imaging v1.6.2 github.com/docker/go-units v0.5.0 diff --git a/go.sum b/go.sum index 9725aeda15f..8952d9a32dd 100644 --- a/go.sum +++ b/go.sum @@ -320,10 +320,6 @@ github.com/creack/pty v1.1.19-0.20220421211855-0d412c9fbeb1 h1:Tw0uuY+3UWYiSbR0+ github.com/creack/pty v1.1.19-0.20220421211855-0d412c9fbeb1/go.mod h1:MOBLtS5ELjhRRrroQr9kyvTxUAFNvYEK993ew/Vr4O4= github.com/curioswitch/go-reassign v0.2.0 h1:G9UZyOcpk/d7Gd6mqYgd8XYWFMw/znxwGDUstnC9DIo= github.com/curioswitch/go-reassign v0.2.0/go.mod h1:x6OpXuWvgfQaMGks2BZybTngWjT84hqJfKoO8Tt/Roc= -github.com/d2r2/go-i2c v0.0.0-20191123181816-73a8a799d6bc h1:HLRSIWzUGMLCq4ldt0W1GLs3nnAxa5EGoP+9qHgh6j0= -github.com/d2r2/go-i2c v0.0.0-20191123181816-73a8a799d6bc/go.mod h1:AwxDPnsgIpy47jbGXZHA9Rv7pDkOJvQbezPuK1Y+nNk= -github.com/d2r2/go-logger v0.0.0-20210606094344-60e9d1233e22 h1:nO+SY4KOMsF/LsZ5EtbSKhiT3M6sv/igo2PEru/xEHI= -github.com/d2r2/go-logger v0.0.0-20210606094344-60e9d1233e22/go.mod h1:eSx+YfcVy5vCjRZBNIhpIpfCGFMQ6XSOSQkDk7+VCpg= github.com/daixiang0/gci v0.2.8/go.mod h1:+4dZ7TISfSmqfAGv59ePaHfNzgGtIkHAhhdKggP1JAc= github.com/daixiang0/gci v0.13.5 h1:kThgmH1yBmZSBCh1EJVxQ7JsHpm5Oms0AMed/0LaH4c= github.com/daixiang0/gci v0.13.5/go.mod h1:12etP2OniiIdP4q+kjUGrC/rUagga7ODbqsom5Eo5Yk=