Skip to content

Commit edfdd4e

Browse files
soypatdeadprogram
soypat
authored and
deadprogram
committed
add l3gd20 gyro driver
1 parent 5e54f08 commit edfdd4e

File tree

4 files changed

+328
-0
lines changed

4 files changed

+328
-0
lines changed

‎examples/l3gd20/main.go‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
package main
2+
3+
import (
4+
"machine"
5+
"time"
6+
7+
"tinygo.org/x/drivers/l3gd20"
8+
)
9+
10+
func main() {
11+
const (
12+
// Default address on most breakout boards.
13+
pcaAddr = 0x40
14+
)
15+
16+
bus := machine.I2C0
17+
18+
err := bus.Configure(machine.I2CConfig{})
19+
if err != nil {
20+
panic(err.Error())
21+
}
22+
23+
gyro := l3gd20.NewI2C(bus, 105)
24+
err = gyro.Configure(l3gd20.Config{Range: l3gd20.Range_250})
25+
if err != nil {
26+
println(err.Error())
27+
}
28+
29+
var x, y, z int32
30+
for {
31+
err = gyro.Update()
32+
if err != nil {
33+
println(err.Error())
34+
}
35+
x, y, z = gyro.AngularVelocity()
36+
println(x, y, z)
37+
time.Sleep(500 * time.Millisecond)
38+
}
39+
}

‎l3gd20/i2c.go‎

Lines changed: 141 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,141 @@
1+
package l3gd20
2+
3+
import (
4+
"encoding/binary"
5+
"time"
6+
7+
"tinygo.org/x/drivers"
8+
)
9+
10+
const (
11+
fifoLen = 32 * 3 * 2
12+
)
13+
14+
type DevI2C struct {
15+
addr uint8
16+
// sensitivity or range.
17+
mul int32
18+
bus drivers.I2C
19+
buf [1]byte
20+
// gyro databuf.
21+
databuf [6]byte
22+
data [3]int32
23+
}
24+
25+
func NewI2C(bus drivers.I2C, addr uint8) *DevI2C {
26+
return &DevI2C{
27+
addr: addr,
28+
bus: bus,
29+
mul: sensMul250,
30+
}
31+
}
32+
33+
// Initializes and configures the device.
34+
func (d *DevI2C) Configure(cfg Config) error {
35+
err := cfg.validate()
36+
if err != nil {
37+
return err
38+
}
39+
err = d.Reboot()
40+
if err != nil {
41+
return err
42+
}
43+
// Reset then switch to normal mode and enable all three channels.
44+
err = d.write8(CTRL_REG1, 0)
45+
if err != nil {
46+
return err
47+
}
48+
err = d.write8(CTRL_REG1, reg1NormalBits)
49+
if err != nil {
50+
return err
51+
}
52+
// Reset REG2 values to default
53+
err = d.write8(CTRL_REG3, 0)
54+
if err != nil {
55+
return err
56+
}
57+
58+
// Set sensitivity
59+
switch cfg.Range {
60+
case 1: // debugging range
61+
d.mul = 1
62+
cfg.Range = Range_2000
63+
case Range_250:
64+
d.mul = sensMul250
65+
case Range_500:
66+
d.mul = sensMul500
67+
case Range_2000:
68+
d.mul = sensMul2000
69+
default:
70+
return ErrBadRange
71+
}
72+
err = d.write8(CTRL_REG4, cfg.Range)
73+
if err != nil {
74+
return err
75+
}
76+
// Finally verify whomai register and return error if
77+
// board is not who it says it is. Some counterfeit boards
78+
// have incorrect whomai but can still be used.
79+
whoami, err := d.read8(WHOAMI)
80+
if err != nil {
81+
return err
82+
}
83+
if whoami != expectedWHOAMI && whoami != expectedWHOAMI_H {
84+
return ErrBadIdentity
85+
}
86+
return nil
87+
}
88+
89+
func (d *DevI2C) Update() error {
90+
err := d.bus.ReadRegister(d.addr, OUT_X_L, d.databuf[:2])
91+
if err != nil {
92+
return err
93+
}
94+
err = d.bus.ReadRegister(d.addr, OUT_Y_L, d.databuf[2:4])
95+
if err != nil {
96+
return err
97+
}
98+
err = d.bus.ReadRegister(d.addr, OUT_Z_L, d.databuf[4:6])
99+
if err != nil {
100+
return err
101+
}
102+
x := int16(binary.LittleEndian.Uint16(d.databuf[0:]))
103+
y := int16(binary.LittleEndian.Uint16(d.databuf[2:]))
104+
z := int16(binary.LittleEndian.Uint16(d.databuf[4:]))
105+
d.data[0] = d.mul * int32(x)
106+
d.data[1] = d.mul * int32(y)
107+
d.data[2] = d.mul * int32(z)
108+
return nil
109+
}
110+
111+
// Reboot sets reboot bit in CTRL_REG5 to true and unsets it.
112+
func (d *DevI2C) Reboot() error {
113+
reg5, err := d.read8(CTRL_REG5)
114+
if err != nil {
115+
return err
116+
}
117+
// Write reboot bit and then unset it.
118+
err = d.write8(CTRL_REG5, reg5|reg5RebootBit)
119+
if err != nil {
120+
return err
121+
}
122+
time.Sleep(50 * time.Microsecond)
123+
return d.write8(CTRL_REG5, reg5&^reg5RebootBit)
124+
}
125+
126+
// AngularVelocity returns result in microradians per second.
127+
func (d *DevI2C) AngularVelocity() (x, y, z int32) {
128+
return d.data[0], d.data[1], d.data[2]
129+
}
130+
131+
// func (d DevI2C) Update(measurement)
132+
133+
func (d DevI2C) read8(reg uint8) (byte, error) {
134+
err := d.bus.ReadRegister(d.addr, reg, d.buf[:1])
135+
return d.buf[0], err
136+
}
137+
138+
func (d DevI2C) write8(reg uint8, val byte) error {
139+
d.buf[0] = val
140+
return d.bus.WriteRegister(d.addr, reg, d.buf[:1])
141+
}

