Adding support for the LSM303DLHC e-Compass

lsm303agr/lsm303agr.go

@@ -36,7 +36,7 @@ type Configuration struct {
 	MagDataRate    uint8
 }
 
-var errNotConnected = errors.New("lsm303agr: failed to communicate with either acel or magnet sensor")
+var errNotConnected = errors.New("lsm303agr: failed to communicate with either accel or magnet sensor")
 
 // New creates a new LSM303AGR connection. The I2C bus must already be configured.
 //

lsm303dlhc/lsm303dlhc.go

@@ -0,0 +1,232 @@
+// Package lsm303dlhc implements a driver for the LSM303dlhc,
+// a 3 axis accelerometer/magnetic sensor which is included on BBC micro:bits v1.5.
+//
+// Datasheet: https://www.st.com/resource/en/datasheet/lsm303dlhc.pdf
+package lsm303dlhc // import "tinygo.org/x/drivers/lsm303dlhc"
+
+import (
+	"math"
+
+	"tinygo.org/x/drivers"
+	"tinygo.org/x/drivers/internal/legacy"
+)
+
+// Device wraps an I2C connection to a LSM303dlhc device.
+type Device struct {
+	bus            drivers.I2C
+	AccelAddress   uint8
+	MagAddress     uint8
+	AccelPowerMode uint8
+	AccelRange     uint8
+	AccelDataRate  uint8
+	MagPowerMode   uint8
+	MagSystemMode  uint8
+	MagDataRate    uint8
+	buf            [6]uint8
+}
+
+// Configuration for LSM303dlhc device.
+type Configuration struct {
+	AccelPowerMode uint8
+	AccelRange     uint8
+	AccelDataRate  uint8
+	MagPowerMode   uint8
+	MagSystemMode  uint8
+	MagDataRate    uint8
+}
+
+// commented out "Connected" related lines since the DLHC sensor does not have the WHO_AM_I registers
+
+// var errNotConnected = errors.New("lsm303dlhc: failed to communicate with either accel or magnet sensor")
+
+// New creates a new LSM303DLHC 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:          bus,
+		AccelAddress: ACCEL_ADDRESS,
+		MagAddress:   MAG_ADDRESS,
+	}
+}
+
+// Connected returns whether both sensor on LSM303dlhc has been found.
+// It does two "who am I" requests and checks the responses.
+// func (d *Device) Connected() bool {
+// 	data1, data2 := []byte{0}, []byte{0}
+// 	legacy.ReadRegister(d.bus, uint8(d.AccelAddress), ACCEL_WHO_AM_I, data1)
+// 	legacy.ReadRegister(d.bus, uint8(d.MagAddress), MAG_WHO_AM_I, data2)
+// 	return data1[0] == 0x33 && data2[0] == 0x40
+// }
+
+// Configure sets up the LSM303dlhc device for communication.
+func (d *Device) Configure(cfg Configuration) (err error) {
+
+	// Verify unit communication
+	// if !d.Connected() {
+	// 	return errNotConnected
+	// }
+
+	if cfg.AccelDataRate != 0 {
+		d.AccelDataRate = cfg.AccelDataRate
+	} else {
+		d.AccelDataRate = ACCEL_DATARATE_100HZ
+	}
+
+	if cfg.AccelPowerMode != 0 {
+		d.AccelPowerMode = cfg.AccelPowerMode
+	} else {
+		d.AccelPowerMode = ACCEL_POWER_NORMAL
+	}
+
+	if cfg.AccelRange != 0 {
+		d.AccelRange = cfg.AccelRange
+	} else {
+		d.AccelRange = ACCEL_RANGE_2G
+	}
+
+	if cfg.MagPowerMode != 0 {
+		d.MagPowerMode = cfg.MagPowerMode
+	} else {
+		d.MagPowerMode = MAG_POWER_NORMAL
+	}
+
+	if cfg.MagDataRate != 0 {
+		d.MagDataRate = cfg.MagDataRate
+	} else {
+		d.MagDataRate = MAG_DATARATE_10HZ
+	}
+
+	if cfg.MagSystemMode != 0 {
+		d.MagSystemMode = cfg.MagSystemMode
+	} else {
+		d.MagSystemMode = MAG_SYSTEM_CONTINUOUS
+	}
+
+	data := d.buf[:1]
+
+	data[0] = byte(d.AccelDataRate<<4 | d.AccelPowerMode | 0x07)
+	err = legacy.WriteRegister(d.bus, uint8(d.AccelAddress), ACCEL_CTRL_REG1_A, data)
+	if err != nil {
+		return
+	}
+
+	data[0] = byte(0x80 | d.AccelRange<<4)
+	err = legacy.WriteRegister(d.bus, uint8(d.AccelAddress), ACCEL_CTRL_REG4_A, data)
+	if err != nil {
+		return
+	}
+
+	data[0] = byte(0xC0)
+	err = legacy.WriteRegister(d.bus, uint8(d.AccelAddress), CRA_REG_M, data)
+	if err != nil {
+		return
+	}
+
+	// Temperature compensation is on for magnetic sensor
+	data[0] = byte(0x80 | d.MagPowerMode<<4 | d.MagDataRate<<2 | d.MagSystemMode)
+	err = legacy.WriteRegister(d.bus, uint8(d.MagAddress), MAG_MR_REG_M, data)
+	if err != nil {
+		return
+	}
+
+	return nil
+}
+
+// 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() (x, y, z int32, err error) {
+	data := d.buf[:6]
+	err = legacy.ReadRegister(d.bus, uint8(d.AccelAddress), ACCEL_OUT_AUTO_INC, data)
+	if err != nil {
+		return
+	}
+
+	rangeFactor := int16(0)
+	switch d.AccelRange {
+	case ACCEL_RANGE_2G:
+		rangeFactor = 1
+	case ACCEL_RANGE_4G:
+		rangeFactor = 2
+	case ACCEL_RANGE_8G:
+		rangeFactor = 4
+	case ACCEL_RANGE_16G:
+		rangeFactor = 12 // the readings in 16G are a bit lower
+	}
+
+	x = int32(int32(int16((uint16(data[1])<<8|uint16(data[0])))>>4*rangeFactor) * 1000000 / 1024)
+	y = int32(int32(int16((uint16(data[3])<<8|uint16(data[2])))>>4*rangeFactor) * 1000000 / 1024)
+	z = int32(int32(int16((uint16(data[5])<<8|uint16(data[4])))>>4*rangeFactor) * 1000000 / 1024)
+	return
+}
+
+// ReadPitchRoll reads the current pitch and roll angles from the device and
+// returns it in micro-degrees. When the z axis is pointing straight to Earth
+// the returned values of pitch and roll would be zero.
+func (d *Device) ReadPitchRoll() (pitch, roll int32, err error) {
+
+	x, y, z, err := d.ReadAcceleration()
+	if err != nil {
+		return
+	}
+	xf, yf, zf := float64(x), float64(y), float64(z)
+	pitch = int32((math.Round(math.Atan2(yf, math.Sqrt(math.Pow(xf, 2)+math.Pow(zf, 2)))*(180/math.Pi)*100) / 100) * 1000000)
+	roll = int32((math.Round(math.Atan2(xf, math.Sqrt(math.Pow(yf, 2)+math.Pow(zf, 2)))*(180/math.Pi)*100) / 100) * 1000000)
+	return
+
+}
+
+// ReadMagneticField reads the current magnetic field from the device and returns
+// it in mG (milligauss). 1 mG = 0.1 µT (microtesla).
+func (d *Device) ReadMagneticField() (x, y, z int32, err error) {
+
+	if d.MagSystemMode == MAG_SYSTEM_SINGLE {
+		cmd := d.buf[:1]
+		cmd[0] = byte(0x80 | d.MagPowerMode<<4 | d.MagDataRate<<2 | d.MagSystemMode)
+		err = legacy.WriteRegister(d.bus, uint8(d.MagAddress), MAG_MR_REG_M, cmd)
+		if err != nil {
+			return
+		}
+	}
+
+	data := d.buf[0:6]
+	legacy.ReadRegister(d.bus, uint8(d.MagAddress), MAG_OUT_AUTO_INC, data)
+
+	x = int32(int16((uint16(data[1])<<8 | uint16(data[0]))))
+	y = int32(int16((uint16(data[3])<<8 | uint16(data[2]))))
+	z = int32(int16((uint16(data[5])<<8 | uint16(data[4]))))
+	return
+}
+
+// ReadCompass reads the current compass heading from the device and returns
+// it in micro-degrees. When the z axis is pointing straight to Earth and
+// the y axis is pointing to North, the heading would be zero.
+//
+// However, the heading may be off due to electronic compasses would be effected
+// by strong magnetic fields and require constant calibration.
+func (d *Device) ReadCompass() (h int32, err error) {
+
+	x, y, _, err := d.ReadMagneticField()
+	if err != nil {
+		return
+	}
+	xf, yf := float64(x), float64(y)
+	h = int32(float32((180/math.Pi)*math.Atan2(yf, xf)) * 1000000)
+	return
+}
+
+// ReadTemperature returns the temperature in Celsius milli degrees (°C/1000)
+func (d *Device) ReadTemperature() (t int32, err error) {
+
+	data := d.buf[:2]
+	err = legacy.ReadRegister(d.bus, uint8(d.MagAddress), TEMP_OUT_AUTO_INC, data)
+	if err != nil {
+		return
+	}
+
+	r := int16((uint16(data[1])<<8 | uint16(data[0]))) >> 4 // temperature offset from 25 °C
+	t = 25000 + int32((float32(r)/8)*1000)
+	return
+}

