mirror of
https://github.com/tinygo-org/drivers.git
synced 2026-08-11 10:23:44 +00:00
lsmXXX: unified, error handling, memory management
This commit is contained in:
Yurii Soldak
Ron Evans
+91
-39
@@ -5,7 +5,11 @@
|
||||
//
|
||||
package lsm6ds3 // import "tinygo.org/x/drivers/lsm6ds3"
|
||||
|
||||
import "tinygo.org/x/drivers"
|
||||
import (
|
||||
"errors"
|
||||
|
||||
"tinygo.org/x/drivers"
|
||||
)
|
||||
|
||||
type AccelRange uint8
|
||||
type AccelSampleRate uint8
|
||||
@@ -23,8 +27,7 @@ type Device struct {
|
||||
accelBandWidth AccelBandwidth
|
||||
gyroRange GyroRange
|
||||
gyroSampleRate GyroSampleRate
|
||||
dataBufferSix []uint8
|
||||
dataBufferTwo []uint8
|
||||
buf [6]uint8
|
||||
}
|
||||
|
||||
// Configuration for LSM6DS3 device.
|
||||
@@ -38,16 +41,26 @@ type Configuration struct {
|
||||
ResetStepCounter bool
|
||||
}
|
||||
|
||||
// New creates a new LSM6DS3 connection. The I2C bus must already be
|
||||
// configured.
|
||||
var errNotConnected = errors.New("lsm6ds3: failed to communicate with acel/gyro sensor")
|
||||
|
||||
// New creates a new LSM6DS3 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, Address: Address}
|
||||
func New(bus drivers.I2C) *Device {
|
||||
return &Device{
|
||||
bus: bus,
|
||||
Address: Address,
|
||||
}
|
||||
}
|
||||
|
||||
// Configure sets up the device for communication.
|
||||
func (d *Device) Configure(cfg Configuration) {
|
||||
func (d *Device) Configure(cfg Configuration) (err error) {
|
||||
|
||||
// Verify unit communication
|
||||
if !d.Connected() {
|
||||
return errNotConnected
|
||||
}
|
||||
|
||||
if cfg.AccelRange != 0 {
|
||||
d.accelRange = cfg.AccelRange
|
||||
} else {
|
||||
@@ -78,44 +91,67 @@ func (d *Device) Configure(cfg Configuration) {
|
||||
d.gyroSampleRate = GYRO_SR_104
|
||||
}
|
||||
|
||||
d.dataBufferSix = make([]uint8, 6)
|
||||
d.dataBufferTwo = make([]uint8, 2)
|
||||
data := d.buf[:1]
|
||||
|
||||
if cfg.IsPedometer { // CONFIGURE AS PEDOMETER
|
||||
// Configure accelerometer: 2G + 26Hz
|
||||
d.bus.WriteRegister(uint8(d.Address), CTRL1_XL, []byte{uint8(ACCEL_2G) | uint8(ACCEL_SR_26)})
|
||||
data[0] = uint8(ACCEL_2G) | uint8(ACCEL_SR_26)
|
||||
err = d.bus.WriteRegister(uint8(d.Address), CTRL1_XL, data)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
// Configure Zen_G, Yen_G, Xen_G, reset steps
|
||||
data[0] = 0x3C
|
||||
if cfg.ResetStepCounter {
|
||||
d.bus.WriteRegister(uint8(d.Address), CTRL10_C, []byte{0x3E})
|
||||
} else {
|
||||
d.bus.WriteRegister(uint8(d.Address), CTRL10_C, []byte{0x3C})
|
||||
data[0] |= 0x02
|
||||
}
|
||||
err = d.bus.WriteRegister(uint8(d.Address), CTRL10_C, data)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
// Enable pedometer
|
||||
d.bus.WriteRegister(uint8(d.Address), TAP_CFG, []byte{0x40})
|
||||
data[0] = 0x40
|
||||
err = d.bus.WriteRegister(uint8(d.Address), TAP_CFG, data)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
} else { // NORMAL USE
|
||||
// Configure accelerometer
|
||||
data := make([]uint8, 1)
|
||||
data[0] = uint8(d.accelRange) | uint8(d.accelSampleRate) | uint8(d.accelBandWidth)
|
||||
d.bus.WriteRegister(uint8(d.Address), CTRL1_XL, data)
|
||||
err = d.bus.WriteRegister(uint8(d.Address), CTRL1_XL, data)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
// Set ODR bit
|
||||
d.bus.ReadRegister(uint8(d.Address), CTRL4_C, data)
|
||||
err = d.bus.ReadRegister(uint8(d.Address), CTRL4_C, data)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
data[0] = data[0] &^ BW_SCAL_ODR_ENABLED
|
||||
data[0] |= BW_SCAL_ODR_ENABLED
|
||||
d.bus.WriteRegister(uint8(d.Address), CTRL4_C, data)
|
||||
err = d.bus.WriteRegister(uint8(d.Address), CTRL4_C, data)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
|
||||
// Configure gyroscope
|
||||
data[0] = uint8(d.gyroRange) | uint8(d.gyroSampleRate)
|
||||
d.bus.WriteRegister(uint8(d.Address), CTRL2_G, data)
|
||||
err = d.bus.WriteRegister(uint8(d.Address), CTRL2_G, data)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
}
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
// Connected returns whether a LSM6DS3 has been found.
|
||||
// It does a "who am I" request and checks the response.
|
||||
func (d *Device) Connected() bool {
|
||||
data := []byte{0}
|
||||
data := d.buf[:1]
|
||||
d.bus.ReadRegister(uint8(d.Address), WHO_AM_I, data)
|
||||
return data[0] == 0x69
|
||||
}
|
||||
@@ -124,8 +160,12 @@ func (d *Device) Connected() bool {
|
||||
// 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) {
|
||||
d.bus.ReadRegister(uint8(d.Address), OUTX_L_XL, d.dataBufferSix)
|
||||
func (d *Device) ReadAcceleration() (x, y, z int32, err error) {
|
||||
data := d.buf[:6]
|
||||
err = d.bus.ReadRegister(uint8(d.Address), OUTX_L_XL, data)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
// k comes from "Table 3. Mechanical characteristics" 3 of the datasheet * 1000
|
||||
k := int32(61) // 2G
|
||||
if d.accelRange == ACCEL_4G {
|
||||
@@ -135,9 +175,9 @@ func (d *Device) ReadAcceleration() (x int32, y int32, z int32) {
|
||||
} else if d.accelRange == ACCEL_16G {
|
||||
k = 488
|
||||
}
|
||||
x = int32(int16((uint16(d.dataBufferSix[1])<<8)|uint16(d.dataBufferSix[0]))) * k
|
||||
y = int32(int16((uint16(d.dataBufferSix[3])<<8)|uint16(d.dataBufferSix[2]))) * k
|
||||
z = int32(int16((uint16(d.dataBufferSix[5])<<8)|uint16(d.dataBufferSix[4]))) * k
|
||||
x = int32(int16((uint16(data[1])<<8)|uint16(data[0]))) * k
|
||||
y = int32(int16((uint16(data[3])<<8)|uint16(data[2]))) * k
|
||||
z = int32(int16((uint16(data[5])<<8)|uint16(data[4]))) * k
|
||||
return
|
||||
}
|
||||
|
||||
@@ -145,8 +185,12 @@ func (d *Device) ReadAcceleration() (x int32, y int32, z int32) {
|
||||
// µ°/s (micro-degrees/sec). This means that if you were to do a complete
|
||||
// rotation along one axis and while doing so integrate all values over time,
|
||||
// you would get a value close to 360000000.
|
||||
func (d *Device) ReadRotation() (x int32, y int32, z int32) {
|
||||
d.bus.ReadRegister(uint8(d.Address), OUTX_L_G, d.dataBufferSix)
|
||||
func (d *Device) ReadRotation() (x, y, z int32, err error) {
|
||||
data := d.buf[:6]
|
||||
err = d.bus.ReadRegister(uint8(d.Address), OUTX_L_G, data)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
// k comes from "Table 3. Mechanical characteristics" 3 of the datasheet * 1000
|
||||
k := int32(4375) // 125DPS
|
||||
if d.gyroRange == GYRO_250DPS {
|
||||
@@ -158,24 +202,32 @@ func (d *Device) ReadRotation() (x int32, y int32, z int32) {
|
||||
} else if d.gyroRange == GYRO_2000DPS {
|
||||
k = 70000
|
||||
}
|
||||
x = int32(int16((uint16(d.dataBufferSix[1])<<8)|uint16(d.dataBufferSix[0]))) * k
|
||||
y = int32(int16((uint16(d.dataBufferSix[3])<<8)|uint16(d.dataBufferSix[2]))) * k
|
||||
z = int32(int16((uint16(d.dataBufferSix[5])<<8)|uint16(d.dataBufferSix[4]))) * k
|
||||
x = int32(int16((uint16(data[1])<<8)|uint16(data[0]))) * k
|
||||
y = int32(int16((uint16(data[3])<<8)|uint16(data[2]))) * k
|
||||
z = int32(int16((uint16(data[5])<<8)|uint16(data[4]))) * k
|
||||
return
|
||||
}
|
||||
|
||||
// ReadTemperature returns the temperature in celsius milli degrees (°C/1000)
|
||||
func (d *Device) ReadTemperature() (int32, error) {
|
||||
d.bus.ReadRegister(uint8(d.Address), OUT_TEMP_L, d.dataBufferTwo)
|
||||
|
||||
func (d *Device) ReadTemperature() (t int32, err error) {
|
||||
data := d.buf[:2]
|
||||
err = d.bus.ReadRegister(uint8(d.Address), OUT_TEMP_L, data)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
// From "Table 5. Temperature sensor characteristics"
|
||||
// temp = value/16 + 25
|
||||
t := 25000 + (int32(int16((int16(d.dataBufferTwo[1])<<8)|int16(d.dataBufferTwo[0])))*125)/2
|
||||
return t, nil
|
||||
t = 25000 + (int32(int16((int16(data[1])<<8)|int16(data[0])))*125)/2
|
||||
return
|
||||
}
|
||||
|
||||
// ReadSteps returns the steps of the pedometer
|
||||
func (d *Device) ReadSteps() int32 {
|
||||
d.bus.ReadRegister(uint8(d.Address), STEP_COUNTER_L, d.dataBufferTwo)
|
||||
return int32(int16((uint16(d.dataBufferTwo[1]) << 8) | uint16(d.dataBufferTwo[0])))
|
||||
func (d *Device) ReadSteps() (s int32, err error) {
|
||||
data := d.buf[:2]
|
||||
err = d.bus.ReadRegister(uint8(d.Address), STEP_COUNTER_L, data)
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
s = int32(int16((uint16(data[1]) << 8) | uint16(data[0])))
|
||||
return
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user