‎l3gd20/l3gd20.go‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
package l3gd20
2+
3+
import "errors"
4+
5+
var (
6+
ErrBadIdentity = errors.New("got unexpected identity from WHOMAI")
7+
ErrBadRange = errors.New("bad range configuration value")
8+
)
9+
10+
// Sensitivity factors
11+
const (
12+
// range at bits 4-5
13+
// 00 = 250 dps
14+
// 01 = 500 dps
15+
// 10 = 2000 dps
16+
// 11 = 2000 dps
17+
rangePos = 4
18+
Range_250 = 0b00 << rangePos // 8.75 mdps/digit
19+
Range_500 = 0b01 << rangePos // 17.5 mdps/digit
20+
Range_2000 = 0b11 << rangePos // 70 mdps/digit
21+
rangebits = Range_250 | Range_500 | Range_2000
22+
23+
// Sensitivities for degrees
24+
sensDiv250dps = 800
25+
sensDiv500dps = 400
26+
sensDiv2000dps = 100
27+
sens_250 = 7. / sensDiv250dps // Sensitivity at 250 dps
28+
sens_500 = 7. / sensDiv500dps // Sensitivity at 500 dps
29+
sens_2000 = 7. / sensDiv2000dps // Sensitivity at 500 dp
30+
31+
// 1e6*Pi/180. = 17453.292519943298 (constant for Degree to micro radians conversion)
32+
// sensitivities for radians
33+
sensMul250 = 7 * 1745329 / 100 / sensDiv250dps
34+
sensMul500 = 7 * 1745329 / 100 / sensDiv500dps
35+
sensMul2000 = 7 * 1745329 / 100 / sensDiv2000dps
36+
)
37+
38+
type Config struct {
39+
Range uint8
40+
}
41+
42+
// validate scans config for invalid data and returns non-nil
43+
// error indicating what data must be modified.
44+
func (cfg *Config) validate() error {
45+
if cfg.Range&^rangebits != 0 && cfg.Range != 1 {
46+
return ErrBadRange
47+
}
48+
return nil
49+
}

‎l3gd20/registers.go‎

