/**
* 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)
}
}
}
}
/**
* Summarize the sensor data and send it off to be processed
**/
func sendSensorData(sensorChannel chan SensorData, when int64, gyroscope *sensors.L3GD20, accelerometer *sensors.LSM303ACCEL, mx, my, mz float32, count int) {
var (
err error
data SensorData
)
data.When = when
data.Count = count
data.Mx = mx
data.My = my
data.Mz = mz
data.Gx, data.Gy, data.Gz, err = gyroscope.Evaluate()
if err == nil {
data.Ax, data.Ay, data.Az, err = accelerometer.Evaluate()
if err == nil {
sensorChannel <- data // send the data over the sensorChannel
}
}
}
/**
* 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
}
/**
* This test program reads data from the three sensors
* gyroscope, accelerometer, and magnetometer, runs it
* through the filter, and prints the computed
* Yaw, Pitch, and Roll every 100 iterations.
* Each iteration takes approximately 3 milliseconds
* on a RaspberryPi model B.
**/
func main() {
const (
d2r = math.Pi / 180.0 // Used to convert degrees to radians
r2d = 180.0 / math.Pi // Used to convert radians to degrees
)
var (
gyroscope *sensors.L3GD20 = nil
accelerometer *sensors.LSM303ACCEL = nil
magnetometer *sensors.LSM303MAG = nil
imu *imus.ImuMayhony = nil
err error
dbgPrint bool
gx, gy, gz float32 // Gyroscope rate in degrees
ax, ay, az float32 // Accelerometer data
mx, my, mz float32 // Magnetometer data
yaw, pitch, roll float32 // IMU results in radians
)
gyroscope, err = sensors.NewL3GD20()
if err != nil {
fmt.Printf("Error: getting device L3GD20, err=%v\n", err)
return
}
accelerometer, err = sensors.NewLSM303ACCEL()
if err != nil {
fmt.Printf("Error: getting device LSM303ACCEL, err=%v\n", err)
return
}
magnetometer, err = sensors.NewLSM303MAG()
if err != nil {
fmt.Printf("Error: getting device LSM303MAG, err=%v\n", err)
return
}
imu = imus.NewImuMayhony()
maxIterations := 10000
for {
startTime := time.Now().UnixNano()
for i := 0; i < maxIterations; i++ {
dbgPrint = (i % 100) == 0
now := time.Now().UnixNano()
gx, gy, gz, err = gyroscope.ReadXYZ()
if err == nil {
ax, ay, az, err = accelerometer.ReadXYZ()
if err == nil {
mx, my, mz, err = magnetometer.ReadXYZ()
if err == nil {
yaw, pitch, roll = imu.Update(now, gx*d2r, gy*d2r, gz*d2r, ax, ay, az, mx, my, mz)
if dbgPrint {
fmt.Printf(" YPR(%10.5f, %10.5f, %10.5f)\n", yaw*r2d, pitch*r2d, roll*r2d)
}
} else {
fmt.Printf("Error: reading magnetometer, err=%v\n", err)
}
} else {
fmt.Printf("Error: reading accelerometer, err=%v\n", err)
}
} else {
fmt.Printf("Error: reading gyroscope, err=%v\n", err)
}
}
finishTime := time.Now().UnixNano()
delta := finishTime - startTime
fmt.Printf("%4.2f milliseconds per iteration\n", (float64(delta)/float64(maxIterations))/1000000.0)
}
}