Skip to content

lsm6ds3tr: avoid unnecessary heap allocations (alternative) #769

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
wants to merge 1 commit into from
+38 −31
Closed

lsm6ds3tr: avoid unnecessary heap allocations (alternative) #769

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
69 changes: 38 additions & 31 deletions lsm6ds3tr/lsm6ds3tr.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@ import (
"errors"

"tinygo.org/x/drivers"
"tinygo.org/x/drivers/internal/legacy"
)

type AccelRange uint8
Expand Down Expand Up @@ -84,30 +83,20 @@ func (d *Device) doConfigure(cfg Configuration) (err error) {
d.gyroSampleRate = GYRO_SR_104
}

data := d.buf[:1]

// Configure accelerometer
data[0] = uint8(d.accelRange) | uint8(d.accelSampleRate)
err = legacy.WriteRegister(d.bus, uint8(d.Address), CTRL1_XL, data)
err = d.writeValue(CTRL1_XL, uint8(d.accelRange)|uint8(d.accelSampleRate))
if err != nil {
return
}

// Set ODR bit
err = legacy.ReadRegister(d.bus, uint8(d.Address), CTRL4_C, data)
if err != nil {
return
}
data[0] = data[0] &^ BW_SCAL_ODR_ENABLED
data[0] |= BW_SCAL_ODR_ENABLED
err = legacy.WriteRegister(d.bus, uint8(d.Address), CTRL4_C, data)
// Enable ODR scaling
err = d.setBits(CTRL4_C, BW_SCAL_ODR_ENABLED)
if err != nil {
return
}

// Configure gyroscope
data[0] = uint8(d.gyroRange) | uint8(d.gyroSampleRate)
err = legacy.WriteRegister(d.bus, uint8(d.Address), CTRL2_G, data)
err = d.writeValue(CTRL2_G, uint8(d.gyroRange)|uint8(d.gyroSampleRate))
if err != nil {
return
}
Expand All @@ -118,18 +107,19 @@ func (d *Device) doConfigure(cfg Configuration) (err error) {
// Connected returns whether a LSM6DS3TR has been found.
// It does a "who am I" request and checks the response.
func (d *Device) Connected() bool {
data := d.buf[:1]
legacy.ReadRegister(d.bus, uint8(d.Address), WHO_AM_I, data)
return data[0] == 0x6A
err := d.readValue(WHO_AM_I, 1)
if err != nil {
return false
}
return d.buf[0] == 0x6A
}

// 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.Address), OUTX_L_XL, data)
err = d.readValue(OUTX_L_XL, 6)
if err != nil {
return
}
Expand All @@ -142,9 +132,9 @@ func (d *Device) ReadAcceleration() (x, y, z int32, err error) {
} else if d.accelRange == ACCEL_16G {
k = 488
}
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
x = int32(int16((uint16(d.buf[1])<<8)|uint16(d.buf[0]))) * k
y = int32(int16((uint16(d.buf[3])<<8)|uint16(d.buf[2]))) * k
z = int32(int16((uint16(d.buf[5])<<8)|uint16(d.buf[4]))) * k
return
}

Expand All @@ -153,8 +143,7 @@ func (d *Device) ReadAcceleration() (x, y, z int32, err error) {
// 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, y, z int32, err error) {
data := d.buf[:6]
err = legacy.ReadRegister(d.bus, uint8(d.Address), OUTX_L_G, data)
err = d.readValue(OUTX_L_G, 6)
if err != nil {
return
}
Expand All @@ -169,21 +158,39 @@ func (d *Device) ReadRotation() (x, y, z int32, err error) {
} else if d.gyroRange == GYRO_2000DPS {
k = 70000
}
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
x = int32(int16((uint16(d.buf[1])<<8)|uint16(d.buf[0]))) * k
y = int32(int16((uint16(d.buf[3])<<8)|uint16(d.buf[2]))) * k
z = int32(int16((uint16(d.buf[5])<<8)|uint16(d.buf[4]))) * k
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.Address), OUT_TEMP_L, data)
err = d.readValue(OUT_TEMP_L, 2)
if err != nil {
return
}
// From "Table 5. Temperature sensor characteristics"
// temp = value/256 + 25
t = 25000 + (int32(int16((int16(data[1])<<8)|int16(data[0])))*125)/32
t = 25000 + (int32(int16((int16(d.buf[2])<<8)|int16(d.buf[1])))*125)/32
return
}

func (d *Device) readValue(reg, size uint8) (err error) {
d.buf[0] = reg
return d.bus.Tx(d.Address, d.buf[0:1], d.buf[0:size])
}

func (d *Device) writeValue(reg, value uint8) error {
d.buf[0] = reg
d.buf[1] = value
return d.bus.Tx(d.Address, d.buf[0:2], nil)
}

func (d *Device) setBits(reg, bits uint8) (err error) {
err = d.readValue(reg, 1)
if err != nil {
return
}
return d.writeValue(reg, (d.buf[0]&^bits)|bits)
}