Merge pull request #817 from cyoung/icm20948

ICM-20948 support.

main/sensors.go

@@ -19,6 +19,12 @@ const (
 	numRetries uint8 = 5
 	calCLimit        = 0.15
 	calDLimit        = 10.0
+
+	// WHO_AM_I values to differentiate between the different IMUs.
+	MPUREG_WHO_AM_I     = 0x75
+	MPUREG_WHO_AM_I_VAL = 0x71 // Expected value.
+	ICMREG_WHO_AM_I     = 0x00
+	ICMREG_WHO_AM_I_VAL = 0xEA // Expected value.
 )
 
 var (
@@ -123,13 +129,36 @@ func tempAndPressureSender() {
 }
 
 func initIMU() (ok bool) {
-	imu, err := sensors.NewMPU9250(&i2cbus)
-	if err == nil {
-		myIMUReader = imu
-		return true
+	// Check if the chip is the ICM-20948 or MPU-9250.
+	v, err := i2cbus.ReadByteFromReg(0x68, ICMREG_WHO_AM_I)
+	if err != nil {
+		log.Printf("Error identifying IMU: %s\n", err.Error())
+		return false
+	}
+	v2, err := i2cbus.ReadByteFromReg(0x68, MPUREG_WHO_AM_I)
+	if err != nil {
+		log.Printf("Error identifying IMU: %s\n", err.Error())
+		return false
 	}
 
-	// TODO westphae: try to connect to MPU9150 or other IMUs.
+	if v == ICMREG_WHO_AM_I_VAL {
+		log.Println("ICM-20948 detected.")
+		imu, err := sensors.NewICM20948(&i2cbus)
+		if err == nil {
+			myIMUReader = imu
+			return true
+		}
+	} else if v2 == MPUREG_WHO_AM_I_VAL {
+		log.Println("MPU-9250 detected.")
+		imu, err := sensors.NewMPU9250(&i2cbus)
+		if err == nil {
+			myIMUReader = imu
+			return true
+		}
+	} else {
+		log.Printf("Could not identify MPU. v=%02x, v2=%02x.\n", v, v2)
+		return false
+	}
 
 	return false
 }

sensors/icm20948.go

@@ -0,0 +1,98 @@
+// Package sensors provides a stratux interface to sensors used for AHRS calculations.
+package sensors
+
+import (
+	"../goflying/icm20948"
+	"github.com/kidoman/embd"
+)
+
+const (
+	gyroRange  = 250 // gyroRange is the default range to use for the Gyro.
+	accelRange = 4   // accelRange is the default range to use for the Accel.
+	updateFreq = 50  // updateFreq is the rate at which to update the sensor values.
+)
+
+// ICM20948 represents an InvenSense ICM-20948 attached to the I2C bus and satisfies
+// the IMUReader interface.
+type ICM20948 struct {
+	mpu *icm20948.ICM20948
+}
+
+// NewICM20948 returns an instance of the ICM-20948 IMUReader, connected to an
+// ICM-20948 attached on the I2C bus with either valid address.
+func NewICM20948(i2cbus *embd.I2CBus) (*ICM20948, error) {
+	var (
+		m   ICM20948
+		mpu *icm20948.ICM20948
+		err error
+	)
+
+	mpu, err = icm20948.NewICM20948(i2cbus, gyroRange, accelRange, updateFreq, true, false)
+	if err != nil {
+		return nil, err
+	}
+
+	// Set Gyro (Accel) LPFs to 25 Hz to filter out prop/glareshield vibrations above 1200 (1260) RPM
+	mpu.SetGyroLPF(25)
+	mpu.SetAccelLPF(25)
+
+	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 *ICM20948) Read() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64, GAError, MAGError error) {
+	var (
+		data *icm20948.MPUData
+		i    int8
+	)
+	data = new(icm20948.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 *ICM20948) ReadOne() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64, GAError, MAGError error) {
+	var (
+		data *icm20948.MPUData
+	)
+	data = new(icm20948.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 *ICM20948) Close() {
+	m.mpu.CloseMPU()
+}

sensors/mpu9250.go

@@ -7,9 +7,9 @@ import (
 )
 
 const (
-	gyroRange  = 250 // gyroRange is the default range to use for the Gyro.
-	accelRange = 4   // accelRange is the default range to use for the Accel.
-	updateFreq = 50  // updateFreq is the rate at which to update the sensor values.
+	mpu9250GyroRange  = 250 // mpu9250GyroRange is the default range to use for the Gyro.
+	mpu9250AccelRange = 4   // mpu9250AccelRange is the default range to use for the Accel.
+	mpu9250UpdateFreq = 50  // mpu9250UpdateFreq is the rate at which to update the sensor values.
 )
 
 // MPU9250 represents an InvenSense MPU9250 attached to the I2C bus and satisfies
@@ -27,7 +27,7 @@ func NewMPU9250(i2cbus *embd.I2CBus) (*MPU9250, error) {
 		err error
 	)
 
-	mpu, err = mpu9250.NewMPU9250(i2cbus, gyroRange, accelRange, updateFreq, true, false)
+	mpu, err = mpu9250.NewMPU9250(i2cbus, mpu9250GyroRange, mpu9250AccelRange, mpu9250UpdateFreq, true, false)
 	if err != nil {
 		return nil, err
 	}