Merge pull request #824 from westphae/bmx160

Add support for bmx160 MPU

main/sensors.go

@@ -25,6 +25,8 @@ const (
 	MPUREG_WHO_AM_I_VAL = 0x71 // Expected value.
 	ICMREG_WHO_AM_I     = 0x00
 	ICMREG_WHO_AM_I_VAL = 0xEA // Expected value.
+	BMXREG_WHO_AM_I     = 0x00
+	BMXREG_WHO_AM_I_VAL = 0xD8 // Expected value.
 )
 
 var (
@@ -140,6 +142,11 @@ func initIMU() (ok bool) {
 		log.Printf("Error identifying IMU: %s\n", err.Error())
 		return false
 	}
+	v3, err := i2cbus.ReadByteFromReg(0x68, BMXREG_WHO_AM_I)
+	if err != nil {
+		log.Printf("Error identifying IMU: %s\n", err.Error())
+		return false
+	}
 
 	if v == ICMREG_WHO_AM_I_VAL {
 		log.Println("ICM-20948 detected.")
@@ -155,8 +162,15 @@ func initIMU() (ok bool) {
 			myIMUReader = imu
 			return true
 		}
+	} else if v3 == BMXREG_WHO_AM_I_VAL {
+		log.Println("BMX-160 detected.")
+		imu, err := sensors.NewBMX160(&i2cbus)
+		if err == nil {
+			myIMUReader = imu
+			return true
+		}
 	} else {
-		log.Printf("Could not identify MPU. v=%02x, v2=%02x.\n", v, v2)
+		log.Printf("Could not identify MPU. v=%02x, v2=%02x, v3=%02x.\n", v, v2, v3)
 		return false
 	}
 

sensors/bmx160.go

@@ -0,0 +1,97 @@
+// Package sensors provides a stratux interface to sensors used for AHRS calculations.
+package sensors
+
+import (
+	"../goflying/bmx160"
+	"github.com/kidoman/embd"
+)
+
+const (
+	bmx160gyroRange  = 125 // gyroRange is the default range to use for the Gyro.
+	bmx160accelRange = 4   // accelRange is the default range to use for the Accel.
+	bmx160updateFreq = 200  // updateFreq is the rate at which to update the sensor values.
+)
+
+// BMX160 is a Bosch BMX160 attached to the I2C bus and satisfies
+// the IMUReader interface.
+type BMX160 struct {
+	mpu *bmx160.BMX160
+}
+
+// NewBMX160 returns an instance of the BMX160 IMUReader, connected to an
+// BMX160 attached on the I2C bus with either valid address.
+func NewBMX160(i2cbus *embd.I2CBus) (*BMX160, error) {
+	var (
+		m   BMX160
+		mpu *bmx160.BMX160
+		err error
+	)
+
+	mpu, err = bmx160.NewBMX160(i2cbus, bmx160gyroRange, bmx160accelRange, bmx160updateFreq, false, false)
+	if err != nil {
+		return nil, err
+	}
+
+	// Set Gyro (Accel) LPFs to 20 (21) Hz to filter out prop/glareshield vibrations above 1200 (1260) RPM
+        // This is done in the constructor of BMX160
+
+	m.mpu = mpu
+	return &m, nil
+}
+
+// Read returns the average (since last reading) time, Gyro X-Y-Z, Accel X-Y-Z, Mag X-Y-Z,
+// error reading Gyro/Accel, and error reading Mag.
+func (m *BMX160) Read() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64, GAError, MAGError error) {
+	var (
+		data *bmx160.MPUData
+		i    int8
+	)
+	data = new(bmx160.MPUData)
+
+	for data.N == 0 && i < 5 {
+		data = <-m.mpu.CAvg
+		T = data.T.UnixNano()
+		G1 = data.G1
+		G2 = data.G2
+		G3 = data.G3
+		A1 = data.A1
+		A2 = data.A2
+		A3 = data.A3
+		M1 = data.M1
+		M2 = data.M2
+		M3 = data.M3
+		GAError = data.GAError
+		MAGError = data.MagError
+		i++
+	}
+	return
+}
+
+// ReadOne returns the most recent time, Gyro X-Y-Z, Accel X-Y-Z, Mag X-Y-Z,
+// error reading Gyro/Accel, and error reading Mag.
+func (m *BMX160) ReadOne() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64, GAError, MAGError error) {
+	var (
+		data *bmx160.MPUData
+	)
+	data = new(bmx160.MPUData)
+
+	data = <-m.mpu.C
+	T = data.T.UnixNano()
+	G1 = data.G1
+	G2 = data.G2
+	G3 = data.G3
+	A1 = data.A1
+	A2 = data.A2
+	A3 = data.A3
+	M1 = data.M1
+	M2 = data.M2
+	M3 = data.M3
+	GAError = data.GAError
+	MAGError = data.MagError
+	return
+}
+
+// Close stops reading the MPU.
+func (m *BMX160) Close() {
+	m.mpu.CloseMPU()
+}