From a9b36f8bd45536775ce3d06120280ef47d183b8e Mon Sep 17 00:00:00 2001
From: fchiesadoc <32172349+FranCDoc@users.noreply.github.com>
Date: Sun, 27 Aug 2023 06:38:33 -0300
Subject: [PATCH] Mpu9150 (#596)

mpu9150: add mpu9150 driver
---
 examples/mpu9150/main.go |  22 +++++++
 go.mod                   |   2 +-
 go.sum                   |   2 +-
 mpu9150/mpu9150.go       |  99 +++++++++++++++++++++++++++++
 mpu9150/registers.go     | 131 +++++++++++++++++++++++++++++++++++++++
 smoketest.sh             |   3 +-
 6 files changed, 256 insertions(+), 3 deletions(-)
 create mode 100644 examples/mpu9150/main.go
 create mode 100644 mpu9150/mpu9150.go
 create mode 100644 mpu9150/registers.go

diff --git a/examples/mpu9150/main.go b/examples/mpu9150/main.go
new file mode 100644
index 0000000..42a86b1
--- /dev/null
+++ b/examples/mpu9150/main.go
@@ -0,0 +1,22 @@
+// Connects to an MPU9150 I2C accelerometer/gyroscope.
+package main
+
+import (
+	"machine"
+	"time"
+
+	"tinygo.org/x/drivers/mpu9150"
+)
+
+func main() {
+	machine.I2C0.Configure(machine.I2CConfig{})
+
+	accel := mpu9150.New(machine.I2C0)
+	accel.Configure()
+
+	for {
+		x, y, z := accel.ReadAcceleration(mpu9150.ACCEL_XOUT_H)
+		println(x, y, z)
+		time.Sleep(time.Millisecond * 100)
+	}
+}
diff --git a/go.mod b/go.mod
index 6986c65..ca7e480 100644
--- a/go.mod
+++ b/go.mod
@@ -9,4 +9,4 @@ require (
 	golang.org/x/net v0.7.0
 	tinygo.org/x/tinyfont v0.3.0
 	tinygo.org/x/tinyterm v0.1.0
-)
+)
\ No newline at end of file
diff --git a/go.sum b/go.sum
index 08ffffd..f6f23c1 100644
--- a/go.sum
+++ b/go.sum
@@ -59,4 +59,4 @@ tinygo.org/x/tinyfont v0.3.0 h1:HIRLQoI3oc+2CMhPcfv+Ig88EcTImE/5npjqOnMD4lM=
 tinygo.org/x/tinyfont v0.3.0/go.mod h1:+TV5q0KpwSGRWnN+ITijsIhrWYJkoUCp9MYELjKpAXk=
 tinygo.org/x/tinyfs v0.1.0/go.mod h1:ysc8Y92iHfhTXeyEM9+c7zviUQ4fN9UCFgSOFfMWv20=
 tinygo.org/x/tinyterm v0.1.0 h1:80i+j+KWoxCFa/Xfp6pWbh79x+8zUdMXC1vaKj2QhkY=
