diff --git a/examples/lsm9ds1/main.go b/examples/lsm9ds1/main.go new file mode 100644 index 0000000..8028ef1 --- /dev/null +++ b/examples/lsm9ds1/main.go @@ -0,0 +1,103 @@ +// LSM9DS1, 9 axis Inertial Measurement Unit (IMU) +package main + +import ( + "fmt" + "machine" + "time" + + "tinygo.org/x/drivers/lsm9ds1" +) + +const ( + PLOTTER = false + SHOW_ACCELERATION = true + SHOW_ROTATION = true + SHOW_MAGNETIC_FIELD = true + SHOW_TEMPERATURE = true +) + +func main() { + + // I2C configure + machine.I2C0.Configure(machine.I2CConfig{}) + + // LSM9DS1 setup + device := lsm9ds1.New(machine.I2C0) + err := device.Configure(lsm9ds1.Configuration{ + AccelRange: lsm9ds1.ACCEL_2G, + AccelSampleRate: lsm9ds1.ACCEL_SR_119, + GyroRange: lsm9ds1.GYRO_250DPS, + GyroSampleRate: lsm9ds1.GYRO_SR_119, + MagRange: lsm9ds1.MAG_4G, + MagSampleRate: lsm9ds1.MAG_SR_40, + }) + if err != nil { + for { + println("Failed to configure", err.Error()) + time.Sleep(time.Second) + } + } + + for { + + if con, err := device.Connected(); !con || err != nil { + println("LSM9DS1 not connected") + time.Sleep(time.Second) + continue + } + + ax, ay, az, _ := device.ReadAcceleration() + gx, gy, gz, _ := device.ReadRotation() + mx, my, mz, _ := device.ReadMagneticField() + t, _ := device.ReadTemperature() + + if PLOTTER { + printPlotter(ax, ay, az, gx, gy, gz, mx, my, mz, t) + time.Sleep(time.Millisecond * 100) + } else { + printMonitor(ax, ay, az, gx, gy, gz, mx, my, mz, t) + time.Sleep(time.Millisecond * 1000) + } + + } + +} + +// Arduino IDE's Serial Plotter +func printPlotter(ax, ay, az, gx, gy, gz, mx, my, mz, t int32) { + if SHOW_ACCELERATION { + fmt.Printf("AX:%f, AY:%f, AZ:%f,", axis(ax), axis(ay), axis(az)) + } + if SHOW_ROTATION { + fmt.Printf("GX:%f, GY:%f, GZ:%f,", axis(gx), axis(gy), axis(gz)) + } + if SHOW_MAGNETIC_FIELD { + fmt.Printf("MX:%d, MY:%d, MZ:%d,", mx, my, mz) + } + if SHOW_TEMPERATURE { + fmt.Printf("T:%f", float32(t)/1000) + } + println() +} + +// Any Serial Monitor +func printMonitor(ax, ay, az, gx, gy, gz, mx, my, mz, t int32) { + if SHOW_ACCELERATION { + fmt.Printf("Acceleration (g): %f, %f, %f\r\n", axis(ax), axis(ay), axis(az)) + } + if SHOW_ROTATION { + fmt.Printf("Rotation (dps): %f, %f, %f\r\n", axis(gx), axis(gy), axis(gz)) + } + if SHOW_MAGNETIC_FIELD { + fmt.Printf("Magnetic field (nT): %d, %d, %d\r\n", mx, my, mz) + } + if SHOW_TEMPERATURE { + fmt.Printf("Temperature C: %f\r\n", float32(t)/1000) + } + println() +} + +func axis(raw int32) float32 { + return float32(raw) / 1000000 +} diff --git a/lsm9ds1/lsm9ds1.go b/lsm9ds1/lsm9ds1.go new file mode 100644 index 0000000..25f5ed5 --- /dev/null +++ b/lsm9ds1/lsm9ds1.go @@ -0,0 +1,227 @@ +// LSM9DS1, 9 axis Inertial Measurement Unit (IMU) +// +// Datasheet: https://www.st.com/resource/en/datasheet/lsm6ds3.pdf +// +package lsm9ds1 // import "tinygo.org/x/drivers/lsm9ds1" + +import ( + "errors" + + "tinygo.org/x/drivers" +) + +type AccelRange uint8 +type AccelSampleRate uint8 +type AccelBandwidth uint8 + +type GyroRange uint8 +type GyroSampleRate uint8 + +type MagRange uint8 +type MagSampleRate uint8 + +// Device wraps connection to a LSM9DS1 device. +type Device struct { + bus drivers.I2C + AccelAddress uint8 + MagAddress uint8 + accelMultiplier int32 + gyroMultiplier int32 + magMultiplier int32 + dataBufferSix []uint8 + dataBufferTwo []uint8 +} + +// Configuration for LSM9DS1 device. +type Configuration struct { + AccelRange AccelRange + AccelSampleRate AccelSampleRate + AccelBandWidth AccelBandwidth + GyroRange GyroRange + GyroSampleRate GyroSampleRate + MagRange MagRange + MagSampleRate MagSampleRate +} + +var errNotConnected = errors.New("lsm9ds1: failed to communicate with either acel/gyro or magnet sensor") + +// New creates a new LSM9DS1 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, + dataBufferSix: make([]uint8, 6), + dataBufferTwo: make([]uint8, 2), + } +} + +// Connected returns whether both sensor on LSM9DS1 has been found. +// It does two "who am I" requests and checks the responses. +// 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 +} + +// ReadAcceleration reads the current acceleration from the device and returns +// it in µg (micro-gravity). When one of the axes is pointing straight to Earth +// and the sensor is not moving the returned value will be around 1000000 or +// -1000000. +func (d *Device) ReadAcceleration() (x, y, z int32, err error) { + err = d.bus.ReadRegister(uint8(d.AccelAddress), OUT_X_L_XL, d.dataBufferSix) + 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 + return +} + +// ReadRotation reads the current rotation from the device and returns it in +// µ°/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, y, z int32, err error) { + err = d.bus.ReadRegister(uint8(d.AccelAddress), OUT_X_L_G, d.dataBufferSix) + 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 + 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) + 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 + 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) + 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 +} + +// --- end of public methods -------------------------------------------------- + +// doConfigure is called by public Configure methods after all +// necessary board-specific initialisations are taken care of +func (d *Device) doConfigure(cfg Configuration) (err error) { + + // Verify unit communication + if con, err := d.Connected(); !con || err != nil { + return errNotConnected + } + + // Multipliers come from "Table 3. Sensor characteristics" of the datasheet * 1000 + switch cfg.AccelRange { + case ACCEL_2G: + d.accelMultiplier = 61 + case ACCEL_4G: + d.accelMultiplier = 122 + case ACCEL_8G: + d.accelMultiplier = 244 + case ACCEL_16G: + d.accelMultiplier = 732 + } + switch cfg.GyroRange { + case GYRO_250DPS: + d.gyroMultiplier = 8750 + case GYRO_500DPS: + d.gyroMultiplier = 17500 + case GYRO_2000DPS: + d.gyroMultiplier = 70000 + } + switch cfg.MagRange { + case MAG_4G: + d.magMultiplier = 14 + case MAG_8G: + d.magMultiplier = 29 + case MAG_12G: + d.magMultiplier = 43 + case MAG_16G: + d.magMultiplier = 58 + } + + data := make([]byte, 1) + + // Configure accelerometer + // Sample rate & measurement range + data[0] = uint8(cfg.AccelSampleRate)<<5 | uint8(cfg.AccelRange)<<3 + err = d.bus.WriteRegister(d.AccelAddress, CTRL_REG6_XL, data) + if err != nil { + return + } + + // Configure gyroscope + // Sample rate & measurement range + data[0] = uint8(cfg.GyroSampleRate)<<5 | uint8(cfg.GyroRange)<<3 + err = d.bus.WriteRegister(d.AccelAddress, CTRL_REG1_G, data) + if err != nil { + return + } + + // Configure magnetometer + + // Temperature compensation enabled + // High-performance mode XY axis + // Sample rate + data[0] = 0b10000000 | 0b01000000 | uint8(cfg.MagSampleRate)<<2 + err = d.bus.WriteRegister(d.MagAddress, CTRL_REG1_M, data) + if err != nil { + return + } + + // Measurement range + data[0] = uint8(cfg.MagRange) << 5 + err = d.bus.WriteRegister(d.MagAddress, CTRL_REG2_M, data) + if err != nil { + return + } + + // Continuous-conversion mode + // https://electronics.stackexchange.com/questions/237397/continuous-conversion-vs-single-conversion-mode + data[0] = 0b00000000 + err = d.bus.WriteRegister(d.MagAddress, CTRL_REG3_M, data) + if err != nil { + return + } + + // High-performance mode Z axis + data[0] = 0b00001000 + err = d.bus.WriteRegister(d.MagAddress, CTRL_REG4_M, data) + if err != nil { + return + } + + return nil +} diff --git a/lsm9ds1/lsm9ds1_generic.go b/lsm9ds1/lsm9ds1_generic.go new file mode 100644 index 0000000..cc276f3 --- /dev/null +++ b/lsm9ds1/lsm9ds1_generic.go @@ -0,0 +1,9 @@ +//go:build !nano_33_ble +// +build !nano_33_ble + +package lsm9ds1 + +// Configure sets up the device for communication. +func (d *Device) Configure(cfg Configuration) error { + return d.doConfigure(cfg) +} diff --git a/lsm9ds1/lsm9ds1_nano_33_ble.go b/lsm9ds1/lsm9ds1_nano_33_ble.go new file mode 100644 index 0000000..3c57f10 --- /dev/null +++ b/lsm9ds1/lsm9ds1_nano_33_ble.go @@ -0,0 +1,25 @@ +//go:build nano_33_ble +// +build nano_33_ble + +// Nano 33 BLE [Sense] has LSM9DS1 unit on-board. +// This custom Configure function powers unit up +// and enables I2C, so unit can can be accessed. +package lsm9ds1 + +import ( + "machine" + "time" +) + +// Configure sets up the device for communication. +func (d *Device) Configure(cfg Configuration) error { + // Following lines are Nano 33 BLE specific, they have nothing to do with sensor per se + machine.LSM_PWR.Configure(machine.PinConfig{Mode: machine.PinOutput}) + machine.LSM_PWR.High() + machine.I2C_PULLUP.Configure(machine.PinConfig{Mode: machine.PinOutput}) + machine.I2C_PULLUP.High() + // Wait a moment + time.Sleep(10 * time.Millisecond) + // Common initialisation code + return d.doConfigure(cfg) +} diff --git a/lsm9ds1/registers.go b/lsm9ds1/registers.go new file mode 100644 index 0000000..6d9861b --- /dev/null +++ b/lsm9ds1/registers.go @@ -0,0 +1,102 @@ +package lsm9ds1 + +// Constants/addresses used for I2C. + +const ( + + // Constants/addresses used for I2C. + ACCEL_ADDRESS = 0x6B + MAG_ADDRESS = 0x1E + + // Table 21. Accelerometer and gyroscope register address map + WHO_AM_I = 0x0F // value 0x68 + CTRL_REG1_G = 0x10 + OUT_X_L_G = 0x18 + OUT_X_H_G = 0x19 + OUT_Y_L_G = 0x1A + OUT_Y_H_G = 0x1B + OUT_Z_L_G = 0x1C + OUT_Z_H_G = 0x1D + OUT_TEMP_L = 0x15 + OUT_TEMP_H = 0x16 + CTRL_REG6_XL = 0x20 + STATUS_REG = 0x27 + OUT_X_L_XL = 0x28 + OUT_X_H_XL = 0x29 + OUT_Y_L_XL = 0x2A + OUT_Y_H_XL = 0x2B + OUT_Z_L_XL = 0x2C + OUT_Z_H_XL = 0x2D + + // Table 22. Magnetic sensor register address map + OFFSET_X_REG_L_M = 0x05 + OFFSET_X_REG_H_M = 0x06 + OFFSET_Y_REG_L_M = 0x07 + OFFSET_Y_REG_H_M = 0x08 + OFFSET_Z_REG_L_M = 0x09 + OFFSET_Z_REG_H_M = 0x0A + WHO_AM_I_M = 0x0F // value 0x3D + CTRL_REG1_M = 0x20 // TEMP_COMP OM1 OM0 DO2 DO1 DO0 FAST_ODR ST + CTRL_REG2_M = 0x21 // 0 FS1 FS0 0 REBOOT SOFT_RST 0 0 + CTRL_REG3_M = 0x22 // 0 LP 0 0 SIM MD1 MD0 + CTRL_REG4_M = 0x23 // 0 0 0 0 OMZ1 OMZ0 BLE 0 + STATUS_REG_M = 0x27 + OUT_X_L_M = 0x28 + OUT_X_H_M = 0x29 + OUT_Y_L_M = 0x2A + OUT_Y_H_M = 0x2B + OUT_Z_L_M = 0x2C + OUT_Z_H_M = 0x2D + + // Table 67. CTRL_REG6_XL register description + ACCEL_2G AccelRange = 0b00 + ACCEL_4G AccelRange = 0b10 + ACCEL_8G AccelRange = 0b11 + ACCEL_16G AccelRange = 0b01 + + // Table 68. ODR register setting (accelerometer only mode) + ACCEL_SR_OFF AccelSampleRate = 0b000 + ACCEL_SR_10 AccelSampleRate = 0b001 + ACCEL_SR_50 AccelSampleRate = 0b010 + ACCEL_SR_119 AccelSampleRate = 0b011 + ACCEL_SR_238 AccelSampleRate = 0b100 + ACCEL_SR_476 AccelSampleRate = 0b101 + ACCEL_SR_952 AccelSampleRate = 0b110 + + // Table 67. CTRL_REG6_XL register description + ACCEL_BW_50 AccelBandwidth = 0b11 + ACCEL_BW_105 AccelBandwidth = 0b10 + ACCEL_BW_211 AccelBandwidth = 0b01 + ACCEL_BW_408 AccelBandwidth = 0b00 + + // Table 45. CTRL_REG1_G register description + GYRO_250DPS GyroRange = 0b00 + GYRO_500DPS GyroRange = 0b01 + GYRO_2000DPS GyroRange = 0b11 + + // Table 9. Gyroscope operating modes + // Table 46. ODR and BW configuration setting (after LPF1) + GYRO_SR_OFF GyroSampleRate = 0b000 + GYRO_SR_15 GyroSampleRate = 0b001 + GYRO_SR_60 GyroSampleRate = 0b010 + GYRO_SR_119 GyroSampleRate = 0b011 + GYRO_SR_238 GyroSampleRate = 0b100 + GYRO_SR_476 GyroSampleRate = 0b101 + GYRO_SR_952 GyroSampleRate = 0b110 + + // Table 114. Full-scale selection + MAG_4G MagRange = 0b00 + MAG_8G MagRange = 0b01 + MAG_12G MagRange = 0b10 + MAG_16G MagRange = 0b11 + + // Table 111. Output data rate configuration + MAG_SR_06 MagSampleRate = 0b000 + MAG_SR_1 MagSampleRate = 0b001 + MAG_SR_2 MagSampleRate = 0b010 + MAG_SR_5 MagSampleRate = 0b011 + MAG_SR_10 MagSampleRate = 0b100 + MAG_SR_20 MagSampleRate = 0b101 + MAG_SR_40 MagSampleRate = 0b110 + MAG_SR_80 MagSampleRate = 0b111 +)