Gyro heading tests.

gen_gdl90.go

@@ -146,10 +146,13 @@ func makeOwnshipReport() bool {
 	msg[9] = tmp[1]  // Longitude.
 	msg[10] = tmp[2] // Longitude.
 
-	//TODO: 0xFFF "invalid altitude."
-	//FIXME: This is **PRESSURE ALTITUDE**
 
-	alt := uint16(mySituation.alt)
+	// This is **PRESSURE ALTITUDE**
+	alt := uint16(0xFFF) // 0xFFF "invalid altitude."
+
+	if isTempPressValid() {
+		alt = uint16(mySituation.pressure_alt)
+	}
 	alt = (alt + 1000) / 25
 	alt = alt & 0xFFF // Should fit in 12 bits.
 
@@ -201,7 +204,7 @@ func makeOwnshipGeometricAltitudeReport() bool {
 	msg := make([]byte, 5)
 	// See p.28.
 	msg[0] = 0x0B                 // Message type "Ownship Geo Alt".
-	alt := int16(mySituation.alt) //FIXME.
+	alt := int16(mySituation.alt) // GPS Altitude.
 	alt = alt / 5
 	msg[1] = byte(alt >> 8)     // Altitude.
 	msg[2] = byte(alt & 0x00FF) // Altitude.

ry835ai.go

@@ -43,6 +43,7 @@ type SituationData struct {
 	// From MPU6050 accel/gyro.
 	pitch            float64
 	roll             float64
+	gyro_heading     float64
 	lastAttitudeTime time.Time
 }
 
@@ -75,6 +76,10 @@ func processNMEALine(l string) bool {
 				return false
 			}
 			trueCourse = uint16(tc)
+			//FIXME: Experimental. Set heading to true heading on the MPU6050 reader.
+			if myMPU6050 != nil && globalStatus.RY835AI_connected && globalSettings.AHRS_Enabled {
+				myMPU6050.ResetHeading(float64(tc))
+			}
 		} else {
 			// No movement.
 			mySituation.trueCourse = 0
@@ -207,6 +212,7 @@ func readBMP180() (float64, float64, error) { // ºCelsius, Meters
 		return temp, 0.0, err
 	}
 	altitude, err := myBMP180.Altitude()
+	altitude = float64(1/0.3048) * altitude // Convert meters to feet.
 	if err != nil {
 		return temp, altitude, err
 	}
@@ -269,12 +275,13 @@ func attitudeReaderSender() {
 		} else {
 			mySituation.pitch = pitch
 			mySituation.roll = roll
+			mySituation.gyro_heading = myMPU6050.Heading() //FIXME. Experimental.
 			mySituation.lastAttitudeTime = time.Now()
 		}
 
 		// Send, if valid.
-		//		if isGPSGroundTrackValid()
-		s := fmt.Sprintf("XATTStratux,%d,%f,%f", mySituation.trueCourse, mySituation.pitch, mySituation.roll)
+		//		if isGPSGroundTrackValid(), etc.
+		s := fmt.Sprintf("XATTStratux,%f,%f,%f", mySituation.gyro_heading, mySituation.pitch, mySituation.roll)
 
 		sendMsg([]byte(s), NETWORK_AHRS)
 
@@ -296,7 +303,7 @@ func isAHRSValid() bool {
 }
 
 func isTempPressValid() bool {
-	return time.Since(mySituation.lastTempPressTime).Seconds < 15
+	return time.Since(mySituation.lastTempPressTime).Seconds() < 15
 }
 
 func initAHRS() error {

sensortest.go

@@ -32,7 +32,7 @@ func main() {
 	outConn, err := net.DialUDP("udp", nil, addr)
 	for {
 		pitch, roll := readMPU6050()
-		s := fmt.Sprintf("XATTMy Sim,180.0,%f,%f", pitch, roll)
+		s := fmt.Sprintf("XATTMy Sim,%f,%f,%f", attSensor.Heading(), pitch, roll)
 		fmt.Printf("%f, %f\n", pitch, roll)
 		outConn.Write([]byte(s))
 		time.Sleep(50 * time.Millisecond)