From 39f44ef478b88f8ee553c01296d5f4478d6c6bdb Mon Sep 17 00:00:00 2001 From: Alan Wang <44191076+alankrantas@users.noreply.github.com> Date: Wed, 1 Jul 2020 22:59:25 +0800 Subject: [PATCH] lsm303agr (#162) * lsm303agr: add support for lsm303agr digital compass --- Makefile | 2 + examples/lsm303agr/main.go | 39 +++++++ lsm303agr/lsm303agr.go | 203 +++++++++++++++++++++++++++++++++++++ lsm303agr/registers.go | 68 +++++++++++++ 4 files changed, 312 insertions(+) create mode 100644 examples/lsm303agr/main.go create mode 100644 lsm303agr/lsm303agr.go create mode 100644 lsm303agr/registers.go diff --git a/Makefile b/Makefile index 03c678b..fc7b22d 100644 --- a/Makefile +++ b/Makefile @@ -67,6 +67,8 @@ smoke-test: @md5sum ./build/test.hex tinygo build -size short -o ./build/test.hex -target=circuitplay-express ./examples/lis3dh/main.go @md5sum ./build/test.hex + tinygo build -size short -o ./build/test.hex -target=microbit ./examples/lsm303agr/main.go + @md5sum ./build/test.hex tinygo build -size short -o ./build/test.hex -target=arduino-nano33 ./examples/lsm6ds3/main.go @md5sum ./build/test.hex tinygo build -size short -o ./build/test.hex -target=itsybitsy-m0 ./examples/mag3110/main.go diff --git a/examples/lsm303agr/main.go b/examples/lsm303agr/main.go new file mode 100644 index 0000000..9dd60c9 --- /dev/null +++ b/examples/lsm303agr/main.go @@ -0,0 +1,39 @@ +package main + +import ( + "machine" + "time" + "tinygo.org/x/drivers/lsm303agr" +) + +func main() { + + machine.I2C0.Configure(machine.I2CConfig{}) + accel_mag := lsm303agr.New(machine.I2C0) + + if !accel_mag.Connected() { + println("LSM303AGR/MAG not connected!") + return + } + + accel_mag.Configure(lsm303agr.Configuration{}) //default settings + + for { + + accel_x, accel_y, accel_z := accel_mag.ReadAcceleration() + pitch, roll := accel_mag.ReadPitchRoll() + mag_x, mag_y, mag_z := accel_mag.ReadMagneticField() + heading := accel_mag.ReadCompass() + temp, _ := accel_mag.ReadTemperature() + + println("ACCEL_X:", accel_x, " ACCEL_Y:", accel_y, " ACCEL_Z:", accel_z) + println("MAG_X:", mag_x, " MAG_Y:", mag_y, " MAG_Z:", mag_z) + println("Pitch:", pitch, " Roll:", roll) + println("Heading:", heading) + println("Temperature:", temp/1000) + println("\n") + + time.Sleep(time.Millisecond * 100) + } + +} diff --git a/lsm303agr/lsm303agr.go b/lsm303agr/lsm303agr.go new file mode 100644 index 0000000..c95f9c4 --- /dev/null +++ b/lsm303agr/lsm303agr.go @@ -0,0 +1,203 @@ +// Package lsm303agr implements a driver for the LSM303AGR, +// a 3 axis accelerometer/magnetic sensor which is included on BBC micro:bits v1.5. +// +// Datasheet: https://www.st.com/resource/en/datasheet/lsm303agr.pdf +// +package lsm303agr // import "tinygo.org/x/drivers/lsm303agr" + +import ( + "machine" + "math" +) + +// Device wraps an I2C connection to a LSM303AGR device. +type Device struct { + bus machine.I2C + AccelAddress uint8 + MagAddress uint8 + AccelPowerMode uint8 + AccelRange uint8 + AccelDataRate uint8 + MagPowerMode uint8 + MagSystemMode uint8 + MagDataRate uint8 +} + +// Configuration for LSM303AGR device. +type Configuration struct { + AccelPowerMode uint8 + AccelRange uint8 + AccelDataRate uint8 + MagPowerMode uint8 + MagSystemMode uint8 + MagDataRate uint8 +} + +// New creates a new LSM303AGR connection. The I2C bus must already be +// configured. +// +// This function only creates the Device object, it does not touch the device. +func New(bus machine.I2C) Device { + return Device{bus: bus, AccelAddress: ACCEL_ADDRESS, MagAddress: MAG_ADDRESS} +} + +// Connected returns whether both sensor on LSM303AGR 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} + d.bus.ReadRegister(uint8(d.AccelAddress), ACCEL_WHO_AM_I, data1) + d.bus.ReadRegister(uint8(d.MagAddress), MAG_WHO_AM_I, data2) + return data1[0] == 0x33 && data2[0] == 0x40 +} + +// Configure sets up the LSM303AGR device for communication. +func (d *Device) Configure(cfg Configuration) { + + 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 + } + + cmd := []byte{0} + + cmd[0] = byte(d.AccelDataRate<<4 | d.AccelPowerMode | 0x07) + d.bus.WriteRegister(uint8(d.AccelAddress), ACCEL_CTRL_REG1_A, cmd) + + cmd[0] = byte(0x80 | d.AccelRange<<4) + d.bus.WriteRegister(uint8(d.AccelAddress), ACCEL_CTRL_REG4_A, cmd) + + cmd[0] = byte(0xC0) + d.bus.WriteRegister(uint8(d.AccelAddress), TEMP_CFG_REG_A, cmd) + + // Temperature compensation is on for magnetic sensor + cmd[0] = byte(0x80 | d.MagPowerMode<<4 | d.MagDataRate<<2 | d.MagSystemMode) + d.bus.WriteRegister(uint8(d.MagAddress), MAG_MR_REG_M, cmd) + +} + +// 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 int32, y int32, z int32) { + + data1, data2, data3, data4, data5, data6 := []byte{0}, []byte{0}, []byte{0}, []byte{0}, []byte{0}, []byte{0} + d.bus.ReadRegister(uint8(d.AccelAddress), ACCEL_OUT_X_H_A, data1) + d.bus.ReadRegister(uint8(d.AccelAddress), ACCEL_OUT_X_L_A, data2) + d.bus.ReadRegister(uint8(d.AccelAddress), ACCEL_OUT_Y_H_A, data3) + d.bus.ReadRegister(uint8(d.AccelAddress), ACCEL_OUT_Y_L_A, data4) + d.bus.ReadRegister(uint8(d.AccelAddress), ACCEL_OUT_Z_H_A, data5) + d.bus.ReadRegister(uint8(d.AccelAddress), ACCEL_OUT_Z_L_A, data6) + + 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(data1[0])<<8|uint16(data2[0])))>>4*rangeFactor) * 1000000 / 1024) + y = int32(int32(int16((uint16(data3[0])<<8|uint16(data4[0])))>>4*rangeFactor) * 1000000 / 1024) + z = int32(int32(int16((uint16(data5[0])<<8|uint16(data6[0])))>>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 int32, roll int32) { + + x, y, z := d.ReadAcceleration() + 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 int32, y int32, z int32) { + + if d.MagSystemMode == MAG_SYSTEM_SINGLE { + cmd := []byte{0} + cmd[0] = byte(0x80 | d.MagPowerMode<<4 | d.MagDataRate<<2 | d.MagSystemMode) + d.bus.WriteRegister(uint8(d.MagAddress), MAG_MR_REG_M, cmd) + } + + data1, data2, data3, data4, data5, data6 := []byte{0}, []byte{0}, []byte{0}, []byte{0}, []byte{0}, []byte{0} + d.bus.ReadRegister(uint8(d.MagAddress), MAG_OUT_X_H_M, data1) + d.bus.ReadRegister(uint8(d.MagAddress), MAG_OUT_X_L_M, data2) + d.bus.ReadRegister(uint8(d.MagAddress), MAG_OUT_Y_H_M, data3) + d.bus.ReadRegister(uint8(d.MagAddress), MAG_OUT_Y_L_M, data4) + d.bus.ReadRegister(uint8(d.MagAddress), MAG_OUT_Z_H_M, data5) + d.bus.ReadRegister(uint8(d.MagAddress), MAG_OUT_Z_L_M, data6) + + x = int32(int16((uint16(data1[0])<<8 | uint16(data2[0])))) + y = int32(int16((uint16(data3[0])<<8 | uint16(data4[0])))) + z = int32(int16((uint16(data5[0])<<8 | uint16(data6[0])))) + 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) { + + x, y, _ := d.ReadMagneticField() + 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() (c int32, e error) { + + data1, data2 := []byte{0}, []byte{0} + d.bus.ReadRegister(uint8(d.AccelAddress), OUT_TEMP_H_A, data1) + d.bus.ReadRegister(uint8(d.AccelAddress), OUT_TEMP_L_A, data2) + + t := int16((uint16(data1[0])<<8 | uint16(data2[0]))) >> 4 // temperature offsef from 25 °C + c = int32((float32(25) + float32(t)/8) * 1000) + e = nil + return +} diff --git a/lsm303agr/registers.go b/lsm303agr/registers.go new file mode 100644 index 0000000..6767f2f --- /dev/null +++ b/lsm303agr/registers.go @@ -0,0 +1,68 @@ +package lsm303agr + +const ( + + // Constants/addresses used for I2C. + ACCEL_ADDRESS = 0x19 + MAG_ADDRESS = 0x1E + + // accelerometer registers. + ACCEL_WHO_AM_I = 0x0F + 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 + + // magnetic sensor registers. + MAG_WHO_AM_I = 0x4F + MAG_MR_REG_M = 0x60 + 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 + + // temperature sensor registers. + TEMP_CFG_REG_A = 0x1F + OUT_TEMP_L_A = 0x0C + OUT_TEMP_H_A = 0x0D + + // 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 +)