From 760f8d95569e9331dfdd588d179573650791031e Mon Sep 17 00:00:00 2001 From: Andrey Egorov Date: Wed, 24 Jul 2024 15:40:35 +0300 Subject: [PATCH] Added accel and gyro data. --- go.mod | 2 +- main.go | 25 +++++-- mpu6050/mpu.go | 174 ++++++++++++++++++++++++++++++++++++++++++++----- 3 files changed, 179 insertions(+), 22 deletions(-) diff --git a/go.mod b/go.mod index dcbfd52..b0c90f1 100644 --- a/go.mod +++ b/go.mod @@ -2,4 +2,4 @@ module github.com/CGSG-2021-AE4/mpu-test go 1.22.5 -require golang.org/x/exp v0.0.0-20240719175910-8a7402abbf56 // indirect +require golang.org/x/exp v0.0.0-20240719175910-8a7402abbf56 diff --git a/main.go b/main.go index 2093f5a..f26e90b 100644 --- a/main.go +++ b/main.go @@ -20,11 +20,24 @@ func mainE() error { if err != nil { return err } - log.Println("GET") - temp, err := mpu.GetTemp() - if err != nil { - return err + log.Println("LOOP") + log.Println("ACCEL") + for { + temp, err := mpu.GetTemp() + if err != nil { + return err + } + log.Println("Temp:", temp) + + accel, err := mpu.GetAccel(false) + if err != nil { + return err + } + log.Println("Accel:", accel) + gyro, err := mpu.GetGyro() + if err != nil { + return err + } + log.Println("Gyro:", gyro) } - log.Println(temp) - return nil } diff --git a/mpu6050/mpu.go b/mpu6050/mpu.go index 0506267..72bc785 100644 --- a/mpu6050/mpu.go +++ b/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 }