From 54ab7df155f07d27ec60fda49e5f78ac24f1209f Mon Sep 17 00:00:00 2001
From: cyoung <cyoung@gmail.com>
Date: Fri, 6 Apr 2018 15:08:45 -0400
Subject: [PATCH] Add new ForeFlight AHRS and ID GDL90 messages.

---
 main/gen_gdl90.go | 34 ++++++++++++++++++++++++
 main/gps.go       | 66 +++++++++++++++++++++++++++++++++++++++++++++++
 main/sensors.go   |  2 +-
 3 files changed, 101 insertions(+), 1 deletion(-)

diff --git a/main/gen_gdl90.go b/main/gen_gdl90.go
index 7c2e20739..58584575a 100644
--- a/main/gen_gdl90.go
+++ b/main/gen_gdl90.go
@@ -643,6 +643,39 @@ func makeStratuxHeartbeat() []byte {
 	return prepareMessage(msg)
 }
 
+/*
+
+	ForeFlight "ID Message".
+
+	Sends device information to ForeFlight.
+
+*/
+func makeFFIDMessage() []byte {
+	msg := make([]byte, 39)
+	msg[0] = 0x65 // Message type "ForeFlight".
+	msg[1] = 0    // ID message identifier.
+	msg[2] = 1    // Message version.
+	// Serial number. Set to "invalid" for now.
+	for i := 3; i <= 10; i++ {
+		msg[i] = 0xFF
+	}
+	devShortName := "Stratux" // Temporary. Will be populated in the future with other names.
+	if len(devShortName) > 8 {
+		devShortName = devShortName[:8] // 8 chars.
+	}
+	copy(msg[11:], devShortName)
+
+	devLongName := fmt.Sprintf("%s-%s", stratuxVersion, stratuxBuild)
+	if len(devLongName) > 16 {
+		devLongName = devLongName[:16] // 16 chars.
+	}
+	copy(msg[19:], devLongName)
+
+	msg[38] = 0x01 // Capabilities mask. MSL altitude for Ownship Geometric report.
+
+	return prepareMessage(msg)
+}
+
 func makeHeartbeat() []byte {
 	msg := make([]byte, 7)
 	// See p.10.
@@ -727,6 +760,7 @@ func heartBeatSender() {
 			sendGDL90(makeHeartbeat(), false)
 			sendGDL90(makeStratuxHeartbeat(), false)
 			sendGDL90(makeStratuxStatus(), false)
+			sendGDL90(makeFFIDMessage(), false)
 			makeOwnshipReport()
 			makeOwnshipGeometricAltitudeReport()
 
diff --git a/main/gps.go b/main/gps.go
index 1f2bd49fe..326f34212 100644
--- a/main/gps.go
+++ b/main/gps.go
@@ -1849,6 +1849,71 @@ func makeFFAHRSSimReport() {
 	sendMsg([]byte(s), NETWORK_AHRS_FFSIM, false)
 }
 
+/*
+
+	ForeFlight "AHRS Message".
+
+	Sends AHRS information to ForeFlight.
+
+*/
+
+func makeFFAHRSMessage() {
+	msg := make([]byte, 12)
+	msg[0] = 0x65 // Message type "ForeFlight".
+	msg[1] = 0x01 // AHRS message identifier.
+
+	// Values if invalid
+	pitch := int16(0x7FFF)
+	roll := int16(0x7FFF)
+	hdg := uint16(0xFFFF)
+	ias := uint16(0xFFFF)
+	tas := uint16(0xFFFF)
+
+	if isAHRSValid() {
+		if !isAHRSInvalidValue(mySituation.AHRSPitch) {
+			pitch = roundToInt16(mySituation.AHRSPitch * 10)
+		}
+		if !isAHRSInvalidValue(mySituation.AHRSRoll) {
+			roll = roundToInt16(mySituation.AHRSRoll * 10)
+		}
+	}
+
+	// Roll.
+	msg[2] = byte((roll >> 8) & 0xFF)
+	msg[3] = byte(roll & 0xFF)
+
+	// Pitch.
+	msg[4] = byte((pitch >> 8) & 0xFF)
+	msg[5] = byte(pitch & 0xFF)
+
+	// Heading.
+	msg[6] = byte((hdg >> 8) & 0xFF)
+	msg[7] = byte(hdg & 0xFF)
+
+	// Indicated Airspeed.
+	msg[8] = byte((ias >> 8) & 0xFF)
+	msg[9] = byte(ias & 0xFF)
+
+	// True Airspeed.
+	msg[10] = byte((tas >> 8) & 0xFF)
+	msg[11] = byte(tas & 0xFF)
+
+	sendMsg(prepareMessage(msg), NETWORK_AHRS_GDL90, false)
+}
+
+/*
+	ffAttitudeSender()
+	 Send AHRS message in FF format every 200ms.
+*/
+
+func ffAttitudeSender() {
+	ticker := time.NewTicker(200 * time.Millisecond)
+	for {
+		<-ticker.C
+		makeFFAHRSMessage()
+	}
+}
+
 func makeAHRSGDL90Report() {
 	msg := make([]byte, 24)
 	msg[0] = 0x4c
@@ -2049,6 +2114,7 @@ func pollGPS() {
 	readyToInitGPS = true //TODO: Implement more robust method (channel control) to kill zombie serial readers
 	timer := time.NewTicker(4 * time.Second)
 	go gpsAttitudeSender()
+	go ffAttitudeSender()
 	for {
 		<-timer.C
 		// GPS enabled, was not connected previously?
diff --git a/main/sensors.go b/main/sensors.go
index caa593b9e..f3cf227aa 100644
--- a/main/sensors.go
+++ b/main/sensors.go
@@ -279,7 +279,7 @@ func sensorAttitudeSender() {
 				mySituation.AHRSPitch = pitch / ahrs.Deg
 				mySituation.AHRSGyroHeading = heading / ahrs.Deg
 
-				// TODO westphae: until magnetometer calibration is performed, no mag heading
+				//TODO westphae: until magnetometer calibration is performed, no mag heading
 				mySituation.AHRSMagHeading = ahrs.Invalid
 				mySituation.AHRSSlipSkid = s.SlipSkid()
 				mySituation.AHRSTurnRate = s.RateOfTurn()