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
/**
* 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
		}
	}
}
Ejemplo n.º 3
0
/**
* Read the raw gyro values and display every 100th one
**/
func main() {
	var (
		err        error
		errCnt     int
		pause      time.Duration
		i          int
		x, y, z    int16
		xx, yy, zz int
		px, py, pz int
		gyroscope  *sensors.L3GD20 = nil
	)
	gyroscope, err = sensors.NewL3GD20()
	if err != nil {
		fmt.Printf("Error: getting device L3GD20, err=%v\n", err)
	} else {
		pause = time.Millisecond * 100
		for {
			x, y, z, err = gyroscope.ReadRaw()
			if err != nil {
				errCnt++
			} else {
				i++
				xx += int(x)
				yy += int(y)
				zz += int(z)
				if i == 10 {
					xx = xx / i
					yy = yy / i
					zz = zz / i
					fmt.Printf("%4d, %4d, %4d\n", xx-px, yy-py, zz-pz)
					i = 0
					px = xx
					py = yy
					pz = zz
					xx = 0
					yy = 0
					zz = 0
				}
			}
			time.Sleep(pause)
		}
	}
}
Ejemplo n.º 4
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
}
Ejemplo n.º 5
0
/**
* 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)
	}
}