Added accel and gyro data.

This commit is contained in:
Andrey Egorov 2024-07-24 15:40:35 +03:00
parent 031d784c53
commit 760f8d9556
3 changed files with 179 additions and 22 deletions

2
go.mod
View File

@ -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

19
main.go
View File

@ -20,11 +20,24 @@ func mainE() error {
if err != nil {
return err
}
log.Println("GET")
log.Println("LOOP")
log.Println("ACCEL")
for {
temp, err := mpu.GetTemp()
if err != nil {
return err
}
log.Println(temp)
return nil
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)
}
}

View File

@ -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
}