-tinygo.org/x/tinyterm v0.1.0/go.mod h1:/DDhNnGwNF2/tNgHywvyZuCGnbH3ov49Z/6e8LPLRR4=
+tinygo.org/x/tinyterm v0.1.0/go.mod h1:/DDhNnGwNF2/tNgHywvyZuCGnbH3ov49Z/6e8LPLRR4=
\ No newline at end of file
diff --git a/mpu9150/mpu9150.go b/mpu9150/mpu9150.go
new file mode 100644
index 0000000..1661daf
--- /dev/null
+++ b/mpu9150/mpu9150.go
@@ -0,0 +1,99 @@
+// Package mpu9150 provides a driver for the MPU9150 accelerometer and gyroscope
+// made by InvenSense.
+//
+// Datasheets:
+// https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-9150-Datasheet.pdf
+// https://inertialelements.com/documents/resources_page/MPU9150-register-manual.pdf
+
+package mpu9150
+
+import (
+	"tinygo.org/x/drivers"
+	"tinygo.org/x/drivers/internal/legacy"
+)
+
+// Device wraps an I2C connection to a MPU9150 device.
+type Device struct {
+	bus     drivers.I2C
+	Address uint16
+}
+
+// New creates a new MPU9150 connection. The I2C bus must already be
+// configured.
+//
+// This function only creates the Device object, it does not touch the device.
+func New(bus drivers.I2C) Device {
+	return Device{bus, Address}
+}
+
+// Connected returns whether a MPU9150 has been found.
+// It does a "who am I" request and checks the response.
+func (d Device) Connected() bool {
+	data := []byte{0}
+	legacy.ReadRegister(d.bus, uint8(d.Address), WHO_AM_I, data)
+	return data[0] == 0x68 // 4.32 Register 117 – Who Am I (MPU-9150 Register Map and Descriptions)
+}
+
+// Configure sets up the device for communication.
+func (d Device) Configure() error {
+	return d.SetClockSource(CLOCK_INTERNAL)
+}
+
+// ReadAcceleration reads the current acceleration from the device and returns
+// it in µg (micro-gravity). When one of the axes is pointing straight to Earth
+// and the sensor is not moving the returned value will be around 1000000 or
+// -1000000.
+func (d Device) ReadAcceleration(accel_axis byte) (x int32, y int32, z int32) {
+	data := make([]byte, 6)
+	legacy.ReadRegister(d.bus, uint8(d.Address), accel_axis, data)
+	// Now do two things:
+	// 1. merge the two values to a 16-bit number (and cast to a 32-bit integer)
+	// 2. scale the value to bring it in the -1000000..1000000 range.
+	//    This is done with a trick. What we do here is essentially multiply by
+	//    1000000 and divide by 16384 to get the original scale, but to avoid
+	//    overflow we do it at 1/64 of the value:
+	//      1000000 / 64 = 15625
+	//      16384   / 64 = 256
+	x = int32(int16((uint16(data[0])<<8)|uint16(data[1]))) * 15625 / 256
+	y = int32(int16((uint16(data[2])<<8)|uint16(data[3]))) * 15625 / 256
+	z = int32(int16((uint16(data[4])<<8)|uint16(data[5]))) * 15625 / 256
+	return
+}
+
+// ReadRotation reads the current rotation from the device and returns it in
+// µ°/s (micro-degrees/sec). This means that if you were to do a complete
+// rotation along one axis and while doing so integrate all values over time,
+// you would get a value close to 360000000.
+func (d Device) ReadRotation(gyro_axis byte) (x int32, y int32, z int32) {
+	data := make([]byte, 6)
+	legacy.ReadRegister(d.bus, uint8(d.Address), gyro_axis, data)
+	// First the value is converted from a pair of bytes to a signed 16-bit
+	// value and then to a signed 32-bit value to avoid integer overflow.
+	// Then the value is scaled to µ°/s (micro-degrees per second).
+	// This is done in the following steps:
+	// 1. Multiply by 250 * 1000_000
+	// 2. Divide by 32768
+	// The following calculation (x * 15625 / 2048 * 1000) is essentially the
+	// same but avoids overflow. First both operations are divided by 16 leading
+	// to multiply by 15625000 and divide by 2048, and then part of the multiply
+	// is done after the divide instead of before.
+	x = int32(int16((uint16(data[0])<<8)|uint16(data[1]))) * 15625 / 2048 * 1000
+	y = int32(int16((uint16(data[2])<<8)|uint16(data[3]))) * 15625 / 2048 * 1000
+	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 legacy.WriteRegister(d.bus, 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 legacy.WriteRegister(d.bus, 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 legacy.WriteRegister(d.bus, uint8(d.Address), ACCEL_CONFIG, []uint8{rng})
+}
diff --git a/mpu9150/registers.go b/mpu9150/registers.go
new file mode 100644
index 0000000..6d36acd
--- /dev/null
+++ b/mpu9150/registers.go
@@ -0,0 +1,131 @@
+package mpu9150
+
+// Constants/addresses used for I2C.
+
+// The I2C address which this device listens to.
+const Address = 0x68
+
+// Registers. Names, addresses and comments copied from the datasheet.
+const (
+	// Self test registers
+	SELF_TEST_X = 0x0D
+	SELF_TEST_Y = 0x0E
+	SELF_TEST_Z = 0x0F
+	SELF_TEST_A = 0x10
+
+	SMPLRT_DIV   = 0x19 // Sample rate divider
+	CONFIG       = 0x1A // Configuration
+	GYRO_CONFIG  = 0x1B // Gyroscope configuration
+	ACCEL_CONFIG = 0x1C // Accelerometer configuration
+	FIFO_EN      = 0x23 // FIFO enable
+
+	// I2C pass-through configuration
+	I2C_MST_CTRL   = 0x24
+	I2C_SLV0_ADDR  = 0x25
+	I2C_SLV0_REG   = 0x26
+	I2C_SLV0_CTRL  = 0x27
+	I2C_SLV1_ADDR  = 0x28
+	I2C_SLV1_REG   = 0x29
+	I2C_SLV1_CTRL  = 0x2A
+	I2C_SLV2_ADDR  = 0x2B
+	I2C_SLV2_REG   = 0x2C
+	I2C_SLV2_CTRL  = 0x2D
+	I2C_SLV3_ADDR  = 0x2E
+	I2C_SLV3_REG   = 0x2F
+	I2C_SLV3_CTRL  = 0x30
+	I2C_SLV4_ADDR  = 0x31
+	I2C_SLV4_REG   = 0x32
+	I2C_SLV4_DO    = 0x33
+	I2C_SLV4_CTRL  = 0x34
+	I2C_SLV4_DI    = 0x35
+	I2C_MST_STATUS = 0x36
+
+	// Interrupt configuration
+	INT_PIN_CFG = 0x37 // Interrupt pin/bypass enable configuration
+	INT_ENABLE  = 0x38 // Interrupt enable
+	INT_STATUS  = 0x3A // Interrupt status
+
+	// Accelerometer measurements
+	ACCEL_XOUT_H = 0x3B
+	ACCEL_XOUT_L = 0x3C
+	ACCEL_YOUT_H = 0x3D
+	ACCEL_YOUT_L = 0x3E
+	ACCEL_ZOUT_H = 0x3F
+	ACCEL_ZOUT_L = 0x40
+
+	// Temperature measurement
+	TEMP_OUT_H = 0x41
+	TEMP_OUT_L = 0x42
+
+	// Gyroscope measurements
+	GYRO_XOUT_H = 0x43
+	GYRO_XOUT_L = 0x44
+	GYRO_YOUT_H = 0x45
+	GYRO_YOUT_L = 0x46
+	GYRO_ZOUT_H = 0x47
+	GYRO_ZOUT_L = 0x48
+
+	// External sensor data
+	EXT_SENS_DATA_00 = 0x49
+	EXT_SENS_DATA_01 = 0x4A
+	EXT_SENS_DATA_02 = 0x4B
+	EXT_SENS_DATA_03 = 0x4C
+	EXT_SENS_DATA_04 = 0x4D
+	EXT_SENS_DATA_05 = 0x4E
+	EXT_SENS_DATA_06 = 0x4F
+	EXT_SENS_DATA_07 = 0x50
+	EXT_SENS_DATA_08 = 0x51
+	EXT_SENS_DATA_09 = 0x52
+	EXT_SENS_DATA_10 = 0x53
+	EXT_SENS_DATA_11 = 0x54
+	EXT_SENS_DATA_12 = 0x55
+	EXT_SENS_DATA_13 = 0x56
+	EXT_SENS_DATA_14 = 0x57
+	EXT_SENS_DATA_15 = 0x58
+	EXT_SENS_DATA_16 = 0x59
+	EXT_SENS_DATA_17 = 0x5A
+	EXT_SENS_DATA_18 = 0x5B
+	EXT_SENS_DATA_19 = 0x5C
+	EXT_SENS_DATA_20 = 0x5D
+	EXT_SENS_DATA_21 = 0x5E
+	EXT_SENS_DATA_22 = 0x5F
+	EXT_SENS_DATA_23 = 0x60
+
+	// I2C peripheral data out
+	I2C_SLV0_DO        = 0x63
+	I2C_SLV1_DO        = 0x64
+	I2C_SLV2_DO        = 0x65
+	I2C_SLV3_DO        = 0x66
+	I2C_MST_DELAY_CTRL = 0x67
+	SIGNAL_PATH_RESET  = 0x68
+
+	USER_CTRL   = 0x6A // User control
+	PWR_MGMT_1  = 0x6B // Power Management 1
+	PWR_MGMT_2  = 0x6C // Power Management 2
+	FIFO_COUNTH = 0x72 // FIFO count registers (high bits)
+	FIFO_COUNTL = 0x73 // FIFO count registers (low bits)
+	FIFO_R_W    = 0x74 // FIFO read/write
+	WHO_AM_I    = 0x75 // Who am I
+
+	// Clock settings  (4.28 Register 107 – Power Management 1)
+	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
+
+	// Gyroscope settings (4.4 Register 27 – Gyroscope Configuration)
+	FS_RANGE_250  = 0x00
+	FS_RANGE_500  = 0x01
+	FS_RANGE_1000 = 0x02
+	FS_RANGE_2000 = 0x03
+
+	// Accelerometer settings (4.5 Register 28 – Accelerometer Configuration)
+	AFS_RANGE_2G  = 0x00
+	AFS_RANGE_4G  = 0x01
+	AFS_RANGE_8G  = 0x02
+	AFS_RANGE_16G = 0x03
+)
diff --git a/smoketest.sh b/smoketest.sh
index 9b3ff49..54b7bdb 100755
--- a/smoketest.sh
+++ b/smoketest.sh
@@ -131,4 +131,5 @@ tinygo build -size short -o ./build/test.uf2 -target=pico ./examples/mpu6886/mai
 tinygo build -size short -o ./build/test.hex -target=arduino-nano33 ./examples/ttp229/main.go
 tinygo build -size short -o ./build/test.hex -target=pico ./examples/ndir/main_ndir.go
 tinygo build -size short -o ./build/test.hex -target=microbit ./examples/ndir/main_ndir.go
-tinygo build -size short -o ./build/test.hex -target=arduino-nano33 ./examples/ndir/main_ndir.go
\ No newline at end of file
+tinygo build -size short -o ./build/test.hex -target=arduino-nano33 ./examples/ndir/main_ndir.go
+tinygo build -size short -o ./build/test.uf2 -target=pico ./examples/mpu9150/main.go