mpu/pkg/mpu6050/mpu.go

package mpu6050

import (
	"fmt"
	"io"

	"golang.org/x/exp/io/i2c"
)

const (
	mpuAddr = 0x68

	// MPU registers

	// Power management
	// powerMgmt1 = 0x6B
	// powerMgmt2 = 0x6C
	// Acceleration
	accelXOut0 = 0x3B
	accelYOut0 = 0x3D
	accelZOut0 = 0x3F
	// Temperature
	tempOut0 = 0x41

	// Gyro
	gyroXOut0 = 0x43
	gyroYOut0 = 0x45
	gyroZOut0 = 0x47
	// Configs
	accelConfig = 0x1C
	gyroConfig  = 0x1B
	mpuConfig   = 0x1A

	gravityMS2 = 9.80665

	// Scale modifiers
	AccelScaleModifier2G  = 16384.0
	AccelScaleModifier4G  = 8192.0
	AccelScaleModifier8G  = 4096.0
	AccelScaleModifier16G = 2048.0

	GyroScaleModifier250DEG  = 131.0
	GyroScaleModifier500DEG  = 65.5
	GyroScaleModifier1000DEG = 32.8
	GyroScaleModifier2000DEG = 16.4

	// Accel range
	AccelRange2G  = 0x00
	AccelRange4G  = 0x08
	AccelRange8G  = 0x10
	AccelRange16G = 0x18

	GyroRange250DEG  = 0x00
	GyroRange500DEG  = 0x08
	GyroRange1000DEG = 0x10
	GyroRange2000DEG = 0x18

	FilterBw256 = 0x00
	FilterBw188 = 0x01
	FilterBw98  = 0x02
	FilterBw42  = 0x03
	FilterBw20  = 0x04
	FilterBw10  = 0x05
	FilterBw5   = 0x06
)

type device struct {
	device *i2c.Device
}

type Device interface {
	GetTemp() (float32, error)

	SetAccelRange(r byte) error
	GetAccel(inG bool) ([3]float32, error)

	SetGyroRange(r byte) error
	GetGyro() ([3]float32, error)

	io.Closer
}

func Open() (Device, error) {
	d, err := i2c.Open(&i2c.Devfs{Dev: "/dev/i2c-1"}, mpuAddr)
	if err != nil {
		return nil, fmt.Errorf("open i2c: %w", err)
	}
	return &device{
		device: d,
	}, nil
}

func (d *device) GetTemp() (float32, error) {
	rawTemp, err := d.readWord(tempOut0)
	if err != nil {
		return 0, err
	}
	return (float32(rawTemp) / 340.0) + 36.53, nil
}

func (d *device) SetAccelRange(accelRange byte) error {
	if err := d.device.WriteReg(accelConfig, []byte{0}); err != nil {
		return err
	}
	if err := d.device.WriteReg(accelConfig, []byte{accelRange}); err != nil {
		return err
	}
	return nil
}

func (d *device) readAccelRange() (byte, error) {
	rawData := make([]byte, 1)
	if err := d.device.ReadReg(accelConfig, rawData); err != nil {
		return 0, err
	}
	return rawData[0], nil
}

func (d *device) GetAccel(inG bool) ([3]float32, error) {
	x, err := d.readWord(accelXOut0)
	if err != nil {
		return [3]float32{}, fmt.Errorf("read x: %w", err)
	}
	y, err := d.readWord(accelYOut0)
	if err != nil {
		return [3]float32{}, fmt.Errorf("read y: %w", err)
	}
	z, err := d.readWord(accelZOut0)
	if err != nil {
		return [3]float32{}, fmt.Errorf("read z: %w", err)
	}

	scale := float32(AccelScaleModifier2G)
	accelRange, err := d.readAccelRange()
	if err != nil {
		return [3]float32{}, fmt.Errorf("read accel range: %w", err)
	}
	switch accelRange {
	case AccelRange2G:
		scale = AccelScaleModifier2G
	case AccelRange4G:
		scale = AccelScaleModifier2G
	case AccelRange8G:
		scale = AccelScaleModifier2G
	case AccelRange16G:
		scale = AccelScaleModifier2G
		//default:
		// log.Println("not foudn accel range:", accelRange)
	}
	scale = 1 / scale

	if !inG {
		scale /= gravityMS2
	}
	return [3]float32{
		float32(x) * scale,
		float32(y) * scale,
		float32(z) * scale,
	}, nil
}

func (d *device) SetGyroRange(gyroRange byte) error {
	if err := d.device.WriteReg(gyroConfig, []byte{0}); err != nil {
		return err
	}
	if err := d.device.WriteReg(gyroConfig, []byte{gyroRange}); err != nil {
		return err
	}
	return nil
}

func (d *device) readGyroRange() (byte, error) {
	rawData := make([]byte, 1)
	if err := d.device.ReadReg(gyroConfig, rawData); err != nil {
		return 0, err
	}
	return rawData[0], nil
}

func (d *device) GetGyro() ([3]float32, error) {
	x, err := d.readWord(gyroXOut0)
	if err != nil {
		return [3]float32{}, fmt.Errorf("read x: %w", err)
	}
	y, err := d.readWord(gyroYOut0)
	if err != nil {
		return [3]float32{}, fmt.Errorf("read y: %w", err)
	}
	z, err := d.readWord(gyroZOut0)
	if err != nil {
		return [3]float32{}, fmt.Errorf("read z: %w", err)
	}

	scale := float32(GyroScaleModifier250DEG)
	gyroRange, err := d.readGyroRange()
	if err != nil {
		return [3]float32{}, fmt.Errorf("read gyro range: %w", err)
	}
	switch gyroRange {
	case GyroRange250DEG:
		scale = GyroScaleModifier250DEG
	case GyroRange500DEG:
		scale = GyroScaleModifier500DEG
	case GyroRange1000DEG:
		scale = GyroScaleModifier1000DEG
	case GyroRange2000DEG:
		scale = GyroScaleModifier2000DEG
		// default:
		//   log.Println("not found gyro range:", gyroRange)
	}
	return [3]float32{
		float32(x) / scale,
		float32(y) / scale,
		float32(z) / scale,
	}, nil
}

func (d *device) Close() error {
	defer func() {
		d.device = nil
	}()
	return d.device.Close()
}