diff --git a/sensor/hmc5883l/hmc5883l.go b/sensor/hmc5883l/hmc5883l.go new file mode 100644 index 0000000..58fedc3 --- /dev/null +++ b/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) +}