tinygo-org/drivers
1
0
Fork 0
mirror of https://github.com/tinygo-org/drivers.git synced 2026-07-26 10:38:41 +00:00
Code Issues Packages Projects Releases 10 Wiki Activity

add l3gd20 gyro driver

Browse Source
This commit is contained in:
soypat
2022-03-24 11:10:45 -03:00
committed by Ron Evans
parent 5e54f08605
commit edfdd4e09e
4 changed files with 328 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
examples/l3gd20/main.go
+39
View File
@@ -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)
}
}
l3gd20/i2c.go
+141
View File
@@ -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])
}
l3gd20/l3gd20.go
+49
View File
@@ -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
}
l3gd20/registers.go
+99
View File
@@ -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
)