diff --git a/examples/l3gd20/main.go b/examples/l3gd20/main.go new file mode 100644 index 0000000..5536c40 --- /dev/null +++ b/examples/l3gd20/main.go @@ -0,0 +1,39 @@ +package main + +import ( + "machine" + "time" + + "tinygo.org/x/drivers/l3gd20" +) + +func main() { + const ( + // Default address on most breakout boards. + pcaAddr = 0x40 + ) + + bus := machine.I2C0 + + err := bus.Configure(machine.I2CConfig{}) + if err != nil { + panic(err.Error()) + } + + gyro := l3gd20.NewI2C(bus, 105) + err = gyro.Configure(l3gd20.Config{Range: l3gd20.Range_250}) + if err != nil { + println(err.Error()) + } + + var x, y, z int32 + for { + err = gyro.Update() + if err != nil { + println(err.Error()) + } + x, y, z = gyro.AngularVelocity() + println(x, y, z) + time.Sleep(500 * time.Millisecond) + } +} diff --git a/l3gd20/i2c.go b/l3gd20/i2c.go new file mode 100644 index 0000000..0511ede --- /dev/null +++ b/l3gd20/i2c.go @@ -0,0 +1,141 @@ +package l3gd20 + +import ( + "encoding/binary" + "time" + + "tinygo.org/x/drivers" +) + +const ( + fifoLen = 32 * 3 * 2 +) + +type DevI2C struct { + addr uint8 + // sensitivity or range. + mul int32 + bus drivers.I2C + buf [1]byte + // gyro databuf. + databuf [6]byte + data [3]int32 +} + +func NewI2C(bus drivers.I2C, addr uint8) *DevI2C { + return &DevI2C{ + addr: addr, + bus: bus, + mul: sensMul250, + } +} + +// Initializes and configures the device. +func (d *DevI2C) Configure(cfg Config) error { + err := cfg.validate() + if err != nil { + return err + } + err = d.Reboot() + if err != nil { + return err + } + // Reset then switch to normal mode and enable all three channels. + err = d.write8(CTRL_REG1, 0) + if err != nil { + return err + } + err = d.write8(CTRL_REG1, reg1NormalBits) + if err != nil { + return err + } + // Reset REG2 values to default + err = d.write8(CTRL_REG3, 0) + if err != nil { + return err + } + + // Set sensitivity + switch cfg.Range { + case 1: // debugging range + d.mul = 1 + cfg.Range = Range_2000 + case Range_250: + d.mul = sensMul250 + case Range_500: + d.mul = sensMul500 + case Range_2000: + d.mul = sensMul2000 + default: + return ErrBadRange + } + err = d.write8(CTRL_REG4, cfg.Range) + if err != nil { + return err + } + // Finally verify whomai register and return error if + // board is not who it says it is. Some counterfeit boards + // have incorrect whomai but can still be used. + whoami, err := d.read8(WHOAMI) + if err != nil { + return err + } + if whoami != expectedWHOAMI && whoami != expectedWHOAMI_H { + return ErrBadIdentity + } + return nil +} + +func (d *DevI2C) Update() error { + err := d.bus.ReadRegister(d.addr, OUT_X_L, d.databuf[:2]) + if err != nil { + return err + } + err = d.bus.ReadRegister(d.addr, OUT_Y_L, d.databuf[2:4]) + if err != nil { + return err + } + err = d.bus.ReadRegister(d.addr, OUT_Z_L, d.databuf[4:6]) + if err != nil { + return err + } + x := int16(binary.LittleEndian.Uint16(d.databuf[0:])) + y := int16(binary.LittleEndian.Uint16(d.databuf[2:])) + z := int16(binary.LittleEndian.Uint16(d.databuf[4:])) + d.data[0] = d.mul * int32(x) + d.data[1] = d.mul * int32(y) + d.data[2] = d.mul * int32(z) + return nil +} + +// Reboot sets reboot bit in CTRL_REG5 to true and unsets it. +func (d *DevI2C) Reboot() error { + reg5, err := d.read8(CTRL_REG5) + if err != nil { + return err + } + // Write reboot bit and then unset it. + err = d.write8(CTRL_REG5, reg5|reg5RebootBit) + if err != nil { + return err + } + time.Sleep(50 * time.Microsecond) + return d.write8(CTRL_REG5, reg5&^reg5RebootBit) +} + +// AngularVelocity returns result in microradians per second. +func (d *DevI2C) AngularVelocity() (x, y, z int32) { + return d.data[0], d.data[1], d.data[2] +} + +// func (d DevI2C) Update(measurement) + +func (d DevI2C) read8(reg uint8) (byte, error) { + err := d.bus.ReadRegister(d.addr, reg, d.buf[:1]) + return d.buf[0], err +} + +func (d DevI2C) write8(reg uint8, val byte) error { + d.buf[0] = val + return d.bus.WriteRegister(d.addr, reg, d.buf[:1]) +} diff --git a/l3gd20/l3gd20.go b/l3gd20/l3gd20.go new file mode 100644 index 0000000..af5b852 --- /dev/null +++ b/l3gd20/l3gd20.go @@ -0,0 +1,49 @@ +package l3gd20 + +import "errors" + +var ( + ErrBadIdentity = errors.New("got unexpected identity from WHOMAI") + ErrBadRange = errors.New("bad range configuration value") +) + +// Sensitivity factors +const ( + // range at bits 4-5 + // 00 = 250 dps + // 01 = 500 dps + // 10 = 2000 dps + // 11 = 2000 dps + rangePos = 4 + Range_250 = 0b00 << rangePos // 8.75 mdps/digit + Range_500 = 0b01 << rangePos // 17.5 mdps/digit + Range_2000 = 0b11 << rangePos // 70 mdps/digit + rangebits = Range_250 | Range_500 | Range_2000 + + // Sensitivities for degrees + sensDiv250dps = 800 + sensDiv500dps = 400 + sensDiv2000dps = 100 + sens_250 = 7. / sensDiv250dps // Sensitivity at 250 dps + sens_500 = 7. / sensDiv500dps // Sensitivity at 500 dps + sens_2000 = 7. / sensDiv2000dps // Sensitivity at 500 dp + + // 1e6*Pi/180. = 17453.292519943298 (constant for Degree to micro radians conversion) + // sensitivities for radians + sensMul250 = 7 * 1745329 / 100 / sensDiv250dps + sensMul500 = 7 * 1745329 / 100 / sensDiv500dps + sensMul2000 = 7 * 1745329 / 100 / sensDiv2000dps +) + +type Config struct { + Range uint8 +} + +// validate scans config for invalid data and returns non-nil +// error indicating what data must be modified. +func (cfg *Config) validate() error { + if cfg.Range&^rangebits != 0 && cfg.Range != 1 { + return ErrBadRange + } + return nil +} diff --git a/l3gd20/registers.go b/l3gd20/registers.go new file mode 100644 index 0000000..546c095 --- /dev/null +++ b/l3gd20/registers.go @@ -0,0 +1,99 @@ +package l3gd20 + +// Expected identification number for L3GD20. +const ( + // For L3GD20 + expectedWHOAMI = 0xD4 + // For L3GD20H + expectedWHOAMI_H = 0xD7 +) + +// Register bits masks +const ( + reg5RebootBit = 1 << 7 + reg5FIFOEnableBit = 1 << 6 + reg1NormalBits = 0b1111 +) + +// Register addresses. Comments from https://github.com/adafruit/Adafruit_L3GD20_U/blob/master/Adafruit_L3GD20_U.cpp +const ( + // The Slave ADdress (SAD) associated with the L3GD20 is 110101xb + I2CAddr = 0b1101011 + // The SDO pin can be used to modify the less significant bit of the device address. + I2CAddrSDOLow = 0b1101010 + WHOAMI uint8 = 0x0F + // CTRL_REG1 (0x20) + // ==================================================================== + // BIT Symbol Description Default + // --- ------ --------------------------------------------- ------- + // 7-6 DR1/0 Output data rate 00 + // 5-4 BW1/0 Bandwidth selection 00 + // 3 PD 0 = Power-down mode, 1 = normal/sleep mode 0 + // 2 ZEN Z-axis enable (0 = disabled, 1 = enabled) 1 + // 1 YEN Y-axis enable (0 = disabled, 1 = enabled) 1 + // 0 XEN X-axis enable (0 = disabled, 1 = enabled) 1 + CTRL_REG1 uint8 = 0x20 + // Set CTRL_REG2 (0x21) + // ==================================================================== + // BIT Symbol Description Default + // --- ------ --------------------------------------------- ------- + // 5-4 HPM1/0 High-pass filter mode selection 00 + // 3-0 HPCF3..0 High-pass filter cutoff frequency selection 0000 + CTRL_REG2 uint8 = 0x21 + // CTRL_REG3 (0x22) + // ==================================================================== + // BIT Symbol Description Default + // --- ------ --------------------------------------------- ------- + // 7 I1_Int1 Interrupt enable on INT1 (0=disable,1=enable) 0 + // 6 I1_Boot Boot status on INT1 (0=disable,1=enable) 0 + // 5 H-Lactive Interrupt active config on INT1 (0=high,1=low) 0 + // 4 PP_OD Push-Pull/Open-Drain (0=PP, 1=OD) 0 + // 3 I2_DRDY Data ready on DRDY/INT2 (0=disable,1=enable) 0 + // 2 I2_WTM FIFO wtrmrk int on DRDY/INT2 (0=dsbl,1=enbl) 0 + // 1 I2_ORun FIFO overrun int on DRDY/INT2 (0=dsbl,1=enbl) 0 + // 0 I2_Empty FIFI empty int on DRDY/INT2 (0=dsbl,1=enbl) 0 + CTRL_REG3 uint8 = 0x22 + // CTRL_REG4 (0x23) + // ==================================================================== + // BIT Symbol Description Default + // --- ------ --------------------------------------------- ------- + // 7 BDU Block Data Update (0=continuous, 1=LSB/MSB) 0 + // 6 BLE Big/Little-Endian (0=Data LSB, 1=Data MSB) 0 + // 5-4 FS1/0 Full scale selection 00 + // 00 = 250 dps + // 01 = 500 dps + // 10 = 2000 dps + // 11 = 2000 dps + // 0 SIM SPI Mode (0=4-wire, 1=3-wire) 0 + CTRL_REG4 uint8 = 0x23 + // CTRL_REG5 (0x24) + // ==================================================================== + // BIT Symbol Description Default + // --- ------ --------------------------------------------- ------- + // 7 BOOT Reboot memory content (0=normal, 1=reboot) 0 + // 6 FIFO_EN FIFO enable (0=FIFO disable, 1=enable) 0 + // 4 HPen High-pass filter enable (0=disable,1=enable) 0 + // 3-2 INT1_SEL INT1 Selection config 00 + // 1-0 OUT_SEL Out selection config 00 + CTRL_REG5 uint8 = 0x24 + REFERENCE uint8 = 0x25 + OUT_TEMP uint8 = 0x26 + STATUS_REG uint8 = 0x27 + OUT_X_L uint8 = 0x28 + OUT_X_H uint8 = 0x29 + OUT_Y_L uint8 = 0x2A + OUT_Y_H uint8 = 0x2B + OUT_Z_L uint8 = 0x2C + OUT_Z_H uint8 = 0x2D + FIFO_CTRL_REG uint8 = 0x2E + FIFO_SRC_REG uint8 = 0x2F + INT1_CFG uint8 = 0x30 + INT1_SRC uint8 = 0x31 + INT1_TSH_XH uint8 = 0x32 + INT1_TSH_XL uint8 = 0x33 + INT1_TSH_YH uint8 = 0x34 + INT1_TSH_YL uint8 = 0x35 + INT1_TSH_ZH uint8 = 0x36 + INT1_TSH_ZL uint8 = 0x37 + INT1_DURATION uint8 = 0x38 +)