Added accel and gyro data.

mpu6050/mpu.go

@@ -2,6 +2,7 @@ package mpu6050
 
 import (
 	"fmt"
+	"log"
 
 	"golang.org/x/exp/io/i2c"
 )
@@ -15,20 +16,52 @@ const (
 	// powerMgmt1 = 0x6B
 	// powerMgmt2 = 0x6C
 	// Acceleration
-	// accelXOut0 = 0x3B
-	// accelYOut0 = 0x3D
-	// accelZOut0 = 0x3F
+	accelXOut0 = 0x3B
+	accelYOut0 = 0x3D
+	accelZOut0 = 0x3F
 	// Temperature
 	tempOut0 = 0x41
 
-// Gyro
-// gyroXOut0 = 0x43
-// gyroYOut0 = 0x45
-// gyroZOut0 = 0x47
-// Configs
-// accelConfig = 0x1C
-// gyroConfig  = 0x1B
-// mpuConfig   = 0x1A
+	// 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 {
@@ -37,7 +70,9 @@ type device struct {
 
 type Device interface {
 	GetTemp() (float32, error)
-	GetAccel() ([3]float32, error)
+	SetAccelRange(r byte) error
+	GetAccel(inG bool) ([3]float32, error)
+	SetGyroRange(r byte) error
 	GetGyro() ([3]float32, error)
 }
 
@@ -59,10 +94,119 @@ func (d *device) GetTemp() (float32, error) {
 	return (float32(rawTemp) / 340.0) + 36.53, nil
 }
 
-func (d *device) GetAccel() ([3]float32, error) {
-	return [3]float32{}, 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) {
-	return [3]float32{}, nil
+	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
 }