From 88ba3ced1531732e16893e2db7cdcd5bc337299d Mon Sep 17 00:00:00 2001 From: Bryan Souza Date: Tue, 29 Oct 2024 10:57:52 -0700 Subject: [PATCH 1/2] added support for LSM303DLHC e-Compass; --- lsm303dlhc/lsm303dlhc.go | 232 +++++++++++++++++++++++++++++++++++++++ lsm303dlhc/registers.go | 75 +++++++++++++ 2 files changed, 307 insertions(+) create mode 100644 lsm303dlhc/lsm303dlhc.go create mode 100644 lsm303dlhc/registers.go diff --git a/lsm303dlhc/lsm303dlhc.go b/lsm303dlhc/lsm303dlhc.go new file mode 100644 index 000000000..e48cfc1b3 --- /dev/null +++ b/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 +} diff --git a/lsm303dlhc/registers.go b/lsm303dlhc/registers.go new file mode 100644 index 000000000..09206f3d0 --- /dev/null +++ b/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 +) From 4277aa796c47235a1add9351f33fcc2941cf4484 Mon Sep 17 00:00:00 2001 From: Bryan Souza Date: Sat, 2 Nov 2024 08:48:34 -0700 Subject: [PATCH 2/2] fixed the spelling in the Connection error message; Initial support for LSM303DLHC added; --- lsm303agr/lsm303agr.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lsm303agr/lsm303agr.go b/lsm303agr/lsm303agr.go index 35a4c929f..f6848eb25 100644 --- a/lsm303agr/lsm303agr.go +++ b/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. //