From 0c7f3179ae2685a2691f1e2e36ea8a5ff01e1392 Mon Sep 17 00:00:00 2001 From: Yurii Soldak Date: Thu, 19 Jun 2025 15:48:00 +0200 Subject: [PATCH 1/3] lsm6ds3tr: avoid unnecessary heap allocations --- lsm6ds3tr/lsm6ds3tr.go | 57 +++++++++++++++++++++--------------------- 1 file changed, 29 insertions(+), 28 deletions(-) diff --git a/lsm6ds3tr/lsm6ds3tr.go b/lsm6ds3tr/lsm6ds3tr.go index b8be8d8b8..c1d74d6fc 100644 --- a/lsm6ds3tr/lsm6ds3tr.go +++ b/lsm6ds3tr/lsm6ds3tr.go @@ -8,7 +8,6 @@ import ( "errors" "tinygo.org/x/drivers" - "tinygo.org/x/drivers/internal/legacy" ) type AccelRange uint8 @@ -26,7 +25,7 @@ type Device struct { accelSampleRate AccelSampleRate gyroRange GyroRange gyroSampleRate GyroSampleRate - buf [6]uint8 + buf [7]uint8 // cmd + up to 6 bytes of data } // Configuration for LSM6DS3TR device. @@ -84,30 +83,32 @@ 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) + d.buf[0] = CTRL1_XL + d.buf[1] = uint8(d.accelRange) | uint8(d.accelSampleRate) + err = d.bus.Tx(d.Address, d.buf[0:2], nil) if err != nil { return } // Set ODR bit - err = legacy.ReadRegister(d.bus, uint8(d.Address), CTRL4_C, data) + d.buf[0] = CTRL4_C + err = d.bus.Tx(d.Address, d.buf[0:1], d.buf[1:2]) 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) + d.buf[0] = CTRL4_C + d.buf[1] = d.buf[1] &^ BW_SCAL_ODR_ENABLED + d.buf[1] |= BW_SCAL_ODR_ENABLED + err = d.bus.Tx(d.Address, d.buf[0:2], nil) 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) + d.buf[0] = CTRL2_G + d.buf[1] = uint8(d.gyroRange) | uint8(d.gyroSampleRate) + err = d.bus.Tx(d.Address, d.buf[0:2], nil) if err != nil { return } @@ -118,9 +119,9 @@ 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 + d.buf[0] = WHO_AM_I + d.bus.Tx(d.Address, d.buf[0:1], d.buf[1:2]) + return d.buf[1] == 0x6A } // ReadAcceleration reads the current acceleration from the device and returns @@ -128,8 +129,8 @@ func (d *Device) Connected() bool { // 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) + d.buf[0] = OUTX_L_XL + err = d.bus.Tx(d.Address, d.buf[0:1], d.buf[1:7]) if err != nil { return } @@ -142,9 +143,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[2])<<8)|uint16(d.buf[1]))) * k + y = int32(int16((uint16(d.buf[4])<<8)|uint16(d.buf[3]))) * k + z = int32(int16((uint16(d.buf[6])<<8)|uint16(d.buf[5]))) * k return } @@ -153,8 +154,8 @@ 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) + d.buf[0] = OUTX_L_G + err = d.bus.Tx(d.Address, d.buf[0:1], d.buf[1:7]) if err != nil { return } @@ -169,21 +170,21 @@ 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[2])<<8)|uint16(d.buf[1]))) * k + y = int32(int16((uint16(d.buf[4])<<8)|uint16(d.buf[3]))) * k + z = int32(int16((uint16(d.buf[6])<<8)|uint16(d.buf[5]))) * 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) + d.buf[0] = OUT_TEMP_L + err = d.bus.Tx(d.Address, d.buf[0:1], d.buf[1:3]) 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 } From a6949c53ea05e101164ac5ed84e27c30920c3c39 Mon Sep 17 00:00:00 2001 From: Yurii Soldak Date: Sat, 21 Jun 2025 17:39:52 +0200 Subject: [PATCH 2/3] lsm6ds3tr: use helper functions, for readability --- lsm6ds3tr/lsm6ds3tr.go | 54 +++++++++++++++++++++++------------------- 1 file changed, 30 insertions(+), 24 deletions(-) diff --git a/lsm6ds3tr/lsm6ds3tr.go b/lsm6ds3tr/lsm6ds3tr.go index c1d74d6fc..01bea3073 100644 --- a/lsm6ds3tr/lsm6ds3tr.go +++ b/lsm6ds3tr/lsm6ds3tr.go @@ -84,31 +84,19 @@ func (d *Device) doConfigure(cfg Configuration) (err error) { } // Configure accelerometer - d.buf[0] = CTRL1_XL - d.buf[1] = uint8(d.accelRange) | uint8(d.accelSampleRate) - err = d.bus.Tx(d.Address, d.buf[0:2], nil) + err = d.writeValue(CTRL1_XL, uint8(d.accelRange)|uint8(d.accelSampleRate)) if err != nil { return } - // Set ODR bit - d.buf[0] = CTRL4_C - err = d.bus.Tx(d.Address, d.buf[0:1], d.buf[1:2]) - if err != nil { - return - } - d.buf[0] = CTRL4_C - d.buf[1] = d.buf[1] &^ BW_SCAL_ODR_ENABLED - d.buf[1] |= BW_SCAL_ODR_ENABLED - err = d.bus.Tx(d.Address, d.buf[0:2], nil) + // Enable ODR scaling + err = d.setBits(CTRL4_C, BW_SCAL_ODR_ENABLED) if err != nil { return } // Configure gyroscope - d.buf[0] = CTRL2_G - d.buf[1] = uint8(d.gyroRange) | uint8(d.gyroSampleRate) - err = d.bus.Tx(d.Address, d.buf[0:2], nil) + err = d.writeValue(CTRL2_G, uint8(d.gyroRange)|uint8(d.gyroSampleRate)) if err != nil { return } @@ -119,8 +107,10 @@ 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 { - d.buf[0] = WHO_AM_I - d.bus.Tx(d.Address, d.buf[0:1], d.buf[1:2]) + err := d.readValue(WHO_AM_I, 1) + if err != nil { + return false + } return d.buf[1] == 0x6A } @@ -129,8 +119,7 @@ func (d *Device) Connected() bool { // 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) { - d.buf[0] = OUTX_L_XL - err = d.bus.Tx(d.Address, d.buf[0:1], d.buf[1:7]) + err = d.readValue(OUTX_L_XL, 6) if err != nil { return } @@ -154,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) { - d.buf[0] = OUTX_L_G - err = d.bus.Tx(d.Address, d.buf[0:1], d.buf[1:7]) + err = d.readValue(OUTX_L_G, 6) if err != nil { return } @@ -178,8 +166,7 @@ func (d *Device) ReadRotation() (x, y, z int32, err error) { // ReadTemperature returns the temperature in celsius milli degrees (°C/1000) func (d *Device) ReadTemperature() (t int32, err error) { - d.buf[0] = OUT_TEMP_L - err = d.bus.Tx(d.Address, d.buf[0:1], d.buf[1:3]) + err = d.readValue(OUT_TEMP_L, 2) if err != nil { return } @@ -188,3 +175,22 @@ func (d *Device) ReadTemperature() (t int32, err error) { t = 25000 + (int32(int16((int16(d.buf[2])<<8)|int16(d.buf[1])))*125)/32 return } + +func (d *Device) readValue(reg, size uint8) error { + d.buf[0] = reg + return d.bus.Tx(d.Address, d.buf[0:1], d.buf[1:size+1]) +} + +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[1]&^bits)|bits) +} From a35ee44c25e666c65a560eca2680b2d42cfe5e7f Mon Sep 17 00:00:00 2001 From: Yurii Soldak Date: Mon, 23 Jun 2025 15:51:04 +0200 Subject: [PATCH 3/3] lsm6ds3tr: return slice of the internal buffer on readBytes --- lsm6ds3tr/lsm6ds3tr.go | 48 +++++++++++++++++++++++------------------- 1 file changed, 26 insertions(+), 22 deletions(-) diff --git a/lsm6ds3tr/lsm6ds3tr.go b/lsm6ds3tr/lsm6ds3tr.go index 01bea3073..76b91d1da 100644 --- a/lsm6ds3tr/lsm6ds3tr.go +++ b/lsm6ds3tr/lsm6ds3tr.go @@ -25,7 +25,7 @@ type Device struct { accelSampleRate AccelSampleRate gyroRange GyroRange gyroSampleRate GyroSampleRate - buf [7]uint8 // cmd + up to 6 bytes of data + buf [7]uint8 // up to 6 bytes for read + 1 byte for the register address } // Configuration for LSM6DS3TR device. @@ -84,7 +84,7 @@ func (d *Device) doConfigure(cfg Configuration) (err error) { } // Configure accelerometer - err = d.writeValue(CTRL1_XL, uint8(d.accelRange)|uint8(d.accelSampleRate)) + err = d.writeByte(CTRL1_XL, uint8(d.accelRange)|uint8(d.accelSampleRate)) if err != nil { return } @@ -96,7 +96,7 @@ func (d *Device) doConfigure(cfg Configuration) (err error) { } // Configure gyroscope - err = d.writeValue(CTRL2_G, uint8(d.gyroRange)|uint8(d.gyroSampleRate)) + err = d.writeByte(CTRL2_G, uint8(d.gyroRange)|uint8(d.gyroSampleRate)) if err != nil { return } @@ -107,11 +107,11 @@ 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 { - err := d.readValue(WHO_AM_I, 1) + data, err := d.readBytes(WHO_AM_I, 1) if err != nil { return false } - return d.buf[1] == 0x6A + return data[0] == 0x6A } // ReadAcceleration reads the current acceleration from the device and returns @@ -119,7 +119,7 @@ func (d *Device) Connected() bool { // 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) { - err = d.readValue(OUTX_L_XL, 6) + data, err := d.readBytes(OUTX_L_XL, 6) if err != nil { return } @@ -132,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(d.buf[2])<<8)|uint16(d.buf[1]))) * k - y = int32(int16((uint16(d.buf[4])<<8)|uint16(d.buf[3]))) * k - z = int32(int16((uint16(d.buf[6])<<8)|uint16(d.buf[5]))) * 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 } @@ -143,7 +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) { - err = d.readValue(OUTX_L_G, 6) + data, err := d.readBytes(OUTX_L_G, 6) if err != nil { return } @@ -158,39 +158,43 @@ func (d *Device) ReadRotation() (x, y, z int32, err error) { } else if d.gyroRange == GYRO_2000DPS { k = 70000 } - x = int32(int16((uint16(d.buf[2])<<8)|uint16(d.buf[1]))) * k - y = int32(int16((uint16(d.buf[4])<<8)|uint16(d.buf[3]))) * k - z = int32(int16((uint16(d.buf[6])<<8)|uint16(d.buf[5]))) * 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() (t int32, err error) { - err = d.readValue(OUT_TEMP_L, 2) + data, err := d.readBytes(OUT_TEMP_L, 2) if err != nil { return } // From "Table 5. Temperature sensor characteristics" // temp = value/256 + 25 - t = 25000 + (int32(int16((int16(d.buf[2])<<8)|int16(d.buf[1])))*125)/32 + t = 25000 + (int32(int16((int16(data[1])<<8)|int16(data[0])))*125)/32 return } -func (d *Device) readValue(reg, size uint8) error { +func (d *Device) readBytes(reg, size uint8) ([]byte, error) { d.buf[0] = reg - return d.bus.Tx(d.Address, d.buf[0:1], d.buf[1:size+1]) + err := d.bus.Tx(d.Address, d.buf[0:1], d.buf[1:size+1]) + if err != nil { + return nil, err + } + return d.buf[1 : size+1], nil } -func (d *Device) writeValue(reg, value uint8) error { +func (d *Device) writeByte(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) +func (d *Device) setBits(reg, bits uint8) error { + data, err := d.readBytes(reg, 1) if err != nil { - return + return err } - return d.writeValue(reg, (d.buf[1]&^bits)|bits) + return d.writeByte(reg, (data[0]&^bits)|bits) }