Skip to content
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

Jump to bottom

lsmXXX (IMU units): Error handling, etc #368

Merged
merged 1 commit into from
Feb 18, 2022
Merged

lsmXXX (IMU units): Error handling, etc #368

Show file tree
Hide file tree
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
24 changes: 16 additions & 8 deletions examples/lsm303agr/main.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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()
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Are these supposed to be commented out here?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Have no idea, not my code, I've just made it work with refactorings.
I suppose, since it's an example, this shows how to read raw acceleration, instead of Pitch and Roll.

// 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()
Expand Down
19 changes: 13 additions & 6 deletions examples/lsm6ds3/main.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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")
Expand Down
14 changes: 10 additions & 4 deletions examples/lsm6dsox/main.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {

Expand All @@ -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 {
Expand All @@ -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
Expand Down
2 changes: 1 addition & 1 deletion examples/lsm9ds1/main.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
127 changes: 77 additions & 50 deletions lsm303agr/lsm303agr.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
package lsm303agr // import "tinygo.org/x/drivers/lsm303agr"

import (
"errors"
"math"

"tinygo.org/x/drivers"
Expand All @@ -22,6 +23,7 @@ type Device struct {
MagPowerMode uint8
MagSystemMode uint8
MagDataRate uint8
buf [6]uint8
}

// Configuration for LSM303AGR device.
Expand All @@ -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.
Expand All @@ -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
Expand Down Expand Up @@ -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) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Matches other drivers, so 👍 from me.

data := d.buf[:6]
err = d.bus.ReadRegister(uint8(d.AccelAddress), ACCEL_OUT_X_L_A, data)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

👍, this should be much faster.

if err != nil {
return
}

rangeFactor := int16(0)
switch d.AccelRange {
Expand All @@ -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)
Expand All @@ -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
}

Expand All @@ -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
}
Loading