From 4e687b29c397dd1e7fd8008bdc5f1e735a9a266f Mon Sep 17 00:00:00 2001
From: deadprogram <ron@hybridgroup.com>
Date: Thu, 19 Jan 2023 23:24:55 +0100
Subject: [PATCH] mpu6050: add functions to configure clock, and scaling for
 accelerometer and gyroscope

Signed-off-by: deadprogram <ron@hybridgroup.com>
---
 mpu6050/mpu6050.go   | 17 ++++++++++++++++-
 mpu6050/registers.go | 23 +++++++++++++++++++++++
 2 files changed, 39 insertions(+), 1 deletion(-)

diff --git a/mpu6050/mpu6050.go b/mpu6050/mpu6050.go
index effebed..c6e945d 100644
--- a/mpu6050/mpu6050.go
+++ b/mpu6050/mpu6050.go
@@ -32,7 +32,7 @@ func (d Device) Connected() bool {
 
 // Configure sets up the device for communication.
 func (d Device) Configure() error {
-	return d.bus.WriteRegister(uint8(d.Address), PWR_MGMT_1, []uint8{0})
+	return d.SetClockSource(CLOCK_INTERNAL)
 }
 
 // ReadAcceleration reads the current acceleration from the device and returns
@@ -78,3 +78,18 @@ func (d Device) ReadRotation() (x int32, y int32, z int32) {
 	z = int32(int16((uint16(data[4])<<8)|uint16(data[5]))) * 15625 / 2048 * 1000
 	return
 }
+
+// SetClockSource allows the user to configure the clock source.
+func (d Device) SetClockSource(source uint8) error {
+	return d.bus.WriteRegister(uint8(d.Address), PWR_MGMT_1, []uint8{source})
+}
+
+// SetFullScaleGyroRange allows the user to configure the scale range for the gyroscope.
+func (d Device) SetFullScaleGyroRange(rng uint8) error {
+	return d.bus.WriteRegister(uint8(d.Address), GYRO_CONFIG, []uint8{rng})
+}
+
+// SetFullScaleAccelRange allows the user to configure the scale range for the accelerometer.
+func (d Device) SetFullScaleAccelRange(rng uint8) error {
+	return d.bus.WriteRegister(uint8(d.Address), ACCEL_CONFIG, []uint8{rng})
+}
diff --git a/mpu6050/registers.go b/mpu6050/registers.go
index 1206002..4b74369 100644
--- a/mpu6050/registers.go
+++ b/mpu6050/registers.go
@@ -98,6 +98,29 @@ const (
 	I2C_PER3_DO      = 0x66
 	I2C_MST_DELAY_CT = 0x67
 
+	// Clock settings
+	CLOCK_INTERNAL               = 0x00
+	CLOCK_PLL_XGYRO              = 0x01
+	CLOCK_PLL_YGYRO              = 0x02
+	CLOCK_PLL_ZGYRO              = 0x03
+	CLOCK_PLL_EXTERNAL_32_768_KZ = 0x04
+	CLOCK_PLL_EXTERNAL_19_2_MHZ  = 0x05
+	CLOCK_RESERVED               = 0x06
+	CLOCK_STOP                   = 0x07
+
+	// Accelerometer settings
+	AFS_RANGE_2G  = 0x00
+	AFS_RANGE_4G  = 0x01
+	AFS_RANGE_8G  = 0x02
+	AFS_RANGE_16G = 0x03
+
+	// Gyroscope settings
+	FS_RANGE_250  = 0x00
+	FS_RANGE_500  = 0x01
+	FS_RANGE_1000 = 0x02
+	FS_RANGE_2000 = 0x03
+
+	// other registers
 	SIGNAL_PATH_RES = 0x68 // Signal path reset
 	USER_CTRL       = 0x6A // User control
 	PWR_MGMT_1      = 0x6B // Power Management 1