From 428bd7002e0134cdb2ede9499882bacada486033 Mon Sep 17 00:00:00 2001 From: martha-johnston <106617924+martha-johnston@users.noreply.github.com> Date: Mon, 18 Nov 2024 09:32:05 -0800 Subject: [PATCH] RSDK-9021: imu wit movement sensor module (#4563) --- components/movementsensor/imuwit/imu.go | 412 ------------------ components/movementsensor/imuwit/imuhwt905.go | 132 ------ .../movementsensor/register/register.go | 1 - go.mod | 1 - go.sum | 2 - 5 files changed, 548 deletions(-) delete mode 100644 components/movementsensor/imuwit/imu.go delete mode 100644 components/movementsensor/imuwit/imuhwt905.go 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/register/register.go b/components/movementsensor/register/register.go index 0283860c491..92f59a990f5 100644 --- a/components/movementsensor/register/register.go +++ b/components/movementsensor/register/register.go @@ -5,7 +5,6 @@ import ( // Load all movementsensors. _ "go.viam.com/rdk/components/movementsensor/adxl345" _ "go.viam.com/rdk/components/movementsensor/fake" - _ "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" diff --git a/go.mod b/go.mod index 232796e4cb8..971e22b91f5 100644 --- a/go.mod +++ b/go.mod @@ -46,7 +46,6 @@ require ( github.com/grpc-ecosystem/go-grpc-middleware v1.4.0 github.com/grpc-ecosystem/grpc-gateway/v2 v2.15.2 github.com/invopop/jsonschema v0.6.0 - github.com/jacobsa/go-serial v0.0.0-20180131005756-15cf729a72d4 github.com/jedib0t/go-pretty/v6 v6.4.6 github.com/jhump/protoreflect v1.15.1 github.com/kellydunn/golang-geo v0.7.0 diff --git a/go.sum b/go.sum index eeb722a9119..4c432a537fa 100644 --- a/go.sum +++ b/go.sum @@ -757,8 +757,6 @@ github.com/inconshreveable/mousetrap v1.1.0/go.mod h1:vpF70FUmC8bwa3OWnCshd2FqLf github.com/influxdata/influxdb1-client v0.0.0-20191209144304-8bf82d3c094d/go.mod h1:qj24IKcXYK6Iy9ceXlo3Tc+vtHo9lIhSX5JddghvEPo= github.com/invopop/jsonschema v0.6.0 h1:8e+xY8ZEn8gDHUYylSlLHy22P+SLeIRIHv3nM3hCbmY= github.com/invopop/jsonschema v0.6.0/go.mod h1:O9uiLokuu0+MGFlyiaqtWxwqJm41/+8Nj0lD7A36YH0= -github.com/jacobsa/go-serial v0.0.0-20180131005756-15cf729a72d4 h1:G2ztCwXov8mRvP0ZfjE6nAlaCX2XbykaeHdbT6KwDz0= -github.com/jacobsa/go-serial v0.0.0-20180131005756-15cf729a72d4/go.mod h1:2RvX5ZjVtsznNZPEt4xwJXNJrM3VTZoQf7V6gk0ysvs= github.com/jdxcode/netrc v0.0.0-20221124155335-4616370d1a84 h1:2uT3aivO7NVpUPGcQX7RbHijHMyWix/yCnIrCWc+5co= github.com/jdxcode/netrc v0.0.0-20221124155335-4616370d1a84/go.mod h1:Zi/ZFkEqFHTm7qkjyNJjaWH4LQA9LQhGJyF0lTYGpxw= github.com/jedib0t/go-pretty/v6 v6.4.6 h1:v6aG9h6Uby3IusSSEjHaZNXpHFhzqMmjXcPq1Rjl9Jw=