kopia lustrzana https://github.com/cyoung/stratux
commit
7eeb32acf6
2
goflying
2
goflying
|
@ -1 +1 @@
|
||||||
Subproject commit d0268b1b182f79d34af4f3a7524ae0b4c18c41e0
|
Subproject commit 1da95360a858bfdc5af2b4e7d5caa66542f164e1
|
|
@ -19,6 +19,12 @@ const (
|
||||||
numRetries uint8 = 5
|
numRetries uint8 = 5
|
||||||
calCLimit = 0.15
|
calCLimit = 0.15
|
||||||
calDLimit = 10.0
|
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 (
|
var (
|
||||||
|
@ -123,13 +129,36 @@ func tempAndPressureSender() {
|
||||||
}
|
}
|
||||||
|
|
||||||
func initIMU() (ok bool) {
|
func initIMU() (ok bool) {
|
||||||
imu, err := sensors.NewMPU9250(&i2cbus)
|
// Check if the chip is the ICM-20948 or MPU-9250.
|
||||||
if err == nil {
|
v, err := i2cbus.ReadByteFromReg(0x68, ICMREG_WHO_AM_I)
|
||||||
myIMUReader = imu
|
if err != nil {
|
||||||
return true
|
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
|
return false
|
||||||
}
|
}
|
||||||
|
|
|
@ -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()
|
||||||
|
}
|
|
@ -7,9 +7,9 @@ import (
|
||||||
)
|
)
|
||||||
|
|
||||||
const (
|
const (
|
||||||
gyroRange = 250 // gyroRange is the default range to use for the Gyro.
|
mpu9250GyroRange = 250 // mpu9250GyroRange is the default range to use for the Gyro.
|
||||||
accelRange = 4 // accelRange is the default range to use for the Accel.
|
mpu9250AccelRange = 4 // mpu9250AccelRange is the default range to use for the Accel.
|
||||||
updateFreq = 50 // updateFreq is the rate at which to update the sensor values.
|
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
|
// 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
|
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 {
|
if err != nil {
|
||||||
return nil, err
|
return nil, err
|
||||||
}
|
}
|
||||||
|
|
Ładowanie…
Reference in New Issue