lsm303dlhc/registers.go

@@ -0,0 +1,75 @@
+package lsm303dlhc
+
+const (
+
+	// Constants/addresses used for I2C.
+	ACCEL_ADDRESS = 0x19
+	MAG_ADDRESS   = 0x1E
+
+	// i2C 8-bit subaddress (SUB): the 7 LSb represent the actual register address
+	// while the MSB enables address auto increment.
+	// If the MSb of the SUB field is 1, the SUB (register address) is
+	// automatically increased to allow multiple data read/writes.
+	ADDR_AUTO_INC_MASK = 0x80
+
+	// accelerometer registers.
+	ACCEL_CTRL_REG1_A  = 0x20
+	ACCEL_CTRL_REG4_A  = 0x23
+	ACCEL_OUT_X_L_A    = 0x28
+	ACCEL_OUT_X_H_A    = 0x29
+	ACCEL_OUT_Y_L_A    = 0x2A
+	ACCEL_OUT_Y_H_A    = 0x2B
+	ACCEL_OUT_Z_L_A    = 0x2C
+	ACCEL_OUT_Z_H_A    = 0x2D
+	ACCEL_OUT_AUTO_INC = ACCEL_OUT_X_L_A | ADDR_AUTO_INC_MASK
+
+	// magnetic sensor registers.
+	MAG_MR_REG_M     = 0x02
+	MAG_OUT_X_L_M    = 0x68
+	MAG_OUT_X_H_M    = 0x69
+	MAG_OUT_Y_L_M    = 0x6A
+	MAG_OUT_Y_H_M    = 0x6B
+	MAG_OUT_Z_L_M    = 0x6C
+	MAG_OUT_Z_H_M    = 0x6D
+	MAG_OUT_AUTO_INC = MAG_OUT_X_L_M | ADDR_AUTO_INC_MASK
+
+	// temperature sensor registers.
+	CRA_REG_M         = 0x80
+	TEMP_OUT_L_M      = 0x32
+	TEMP_OUT_H_M      = 0x31
+	TEMP_OUT_AUTO_INC = TEMP_OUT_L_M | ADDR_AUTO_INC_MASK
+
+	// accelerometer power mode.
+	ACCEL_POWER_NORMAL = 0x00 // default
+	ACCEL_POWER_LOW    = 0x08
+
+	// accelerometer range.
+	ACCEL_RANGE_2G  = 0x00 // default
+	ACCEL_RANGE_4G  = 0x01
+	ACCEL_RANGE_8G  = 0x02
+	ACCEL_RANGE_16G = 0x03
+
+	// accelerometer data rate.
+	ACCEL_DATARATE_1HZ    = 0x01
+	ACCEL_DATARATE_10HZ   = 0x02
+	ACCEL_DATARATE_25HZ   = 0x03
+	ACCEL_DATARATE_50HZ   = 0x04
+	ACCEL_DATARATE_100HZ  = 0x05 // default
+	ACCEL_DATARATE_200HZ  = 0x06
+	ACCEL_DATARATE_400HZ  = 0x07
+	ACCEL_DATARATE_1344HZ = 0x09 // 5376Hz in low-power mode
+
+	// magnetic sensor power mode.
+	MAG_POWER_NORMAL = 0x00 // default
+	MAG_POWER_LOW    = 0x01
+
+	// magnetic sensor operate mode.
+	MAG_SYSTEM_CONTINUOUS = 0x00 // default
+	MAG_SYSTEM_SINGLE     = 0x01
+
+	// magnetic sensor data rate
+	MAG_DATARATE_10HZ  = 0x00 // default
+	MAG_DATARATE_20HZ  = 0x01
+	MAG_DATARATE_50HZ  = 0x02
+	MAG_DATARATE_100HZ = 0x03
+)