gobot.io/x/gobot/v2@v2.1.0/drivers/i2c/l3gd20h_driver.go (about)

     1  package i2c
     2  
     3  import (
     4  	"bytes"
     5  	"encoding/binary"
     6  	"log"
     7  )
     8  
     9  const (
    10  	l3gd20hDebug          = false
    11  	l3gd20hDefaultAddress = 0x6B
    12  )
    13  
    14  const (
    15  	l3gd20hReg_Ctl1    = 0x20 // output data rate selection, bandwidth selection, power mode, axis X/Y/Z enable
    16  	l3gd20hReg_Ctl4    = 0x23 // block data update, big/little-endian, full scale, level sensitive latch, self test, serial interface mode
    17  	l3gd20hReg_OutXLSB = 0x28 // X-axis angular rate data, LSB
    18  
    19  	l3gd20hCtl1_NormalModeBit = 0x08
    20  	l3gd20hCtl1_EnableZBit    = 0x04
    21  	l3gd20hCtl1_EnableYBit    = 0x02
    22  	l3gd20hCtl1_EnableXBit    = 0x01
    23  
    24  	l3gd20hCtl4_FullScaleRangeBits = 0x30
    25  )
    26  
    27  // L3GD20HScale is for configurable full scale range.
    28  type L3GD20HScale byte
    29  
    30  const (
    31  	// L3GD20HScale250dps is the +/-250 degrees-per-second full scale range (+/-245 from datasheet, but can hold around +/-286).
    32  	L3GD20HScale250dps L3GD20HScale = 0x00
    33  	// L3GD20HScale500dps is the +/-500 degrees-per-second full scale range.
    34  	L3GD20HScale500dps L3GD20HScale = 0x10
    35  	// L3GD20HScale2001dps is the +/-2000 degrees-per-second full scale range by using 0x20 setting.
    36  	L3GD20HScale2001dps L3GD20HScale = 0x20
    37  	// L3GD20HScale2000dps is the +/-2000 degrees-per-second full scale range.
    38  	L3GD20HScale2000dps L3GD20HScale = 0x30
    39  )
    40  
    41  // l3gdhSensibility in °/s, see the mechanical characteristics in the datasheet
    42  var l3gdhSensibility = map[L3GD20HScale]float32{
    43  	L3GD20HScale250dps:  0.00875,
    44  	L3GD20HScale500dps:  0.0175,
    45  	L3GD20HScale2001dps: 0.07,
    46  	L3GD20HScale2000dps: 0.07,
    47  }
    48  
    49  // L3GD20HDriver is the gobot driver for the Adafruit Triple-Axis Gyroscope L3GD20H.
    50  // Device datasheet: http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/DATASHEET/DM00036465.pdf
    51  type L3GD20HDriver struct {
    52  	*Driver
    53  	scale L3GD20HScale
    54  }
    55  
    56  // NewL3GD20HDriver creates a new Gobot driver for the
    57  // L3GD20H I2C Triple-Axis Gyroscope.
    58  //
    59  // Params:
    60  //		c Connector - the Adaptor to use with this Driver
    61  //
    62  // Optional params:
    63  //		i2c.WithBus(int):	bus to use with this driver
    64  //		i2c.WithAddress(int):	address to use with this driver
    65  //
    66  func NewL3GD20HDriver(c Connector, options ...func(Config)) *L3GD20HDriver {
    67  	l := &L3GD20HDriver{
    68  		Driver: NewDriver(c, "L3GD20H", l3gd20hDefaultAddress, options...),
    69  		scale:  L3GD20HScale250dps,
    70  	}
    71  	l.afterStart = l.initialize
    72  
    73  	// TODO: add commands to API
    74  	return l
    75  }
    76  
    77  // WithL3GD20HFullScaleRange option sets the full scale range for the gyroscope.
    78  // Valid settings are of type "L3GD20HScale"
    79  func WithL3GD20HFullScaleRange(val L3GD20HScale) func(Config) {
    80  	return func(c Config) {
    81  		d, ok := c.(*L3GD20HDriver)
    82  		if ok {
    83  			d.scale = val
    84  		} else if l3gd20hDebug {
    85  			log.Printf("Trying to set full scale range of gyroscope for non-L3GD20HDriver %v", c)
    86  		}
    87  	}
    88  }
    89  
    90  // SetScale sets the full scale range of the device (deprecated, use WithL3GD20HFullScaleRange() instead).
    91  func (d *L3GD20HDriver) SetScale(s L3GD20HScale) {
    92  	d.scale = s
    93  }
    94  
    95  // Scale returns the full scale range (deprecated, use FullScaleRange() instead).
    96  func (d *L3GD20HDriver) Scale() L3GD20HScale {
    97  	return d.scale
    98  }
    99  
   100  // FullScaleRange returns the full scale range of the device.
   101  func (d *L3GD20HDriver) FullScaleRange() (uint8, error) {
   102  	d.mutex.Lock()
   103  	defer d.mutex.Unlock()
   104  
   105  	val, err := d.connection.ReadByteData(l3gd20hReg_Ctl4)
   106  	if err != nil {
   107  		return 0, err
   108  	}
   109  	return val & l3gd20hCtl4_FullScaleRangeBits, nil
   110  }
   111  
   112  // XYZ returns the current change in degrees per second, for the 3 axis.
   113  func (d *L3GD20HDriver) XYZ() (x float32, y float32, z float32, err error) {
   114  	d.mutex.Lock()
   115  	defer d.mutex.Unlock()
   116  
   117  	measurements := make([]byte, 6)
   118  	reg := l3gd20hReg_OutXLSB | 0x80 // set auto-increment bit
   119  	if err = d.connection.ReadBlockData(uint8(reg), measurements); err != nil {
   120  		return 0, 0, 0, err
   121  	}
   122  
   123  	var rawX int16
   124  	var rawY int16
   125  	var rawZ int16
   126  	buf := bytes.NewBuffer(measurements)
   127  	binary.Read(buf, binary.LittleEndian, &rawX)
   128  	binary.Read(buf, binary.LittleEndian, &rawY)
   129  	binary.Read(buf, binary.LittleEndian, &rawZ)
   130  
   131  	sensitivity := l3gdhSensibility[d.scale]
   132  
   133  	return float32(rawX) * sensitivity, float32(rawY) * sensitivity, float32(rawZ) * sensitivity, nil
   134  }
   135  
   136  func (d *L3GD20HDriver) initialize() (err error) {
   137  	// reset the gyroscope.
   138  	if err := d.connection.WriteByteData(l3gd20hReg_Ctl1, 0x00); err != nil {
   139  		return err
   140  	}
   141  	// Enable Z, Y and X axis.
   142  	ctl1 := l3gd20hCtl1_NormalModeBit | l3gd20hCtl1_EnableZBit | l3gd20hCtl1_EnableYBit | l3gd20hCtl1_EnableXBit
   143  	if err := d.connection.WriteByteData(l3gd20hReg_Ctl1, uint8(ctl1)); err != nil {
   144  		return err
   145  	}
   146  	// Set the sensitivity scale.
   147  	if err := d.connection.WriteByteData(l3gd20hReg_Ctl4, byte(d.scale)); err != nil {
   148  		return err
   149  	}
   150  	return nil
   151  }