Ejemplo n.º 1
0
/**
* 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)
			}
		}
	}
}
Ejemplo n.º 2
0
/**
* 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
}