mirror of
https://github.com/tinygo-org/drivers.git
synced 2026-07-26 10:38:41 +00:00
added support for LSM303DLHC e-Compass; (#783)
fixed the spelling in the Connection error message; Initial support for LSM303DLHC added; Added LSM303DLHC to smoketest and added an example; Removed unnecessary comments; fixed format error; squashed and ready for merge;
This commit is contained in:
Bryan Souza
GitHub
@@ -0,0 +1,58 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"machine"
|
||||
"time"
|
||||
|
||||
"tinygo.org/x/drivers/lsm303dlhc"
|
||||
)
|
||||
|
||||
func main() {
|
||||
|
||||
// LSM303DLHC is connected to the I2C0 bus on Adafruit Feather M4 via pins: 20(SDA) and 21(SCL).
|
||||
machine.I2C0.Configure(machine.I2CConfig{})
|
||||
|
||||
sensor := lsm303dlhc.New(machine.I2C0)
|
||||
//default settings
|
||||
err := sensor.Configure(lsm303dlhc.Configuration{
|
||||
AccelPowerMode: lsm303dlhc.ACCEL_POWER_NORMAL,
|
||||
AccelRange: lsm303dlhc.ACCEL_RANGE_2G,
|
||||
AccelDataRate: lsm303dlhc.ACCEL_DATARATE_100HZ,
|
||||
MagPowerMode: lsm303dlhc.MAG_POWER_NORMAL,
|
||||
MagSystemMode: lsm303dlhc.MAG_SYSTEM_CONTINUOUS,
|
||||
MagDataRate: lsm303dlhc.MAG_DATARATE_10HZ,
|
||||
})
|
||||
if err != nil {
|
||||
for {
|
||||
println("Failed to configure", err.Error())
|
||||
time.Sleep(time.Second)
|
||||
}
|
||||
}
|
||||
|
||||
for {
|
||||
accel_x, accel_y, accel_z, err := sensor.ReadAcceleration()
|
||||
if err != nil {
|
||||
println("Failed to read accel", err.Error())
|
||||
}
|
||||
println("ACCEL_X:", accel_x, " ACCEL_Y:", accel_y, " ACCEL_Z:", accel_z)
|
||||
|
||||
mag_x, mag_y, mag_z, err := sensor.ReadMagneticField()
|
||||
if err != nil {
|
||||
println("Failed to read mag", err.Error())
|
||||
}
|
||||
println("MAG_X:", mag_x, " MAG_Y:", mag_y, " MAG_Z:", mag_z)
|
||||
|
||||
pitch, roll, _ := sensor.ReadPitchRoll()
|
||||
println("Pitch:", float32(pitch), " Roll:", float32(roll))
|
||||
|
||||
heading, _ := sensor.ReadCompass()
|
||||
println("Heading:", float32(heading), "degrees")
|
||||
|
||||
temp, _ := sensor.ReadTemperature()
|
||||
println("Temperature:", float32(temp)/1000, "*C")
|
||||
|
||||
println("\n")
|
||||
time.Sleep(time.Millisecond * 250)
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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.
|
||||
//
|
||||
|
||||
@@ -0,0 +1,214 @@
|
||||
// Package lsm303dlhc implements a driver for the LSM303dlhc,
|
||||
// a 3 axis accelerometer/magnetic sensor typically available on breakout boards.
|
||||
//
|
||||
// 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
|
||||
}
|
||||
|
||||
// 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,
|
||||
}
|
||||
}
|
||||
|
||||
// Configure sets up the LSM303dlhc device for communication.
|
||||
func (d *Device) Configure(cfg Configuration) (err error) {
|
||||
|
||||
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
|
||||
}
|
||||
@@ -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
|
||||
)
|
||||
@@ -44,6 +44,7 @@ tinygo build -size short -o ./build/test.hex -target=pyportal ./examples/ili9341
|
||||
tinygo build -size short -o ./build/test.hex -target=circuitplay-express ./examples/lis3dh/main.go
|
||||
tinygo build -size short -o ./build/test.hex -target=nano-33-ble ./examples/lps22hb/main.go
|
||||
tinygo build -size short -o ./build/test.hex -target=microbit ./examples/lsm303agr/main.go
|
||||
tinygo build -size short -o ./build/test.hex -target=feather-m4 ./examples/lsm303dlhc/main.go
|
||||
tinygo build -size short -o ./build/test.hex -target=arduino-nano33 ./examples/lsm6ds3/main.go
|
||||
tinygo build -size short -o ./build/test.hex -target=itsybitsy-m0 ./examples/mag3110/main.go
|
||||
tinygo build -size short -o ./build/test.hex -target=itsybitsy-m0 ./examples/mcp23017/main.go
|
||||
|
||||
Reference in New Issue
Block a user