Added hmc5883L magnetometer

sensor/hmc5883l/hmc5883l.go

@@ -0,0 +1,171 @@
+// Package hmc5883l allows interfacing with the HMC5883L magnetometer.
+package hmc5883l
+
+import (
+	"math"
+	"sync"
+	"time"
+
+	"github.com/golang/glog"
+	"github.com/kidoman/embd"
+)
+
+const (
+	magAddress = 0x1E
+
+	// Register addresses.
+	magConfigRegA = 0x00
+	magConfigRegB = 0x01
+	magModeReg    = 0x02
+	magMSBx       = 0x03
+	magLSBx       = 0x04
+	magMSBz       = 0x05
+	magLSBz       = 0x06
+	magMSBy       = 0x07
+	magLSBy       = 0x08
+	magStatusReg  = 0x09
+
+	// ConfigA Params.
+	MagHz75         = 0x00 // ODR = 0.75 Hz
+	Mag1Hz5         = 0x04 // ODR = 1.5 Hz
+	Mag3Hz          = 0x08 // ODR = 3 Hz
+	Mag7Hz5         = 0x0C // ODR = 7.5 Hz
+	Mag15Hz         = 0x10 // ODR = 15 Hz
+	Mag30Hz         = 0x14 // ODR = 30 Hz
+	Mag75Hz         = 0x18 // ODR = 75 Hz
+	MagNormal       = 0x00 // Normal mode
+	MagPositiveBias = 0x01 // Positive bias mode
+	MagNegativeBias = 0x02 // Negative bias mode
+
+	MagCRADefault = Mag15Hz | MagNormal // 15 Hz and normal mode is the default
+
+	// ConfigB Params.
+	MagCRBDefault = 0x20 // Gain 1090
+
+	// Mode Reg Params.
+	MagContinuous = 0x00 // Continuous conversion mode
+	MagSingle     = 0x01 // Single conversion mode
+	MagSleep      = 0x03 // Sleep mode
+
+	MagMRDefault = MagContinuous // Continuous conversion is the default
+
+	pollDelay = 250 // Delay before reading from mag. (ms)
+)
+
+// HMC5883L represents a HMC5883L magnetometer.
+type HMC5883L struct {
+	Bus  embd.I2CBus
+	Poll int
+
+	initialized bool
+	mu          sync.RWMutex
+
+	headings chan float64
+
+	quit chan struct{}
+}
+
+// New creates a new HMC5883L interface. The bus variable controls
+// the I2C bus used to communicate with the device.
+func New(bus embd.I2CBus) *HMC5883L {
+	return &HMC5883L{Bus: bus, Poll: pollDelay}
+}
+
+// Initialize the device
+func (d *HMC5883L) setup() error {
+	d.mu.RLock()
+	if d.initialized {
+		d.mu.RUnlock()
+		return nil
+	}
+	d.mu.RUnlock()
+
+	d.mu.Lock()
+	defer d.mu.Unlock()
+
+	if err := d.Bus.WriteByteToReg(magAddress, magConfigRegA, MagCRADefault); err != nil {
+		return err
+	}
+	if err := d.Bus.WriteByteToReg(magAddress, magConfigRegB, MagCRBDefault); err != nil {
+		return err
+	}
+	if err := d.Bus.WriteByteToReg(magAddress, magModeReg, MagMRDefault); err != nil {
+		return err
+	}
+
+	d.initialized = true
+
+	return nil
+}
+
+func (d *HMC5883L) measureHeading() (float64, error) {
+	if err := d.setup(); err != nil {
+		return 0, err
+	}
+
+	data := make([]byte, 6)
+	if err := d.Bus.ReadFromReg(magAddress, magMSBx, data); err != nil {
+		return 0, err
+	}
+
+	x := int16(data[0])<<8 | int16(data[1])
+	y := int16(data[4])<<8 | int16(data[5])
+
+	heading := math.Atan2(float64(y), float64(x)) / math.Pi * 180
+	if heading < 0 {
+		heading += 360
+	}
+
+	glog.V(3).Infof("Mag X=%v Y=%v HEAD=%v", x, y, heading)
+	return heading, nil
+}
+
+// Heading returns the current heading [0, 360).
+func (d *HMC5883L) Heading() (float64, error) {
+	select {
+	case heading := <-d.headings:
+		return heading, nil
+	default:
+		glog.V(2).Infof("lsm303: no headings available... measuring")
+		return d.measureHeading()
+	}
+}
+
+// Run starts the sensor data acquisition loop.
+func (d *HMC5883L) Run() error {
+	go func() {
+		d.quit = make(chan struct{})
+
+		timer := time.Tick(time.Duration(d.Poll) * time.Millisecond)
+
+		var heading float64
+
+		for {
+			select {
+			case <-timer:
+				h, err := d.measureHeading()
+				if err == nil {
+					heading = h
+				}
+				if err == nil && d.headings == nil {
+					d.headings = make(chan float64)
+				}
+			case d.headings <- heading:
+			case <-d.quit:
+				d.headings = nil
+				return
+			}
+
+		}
+	}()
+
+	return nil
+}
+
+// Close the sensor data acquisition loop and put the HMC5883L into sleep mode.
+func (d *HMC5883L) Close() error {
+	if d.quit != nil {
+		d.quit <- struct{}{}
+	}
+	return d.Bus.WriteByteToReg(magAddress, magModeReg, MagSleep)
+}