diff --git a/main/sensors.go b/main/sensors.go index b754b8dc..bf775119 100644 --- a/main/sensors.go +++ b/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 } diff --git a/sensors/bmx160.go b/sensors/bmx160.go new file mode 100644 index 00000000..25871bec --- /dev/null +++ b/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() +}