Use RY835AI or RY836AI; add more fields to GDL90 report

main/gen_gdl90.go

@@ -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 & 0xFF))
-	data = append(data, byte(crc>>8))
+	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.
+	// Connected hardware: RY83XAI.
-	if globalStatus.RY835AI_connected {
+	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()

main/network.go

@@ -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++

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.
+	var err error
-	return nil
+
+	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(mySituation.Roll*10)
-	roll := int16(float64(mySituation.Roll) * float64(10.0))
+	pitch := int16(mySituation.Pitch*10)
-	hdg := uint16(float64(mySituation.Gyro_heading) * float64(10.0))
+	yaw := uint16(mySituation.Gyro_heading*10)
-	slip_skid := int16(float64(mySituation.SlipSkid) * float64(10.0))
+	slip_skid := int16(mySituation.SlipSkid*10)
-	turn_rate := int16(float64(mySituation.RateOfTurn) * float64(10.0))
+	turn_rate := int16(mySituation.RateOfTurn*10)
-	g := int16(float64(mySituation.GLoad) * float64(10.0))
+	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[8] = byte((yaw >> 8) & 0xFF)
-	msg[9] = byte(hdg & 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())
+			log.Printf("AHRS MPU Error: %s\n", err_pitch.Error())
-			globalStatus.RY835AI_connected = false
+			globalStatus.RY83XAI_connected = false
 			break
 		}
 
 		roll, err_roll := myMPU.Roll()
 
 		if err_roll != nil {
-			log.Printf("readMPU6050(): %s\n", err_roll.Error())
+			log.Printf("AHRS MPU Error: %s\n", err_roll.Error())
-			globalStatus.RY835AI_connected = false
+			globalStatus.RY83XAI_connected = false
 			break
 		}
 
 		heading, err_heading := myMPU.Heading() //FIXME. Experimental.
 		if err_heading != nil {
-			log.Printf("readMPU6050(): %s\n", err_heading.Error())
+			log.Printf("AHRS MPU Error: %s\n", err_heading.Error())
-			globalStatus.RY835AI_connected = false
+			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() {
+func pollRY83XAI() {
-	readyToInitGPS = true //TO-DO: Implement more robust method (channel control) to kill zombie serial readers
+	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?
+		// RY83XAI I2C enabled, was not connected previously?
-		if globalSettings.AHRS_Enabled && !globalStatus.RY835AI_connected {
+		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()
 }

mpu/bmp.go

@@ -0,0 +1 @@
+package mpu

mpu/mpu6050.go

@@ -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)

mpu/mpu9250.go

@@ -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{}{}
+	}
+}