mirror of
https://github.com/tinygo-org/drivers.git
synced 2026-07-26 10:38:41 +00:00
lsmXXX: unified, error handling, memory management
This commit is contained in:
Yurii Soldak
Ron Evans
@@ -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()
|
||||
|
||||
@@ -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")
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
+77
-50
@@ -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
|
||||
}
|
||||
|
||||
+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
|
||||
}
|
||||
|
||||
+54
-27
@@ -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
|
||||
}
|
||||
|
||||
+29
-34
@@ -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
|
||||
|
||||
@@ -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)
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user