From b0c4f3e2cea81cf9edfa971e200939d0a4fa8589 Mon Sep 17 00:00:00 2001
From: Ayke van Laethem <aykevanlaethem@gmail.com>
Date: Sun, 10 Mar 2019 22:58:45 +0100
Subject: [PATCH] mma8653: return acceleration in a standard unit: micro-G.

---
 examples/mma8653/main.go |  4 ++--
 mma8653/mma8653.go       | 49 +++++++++++++++++++++++++++++++---------
 mma8653/registers.go     |  9 ++++++++
 3 files changed, 49 insertions(+), 13 deletions(-)

diff --git a/examples/mma8653/main.go b/examples/mma8653/main.go
index 8086dd8..7af9f96 100644
--- a/examples/mma8653/main.go
+++ b/examples/mma8653/main.go
@@ -12,10 +12,10 @@ func main() {
 	machine.I2C0.Configure(machine.I2CConfig{})
 
 	accel := mma8653.New(machine.I2C0)
-	accel.Configure(mma8653.DataRate200Hz)
+	accel.Configure(mma8653.DataRate200Hz, mma8653.Sensitivity2G)
 
 	for {
-		x, y, z := accel.ReadAcceleration()
+		x, y, z, _ := accel.ReadAcceleration()
 		println(x, y, z)
 		time.Sleep(time.Millisecond * 100)
 	}
diff --git a/mma8653/mma8653.go b/mma8653/mma8653.go
index 96b6d3d..b6d01dc 100644
--- a/mma8653/mma8653.go
+++ b/mma8653/mma8653.go
@@ -11,7 +11,8 @@ import (
 
 // Device wraps an I2C connection to a MMA8653 device.
 type Device struct {
-	bus machine.I2C
+	bus         machine.I2C
+	sensitivity Sensitivity
 }
 
 // New creates a new MMA8653 connection. The I2C bus must already be
@@ -19,7 +20,7 @@ type Device struct {
 //
 // This function only creates the Device object, it does not touch the device.
 func New(bus machine.I2C) Device {
-	return Device{bus}
+	return Device{bus, Sensitivity2G}
 }
 
 // Connected returns whether a MMA8653 has been found.
@@ -31,18 +32,44 @@ func (d Device) Connected() bool {
 }
 
 // Configure sets up the device for communication.
-func (d Device) Configure(speed DataRate) {
-	data := (uint8(speed) << 3) | 1 // set data rate and ACTIVE mode
-	d.bus.WriteRegister(Address, CTRL_REG1, []uint8{data})
+func (d *Device) Configure(speed DataRate, sensitivity Sensitivity) error {
+	// Set mode to STANDBY to be able to change the sensitivity.
+	err := d.bus.WriteRegister(Address, CTRL_REG1, []uint8{0})
+	if err != nil {
+		return err
+	}
+
+	// Set sensitivity (2G, 4G, 8G).
+	err = d.bus.WriteRegister(Address, XYZ_DATA_CFG, []uint8{uint8(sensitivity)})
+	if err != nil {
+		return err
+	}
+	d.sensitivity = sensitivity
+
+	// Set mode to ACTIVE and set the data rate.
+	err = d.bus.WriteRegister(Address, CTRL_REG1, []uint8{(uint8(speed) << 3) | 1})
+	if err != nil {
+		return err
+	}
+	return nil
 }
 
 // ReadAcceleration reads the current acceleration from the device and returns
-// it.
-func (d Device) ReadAcceleration() (x int16, y int16, z int16) {
+// 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() (x int32, y int32, z int32, err error) {
 	data := make([]byte, 6)
-	d.bus.ReadRegister(Address, OUT_X_MSB, data)
-	x = int16((uint16(data[0]) << 8) | uint16(data[1]))
-	y = int16((uint16(data[2]) << 8) | uint16(data[3]))
-	z = int16((uint16(data[4]) << 8) | uint16(data[5]))
+	err = d.bus.ReadRegister(Address, OUT_X_MSB, data)
+	shift := uint32(8)
+	switch d.sensitivity {
+	case Sensitivity4G:
+		shift = 7
+	case Sensitivity8G:
+		shift = 6
+	}
+	x = int32(int16((uint16(data[0])<<8)|uint16(data[1]))) * 15625 >> shift
+	y = int32(int16((uint16(data[2])<<8)|uint16(data[3]))) * 15625 >> shift
+	z = int32(int16((uint16(data[4])<<8)|uint16(data[5]))) * 15625 >> shift
 	return
 }
diff --git a/mma8653/registers.go b/mma8653/registers.go
index 8f3d53e..e6f3145 100644
--- a/mma8653/registers.go
+++ b/mma8653/registers.go
@@ -51,3 +51,12 @@ const (
 	DataRate6Hz                   // 6.25Hz, 160ms  interval
 	DataRate2Hz                   // 1.56Hz, 640ms  interval
 )
+
+type Sensitivity uint8
+
+// Sensitivity constants.
+const (
+	Sensitivity2G Sensitivity = iota
+	Sensitivity4G
+	Sensitivity8G
+)