kopia lustrzana https://github.com/cyoung/stratux
Revert "Add support for bmx160 MPU"
rodzic
fb1ef310f5
commit
ac357d8a28
|
@ -25,8 +25,6 @@ const (
|
||||||
MPUREG_WHO_AM_I_VAL = 0x71 // Expected value.
|
MPUREG_WHO_AM_I_VAL = 0x71 // Expected value.
|
||||||
ICMREG_WHO_AM_I = 0x00
|
ICMREG_WHO_AM_I = 0x00
|
||||||
ICMREG_WHO_AM_I_VAL = 0xEA // Expected value.
|
ICMREG_WHO_AM_I_VAL = 0xEA // Expected value.
|
||||||
BMXREG_WHO_AM_I = 0x00
|
|
||||||
BMXREG_WHO_AM_I_VAL = 0xD8 // Expected value.
|
|
||||||
)
|
)
|
||||||
|
|
||||||
var (
|
var (
|
||||||
|
@ -142,11 +140,6 @@ func initIMU() (ok bool) {
|
||||||
log.Printf("Error identifying IMU: %s\n", err.Error())
|
log.Printf("Error identifying IMU: %s\n", err.Error())
|
||||||
return false
|
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 {
|
if v == ICMREG_WHO_AM_I_VAL {
|
||||||
log.Println("ICM-20948 detected.")
|
log.Println("ICM-20948 detected.")
|
||||||
|
@ -162,15 +155,8 @@ func initIMU() (ok bool) {
|
||||||
myIMUReader = imu
|
myIMUReader = imu
|
||||||
return true
|
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 {
|
} else {
|
||||||
log.Printf("Could not identify MPU. v=%02x, v2=%02x, v3=%02x.\n", v, v2, v3)
|
log.Printf("Could not identify MPU. v=%02x, v2=%02x.\n", v, v2)
|
||||||
return false
|
return false
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1,97 +0,0 @@
|
||||||
// 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()
|
|
||||||
}
|
|
Ładowanie…
Reference in New Issue