Lines changed: 99 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,99 @@
1+
package l3gd20
2+
3+
// Expected identification number for L3GD20.
4+
const (
5+
// For L3GD20
6+
expectedWHOAMI = 0xD4
7+
// For L3GD20H
8+
expectedWHOAMI_H = 0xD7
9+
)
10+
11+
// Register bits masks
12+
const (
13+
reg5RebootBit = 1 << 7
14+
reg5FIFOEnableBit = 1 << 6
15+
reg1NormalBits = 0b1111
16+
)
17+
18+
// Register addresses. Comments from https://github.com/adafruit/Adafruit_L3GD20_U/blob/master/Adafruit_L3GD20_U.cpp
19+
const (
20+
// The Slave ADdress (SAD) associated with the L3GD20 is 110101xb
21+
I2CAddr = 0b1101011
22+
// The SDO pin can be used to modify the less significant bit of the device address.
23+
I2CAddrSDOLow = 0b1101010
24+
WHOAMI uint8 = 0x0F
25+
// CTRL_REG1 (0x20)
26+
// ====================================================================
27+
// BIT Symbol Description Default
28+
// --- ------ --------------------------------------------- -------
29+
// 7-6 DR1/0 Output data rate 00
30+
// 5-4 BW1/0 Bandwidth selection 00
31+
// 3 PD 0 = Power-down mode, 1 = normal/sleep mode 0
32+
// 2 ZEN Z-axis enable (0 = disabled, 1 = enabled) 1
33+
// 1 YEN Y-axis enable (0 = disabled, 1 = enabled) 1
34+
// 0 XEN X-axis enable (0 = disabled, 1 = enabled) 1
35+
CTRL_REG1 uint8 = 0x20
36+
// Set CTRL_REG2 (0x21)
37+
// ====================================================================
38+
// BIT Symbol Description Default
39+
// --- ------ --------------------------------------------- -------
40+
// 5-4 HPM1/0 High-pass filter mode selection 00
41+
// 3-0 HPCF3..0 High-pass filter cutoff frequency selection 0000
42+
CTRL_REG2 uint8 = 0x21
43+
// CTRL_REG3 (0x22)
44+
// ====================================================================
45+
// BIT Symbol Description Default
46+
// --- ------ --------------------------------------------- -------
47+
// 7 I1_Int1 Interrupt enable on INT1 (0=disable,1=enable) 0
48+
// 6 I1_Boot Boot status on INT1 (0=disable,1=enable) 0
49+
// 5 H-Lactive Interrupt active config on INT1 (0=high,1=low) 0
50+
// 4 PP_OD Push-Pull/Open-Drain (0=PP, 1=OD) 0
51+
// 3 I2_DRDY Data ready on DRDY/INT2 (0=disable,1=enable) 0
52+
// 2 I2_WTM FIFO wtrmrk int on DRDY/INT2 (0=dsbl,1=enbl) 0
53+
// 1 I2_ORun FIFO overrun int on DRDY/INT2 (0=dsbl,1=enbl) 0
54+
// 0 I2_Empty FIFI empty int on DRDY/INT2 (0=dsbl,1=enbl) 0
55+
CTRL_REG3 uint8 = 0x22
56+
// CTRL_REG4 (0x23)
57+
// ====================================================================
58+
// BIT Symbol Description Default
59+
// --- ------ --------------------------------------------- -------
60+
// 7 BDU Block Data Update (0=continuous, 1=LSB/MSB) 0
61+
// 6 BLE Big/Little-Endian (0=Data LSB, 1=Data MSB) 0
62+
// 5-4 FS1/0 Full scale selection 00
63+
// 00 = 250 dps
64+
// 01 = 500 dps
65+
// 10 = 2000 dps
66+
// 11 = 2000 dps
67+
// 0 SIM SPI Mode (0=4-wire, 1=3-wire) 0
68+
CTRL_REG4 uint8 = 0x23
69+
// CTRL_REG5 (0x24)
70+
// ====================================================================
71+
// BIT Symbol Description Default
72+
// --- ------ --------------------------------------------- -------
73+
// 7 BOOT Reboot memory content (0=normal, 1=reboot) 0
74+
// 6 FIFO_EN FIFO enable (0=FIFO disable, 1=enable) 0
75+
// 4 HPen High-pass filter enable (0=disable,1=enable) 0
76+
// 3-2 INT1_SEL INT1 Selection config 00
77+
// 1-0 OUT_SEL Out selection config 00
78+
CTRL_REG5 uint8 = 0x24
79+
REFERENCE uint8 = 0x25
80+
OUT_TEMP uint8 = 0x26
81+
STATUS_REG uint8 = 0x27
82+
OUT_X_L uint8 = 0x28
83+
OUT_X_H uint8 = 0x29
84+
OUT_Y_L uint8 = 0x2A
85+
OUT_Y_H uint8 = 0x2B
86+
OUT_Z_L uint8 = 0x2C
87+
OUT_Z_H uint8 = 0x2D
88+
FIFO_CTRL_REG uint8 = 0x2E
89+
FIFO_SRC_REG uint8 = 0x2F
90+
INT1_CFG uint8 = 0x30
91+
INT1_SRC uint8 = 0x31
92+
INT1_TSH_XH uint8 = 0x32
93+
INT1_TSH_XL uint8 = 0x33
94+
INT1_TSH_YH uint8 = 0x34
95+
INT1_TSH_YL uint8 = 0x35
96+
INT1_TSH_ZH uint8 = 0x36
97+
INT1_TSH_ZL uint8 = 0x37
98+
INT1_DURATION uint8 = 0x38
99+
)

0 commit comments

Comments
 (0)