/**
* Loop reading sensors, attempt to summarize at 50Hz
**/
func ReadSensors(sensorChannel chan SensorData) {
var (
count int
now int64 // Current time in nanoseconds
lastTime int64 // Last 100Hz time in nanoseconds
hz int64 // Hz in nanoseconds
magCount int // Read the Magnetometer every 5th summarize
mx, my, mz float32 // Magnetometer data
err error
gyroscope *sensors.L3GD20
accelerometer *sensors.LSM303ACCEL
magnetometer *sensors.LSM303MAG
)
fmt.Printf("Allocating sensors...\n")
gyroscope, accelerometer, magnetometer, err = setup()
if err != nil {
close(sensorChannel)
return
}
fmt.Printf("Reading sensors...\n")
hz = int64(time.Second/50) - 200000 // minus overhead to send sensor data
lastTime = time.Now().UnixNano()
mx, my, mz, err = magnetometer.ReadXYZ()
if err != nil {
fmt.Printf("readSensors: failed to read magnetometer, err=%v\n", err)
}
for {
count++
gyroscope.Measure()
now = time.Now().UnixNano()
if (now - lastTime) >= hz {
sendSensorData(sensorChannel, now, gyroscope, accelerometer, mx, my, mz, count)
lastTime = now
count = 0
magCount++
}
accelerometer.Measure()
now = time.Now().UnixNano()
if (now - lastTime) >= hz {
sendSensorData(sensorChannel, now, gyroscope, accelerometer, mx, my, mz, count)
lastTime = now
count = 0
magCount++
}
if magCount == 5 {
magCount = 0
mx, my, mz, err = magnetometer.ReadXYZ()
if err != nil {
fmt.Printf("readSensors: failed to read magnetometer, err=%v\n", err)
}
}
}
}
/**
* Calibrate the sensors
**/
func calibrate(gyroscope *sensors.L3GD20, accelerometer *sensors.LSM303ACCEL) (err error) {
const (
ITERATIONS int = 100
)
var (
t time.Duration
n int
)
fmt.Printf("Calibrating...\n")
t = time.Millisecond * 100
for n = 0; n < ITERATIONS; n++ {
gyroscope.Measure()
accelerometer.Measure()
time.Sleep(t)
}
err = gyroscope.ComputeBias()
if err == nil {
err = accelerometer.ComputeBias()
}
return
}