1
0
mirror of https://github.com/kidoman/embd synced 2024-12-22 21:00:05 +01:00
embd/sensor/lsm303/lsm303.go

208 lines
4.3 KiB
Go
Raw Normal View History

2013-12-07 23:11:06 +05:30
// Package lsm303 allows interfacing with the LSM303 magnetometer.
package lsm303
import (
"log"
"math"
"sync"
"time"
"github.com/kid0m4n/go-rpi/i2c"
)
const (
magAddress = 0x1E
magConfigRegA = 0x00
2013-12-09 02:49:39 +05:30
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
2013-12-07 23:11:06 +05:30
magModeReg = 0x02
2013-12-09 02:49:39 +05:30
MagContinuous = 0x00 // Continuous conversion mode
2013-12-09 02:51:15 +05:30
MagSingle = 0x01 // Single conversion mode
2013-12-09 02:49:39 +05:30
MagSleep = 0x03 // Sleep mode
2013-12-07 23:11:06 +05:30
2013-12-09 02:49:39 +05:30
MagMRDefault = MagContinuous // Continuous conversion is the default
2013-12-07 23:11:06 +05:30
magDataSignal = 0x02
magData = 0x03
pollDelay = 250
)
2013-12-09 02:49:39 +05:30
// A LSM303 implements access to a LSM303 sensor.
2013-12-07 23:11:06 +05:30
type LSM303 interface {
2013-12-09 02:49:39 +05:30
// SetPollDelay sets the delay between runs of the data acquisition loop.
2013-12-07 23:11:06 +05:30
SetPollDelay(delay int)
2013-12-09 02:49:39 +05:30
// Heading returns the current heading [0, 360).
2013-12-07 23:11:06 +05:30
Heading() (heading float64, err error)
2013-12-09 02:49:39 +05:30
// Run starts the sensor data acquisition loop.
2013-12-07 23:11:06 +05:30
Run() error
// Close closes the sensor data acquisition loop and puts the LSM303 into sleep mode.
2013-12-07 23:11:06 +05:30
Close() error
}
type lsm303 struct {
bus i2c.Bus
initialized bool
mu *sync.RWMutex
headings chan float64
poll int
quit chan struct{}
debug bool
}
2013-12-09 02:49:39 +05:30
// Default instance of the LSM303 sensor.
2013-12-07 23:11:06 +05:30
var Default = New(i2c.Default)
// New creates a new LSM303 interface. The bus variable controls
// the I2C bus used to communicate with the device.
func New(bus i2c.Bus) LSM303 {
return &lsm303{bus: bus, mu: new(sync.RWMutex), poll: pollDelay}
}
// Initialize the device
func (d *lsm303) setup() (err error) {
d.mu.RLock()
if d.initialized {
d.mu.RUnlock()
return
}
d.mu.RUnlock()
d.mu.Lock()
defer d.mu.Unlock()
if err = d.bus.WriteByteToReg(magAddress, magConfigRegA, MagCRADefault); err != nil {
2013-12-07 23:11:06 +05:30
return
}
if err = d.bus.WriteByteToReg(magAddress, magModeReg, MagMRDefault); err != nil {
2013-12-07 23:11:06 +05:30
return
}
d.initialized = true
return
}
// SetPollDelay sets the delay between runs of the data acquisition loop.
func (d *lsm303) SetPollDelay(delay int) {
d.poll = delay
}
func (d *lsm303) measureHeading() (heading float64, err error) {
if err = d.setup(); err != nil {
return
}
if _, err = d.bus.ReadByteFromReg(magAddress, magDataSignal); err != nil {
return
}
data := make([]byte, 6)
if err = d.bus.ReadFromReg(magAddress, magData, data); err != nil {
return
}
x := int16(data[0])<<8 | int16(data[1])
y := int16(data[2])<<8 | int16(data[3])
heading = math.Atan2(float64(y), float64(x)) / math.Pi * 180
if heading < 0 {
heading += 360
}
return
}
2013-12-09 02:49:39 +05:30
// Heading returns the current heading [0, 360).
2013-12-07 23:11:06 +05:30
func (d *lsm303) Heading() (heading float64, err error) {
select {
case heading = <-d.headings:
return
default:
if d.debug {
log.Print("lsm303: no headings available... measuring")
}
return d.measureHeading()
}
}
2013-12-09 02:49:39 +05:30
// Run starts the sensor data acquisition loop.
2013-12-07 23:11:06 +05:30
func (d *lsm303) Run() (err 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
}
// Close the sensor data acquisition loop and put the LSM303 into sleep mode.
func (d *lsm303) Close() (err error) {
if d.quit != nil {
d.quit <- struct{}{}
}
err = d.bus.WriteByteToReg(magAddress, magModeReg, MagSleep)
2013-12-07 23:11:06 +05:30
return
}
2013-12-09 02:49:39 +05:30
// SetPollDelay sets the delay between runs of the data acquisition loop.
func SetPollDelay(delay int) {
Default.SetPollDelay(delay)
}
// Heading returns the current heading [0, 360).
2013-12-07 23:11:06 +05:30
func Heading() (heading float64, err error) {
return Default.Heading()
}
2013-12-09 02:49:39 +05:30
// Run starts the sensor data acquisition loop.
2013-12-07 23:11:06 +05:30
func Run() (err error) {
return Default.Run()
}
2013-12-09 02:49:39 +05:30
// Close closes the sensor data acquisition loop and put the LSM303 into sleep mode.
2013-12-07 23:11:06 +05:30
func Close() (err error) {
return Default.Close()
}