kopia lustrzana https://github.com/cyoung/stratux
Use RY835AI or RY836AI; add more fields to GDL90 report
Eric Westphal
rodzic
ddaf673006
commit
d91a2a10e7
|
|
@ -163,8 +163,8 @@ func prepareMessage(data []byte) []byte {
|
|||
// Compute CRC before modifying the message.
|
||||
crc := crcCompute(data)
|
||||
// Add the two CRC16 bytes before replacing control characters.
|
||||
data = append(data, byte(crc&0xFF))
|
||||
data = append(data, byte(crc>>8))
|
||||
data = append(data, byte(crc & 0xFF))
|
||||
data = append(data, byte((crc >> 8) & 0xFF))
|
||||
|
||||
tmp := []byte{0x7E} // Flag start.
|
||||
|
||||
|
|
@ -426,8 +426,8 @@ func makeStratuxStatus() []byte {
|
|||
|
||||
// Connected hardware: number of radios.
|
||||
msg[15] = msg[15] | (byte(globalStatus.Devices) & 0x3)
|
||||
// Connected hardware: RY835AI.
|
||||
if globalStatus.RY835AI_connected {
|
||||
// Connected hardware: RY83XAI.
|
||||
if globalStatus.RY83XAI_connected {
|
||||
msg[15] = msg[15] | (1 << 2)
|
||||
}
|
||||
|
||||
|
|
@ -975,7 +975,7 @@ type status struct {
|
|||
GPS_satellites_tracked uint16
|
||||
GPS_connected bool
|
||||
GPS_solution string
|
||||
RY835AI_connected bool
|
||||
RY83XAI_connected bool
|
||||
Uptime int64
|
||||
Clock time.Time
|
||||
UptimeClock time.Time
|
||||
|
|
@ -1290,7 +1290,7 @@ func main() {
|
|||
//FIXME: Only do this if data logging is enabled.
|
||||
initDataLog()
|
||||
|
||||
initRY835AI()
|
||||
initRY83XAI()
|
||||
|
||||
// Start the heartbeat message loop in the background, once per second.
|
||||
go heartBeatSender()
|
||||
|
|
|
|||
|
|
@ -135,7 +135,10 @@ func sendToAllConnectedClients(msg networkMessage) {
|
|||
if sleepFlag {
|
||||
continue
|
||||
}
|
||||
netconn.Conn.Write(msg.msg) // Write immediately.
|
||||
_, err := netconn.Conn.Write(msg.msg) // Write immediately.
|
||||
if err != nil {
|
||||
log.Printf("GDL Message error: %s\n", err.Error())
|
||||
}
|
||||
totalNetworkMessagesSent++
|
||||
globalStatus.NetworkDataMessagesSent++
|
||||
globalStatus.NetworkDataMessagesSentNonqueueable++
|
||||
|
|
|
|||
156
main/ry83Xai.go
156
main/ry83Xai.go
|
|
@ -4,7 +4,7 @@
|
|||
that can be found in the LICENSE file, herein included
|
||||
as part of this header.
|
||||
|
||||
ry835ai.go: GPS functions, GPS init, AHRS status messages, other external sensor monitoring.
|
||||
ry83Xai.go: GPS functions, GPS init, AHRS status messages, other external sensor monitoring.
|
||||
*/
|
||||
|
||||
package main
|
||||
|
|
@ -28,6 +28,7 @@ import (
|
|||
"os/exec"
|
||||
|
||||
"../mpu"
|
||||
"errors"
|
||||
)
|
||||
|
||||
const (
|
||||
|
|
@ -37,6 +38,7 @@ const (
|
|||
SAT_TYPE_GALILEO = 3 // GAxxx; NMEA IDs unknown
|
||||
SAT_TYPE_BEIDOU = 4 // GBxxx; NMEA IDs 201-235
|
||||
SAT_TYPE_SBAS = 10 // NMEA IDs 33-54
|
||||
MPURETRYNUM = 5 // Number of times to retry connecting to MPU
|
||||
)
|
||||
|
||||
type SatelliteInfo struct {
|
||||
|
|
@ -432,9 +434,9 @@ func validateNMEAChecksum(s string) (string, bool) {
|
|||
// changes while on the ground and "movement" is really only changes in GPS fix as it settles down.
|
||||
//TODO: Some more robust checking above current and last speed.
|
||||
//TODO: Dynamic adjust for gain based on groundspeed
|
||||
//westphae: Do I need to do anything here?
|
||||
//TODO westphae: Do I need to do anything here?
|
||||
func setTrueCourse(groundSpeed uint16, trueCourse float64) {
|
||||
if myMPU != nil && globalStatus.RY835AI_connected && globalSettings.AHRS_Enabled {
|
||||
if myMPU != nil && globalStatus.RY83XAI_connected && globalSettings.AHRS_Enabled {
|
||||
if mySituation.GroundSpeed >= 7 && groundSpeed >= 7 {
|
||||
myMPU.ResetHeading(trueCourse, 0.10)
|
||||
}
|
||||
|
|
@ -942,7 +944,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
|
|||
tmpSituation := mySituation // If we decide to not use the data in this message, then don't make incomplete changes in mySituation.
|
||||
|
||||
//$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A
|
||||
/* check RY835 man for NMEA version, if >2.2, add mode field
|
||||
/* check RY83XAI man for NMEA version, if >2.2, add mode field
|
||||
Where:
|
||||
RMC Recommended Minimum sentence C
|
||||
123519 Fix taken at 12:35:19 UTC
|
||||
|
|
@ -1363,10 +1365,31 @@ func initBMP180() error {
|
|||
return nil
|
||||
}
|
||||
|
||||
//TODO westphae: set up myMPU as MPU6050 or MPU9250, depending on which exists
|
||||
func initMPU() error {
|
||||
myMPU, _ = mpu.NewMPU6050() //TODO: error checking.
|
||||
return nil
|
||||
var err error
|
||||
|
||||
for i:=0; i < MPURETRYNUM; i++ {
|
||||
myMPU, err = mpu.NewMPU9250()
|
||||
if err != nil {
|
||||
time.Sleep(100 * time.Millisecond)
|
||||
} else {
|
||||
log.Println("AHRS: Successfully initialized MPU9250")
|
||||
return nil
|
||||
}
|
||||
}
|
||||
|
||||
for i := 0; i < MPURETRYNUM; i++ {
|
||||
myMPU, err = mpu.NewMPU6050()
|
||||
if err != nil {
|
||||
time.Sleep(100 * time.Millisecond)
|
||||
} else {
|
||||
log.Println("AHRS: Successfully initialized MPU6050")
|
||||
return nil
|
||||
}
|
||||
}
|
||||
|
||||
log.Println("AHRS Error: couldn't initialize MPU9250 or MPU6050")
|
||||
return errors.New("AHRS Error: couldn't initialize MPU9250 or MPU6050")
|
||||
}
|
||||
|
||||
func initI2C() error {
|
||||
|
|
@ -1377,21 +1400,21 @@ func initI2C() error {
|
|||
// Unused at the moment. 5 second update, since read functions in bmp180 are slow.
|
||||
func tempAndPressureReader() {
|
||||
timer := time.NewTicker(5 * time.Second)
|
||||
for globalStatus.RY835AI_connected && globalSettings.AHRS_Enabled {
|
||||
for globalStatus.RY83XAI_connected && globalSettings.AHRS_Enabled {
|
||||
<-timer.C
|
||||
// Read temperature and pressure altitude.
|
||||
temp, alt, err_bmp180 := readBMP180()
|
||||
// Process.
|
||||
if err_bmp180 != nil {
|
||||
log.Printf("readBMP180(): %s\n", err_bmp180.Error())
|
||||
globalStatus.RY835AI_connected = false
|
||||
globalStatus.RY83XAI_connected = false
|
||||
} else {
|
||||
mySituation.Temp = temp
|
||||
mySituation.Pressure_alt = alt
|
||||
mySituation.LastTempPressTime = stratuxClock.Time
|
||||
}
|
||||
}
|
||||
globalStatus.RY835AI_connected = false
|
||||
globalStatus.RY83XAI_connected = false
|
||||
}
|
||||
|
||||
func makeFFAHRSSimReport() {
|
||||
|
|
@ -1401,19 +1424,23 @@ func makeFFAHRSSimReport() {
|
|||
}
|
||||
|
||||
func makeAHRSGDL90Report() {
|
||||
msg := make([]byte, 16)
|
||||
msg := make([]byte, 24)
|
||||
msg[0] = 0x4c
|
||||
msg[1] = 0x45
|
||||
msg[2] = 0x01
|
||||
msg[3] = 0x00
|
||||
msg[3] = 0x01
|
||||
|
||||
pitch := int16(float64(mySituation.Pitch) * float64(10.0))
|
||||
roll := int16(float64(mySituation.Roll) * float64(10.0))
|
||||
hdg := uint16(float64(mySituation.Gyro_heading) * float64(10.0))
|
||||
slip_skid := int16(float64(mySituation.SlipSkid) * float64(10.0))
|
||||
turn_rate := int16(float64(mySituation.RateOfTurn) * float64(10.0))
|
||||
g := int16(float64(mySituation.GLoad) * float64(10.0))
|
||||
roll := int16(mySituation.Roll*10)
|
||||
pitch := int16(mySituation.Pitch*10)
|
||||
yaw := uint16(mySituation.Gyro_heading*10)
|
||||
slip_skid := int16(mySituation.SlipSkid*10)
|
||||
turn_rate := int16(mySituation.RateOfTurn*10)
|
||||
g := int16(mySituation.GLoad*10)
|
||||
airspeed := 0x7FFF // Can add this once we can read airspeed
|
||||
palt := uint16(mySituation.Pressure_alt+5000)
|
||||
vs := int16(mySituation.GPSVertVel) //TODO: record BMP rate of climb
|
||||
|
||||
//TODO westphae: invalidate each with 0x7FFF when data is invalid
|
||||
// Roll.
|
||||
msg[4] = byte((roll >> 8) & 0xFF)
|
||||
msg[5] = byte(roll & 0xFF)
|
||||
|
|
@ -1423,14 +1450,14 @@ func makeAHRSGDL90Report() {
|
|||
msg[7] = byte(pitch & 0xFF)
|
||||
|
||||
// Heading.
|
||||
msg[8] = byte((hdg >> 8) & 0xFF)
|
||||
msg[9] = byte(hdg & 0xFF)
|
||||
msg[8] = byte((yaw >> 8) & 0xFF)
|
||||
msg[9] = byte(yaw & 0xFF)
|
||||
|
||||
// Slip/skid.
|
||||
msg[10] = byte((slip_skid >> 8) & 0xFF)
|
||||
msg[11] = byte(slip_skid & 0xFF)
|
||||
|
||||
// Yaw rate.
|
||||
// Turn rate.
|
||||
msg[12] = byte((turn_rate >> 8) & 0xFF)
|
||||
msg[13] = byte(turn_rate & 0xFF)
|
||||
|
||||
|
|
@ -1438,33 +1465,78 @@ func makeAHRSGDL90Report() {
|
|||
msg[14] = byte((g >> 8) & 0xFF)
|
||||
msg[15] = byte(g & 0xFF)
|
||||
|
||||
// Indicated Airspeed
|
||||
msg[16] = byte((airspeed >> 8) & 0xFF)
|
||||
msg[17] = byte(airspeed & 0xFF)
|
||||
|
||||
// Pressure Altitude
|
||||
//TODO westphae: this is just for testing; 0x7FFF it until BMP280 is working
|
||||
msg[18] = byte((palt >> 8) & 0xFF)
|
||||
msg[19] = byte(palt & 0xFF)
|
||||
|
||||
// Vertical Speed
|
||||
msg[20] = byte((vs >> 8) & 0xFF)
|
||||
msg[21] = byte(vs & 0xFF)
|
||||
|
||||
// Reserved
|
||||
msg[22] = 0x7F
|
||||
msg[23] = 0xFF
|
||||
|
||||
sendMsg(prepareMessage(msg), NETWORK_AHRS_GDL90, false)
|
||||
}
|
||||
|
||||
func attitudeReaderSender() {
|
||||
timer := time.NewTicker(100 * time.Millisecond) // ~10Hz update.
|
||||
for globalStatus.RY835AI_connected && globalSettings.AHRS_Enabled {
|
||||
for globalStatus.RY83XAI_connected && globalSettings.AHRS_Enabled {
|
||||
<-timer.C
|
||||
// Read pitch and roll.
|
||||
pitch, err_pitch := myMPU.Pitch()
|
||||
if err_pitch != nil {
|
||||
log.Printf("readMPU6050(): %s\n", err_pitch.Error())
|
||||
globalStatus.RY835AI_connected = false
|
||||
log.Printf("AHRS MPU Error: %s\n", err_pitch.Error())
|
||||
globalStatus.RY83XAI_connected = false
|
||||
break
|
||||
}
|
||||
|
||||
roll, err_roll := myMPU.Roll()
|
||||
|
||||
if err_roll != nil {
|
||||
log.Printf("readMPU6050(): %s\n", err_roll.Error())
|
||||
globalStatus.RY835AI_connected = false
|
||||
log.Printf("AHRS MPU Error: %s\n", err_roll.Error())
|
||||
globalStatus.RY83XAI_connected = false
|
||||
break
|
||||
}
|
||||
|
||||
heading, err_heading := myMPU.Heading() //FIXME. Experimental.
|
||||
if err_heading != nil {
|
||||
log.Printf("readMPU6050(): %s\n", err_heading.Error())
|
||||
globalStatus.RY835AI_connected = false
|
||||
log.Printf("AHRS MPU Error: %s\n", err_heading.Error())
|
||||
globalStatus.RY83XAI_connected = false
|
||||
break
|
||||
}
|
||||
|
||||
headingMag, err_headingMag := myMPU.MagHeading()
|
||||
if err_headingMag != nil {
|
||||
log.Printf("AHRS MPU Error: %s\n", err_headingMag.Error())
|
||||
globalStatus.RY83XAI_connected = false
|
||||
break
|
||||
}
|
||||
|
||||
slipSkid, err_slipSkid := myMPU.SlipSkid()
|
||||
if err_slipSkid != nil {
|
||||
log.Printf("AHRS MPU Error: %s\n", err_slipSkid.Error())
|
||||
globalStatus.RY83XAI_connected = false
|
||||
break
|
||||
}
|
||||
|
||||
turnRate, err_turnRate := myMPU.RateOfTurn()
|
||||
if err_turnRate != nil {
|
||||
log.Printf("AHRS MPU Error: %s\n", err_turnRate.Error())
|
||||
globalStatus.RY83XAI_connected = false
|
||||
break
|
||||
}
|
||||
|
||||
gLoad, err_gLoad := myMPU.GLoad()
|
||||
if err_gLoad != nil {
|
||||
log.Printf("AHRS MPU Error: %s\n", err_gLoad.Error())
|
||||
globalStatus.RY83XAI_connected = false
|
||||
break
|
||||
}
|
||||
|
||||
|
|
@ -1473,6 +1545,10 @@ func attitudeReaderSender() {
|
|||
mySituation.Pitch = pitch
|
||||
mySituation.Roll = roll
|
||||
mySituation.Gyro_heading = heading
|
||||
mySituation.Mag_heading = headingMag
|
||||
mySituation.SlipSkid = slipSkid
|
||||
mySituation.RateOfTurn = turnRate
|
||||
mySituation.GLoad = gLoad
|
||||
mySituation.LastAttitudeTime = stratuxClock.Time
|
||||
|
||||
// Send, if valid.
|
||||
|
|
@ -1483,7 +1559,7 @@ func attitudeReaderSender() {
|
|||
|
||||
mySituation.mu_Attitude.Unlock()
|
||||
}
|
||||
globalStatus.RY835AI_connected = false
|
||||
globalStatus.RY83XAI_connected = false
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -1559,27 +1635,29 @@ func isTempPressValid() bool {
|
|||
|
||||
func initAHRS() error {
|
||||
if err := initI2C(); err != nil { // I2C bus.
|
||||
log.Println("AHRS Error: Couldn't initialize i2c bus")
|
||||
return err
|
||||
}
|
||||
if err := initBMP180(); err != nil { // I2C temperature and pressure altitude.
|
||||
log.Println("AHRS Error: No BMP180, Closing i2c bus")
|
||||
i2cbus.Close()
|
||||
return err
|
||||
}
|
||||
// TODO westphae: Initialize MPU9250 here
|
||||
if err := initMPU(); err != nil { // I2C accel/gyro.
|
||||
log.Println("AHRS Error: Couldn't init MPU, closing i2c bus")
|
||||
i2cbus.Close()
|
||||
myBMP180.Close()
|
||||
return err
|
||||
}
|
||||
globalStatus.RY835AI_connected = true
|
||||
globalStatus.RY83XAI_connected = true
|
||||
go attitudeReaderSender()
|
||||
go tempAndPressureReader()
|
||||
//go tempAndPressureReader()
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
func pollRY835AI() {
|
||||
readyToInitGPS = true //TO-DO: Implement more robust method (channel control) to kill zombie serial readers
|
||||
func pollRY83XAI() {
|
||||
readyToInitGPS = true //TODO: Implement more robust method (channel control) to kill zombie serial readers
|
||||
timer := time.NewTicker(4 * time.Second)
|
||||
for {
|
||||
<-timer.C
|
||||
|
|
@ -1590,22 +1668,22 @@ func pollRY835AI() {
|
|||
go gpsSerialReader()
|
||||
}
|
||||
}
|
||||
// RY835AI I2C enabled, was not connected previously?
|
||||
if globalSettings.AHRS_Enabled && !globalStatus.RY835AI_connected {
|
||||
// RY83XAI I2C enabled, was not connected previously?
|
||||
if globalSettings.AHRS_Enabled && !globalStatus.RY83XAI_connected {
|
||||
err := initAHRS()
|
||||
if err != nil {
|
||||
log.Printf("initAHRS(): %s\ndisabling AHRS sensors.\n", err.Error())
|
||||
globalStatus.RY835AI_connected = false
|
||||
globalStatus.RY83XAI_connected = false
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func initRY835AI() {
|
||||
func initRY83XAI() {
|
||||
mySituation.mu_GPS = &sync.Mutex{}
|
||||
mySituation.mu_Attitude = &sync.Mutex{}
|
||||
satelliteMutex = &sync.Mutex{}
|
||||
Satellites = make(map[string]SatelliteInfo)
|
||||
|
||||
go pollRY835AI()
|
||||
go pollRY83XAI()
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1 @@
|
|||
package mpu
|
||||
|
|
@ -7,6 +7,7 @@ import (
|
|||
"log"
|
||||
"math"
|
||||
"time"
|
||||
"errors"
|
||||
)
|
||||
|
||||
//https://www.olimex.com/Products/Modules/Sensors/MOD-MPU6050/resources/RM-MPU-60xxA_rev_4.pdf
|
||||
|
|
@ -42,14 +43,19 @@ type MPU6050 struct {
|
|||
// New returns a handle to a MPU6050 sensor.
|
||||
func NewMPU6050() (*MPU6050, error) {
|
||||
n := &MPU6050{poll: pollDelay}
|
||||
n.startUp()
|
||||
if err := n.startUp(); err != nil {
|
||||
return nil, err
|
||||
}
|
||||
return n, nil
|
||||
}
|
||||
|
||||
func (d *MPU6050) startUp() error {
|
||||
mpu_sample_rate := 10 // 10 Hz read rate of hardware IMU
|
||||
yaw_mix_factor := 0 // must be zero if no magnetometer
|
||||
mpu.InitMPU(mpu_sample_rate, yaw_mix_factor)
|
||||
err := mpu.InitMPU(mpu_sample_rate, yaw_mix_factor)
|
||||
if err != 0 {
|
||||
return errors.New("MPU6050 Error: couldn't start MPU")
|
||||
}
|
||||
|
||||
d.pitch_history = make([]float64, 0)
|
||||
d.roll_history = make([]float64, 0)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,192 @@
|
|||
// Package MPU9250 provides a stratux interface to the MPU9250 IMU
|
||||
package mpu
|
||||
|
||||
import (
|
||||
"errors"
|
||||
"github.com/westphae/goflying/mpu9250"
|
||||
"math"
|
||||
"time"
|
||||
"log"
|
||||
)
|
||||
|
||||
const (
|
||||
DECAY = 0.98
|
||||
GYRORANGE = 250
|
||||
ACCELRANGE = 4
|
||||
UPDATEFREQ = 100
|
||||
CALIBTIME int64 = 5*60*1000000000
|
||||
)
|
||||
|
||||
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{}
|
||||
}
|
||||
|
||||
func NewMPU9250() (*MPU9250, error) {
|
||||
var (
|
||||
m MPU9250
|
||||
mpu *mpu9250.MPU9250
|
||||
err error
|
||||
)
|
||||
|
||||
mpu, err = mpu9250.NewMPU9250(GYRORANGE, ACCELRANGE, UPDATEFREQ, false)
|
||||
if err != nil {
|
||||
log.Println("AHRS Error: couldn't initialize MPU9250")
|
||||
return nil, err
|
||||
}
|
||||
|
||||
err = mpu.CalibrateGyro(1)
|
||||
if err != nil {
|
||||
log.Printf("AHRS: Gyro calibration failed: %s\n", err.Error())
|
||||
} else {
|
||||
log.Println("AHRS: Gyro calibration successful")
|
||||
m.nextCalibrateT = time.Now().UnixNano() + CALIBTIME
|
||||
}
|
||||
|
||||
m.mpu = mpu
|
||||
m.valid = true
|
||||
|
||||
time.Sleep(100 * time.Millisecond)
|
||||
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:
|
||||
Ts, Gx, Gy, Gz, Ax, Ay, Az, Mx, My, _, gaError, magError := m.mpu.Read()
|
||||
|
||||
if gaError == nil {
|
||||
m.T = Ts
|
||||
smooth(&m.turnRate, Gz)
|
||||
smooth(&m.gLoad, math.Sqrt(Ax * Ax + Ay * Ay + Az * Az))
|
||||
smooth(&m.slipSkid, Ay / Az)
|
||||
|
||||
// Quick and dirty calcs just to test - these are no good for pitch >> 0
|
||||
m.pitch += 0.1 * Gx
|
||||
m.roll += 0.1 * Gy
|
||||
m.heading -= 0.1 * Gz
|
||||
|
||||
if m.pitch > 90 {
|
||||
m.pitch = 180-m.pitch
|
||||
}
|
||||
if m.pitch < -90 {
|
||||
m.pitch = -180 - m.pitch
|
||||
}
|
||||
if (m.roll > 180) || (m.roll < -180) {
|
||||
m.roll = -m.roll
|
||||
}
|
||||
if m.heading > 360 {
|
||||
m.heading -= 360
|
||||
}
|
||||
if m.heading < 0 {
|
||||
m.heading += 360
|
||||
}
|
||||
}
|
||||
|
||||
if magError == nil {
|
||||
smooth(&m.headingMag, math.Atan2(My, Mx))
|
||||
}
|
||||
|
||||
// Calibrate if past-due
|
||||
if time.Now().UnixNano() > m.nextCalibrateT {
|
||||
err := m.mpu.CalibrateGyro(1)
|
||||
if err != nil {
|
||||
log.Printf("AHRS: Error calibrating gyro, %s\n", err)
|
||||
} else {
|
||||
log.Println("AHRS: Gyro calibration successful")
|
||||
m.nextCalibrateT = time.Now().UnixNano() + CALIBTIME
|
||||
}
|
||||
}
|
||||
case <-m.quit:
|
||||
m.mpu.CloseMPU()
|
||||
return
|
||||
}
|
||||
}
|
||||
}()
|
||||
}
|
||||
|
||||
func smooth(val *float64, new float64) {
|
||||
*val = DECAY * *val + (1-DECAY)*new
|
||||
}
|
||||
|
||||
func (m *MPU9250) ResetHeading(newHeading float64, gain float64) {
|
||||
m.heading = newHeading
|
||||
}
|
||||
|
||||
func (m *MPU9250) Pitch() (float64, error) {
|
||||
if m.valid {
|
||||
return m.pitch, nil
|
||||
} else {
|
||||
return 0, errors.New("MPU error: data not available")
|
||||
}
|
||||
}
|
||||
|
||||
func (m *MPU9250) Roll() (float64, error) {
|
||||
if m.valid {
|
||||
return m.roll, nil
|
||||
} else {
|
||||
return 0, errors.New("MPU error: data not available")
|
||||
}
|
||||
}
|
||||
|
||||
func (m *MPU9250) Heading() (float64, error) {
|
||||
if m.valid {
|
||||
return m.heading, nil
|
||||
} else {
|
||||
return 0, errors.New("MPU error: data not available")
|
||||
}
|
||||
}
|
||||
|
||||
func (m *MPU9250) MagHeading() (float64, error) {
|
||||
if m.valid {
|
||||
return m.headingMag, nil
|
||||
} else {
|
||||
return 0, errors.New("MPU error: data not available")
|
||||
}
|
||||
}
|
||||
|
||||
func (m *MPU9250) SlipSkid() (float64, error) {
|
||||
if m.valid {
|
||||
return m.slipSkid, nil
|
||||
} else {
|
||||
return 0, errors.New("MPU error: data not available")
|
||||
}
|
||||
}
|
||||
|
||||
func (m *MPU9250) RateOfTurn() (float64, error) {
|
||||
if m.valid {
|
||||
return m.turnRate, nil
|
||||
} else {
|
||||
return 0, errors.New("MPU error: data not available")
|
||||
}
|
||||
}
|
||||
|
||||
func (m *MPU9250) GLoad() (float64, error) {
|
||||
if m.valid {
|
||||
return m.gLoad, nil
|
||||
} else {
|
||||
return 0, errors.New("MPU error: data not available")
|
||||
}
|
||||
}
|
||||
|
||||
func (m *MPU9250) Close() {
|
||||
if m.quit != nil {
|
||||
m.quit <- struct{}{}
|
||||
}
|
||||
}
|
||||
Ładowanie…
Reference in New Issue