kopia lustrzana https://github.com/cyoung/stratux
AHRSProvider calcs supplemental AHRS info instead of sensor.
Eric Westphal
rodzic
e78146ab99
commit
36595433be
|
|
@ -159,8 +159,6 @@ func sensorAttitudeSender() {
|
|||
a1, a2, a3, b1, b2, b3, m1, m2, m3 float64 // IMU measurements
|
||||
ff *[3][3]float64 // Sensor orientation matrix
|
||||
mpuError, magError error
|
||||
headingMag, slipSkid, turnRate, gLoad float64
|
||||
errHeadingMag, errSlipSkid, errTurnRate, errGLoad error
|
||||
failnum uint8
|
||||
analysisLogger *ahrs.AHRSLogger
|
||||
)
|
||||
|
|
@ -221,7 +219,7 @@ func sensorAttitudeSender() {
|
|||
t = stratuxClock.Time
|
||||
m.T = float64(t.UnixNano() / 1000) / 1e6
|
||||
|
||||
_, b1, b2, b3, a1, a2, a3, m1, m2, m3, mpuError, magError = myIMUReader.ReadRaw()
|
||||
_, b1, b2, b3, a1, a2, a3, m1, m2, m3, mpuError, magError = myIMUReader.Read()
|
||||
// This is how the RY83XAI is wired up
|
||||
//m.A1, m.A2, m.A3 = -a2, +a1, -a3
|
||||
//m.B1, m.B2, m.B3 = +b2, -b1, +b3
|
||||
|
|
@ -248,8 +246,8 @@ func sensorAttitudeSender() {
|
|||
log.Printf("AHRS Gyro/Accel Error: failed to read %d times, restarting: %s\n", failnum, mpuError)
|
||||
myIMUReader.Close()
|
||||
globalStatus.IMUConnected = false
|
||||
continue
|
||||
}
|
||||
continue
|
||||
}
|
||||
if magError != nil {
|
||||
log.Printf("AHRS Magnetometer Error, not using for this run: %s\n", magError)
|
||||
|
|
@ -299,34 +297,15 @@ func sensorAttitudeSender() {
|
|||
if s.Valid() {
|
||||
mySituation.mu_Attitude.Lock()
|
||||
|
||||
roll, pitch, heading = s.CalcRollPitchHeading()
|
||||
roll, pitch, heading = s.RollPitchHeading()
|
||||
mySituation.Roll = roll / ahrs.Deg
|
||||
mySituation.Pitch = pitch / ahrs.Deg
|
||||
mySituation.Gyro_heading = heading / ahrs.Deg
|
||||
|
||||
if headingMag, errHeadingMag = myIMUReader.MagHeading(); errHeadingMag != nil {
|
||||
log.Printf("AHRS MPU Error: %s\n", errHeadingMag.Error())
|
||||
} else {
|
||||
mySituation.Mag_heading = headingMag
|
||||
}
|
||||
|
||||
if slipSkid, errSlipSkid = myIMUReader.SlipSkid(); errSlipSkid != nil {
|
||||
log.Printf("AHRS MPU Error: %s\n", errSlipSkid.Error())
|
||||
} else {
|
||||
mySituation.SlipSkid = slipSkid
|
||||
}
|
||||
|
||||
if turnRate, errTurnRate = myIMUReader.RateOfTurn(); errTurnRate != nil {
|
||||
log.Printf("AHRS MPU Error: %s\n", errTurnRate.Error())
|
||||
} else {
|
||||
mySituation.RateOfTurn = turnRate
|
||||
}
|
||||
|
||||
if gLoad, errGLoad = myIMUReader.GLoad(); errGLoad != nil {
|
||||
log.Printf("AHRS MPU Error: %s\n", errGLoad.Error())
|
||||
} else {
|
||||
mySituation.GLoad = gLoad
|
||||
}
|
||||
mySituation.Mag_heading = s.MagHeading()
|
||||
mySituation.SlipSkid = s.SlipSkid()
|
||||
mySituation.RateOfTurn = s.RateOfTurn()
|
||||
mySituation.GLoad = s.GLoad()
|
||||
|
||||
mySituation.LastAttitudeTime = t
|
||||
mySituation.mu_Attitude.Unlock()
|
||||
|
|
@ -342,7 +321,7 @@ func sensorAttitudeSender() {
|
|||
}
|
||||
|
||||
func getMinAccelDirection() (i int, err error) {
|
||||
_, _, _, _, a1, a2, a3, _, _, _, err, _ := myIMUReader.ReadRaw()
|
||||
_, _, _, _, a1, a2, a3, _, _, _, err, _ := myIMUReader.Read()
|
||||
if err != nil {
|
||||
return
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2,14 +2,14 @@
|
|||
package sensors
|
||||
|
||||
// IMUReader provides an interface to various Inertial Measurement Unit sensors,
|
||||
// such as the InvenSense MPU9150 or MPU9250.
|
||||
// such as the InvenSense MPU9150 or MPU9250. It is a light abstraction on top
|
||||
// of the current goflying MPU9250 driver so that it can accommodate other types
|
||||
// of drivers.
|
||||
type IMUReader interface {
|
||||
// ReadRaw returns the time, Gyro X-Y-Z, Accel X-Y-Z, Mag X-Y-Z, error reading Gyro/Accel, and error reading Mag.
|
||||
ReadRaw() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64, GAError, MagError error)
|
||||
Calibrate(duration, retries int) error // Calibrate kicks off a calibration for specified duration (s) and retries.
|
||||
Close() // Close stops reading the MPU.
|
||||
MagHeading() (hdg float64, MagError error) // MagHeading returns the magnetic heading in degrees.
|
||||
SlipSkid() (slipSkid float64, err error) // SlipSkid returns the slip/skid angle in degrees.
|
||||
RateOfTurn() (turnRate float64, err error) // RateOfTurn returns the turn rate in degrees per second.
|
||||
GLoad() (gLoad float64, err error) // GLoad returns the current G load, in G's.
|
||||
// Calibrate kicks off a calibration for specified duration (s) and retries.
|
||||
Calibrate(duration, retries int) error
|
||||
// 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.
|
||||
Read() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64, GAError, MagError error)
|
||||
// Close stops reading the MPU.
|
||||
Close()
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2,16 +2,13 @@
|
|||
package sensors
|
||||
|
||||
import (
|
||||
"errors"
|
||||
"math"
|
||||
"log"
|
||||
"time"
|
||||
|
||||
"github.com/westphae/goflying/mpu9250"
|
||||
"log"
|
||||
)
|
||||
|
||||
const (
|
||||
decay = 0.8 // decay is the decay constant used for exponential smoothing of sensor values.
|
||||
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 = 1000 // updateFreq is the rate at which to update the sensor values.
|
||||
|
|
@ -21,15 +18,6 @@ const (
|
|||
// the IMUReader interface.
|
||||
type MPU9250 struct {
|
||||
mpu *mpu9250.MPU9250
|
||||
pitch, roll, heading float64
|
||||
headingMag float64
|
||||
slipSkid float64
|
||||
turnRate float64
|
||||
gLoad float64
|
||||
T int64
|
||||
valid bool
|
||||
nextCalibrateT int64
|
||||
quit chan struct{}
|
||||
}
|
||||
|
||||
// NewMPU9250 returns an instance of the MPU9250 IMUReader, connected to an
|
||||
|
|
@ -51,95 +39,17 @@ func NewMPU9250() (*MPU9250, error) {
|
|||
log.Println("AHRS Info: Setting MPU9250 LPF")
|
||||
mpu.SetGyroLPF(21)
|
||||
mpu.SetAccelLPF(21)
|
||||
time.Sleep(100 * time.Millisecond)
|
||||
|
||||
m.mpu = mpu
|
||||
m.valid = true
|
||||
|
||||
time.Sleep(100 * time.Millisecond)
|
||||
log.Println("AHRS Info: monitoring IMU")
|
||||
m.run()
|
||||
|
||||
return &m, nil
|
||||
}
|
||||
|
||||
func (m *MPU9250) run() {
|
||||
time.Sleep(100 * time.Millisecond)
|
||||
go func() {
|
||||
m.quit = make(chan struct{})
|
||||
clock := time.NewTicker(100 * time.Millisecond)
|
||||
|
||||
for {
|
||||
select {
|
||||
case <-clock.C:
|
||||
data := <-m.mpu.CAvg
|
||||
|
||||
if data.GAError == nil && data.N > 0 {
|
||||
m.T = data.T.UnixNano()
|
||||
smooth(&m.turnRate, -data.G3) // TODO westphae: gross approx, depends on attitude!
|
||||
smooth(&m.gLoad, data.A3)
|
||||
smooth(&m.slipSkid, math.Atan2(-data.A1, data.A3)*180/math.Pi)
|
||||
}
|
||||
|
||||
if data.MagError == nil && data.NM > 0 {
|
||||
hM := math.Atan2(-data.M2, data.M1) * 180 / math.Pi
|
||||
if hM-m.headingMag < -180 {
|
||||
hM += 360
|
||||
}
|
||||
smooth(&m.headingMag, hM)
|
||||
for m.headingMag < 0 {
|
||||
m.headingMag += 360
|
||||
}
|
||||
for m.headingMag >= 360 {
|
||||
m.headingMag -= 360
|
||||
}
|
||||
}
|
||||
case <-m.quit:
|
||||
m.mpu.CloseMPU()
|
||||
return
|
||||
}
|
||||
}
|
||||
}()
|
||||
}
|
||||
|
||||
func smooth(val *float64, new float64) {
|
||||
*val = decay**val + (1-decay)*new
|
||||
}
|
||||
|
||||
// MagHeading returns the magnetic heading in degrees.
|
||||
func (m *MPU9250) MagHeading() (float64, error) {
|
||||
if m.valid {
|
||||
return m.headingMag, nil
|
||||
}
|
||||
return 0, errors.New("MPU error: data not available")
|
||||
}
|
||||
|
||||
// SlipSkid returns the slip/skid angle in degrees.
|
||||
func (m *MPU9250) SlipSkid() (float64, error) {
|
||||
if m.valid {
|
||||
return m.slipSkid, nil
|
||||
}
|
||||
return 0, errors.New("MPU error: data not available")
|
||||
}
|
||||
|
||||
// RateOfTurn returns the turn rate in degrees per second.
|
||||
func (m *MPU9250) RateOfTurn() (float64, error) {
|
||||
if m.valid {
|
||||
return m.turnRate, nil
|
||||
}
|
||||
return 0, errors.New("MPU error: data not available")
|
||||
}
|
||||
|
||||
// GLoad returns the current G load, in G's.
|
||||
func (m *MPU9250) GLoad() (float64, error) {
|
||||
if m.valid {
|
||||
return m.gLoad, nil
|
||||
}
|
||||
return 0, errors.New("MPU error: data not available")
|
||||
}
|
||||
|
||||
// ReadRaw returns the time, Gyro X-Y-Z, Accel X-Y-Z, Mag X-Y-Z, error reading Gyro/Accel, and error reading Mag.
|
||||
func (m *MPU9250) ReadRaw() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64, GAError, MAGError error) {
|
||||
data := <-m.mpu.C
|
||||
// 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 *MPU9250) Read() (T int64, G1, G2, G3, A1, A2, A3, M1, M2, M3 float64, GAError, MAGError error) {
|
||||
data := <-m.mpu.CAvg
|
||||
T = data.T.UnixNano()
|
||||
G1 = data.G1
|
||||
G2 = data.G2
|
||||
|
|
@ -175,7 +85,5 @@ func (m *MPU9250) Calibrate(dur, retries int) (err error) {
|
|||
|
||||
// Close stops reading the MPU.
|
||||
func (m *MPU9250) Close() {
|
||||
if m.quit != nil {
|
||||
m.quit <- struct{}{}
|
||||
}
|
||||
m.mpu.CloseMPU()
|
||||
}
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue