From 45dce188f50dee37e8910dfd09f539daa3ee3041 Mon Sep 17 00:00:00 2001 From: Yurii Soldak Date: Sat, 29 Jan 2022 01:15:16 +0100 Subject: [PATCH] lsmXXX: unified, error handling, memory management --- examples/lsm303agr/main.go | 24 ++++-- examples/lsm6ds3/main.go | 19 +++-- examples/lsm6dsox/main.go | 14 +++- examples/lsm9ds1/main.go | 2 +- lsm303agr/lsm303agr.go | 127 +++++++++++++++++++------------- lsm6ds3/lsm6ds3.go | 130 +++++++++++++++++++++++---------- lsm6dsox/lsm6dsox.go | 81 +++++++++++++------- lsm9ds1/lsm9ds1.go | 63 ++++++++-------- lsm9ds1/lsm9ds1_nano_33_ble.go | 2 +- 9 files changed, 292 insertions(+), 170 deletions(-) diff --git a/examples/lsm303agr/main.go b/examples/lsm303agr/main.go index 109290a52..fadb1dc1c 100644 --- a/examples/lsm303agr/main.go +++ b/examples/lsm303agr/main.go @@ -13,7 +13,13 @@ func main() { machine.I2C0.Configure(machine.I2CConfig{}) sensor := lsm303agr.New(machine.I2C0) - sensor.Configure(lsm303agr.Configuration{}) //default settings + err := sensor.Configure(lsm303agr.Configuration{}) //default settings + if err != nil { + for { + println("Failed to configure", err.Error()) + time.Sleep(time.Second) + } + } // you can specify the following options to adjust accuracy, sensor range or save power. // see https://github.com/tinygo-org/drivers/blob/release/lsm303agr/registers.go for details: @@ -28,22 +34,24 @@ func main() { }) */ - if !sensor.Connected() { - println("LSM303AGR/MAG not connected!") - return - } - for { + + if !sensor.Connected() { + println("LSM303AGR/MAG not connected!") + time.Sleep(time.Second) + continue + } + // accel_x, accel_y, accel_z := sensor.ReadAcceleration() // println("ACCEL_X:", accel_x/100000, " ACCEL_Y:", accel_y/100000, " ACCEL_Z:", accel_z/100000) // mag_x, mag_y, mag_z := sensor.ReadMagneticField() // println("MAG_X:", mag_x/100000, " MAG_Y:", mag_y/100000, " MAG_Z:", mag_z/100000) - pitch, roll := sensor.ReadPitchRoll() + pitch, roll, _ := sensor.ReadPitchRoll() println("Pitch:", float32(pitch)/100000, " Roll:", float32(roll)/100000) - heading := sensor.ReadCompass() + heading, _ := sensor.ReadCompass() println("Heading:", float32(heading)/100000, "degrees") temp, _ := sensor.ReadTemperature() diff --git a/examples/lsm6ds3/main.go b/examples/lsm6ds3/main.go index 88b85d138..1cee5fad5 100644 --- a/examples/lsm6ds3/main.go +++ b/examples/lsm6ds3/main.go @@ -12,16 +12,23 @@ func main() { machine.I2C0.Configure(machine.I2CConfig{}) accel := lsm6ds3.New(machine.I2C0) - accel.Configure(lsm6ds3.Configuration{}) - if !accel.Connected() { - println("LSM6DS3 not connected") - return + err := accel.Configure(lsm6ds3.Configuration{}) + if err != nil { + for { + println("Failed to configure", err.Error()) + time.Sleep(time.Second) + } } for { - x, y, z := accel.ReadAcceleration() + if !accel.Connected() { + println("LSM6DS3 not connected") + time.Sleep(time.Second) + continue + } + x, y, z, _ := accel.ReadAcceleration() println("Acceleration:", float32(x)/1000000, float32(y)/1000000, float32(z)/1000000) - x, y, z = accel.ReadRotation() + x, y, z, _ = accel.ReadRotation() println("Gyroscope:", float32(x)/1000000, float32(y)/1000000, float32(z)/1000000) x, _ = accel.ReadTemperature() println("Degrees C", float32(x)/1000, "\n\n") diff --git a/examples/lsm6dsox/main.go b/examples/lsm6dsox/main.go index e962d29db..cc521aa44 100644 --- a/examples/lsm6dsox/main.go +++ b/examples/lsm6dsox/main.go @@ -26,12 +26,18 @@ func main() { machine.I2C0.Configure(machine.I2CConfig{}) device := lsm6dsox.New(machine.I2C0) - device.Configure(lsm6dsox.Configuration{ + err := device.Configure(lsm6dsox.Configuration{ AccelRange: lsm6dsox.ACCEL_2G, AccelSampleRate: lsm6dsox.ACCEL_SR_104, GyroRange: lsm6dsox.GYRO_250DPS, GyroSampleRate: lsm6dsox.GYRO_SR_104, }) + if err != nil { + for { + println("Failed to configure", err.Error()) + time.Sleep(time.Second) + } + } for { @@ -46,8 +52,8 @@ func main() { calibrateGyro(device) } - ax, ay, az := device.ReadAcceleration() - gx, gy, gz := device.ReadRotation() + ax, ay, az, _ := device.ReadAcceleration() + gx, gy, gz, _ := device.ReadRotation() t, _ := device.ReadTemperature() if PLOTTER { @@ -64,7 +70,7 @@ func main() { func calibrateGyro(device *lsm6dsox.Device) { for i := 0; i < 100; i++ { - gx, gy, gz := device.ReadRotation() + gx, gy, gz, _ := device.ReadRotation() cal[0] += float32(gx) / 1000000 cal[1] += float32(gy) / 1000000 cal[2] += float32(gz) / 1000000 diff --git a/examples/lsm9ds1/main.go b/examples/lsm9ds1/main.go index 8028ef174..6c0aef471 100644 --- a/examples/lsm9ds1/main.go +++ b/examples/lsm9ds1/main.go @@ -41,7 +41,7 @@ func main() { for { - if con, err := device.Connected(); !con || err != nil { + if !device.Connected() { println("LSM9DS1 not connected") time.Sleep(time.Second) continue diff --git a/lsm303agr/lsm303agr.go b/lsm303agr/lsm303agr.go index 665a7e997..2eb4cfb07 100644 --- a/lsm303agr/lsm303agr.go +++ b/lsm303agr/lsm303agr.go @@ -6,6 +6,7 @@ package lsm303agr // import "tinygo.org/x/drivers/lsm303agr" import ( + "errors" "math" "tinygo.org/x/drivers" @@ -22,6 +23,7 @@ type Device struct { MagPowerMode uint8 MagSystemMode uint8 MagDataRate uint8 + buf [6]uint8 } // Configuration for LSM303AGR device. @@ -34,12 +36,17 @@ type Configuration struct { MagDataRate uint8 } -// New creates a new LSM303AGR connection. The I2C bus must already be -// configured. +var errNotConnected = errors.New("lsm303agr: failed to communicate with either acel or magnet sensor") + +// 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 drivers.I2C) Device { - return Device{bus: bus, AccelAddress: ACCEL_ADDRESS, MagAddress: MAG_ADDRESS} +func New(bus drivers.I2C) *Device { + return &Device{ + bus: bus, + AccelAddress: ACCEL_ADDRESS, + MagAddress: MAG_ADDRESS, + } } // Connected returns whether both sensor on LSM303AGR has been found. @@ -52,7 +59,12 @@ func (d *Device) Connected() bool { } // Configure sets up the LSM303AGR 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.AccelDataRate != 0 { d.AccelDataRate = cfg.AccelDataRate @@ -90,36 +102,46 @@ func (d *Device) Configure(cfg Configuration) { d.MagSystemMode = MAG_SYSTEM_CONTINUOUS } - cmd := []byte{0} + data := d.buf[:1] - cmd[0] = byte(d.AccelDataRate<<4 | d.AccelPowerMode | 0x07) - d.bus.WriteRegister(uint8(d.AccelAddress), ACCEL_CTRL_REG1_A, cmd) + data[0] = byte(d.AccelDataRate<<4 | d.AccelPowerMode | 0x07) + err = d.bus.WriteRegister(uint8(d.AccelAddress), ACCEL_CTRL_REG1_A, data) + if err != nil { + return + } - cmd[0] = byte(0x80 | d.AccelRange<<4) - d.bus.WriteRegister(uint8(d.AccelAddress), ACCEL_CTRL_REG4_A, cmd) + data[0] = byte(0x80 | d.AccelRange<<4) + err = d.bus.WriteRegister(uint8(d.AccelAddress), ACCEL_CTRL_REG4_A, data) + if err != nil { + return + } - cmd[0] = byte(0xC0) - d.bus.WriteRegister(uint8(d.AccelAddress), TEMP_CFG_REG_A, cmd) + data[0] = byte(0xC0) + err = d.bus.WriteRegister(uint8(d.AccelAddress), TEMP_CFG_REG_A, data) + if err != nil { + return + } // 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) + data[0] = byte(0x80 | d.MagPowerMode<<4 | d.MagDataRate<<2 | d.MagSystemMode) + err = d.bus.WriteRegister(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 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) +func (d *Device) ReadAcceleration() (x, y, z int32, err error) { + data := d.buf[:6] + err = d.bus.ReadRegister(uint8(d.AccelAddress), ACCEL_OUT_X_L_A, data) + if err != nil { + return + } rangeFactor := int16(0) switch d.AccelRange { @@ -133,18 +155,21 @@ func (d *Device) ReadAcceleration() (x int32, y int32, z int32) { 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) + 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 int32, roll int32) { +func (d *Device) ReadPitchRoll() (pitch, roll int32, err error) { - x, y, z := d.ReadAcceleration() + 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) @@ -154,25 +179,23 @@ func (d *Device) ReadPitchRoll() (pitch int32, roll int32) { // 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) { +func (d *Device) ReadMagneticField() (x, y, z int32, err error) { if d.MagSystemMode == MAG_SYSTEM_SINGLE { - cmd := []byte{0} + cmd := d.buf[:1] cmd[0] = byte(0x80 | d.MagPowerMode<<4 | d.MagDataRate<<2 | d.MagSystemMode) - d.bus.WriteRegister(uint8(d.MagAddress), MAG_MR_REG_M, cmd) + err = d.bus.WriteRegister(uint8(d.MagAddress), MAG_MR_REG_M, cmd) + if err != nil { + return + } } - 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) + data := d.buf[0:6] + d.bus.ReadRegister(uint8(d.MagAddress), MAG_OUT_X_L_M, data) - 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])))) + 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 } @@ -182,23 +205,27 @@ func (d *Device) ReadMagneticField() (x int32, y int32, z int32) { // // 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) { +func (d *Device) ReadCompass() (h int32, err error) { - x, y, _ := d.ReadMagneticField() + 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() (c int32, e error) { +func (d *Device) ReadTemperature() (t int32, err 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) + data := d.buf[:2] + err = d.bus.ReadRegister(uint8(d.AccelAddress), OUT_TEMP_L_A, data) + if err != nil { + return + } - 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 + r := int16((uint16(data[1])<<8 | uint16(data[0]))) >> 4 // temperature offset from 25 °C + t = 25000 + int32((float32(r)/8)*1000) return } diff --git a/lsm6ds3/lsm6ds3.go b/lsm6ds3/lsm6ds3.go index 79d5f4ba4..7db9ee510 100644 --- a/lsm6ds3/lsm6ds3.go +++ b/lsm6ds3/lsm6ds3.go @@ -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 } diff --git a/lsm6dsox/lsm6dsox.go b/lsm6dsox/lsm6dsox.go index 050728f9a..42c5ac1c7 100644 --- a/lsm6dsox/lsm6dsox.go +++ b/lsm6dsox/lsm6dsox.go @@ -5,7 +5,11 @@ // package lsm6dsox // import "tinygo.org/x/drivers/lsm6dsox" -import "tinygo.org/x/drivers" +import ( + "errors" + + "tinygo.org/x/drivers" +) type AccelRange uint8 type AccelSampleRate uint8 @@ -17,10 +21,9 @@ type GyroSampleRate uint8 type Device struct { bus drivers.I2C Address uint16 - dataBufferSix []uint8 - dataBufferTwo []uint8 accelMultiplier int32 gyroMultiplier int32 + buf [6]uint8 } // Configuration for LSM6DSOX device. @@ -31,20 +34,25 @@ type Configuration struct { GyroSampleRate GyroSampleRate } +var errNotConnected = errors.New("lsm6dsox: failed to communicate with acel/gyro sensor") + // New creates a new LSM6DSOX 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, - dataBufferSix: make([]uint8, 6), - dataBufferTwo: make([]uint8, 2), + 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 + } // Multipliers come from "Table 2. Mechanical characteristics" of the datasheet * 1000 switch cfg.AccelRange { @@ -68,19 +76,27 @@ func (d *Device) Configure(cfg Configuration) { d.gyroMultiplier = 70000 } - data := make([]uint8, 1) + data := d.buf[:1] // Configure accelerometer data[0] = uint8(cfg.AccelRange) | uint8(cfg.AccelSampleRate) - d.bus.WriteRegister(uint8(d.Address), CTRL1_XL, data) + err = d.bus.WriteRegister(uint8(d.Address), CTRL1_XL, data) + if err != nil { + return + } // Configure gyroscope data[0] = uint8(cfg.GyroRange) | uint8(cfg.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 LSM6DSOX 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] == 0x6C } @@ -89,11 +105,15 @@ 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_A, d.dataBufferSix) - x = int32(int16((uint16(d.dataBufferSix[1])<<8)|uint16(d.dataBufferSix[0]))) * d.accelMultiplier - y = int32(int16((uint16(d.dataBufferSix[3])<<8)|uint16(d.dataBufferSix[2]))) * d.accelMultiplier - z = int32(int16((uint16(d.dataBufferSix[5])<<8)|uint16(d.dataBufferSix[4]))) * d.accelMultiplier +func (d *Device) ReadAcceleration() (x, y, z int32, err error) { + data := d.buf[:6] + err = d.bus.ReadRegister(uint8(d.Address), OUTX_L_A, data) + if err != nil { + return + } + x = int32(int16((uint16(data[1])<<8)|uint16(data[0]))) * d.accelMultiplier + y = int32(int16((uint16(data[3])<<8)|uint16(data[2]))) * d.accelMultiplier + z = int32(int16((uint16(data[5])<<8)|uint16(data[4]))) * d.accelMultiplier return } @@ -101,20 +121,27 @@ 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) - x = int32(int16((uint16(d.dataBufferSix[1])<<8)|uint16(d.dataBufferSix[0]))) * d.gyroMultiplier - y = int32(int16((uint16(d.dataBufferSix[3])<<8)|uint16(d.dataBufferSix[2]))) * d.gyroMultiplier - z = int32(int16((uint16(d.dataBufferSix[5])<<8)|uint16(d.dataBufferSix[4]))) * d.gyroMultiplier +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 + } + x = int32(int16((uint16(data[1])<<8)|uint16(data[0]))) * d.gyroMultiplier + y = int32(int16((uint16(data[3])<<8)|uint16(data[2]))) * d.gyroMultiplier + z = int32(int16((uint16(data[5])<<8)|uint16(data[4]))) * d.gyroMultiplier 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 4. Temperature sensor characteristics" // temp = value/256 + 25 - t := 25000 + (int32(int16((int16(d.dataBufferTwo[1])<<8)|int16(d.dataBufferTwo[0])))*125)/32 - return t, nil + t = 25000 + (int32(int16((int16(data[1])<<8)|int16(data[0])))*125)/32 + return } diff --git a/lsm9ds1/lsm9ds1.go b/lsm9ds1/lsm9ds1.go index 25f5ed5d3..bc56baca1 100644 --- a/lsm9ds1/lsm9ds1.go +++ b/lsm9ds1/lsm9ds1.go @@ -28,8 +28,7 @@ type Device struct { accelMultiplier int32 gyroMultiplier int32 magMultiplier int32 - dataBufferSix []uint8 - dataBufferTwo []uint8 + buf [6]uint8 } // Configuration for LSM9DS1 device. @@ -50,11 +49,9 @@ var errNotConnected = errors.New("lsm9ds1: failed to communicate with either ace // 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, - dataBufferSix: make([]uint8, 6), - dataBufferTwo: make([]uint8, 2), + bus: bus, + AccelAddress: ACCEL_ADDRESS, + MagAddress: MAG_ADDRESS, } } @@ -63,17 +60,11 @@ func New(bus drivers.I2C) *Device { // In a rare case of an I2C bus issue, it can also return an error. // Case of boolean false and error nil means I2C is up, // but "who am I" responses have unexpected values. -func (d *Device) Connected() (connected bool, err error) { - data1, data2 := []byte{0}, []byte{0} - err = d.bus.ReadRegister(d.AccelAddress, WHO_AM_I, data1) - if err != nil { - return false, err - } - err = d.bus.ReadRegister(d.MagAddress, WHO_AM_I_M, data2) - if err != nil { - return false, err - } - return data1[0] == 0x68 && data2[0] == 0x3D, nil +func (d *Device) Connected() bool { + data1, data2 := d.buf[:1], d.buf[1:2] + d.bus.ReadRegister(d.AccelAddress, WHO_AM_I, data1) + d.bus.ReadRegister(d.MagAddress, WHO_AM_I_M, data2) + return data1[0] == 0x68 && data2[0] == 0x3D } // ReadAcceleration reads the current acceleration from the device and returns @@ -81,13 +72,14 @@ func (d *Device) Connected() (connected bool, err error) { // 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.bus.ReadRegister(uint8(d.AccelAddress), OUT_X_L_XL, d.dataBufferSix) + data := d.buf[:6] + err = d.bus.ReadRegister(uint8(d.AccelAddress), OUT_X_L_XL, data) if err != nil { return } - x = int32(int16((uint16(d.dataBufferSix[1])<<8)|uint16(d.dataBufferSix[0]))) * d.accelMultiplier - y = int32(int16((uint16(d.dataBufferSix[3])<<8)|uint16(d.dataBufferSix[2]))) * d.accelMultiplier - z = int32(int16((uint16(d.dataBufferSix[5])<<8)|uint16(d.dataBufferSix[4]))) * d.accelMultiplier + x = int32(int16((uint16(data[1])<<8)|uint16(data[0]))) * d.accelMultiplier + y = int32(int16((uint16(data[3])<<8)|uint16(data[2]))) * d.accelMultiplier + z = int32(int16((uint16(data[5])<<8)|uint16(data[4]))) * d.accelMultiplier return } @@ -96,38 +88,41 @@ 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.bus.ReadRegister(uint8(d.AccelAddress), OUT_X_L_G, d.dataBufferSix) + data := d.buf[:6] + err = d.bus.ReadRegister(uint8(d.AccelAddress), OUT_X_L_G, data) if err != nil { return } - x = int32(int16((uint16(d.dataBufferSix[1])<<8)|uint16(d.dataBufferSix[0]))) * d.gyroMultiplier - y = int32(int16((uint16(d.dataBufferSix[3])<<8)|uint16(d.dataBufferSix[2]))) * d.gyroMultiplier - z = int32(int16((uint16(d.dataBufferSix[5])<<8)|uint16(d.dataBufferSix[4]))) * d.gyroMultiplier + x = int32(int16((uint16(data[1])<<8)|uint16(data[0]))) * d.gyroMultiplier + y = int32(int16((uint16(data[3])<<8)|uint16(data[2]))) * d.gyroMultiplier + z = int32(int16((uint16(data[5])<<8)|uint16(data[4]))) * d.gyroMultiplier return } // ReadMagneticField reads the current magnetic field from the device and returns // it in nT (nanotesla). 1 G (gauss) = 100_000 nT (nanotesla). func (d *Device) ReadMagneticField() (x, y, z int32, err error) { - err = d.bus.ReadRegister(uint8(d.MagAddress), OUT_X_L_M, d.dataBufferSix) + data := d.buf[:6] + err = d.bus.ReadRegister(uint8(d.MagAddress), OUT_X_L_M, data) if err != nil { return } - x = int32(int16((int16(d.dataBufferSix[1])<<8)|int16(d.dataBufferSix[0]))) * d.magMultiplier - y = int32(int16((int16(d.dataBufferSix[3])<<8)|int16(d.dataBufferSix[2]))) * d.magMultiplier - z = int32(int16((int16(d.dataBufferSix[5])<<8)|int16(d.dataBufferSix[4]))) * d.magMultiplier + x = int32(int16((int16(data[1])<<8)|int16(data[0]))) * d.magMultiplier + y = int32(int16((int16(data[3])<<8)|int16(data[2]))) * d.magMultiplier + z = int32(int16((int16(data[5])<<8)|int16(data[4]))) * d.magMultiplier return } // ReadTemperature returns the temperature in Celsius milli degrees (°C/1000) func (d *Device) ReadTemperature() (t int32, err error) { - err = d.bus.ReadRegister(uint8(d.AccelAddress), OUT_TEMP_L, d.dataBufferTwo) + data := d.buf[:2] + err = d.bus.ReadRegister(uint8(d.AccelAddress), 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 + t = 25000 + (int32(int16((int16(data[1])<<8)|int16(data[0])))*125)/2 return } @@ -138,7 +133,7 @@ func (d *Device) ReadTemperature() (t int32, err error) { func (d *Device) doConfigure(cfg Configuration) (err error) { // Verify unit communication - if con, err := d.Connected(); !con || err != nil { + if !d.Connected() { return errNotConnected } @@ -172,7 +167,7 @@ func (d *Device) doConfigure(cfg Configuration) (err error) { d.magMultiplier = 58 } - data := make([]byte, 1) + data := d.buf[:1] // Configure accelerometer // Sample rate & measurement range diff --git a/lsm9ds1/lsm9ds1_nano_33_ble.go b/lsm9ds1/lsm9ds1_nano_33_ble.go index 3c57f100e..3b8be2d47 100644 --- a/lsm9ds1/lsm9ds1_nano_33_ble.go +++ b/lsm9ds1/lsm9ds1_nano_33_ble.go @@ -19,7 +19,7 @@ func (d *Device) Configure(cfg Configuration) error { machine.I2C_PULLUP.Configure(machine.PinConfig{Mode: machine.PinOutput}) machine.I2C_PULLUP.High() // Wait a moment - time.Sleep(10 * time.Millisecond) + time.Sleep(100 * time.Millisecond) // Common initialisation code return d.doConfigure(cfg) }