gobot.io/x/gobot@v1.16.0/drivers/i2c/hmc8553l_driver.go (about)

     1  package i2c
     2  
     3  import (
     4  	"math"
     5  
     6  	"gobot.io/x/gobot"
     7  )
     8  
     9  const (
    10  	defaultAddress = 0x1e // default I2C Address
    11  	registerA      = 0x0  // Address of Configuration register A
    12  	registerB      = 0x01 // Address of Configuration register B
    13  	registerMode   = 0x02 // Address of node register
    14  	xAxisH         = 0x03 // Address of X-axis MSB data register
    15  	zAxisH         = 0x05 // Address of Z-axis MSB data register
    16  	yAxisH         = 0x07 // Address of Y-axis MSB data register
    17  )
    18  
    19  // HMC8553LDriver is a Driver for a HMC6352 digital compass
    20  type HMC8553LDriver struct {
    21  	name       string
    22  	connector  Connector
    23  	connection Connection
    24  	Config
    25  }
    26  
    27  // NewHMC8553LDriver creates a new driver with specified i2c interface
    28  // Params:
    29  //		conn Connector - the Adaptor to use with this Driver
    30  //
    31  // Optional params:
    32  //		i2c.WithBus(int):	bus to use with this driver
    33  //		i2c.WithAddress(int):	address to use with this driver
    34  //
    35  func NewHMC8553LDriver(a Connector, options ...func(Config)) *HMC8553LDriver {
    36  	hmc := &HMC8553LDriver{
    37  		name:      gobot.DefaultName("HMC8553L"),
    38  		connector: a,
    39  		Config:    NewConfig(),
    40  	}
    41  
    42  	for _, option := range options {
    43  		option(hmc)
    44  	}
    45  
    46  	return hmc
    47  }
    48  
    49  // Name returns the name for this Driver
    50  func (h *HMC8553LDriver) Name() string { return h.name }
    51  
    52  // SetName sets the name for this Driver
    53  func (h *HMC8553LDriver) SetName(n string) { h.name = n }
    54  
    55  // Connection returns the connection for this Driver
    56  func (h *HMC8553LDriver) Connection() gobot.Connection { return h.connector.(gobot.Connection) }
    57  
    58  // Start initializes the HMC8553L
    59  func (h *HMC8553LDriver) Start() (err error) {
    60  	bus := h.GetBusOrDefault(h.connector.GetDefaultBus())
    61  	address := h.GetAddressOrDefault(defaultAddress)
    62  	h.connection, err = h.connector.GetConnection(address, bus)
    63  	if err != nil {
    64  		return err
    65  	}
    66  	if err := h.connection.WriteByteData(registerA, 0x70); err != nil {
    67  		return err
    68  	}
    69  	if err := h.connection.WriteByteData(registerB, 0xa0); err != nil {
    70  		return err
    71  	}
    72  	if err := h.connection.WriteByteData(registerMode, 0); err != nil {
    73  		return err
    74  	}
    75  	return
    76  }
    77  
    78  // Halt returns true if devices is halted successfully
    79  func (h *HMC8553LDriver) Halt() (err error) { return }
    80  
    81  // ReadRawData reads the raw values from the X, Y, and Z registers
    82  func (h *HMC8553LDriver) ReadRawData() (x int16, y int16, z int16, err error) {
    83  	unsignedX, err := h.connection.ReadWordData(xAxisH)
    84  	if err != nil {
    85  		return
    86  	}
    87  	unsignedY, err := h.connection.ReadWordData(yAxisH)
    88  	if err != nil {
    89  		return
    90  	}
    91  	unsignedZ, err := h.connection.ReadWordData(zAxisH)
    92  	if err != nil {
    93  		return
    94  	}
    95  	return unsignedToSigned(unsignedX), unsignedToSigned(unsignedY), unsignedToSigned(unsignedZ), nil
    96  }
    97  
    98  // Heading returns the current heading in radians
    99  func (h *HMC8553LDriver) Heading() (heading float64, err error) {
   100  	var x, y int16
   101  	x, y, _, err = h.ReadRawData()
   102  	if err != nil {
   103  		return
   104  	}
   105  	heading = math.Atan2(float64(y), float64(x))
   106  	if heading > 2*math.Pi {
   107  		heading -= 2 * math.Pi
   108  	}
   109  	if heading < 0 {
   110  		heading += 2 * math.Pi
   111  	}
   112  	return
   113  }
   114  
   115  func unsignedToSigned(unsignedValue uint16) int16 {
   116  	if unsignedValue > 32768 {
   117  		return int16(unsignedValue) - ^int16(0) - ^int16(0) - 2
   118  	}
   119  	return int16(unsignedValue)
   120  }