Merge branch 'AvSquirrel-gpsattitude-june2016' into gpsattitude

main/datalog.go

@@ -474,6 +474,7 @@ func dataLog() {
 		makeTable(msg{}, "messages", db)
 		makeTable(esmsg{}, "es_messages", db)
 		makeTable(Dump1090TermMessage{}, "dump1090_terminal", db)
+		makeTable(gpsPerfStats{}, "gps_attitude", db)
 		makeTable(StratuxStartup{}, "startup", db)
 	}
 
@@ -567,6 +568,12 @@ func logESMsg(m esmsg) {
 	}
 }
 
+func logGPSAttitude(gpsPerf gpsPerfStats) {
+	if globalSettings.ReplayLog && isDataLogReady() {
+		dataLogChan <- DataLogRow{tbl: "gps_attitude", data: gpsPerf}
+	}
+}
+
 func logDump1090TermMessage(m Dump1090TermMessage) {
 	if globalSettings.DEBUG && globalSettings.ReplayLog && isDataLogReady() {
 		dataLogChan <- DataLogRow{tbl: "dump1090_terminal", data: m}

main/gen_gdl90.go

@@ -75,6 +75,15 @@ const (
 
 	LON_LAT_RESOLUTION = float32(180.0 / 8388608.0)
 	TRACK_RESOLUTION   = float32(360.0 / 256.0)
+
+	GPS_TYPE_NMEA     = 0x01
+	GPS_TYPE_UBX      = 0x02
+	GPS_TYPE_SIRF     = 0x03
+	GPS_TYPE_MEDIATEK = 0x04
+	GPS_TYPE_FLARM    = 0x05
+	GPS_TYPE_GARMIN   = 0x06
+	// other GPS types to be defined as needed
+
 )
 
 var usage *du.DiskUsage
@@ -994,6 +1003,7 @@ type settings struct {
 	OwnshipModeS         string
 	WatchList            string
 	DeveloperMode        bool
+	AHRS_GDL90_Enabled   bool
 }
 
 type status struct {
@@ -1016,6 +1026,7 @@ type status struct {
 	GPS_position_accuracy                      float32
 	GPS_connected                              bool
 	GPS_solution                               string
+	GPS_detected_type                          uint
 	RY835AI_connected                          bool
 	Uptime                                     int64
 	UptimeClock                                time.Time
@@ -1057,6 +1068,7 @@ func defaultSettings() {
 	globalSettings.ReplayLog = false //TODO: 'true' for debug builds.
 	globalSettings.OwnshipModeS = "F00000"
 	globalSettings.DeveloperMode = false
+	globalSettings.AHRS_GDL90_Enabled = false
 }
 
 func readSettings() {

main/managementinterface.go

@@ -221,6 +221,8 @@ func handleSettingsSetRequest(w http.ResponseWriter, r *http.Request) {
 						globalSettings.GPS_Enabled = val.(bool)
 					case "AHRS_Enabled":
 						globalSettings.AHRS_Enabled = val.(bool)
+					case "AHRS_GDL90_Enabled":
+						globalSettings.AHRS_GDL90_Enabled = val.(bool)
 					case "DEBUG":
 						globalSettings.DEBUG = val.(bool)
 					case "DisplayTrafficSource":

main/network.go

@@ -548,7 +548,7 @@ func networkStatsCounter() {
 /*
 	ffMonitor().
 		Watches for "i-want-to-play-ffm-udp", "i-can-play-ffm-udp", and "i-cannot-play-ffm-udp" UDP messages broadcasted on
-		 port 50113. Tags the client, issues a warning, and disables AHRS.
+		 port 50113. Tags the client, issues a warning, and disables AHRS GDL90 output.
 
 */
 
@@ -587,8 +587,8 @@ func ffMonitor() {
 		}
 		if strings.HasPrefix(s, "i-want-to-play-ffm-udp") || strings.HasPrefix(s, "i-can-play-ffm-udp") || strings.HasPrefix(s, "i-cannot-play-ffm-udp") {
 			p.FFCrippled = true
-			//FIXME: AHRS doesn't need to be disabled globally, just messages need to be filtered.
-			globalSettings.AHRS_Enabled = false
+			//FIXME: AHRS output doesn't need to be disabled globally, just on the ForeFlight client IPs.
+			globalSettings.AHRS_GDL90_Enabled = false
 			if !ff_warned {
 				e := errors.New("Stratux is not supported by your EFB app. Your EFB app is known to regularly make changes that cause compatibility issues with Stratux. See the README for a list of apps that officially support Stratux.")
 				addSystemError(e)

main/ry835ai.go

@@ -12,6 +12,7 @@ package main
 import (
 	"fmt"
 	"log"
+	"math"
 	"strconv"
 	"strings"
 	"sync"
@@ -53,8 +54,8 @@ type SatelliteInfo struct {
 }
 
 type SituationData struct {
-	mu_GPS *sync.Mutex
-
+	mu_GPS     *sync.Mutex
+	mu_GPSPerf *sync.Mutex
 	// From GPS.
 	LastFixSinceMidnightUTC  float32
 	Lat                      float32
@@ -72,6 +73,7 @@ type SituationData struct {
 	GPSVertVel               float32 // GPS vertical velocity, feet per second
 	LastFixLocalTime         time.Time
 	TrueCourse               float32
+	GPSTurnRate              float64 // calculated GPS rate of turn, degrees per second
 	GroundSpeed              uint16
 	LastGroundTrackTime      time.Time
 	GPSTime                  time.Time
@@ -93,6 +95,26 @@ type SituationData struct {
 	LastAttitudeTime time.Time
 }
 
+/*
+myGPSPerfStats used to track short-term position / velocity trends, used to feed dynamic AHRS model. Use floats for better resolution of calculated data.
+*/
+type gpsPerfStats struct {
+	stratuxTime   uint64  // time since Stratux start, msec
+	nmeaTime      float32 // timestamp from NMEA message
+	msgType       string  // NMEA message type
+	gsf           float32 // knots
+	coursef       float32 // true course [degrees]
+	alt           float32 // gps altitude, ft msl
+	vv            float32 // vertical velocity, ft/sec
+	gpsTurnRate   float64 // calculated turn rate, deg/sec. Right turn is positive.
+	gpsPitch      float64 // estimated pitch angle, deg. Calculated from gps ground speed and VV. Equal to flight path angle.
+	gpsRoll       float64 // estimated roll angle from turn rate and groundspeed, deg. Assumes airplane in coordinated turns.
+	gpsLoadFactor float64 // estimated load factor from turn rate and groundspeed, "gee". Assumes airplane in coordinated turns.
+}
+
+var gpsPerf gpsPerfStats
+var myGPSPerfStats []gpsPerfStats
+
 var serialConfig *serial.Config
 var serialPort *serial.Port
 
@@ -117,6 +139,13 @@ p.109 CFG-NAV5 (0x06 0x24)
 Poll Navigation Engine Settings
 */
 
+/*
+	chksumUBX()
+		returns the two-byte Fletcher algorithm checksum of byte array msg.
+		This is used in configuration messages for the u-blox GPS. See p. 97 of the
+		u-blox M8 Receiver Description.
+*/
+
 func chksumUBX(msg []byte) []byte {
 	ret := make([]byte, 2)
 	for i := 0; i < len(msg); i++ {
@@ -126,7 +155,12 @@ func chksumUBX(msg []byte) []byte {
 	return ret
 }
 
-// p.62
+/*
+	makeUBXCFG()
+		creates a UBX-formatted package consisting of two sync characters,
+		class, ID, payload length in bytes (2-byte little endian), payload, and checksum.
+		See p. 95 of the u-blox M8 Receiver Description.
+*/
 func makeUBXCFG(class, id byte, msglen uint16, msg []byte) []byte {
 	ret := make([]byte, 6)
 	ret[0] = 0xB5
@@ -154,6 +188,7 @@ func initGPSSerial() bool {
 	var device string
 	baudrate := int(9600)
 	isSirfIV := bool(false)
+	globalStatus.GPS_detected_type = 0 // reset detected type on each initialization
 
 	if _, err := os.Stat("/dev/ublox8"); err == nil { // u-blox 8 (RY83xAI over USB).
 		device = "/dev/ublox8"
@@ -436,6 +471,362 @@ func setTrueCourse(groundSpeed uint16, trueCourse float64) {
 	}
 }
 
+/*
+calcGPSAttitude estimates turn rate, pitch, and roll based on recent GPS groundspeed, track, and altitude / vertical speed.
+
+Method uses stored performance statistics from myGPSPerfStats[]. Ideally, calculation is based on most recent 1.5 seconds of data,
+assuming 10 Hz sampling frequency. Lower frequency sample rates will increase calculation window for smoother response, at the
+cost of slightly increased lag.
+
+(c) 2016 AvSquirrel (https://github.com/AvSquirrel) . All rights reserved.
+Distributable under the terms of the "BSD-New" License that can be found in
+the LICENSE file, herein included as part of this header.
+*/
+
+func calcGPSAttitude() bool {
+	// check slice length. Return error if empty set or set zero values
+	mySituation.mu_GPSPerf.Lock()
+	defer mySituation.mu_GPSPerf.Unlock()
+	length := len(myGPSPerfStats)
+	index := length - 1
+
+	if length == 0 {
+		log.Printf("GPS attitude: No data received yet. Not calculating attitude.\n")
+		return false
+	} else if length == 1 {
+		//log.Printf("myGPSPerfStats has one data point. Setting statistics to zero.\n")
+		myGPSPerfStats[index].gpsTurnRate = 0
+		myGPSPerfStats[index].gpsPitch = 0
+		myGPSPerfStats[index].gpsRoll = 0
+		return false
+	}
+
+	// check if GPS data was put in the structure more than three seconds ago -- this shouldn't happen unless something is wrong.
+	if (stratuxClock.Milliseconds - myGPSPerfStats[index].stratuxTime) > 3000 {
+		myGPSPerfStats[index].gpsTurnRate = 0
+		myGPSPerfStats[index].gpsPitch = 0
+		myGPSPerfStats[index].gpsRoll = 0
+		log.Printf("GPS attitude: GPS data is more than three seconds old. Setting attitude to zero.\n")
+		return false
+	}
+
+	// check time interval between samples
+	t1 := myGPSPerfStats[index].nmeaTime
+	t0 := myGPSPerfStats[index-1].nmeaTime
+	dt := t1 - t0
+
+	// first time error case: index is more than three seconds ahead of index-1
+	if dt > 3 {
+		log.Printf("GPS attitude: Can't calculate GPS attitude. Reference data is old. dt = %v\n", dt)
+		return false
+	}
+
+	// second case: index is behind index-1. This could be result of day rollover. If time is within n seconds of UTC,
+	// we rebase to the previous day, and will re-rebase the entire slice forward to the current day once all values roll over.
+	// TO-DO: Validate by testing at 0000Z
+	if dt < 0 {
+		log.Printf("GPS attitude: Current GPS time (%.2f) is older than last GPS time (%.2f). Checking for 0000Z rollover.\n", t1, t0)
+		if myGPSPerfStats[index-1].nmeaTime > 86300 && myGPSPerfStats[index].nmeaTime < 100 { // be generous with the time window at rollover
+			myGPSPerfStats[index].nmeaTime += 86400
+		} else {
+			// time decreased, but not due to a recent rollover. Something odd is going on.
+			log.Printf("GPS attitude: Time isn't near 0000Z. Unknown reason for offset. Can't calculate GPS attitude.\n")
+			return false
+		}
+
+		// check time array to see if all timestamps are > 86401 seconds since midnight
+		var tempTime []float64
+		tempTime = make([]float64, length, length)
+		for i := 0; i < length; i++ {
+			tempTime[i] = float64(myGPSPerfStats[i].nmeaTime)
+		}
+		minTime, _ := arrayMin(tempTime)
+		if minTime > 86401.0 {
+			log.Printf("GPS attitude: Rebasing GPS time since midnight to current day.\n")
+			for i := 0; i < length; i++ {
+				myGPSPerfStats[i].nmeaTime -= 86400
+			}
+		}
+
+		// Verify adjustment
+		dt = myGPSPerfStats[index].nmeaTime - myGPSPerfStats[index-1].nmeaTime
+		log.Printf("GPS attitude: New dt = %f\n", dt)
+		if dt > 3 {
+			log.Printf("GPS attitude: Can't calculate GPS attitude. Reference data is old. dt = %v\n", dt)
+			return false
+		} else if dt < 0 {
+			log.Printf("GPS attitude: Something went wrong rebasing the time.\n")
+			return false
+		}
+
+	}
+
+	// If all of the bounds checks pass, begin processing the GPS data.
+
+	// local variables
+	var headingAvg, dh, v_x, v_z, a_c, omega, slope, intercept, dt_avg float64
+	var tempHdg, tempHdgUnwrapped, tempHdgTime, tempSpeed, tempVV, tempSpeedTime, tempRegWeights []float64 // temporary arrays for regression calculation
+	var valid bool
+	var lengthHeading, lengthSpeed int
+
+	center := float64(myGPSPerfStats[index].nmeaTime) // current time for calculating regression weights
+	halfwidth := float64(1.4)                         // width of regression evaluation window. Default of 1.4 seconds for 5 Hz sampling; can increase to 3.5 sec @ 1 Hz
+
+	// frequency detection
+	tempSpeedTime = make([]float64, 0)
+	for i := 1; i < length; i++ {
+		dt = myGPSPerfStats[i].nmeaTime - myGPSPerfStats[i-1].nmeaTime
+		if dt > 0.05 { // avoid double counting messages with same / similar timestamps
+			tempSpeedTime = append(tempSpeedTime, float64(dt))
+		}
+	}
+	//log.Printf("Delta time array is %v.\n",tempSpeedTime)
+	dt_avg, valid = mean(tempSpeedTime)
+	if valid && dt_avg > 0 {
+		if globalSettings.DEBUG {
+			log.Printf("GPS attitude: Average delta time is %.2f s (%.1f Hz)\n", dt_avg, 1/dt_avg)
+		}
+		halfwidth = 7 * dt_avg
+	} else {
+		if globalSettings.DEBUG {
+			log.Printf("GPS attitude: Couldn't determine sample rate\n")
+		}
+		halfwidth = 3.5
+	}
+
+	if halfwidth > 3.5 {
+		halfwidth = 3.5 // limit calculation window to 3.5 seconds of data for 1 Hz or slower samples
+	}
+
+	if globalStatus.GPS_detected_type == GPS_TYPE_UBX { // UBX reports vertical speed, so we can just walk through all of the PUBX messages in order
+		// Speed and VV. Use all values in myGPSPerfStats; perform regression.
+		tempSpeedTime = make([]float64, length, length) // all are length of original slice
+		tempSpeed = make([]float64, length, length)
+		tempVV = make([]float64, length, length)
+		tempRegWeights = make([]float64, length, length)
+
+		for i := 0; i < length; i++ {
+			tempSpeed[i] = float64(myGPSPerfStats[i].gsf)
+			tempVV[i] = float64(myGPSPerfStats[i].vv)
+			tempSpeedTime[i] = float64(myGPSPerfStats[i].nmeaTime)
+			tempRegWeights[i] = triCubeWeight(center, halfwidth, tempSpeedTime[i])
+		}
+
+		// Groundspeed regression estimate.
+		slope, intercept, valid = linRegWeighted(tempSpeedTime, tempSpeed, tempRegWeights)
+		if !valid {
+			log.Printf("GPS attitude: Error calculating speed regression from UBX position messages")
+			return false
+		} else {
+			v_x = (slope*float64(myGPSPerfStats[index].nmeaTime) + intercept) * 1.687810 // units are knots, converted to feet/sec
+		}
+
+		// Vertical speed regression estimate.
+		slope, intercept, valid = linRegWeighted(tempSpeedTime, tempVV, tempRegWeights)
+		if !valid {
+			log.Printf("GPS attitude: Error calculating vertical speed regression from UBX position messages")
+			return false
+		} else {
+			v_z = (slope*float64(myGPSPerfStats[index].nmeaTime) + intercept) // units are feet per sec; no conversion needed
+		}
+
+	} else { // If we need to parse standard NMEA messages, determine if it's RMC or GGA, then fill the temporary slices accordingly. Need to pull from multiple message types since GGA doesn't do course or speed; VTG / RMC don't do altitude, etc. Grrr.
+
+		//v_x = float64(myGPSPerfStats[index].gsf * 1.687810)
+		//v_z = 0
+
+		// first, parse groundspeed from RMC messages.
+		tempSpeedTime = make([]float64, 0)
+		tempSpeed = make([]float64, 0)
+		tempRegWeights = make([]float64, 0)
+
+		for i := 0; i < length; i++ {
+			if myGPSPerfStats[i].msgType == "GPRMC" || myGPSPerfStats[i].msgType == "GNRMC" {
+				tempSpeed = append(tempSpeed, float64(myGPSPerfStats[i].gsf))
+				tempSpeedTime = append(tempSpeedTime, float64(myGPSPerfStats[i].nmeaTime))
+				tempRegWeights = append(tempRegWeights, triCubeWeight(center, halfwidth, float64(myGPSPerfStats[i].nmeaTime)))
+			}
+		}
+		lengthSpeed = len(tempSpeed)
+		if lengthSpeed == 0 {
+			log.Printf("GPS Attitude: No groundspeed data could be parsed from NMEA RMC messages\n")
+			return false
+		} else if lengthSpeed == 1 {
+			v_x = tempSpeed[0] * 1.687810
+		} else {
+			slope, intercept, valid = linRegWeighted(tempSpeedTime, tempSpeed, tempRegWeights)
+			if !valid {
+				log.Printf("GPS attitude: Error calculating speed regression from NMEA RMC position messages")
+				return false
+			} else {
+				v_x = (slope*float64(myGPSPerfStats[index].nmeaTime) + intercept) * 1.687810 // units are knots, converted to feet/sec
+				//log.Printf("Avg speed %f calculated from %d RMC messages\n", v_x, lengthSpeed) // DEBUG
+			}
+		}
+
+		// next, calculate vertical velocity from GGA altitude data.
+		tempSpeedTime = make([]float64, 0)
+		tempVV = make([]float64, 0)
+		tempRegWeights = make([]float64, 0)
+
+		for i := 0; i < length; i++ {
+			if myGPSPerfStats[i].msgType == "GPGGA" || myGPSPerfStats[i].msgType == "GNGGA" {
+				tempVV = append(tempVV, float64(myGPSPerfStats[i].alt))
+				tempSpeedTime = append(tempSpeedTime, float64(myGPSPerfStats[i].nmeaTime))
+				tempRegWeights = append(tempRegWeights, triCubeWeight(center, halfwidth, float64(myGPSPerfStats[i].nmeaTime)))
+			}
+		}
+		lengthSpeed = len(tempVV)
+		if lengthSpeed < 2 {
+			log.Printf("GPS Attitude: Not enough points to calculate vertical speed from NMEA GGA messages\n")
+			return false
+		} else {
+			slope, _, valid = linRegWeighted(tempSpeedTime, tempVV, tempRegWeights)
+			if !valid {
+				log.Printf("GPS attitude: Error calculating vertical speed regression from NMEA GGA messages")
+				return false
+			} else {
+				v_z = slope // units are feet/sec
+				//log.Printf("Avg VV %f calculated from %d GGA messages\n", v_z, lengthSpeed) // DEBUG
+			}
+		}
+
+	}
+
+	// If we're going too slow for processNMEALine() to give us valid heading data, there's no sense in trying to parse it.
+	// However, we need to return a valid level attitude so we don't get the "red X of death" on our AHRS display.
+	// This will also eliminate most of the nuisance error message from the turn rate calculation.
+	if v_x < 6 { // ~3.55 knots
+
+		myGPSPerfStats[index].gpsPitch = 0
+		myGPSPerfStats[index].gpsRoll = 0
+		myGPSPerfStats[index].gpsTurnRate = 0
+		myGPSPerfStats[index].gpsLoadFactor = 1.0
+		mySituation.GPSTurnRate = 0
+
+		// Output format:GPSAtttiude,seconds,nmeaTime,msg_type,GS,Course,Alt,VV,filtered_GS,filtered_course,turn rate,filtered_vv,pitch, roll,load_factor
+		buf := fmt.Sprintf("GPSAttitude,%.1f,%.2f,%s,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f\n", float64(stratuxClock.Milliseconds)/1000, myGPSPerfStats[index].nmeaTime, myGPSPerfStats[index].msgType, myGPSPerfStats[index].gsf, myGPSPerfStats[index].coursef, myGPSPerfStats[index].alt, myGPSPerfStats[index].vv, v_x/1.687810, headingAvg, myGPSPerfStats[index].gpsTurnRate, v_z, myGPSPerfStats[index].gpsPitch, myGPSPerfStats[index].gpsRoll, myGPSPerfStats[index].gpsLoadFactor)
+		if globalSettings.DEBUG {
+			log.Printf("%s", buf) // FIXME. Send to sqlite log or other file?
+		}
+		logGPSAttitude(myGPSPerfStats[index])
+		//replayLog(buf, MSGCLASS_AHRS)
+
+		return true
+	}
+
+	// Heading.  Same method used for UBX and generic.
+	// First, walk through the PerfStats and parse only valid heading data.
+	//log.Printf("Raw heading data:")
+	for i := 0; i < length; i++ {
+		//log.Printf("%.1f,",myGPSPerfStats[i].coursef)
+		if myGPSPerfStats[i].coursef >= 0 { // negative values are used to flag invalid / unavailable course
+			tempHdg = append(tempHdg, float64(myGPSPerfStats[i].coursef))
+			tempHdgTime = append(tempHdgTime, float64(myGPSPerfStats[i].nmeaTime))
+		}
+	}
+	//log.Printf("\n")
+	//log.Printf("tempHdg: %v\n", tempHdg)
+
+	// Next, unwrap the heading so we don't mess up the regression by fitting a line across the 0/360 deg discontinutiy
+	lengthHeading = len(tempHdg)
+	tempHdgUnwrapped = make([]float64, lengthHeading, lengthHeading)
+	tempRegWeights = make([]float64, lengthHeading, lengthHeading)
+
+	if lengthHeading > 1 {
+		tempHdgUnwrapped[0] = tempHdg[0]
+		tempRegWeights[0] = triCubeWeight(center, halfwidth, tempHdgTime[0])
+		for i := 1; i < lengthHeading; i++ {
+			tempRegWeights[i] = triCubeWeight(center, halfwidth, tempHdgTime[i])
+			if math.Abs(tempHdg[i]-tempHdg[i-1]) < 180 { // case 1: if angle change is less than 180 degrees, use the same reference system
+				tempHdgUnwrapped[i] = tempHdgUnwrapped[i-1] + tempHdg[i] - tempHdg[i-1]
+			} else if tempHdg[i] > tempHdg[i-1] { // case 2: heading has wrapped around from NE to NW. Subtract 360 to keep consistent with previous data.
+				tempHdgUnwrapped[i] = tempHdgUnwrapped[i-1] + tempHdg[i] - tempHdg[i-1] - 360
+			} else { // case 3:  heading has wrapped around from NW to NE. Add 360 to keep consistent with previous data.
+				tempHdgUnwrapped[i] = tempHdgUnwrapped[i-1] + tempHdg[i] - tempHdg[i-1] + 360
+			}
+		}
+	} else { //
+		if globalSettings.DEBUG {
+			log.Printf("GPS attitude: Can't calculate turn rate with less than two points.\n")
+		}
+		return false
+	}
+
+	// Finally, calculate turn rate as the slope of the weighted linear regression of unwrapped heading.
+	slope, intercept, valid = linRegWeighted(tempHdgTime, tempHdgUnwrapped, tempRegWeights)
+
+	if !valid {
+		log.Printf("GPS attitude: Regression error calculating turn rate")
+		return false
+	} else {
+		headingAvg = slope*float64(myGPSPerfStats[index].nmeaTime) + intercept
+		dh = slope // units are deg per sec; no conversion needed here
+		//log.Printf("Calculated heading and turn rate: %.3f degrees, %.3f deg/sec\n",headingAvg,dh)
+	}
+
+	myGPSPerfStats[index].gpsTurnRate = dh
+	mySituation.GPSTurnRate = dh
+
+	// pitch angle -- or to be more pedantic, glide / climb angle, since we're just looking a rise-over-run.
+	// roll angle, based on turn rate and ground speed. Only valid for coordinated flight. Differences between airspeed and groundspeed will trip this up.
+	if v_x > 20 { // reduce nuisance 'bounce' at low speeds. 20 ft/sec = 11.9 knots.
+		myGPSPerfStats[index].gpsPitch = math.Atan2(v_z, v_x) * 180.0 / math.Pi
+
+		/*
+			Governing equations for roll calculations
+
+			Physics tells us that
+				a_z = g     (in steady-state flight -- climbing, descending, or level -- this is gravity. 9.81 m/s^2 or 32.2 ft/s^2)
+				a_c = v^2/r (centripetal acceleration)
+
+			We don't know r. However, we do know the tangential velocity (v) and angular velocity (omega). Express omega in radians per unit time, and
+
+				v = omega*r
+
+			By substituting and rearranging terms:
+
+				a_c = v^2 / (v / omega)
+				a_c = v*omega
+
+			Free body diagram time!
+
+				   /|
+			  a_r / |  a_z
+				 /__|
+			   X   a_c
+				\_________________ [For the purpose of this comment, " X" is an airplane in a 20 degree bank. Use your imagination, mkay?)
+
+			Resultant acceleration a_r is what the wings feel; a_r/a_z = load factor. Anyway, trig out the bank angle:
+
+				bank angle = atan(a_c/a_z)
+						   = atan(v*omega/g)
+
+				wing loading = sqrt(a_c^2 + a_z^2) / g
+
+		*/
+
+		g := 32.174                                        // ft-s^-2
+		omega = radians(myGPSPerfStats[index].gpsTurnRate) // need radians/sec
+		a_c = v_x * omega
+		myGPSPerfStats[index].gpsRoll = math.Atan2(a_c, g) * 180 / math.Pi // output is degrees
+		myGPSPerfStats[index].gpsLoadFactor = math.Sqrt(a_c*a_c+g*g) / g
+	} else {
+		myGPSPerfStats[index].gpsPitch = 0
+		myGPSPerfStats[index].gpsRoll = 0
+		myGPSPerfStats[index].gpsLoadFactor = 1
+	}
+
+	// Output format:GPSAtttiude,seconds,nmeaTime,msg_type,GS,Course,Alt,VV,filtered_GS,filtered_course,turn rate,filtered_vv,pitch, roll,load_factor
+	buf := fmt.Sprintf("GPSAttitude,%.1f,%.2f,%s,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f,%0.3f\n", float64(stratuxClock.Milliseconds)/1000, myGPSPerfStats[index].nmeaTime, myGPSPerfStats[index].msgType, myGPSPerfStats[index].gsf, myGPSPerfStats[index].coursef, myGPSPerfStats[index].alt, myGPSPerfStats[index].vv, v_x/1.687810, headingAvg, myGPSPerfStats[index].gpsTurnRate, v_z, myGPSPerfStats[index].gpsPitch, myGPSPerfStats[index].gpsRoll, myGPSPerfStats[index].gpsLoadFactor)
+	if globalSettings.DEBUG {
+		log.Printf("%s", buf) // FIXME. Send to sqlite log or other file?
+	}
+	logGPSAttitude(myGPSPerfStats[index])
+	//replayLog(buf, MSGCLASS_AHRS)
+	return true
+}
+
 func calculateNACp(accuracy float32) uint8 {
 	ret := uint8(0)
 
@@ -477,6 +868,12 @@ func processNMEALine(l string) (sentenceUsed bool) {
 		mySituation.mu_GPS.Unlock()
 	}()
 
+	// Local variables for GPS attitude estimation
+	thisGpsPerf := gpsPerf                              // write to myGPSPerfStats at end of function IFF
+	thisGpsPerf.coursef = -999.9                        // default value of -999.9 indicates invalid heading to regression calculation
+	thisGpsPerf.stratuxTime = stratuxClock.Milliseconds // used for gross indexing
+	updateGPSPerf := false                              // change to true when position or vector info is read
+
 	l_valid, validNMEAcs := validateNMEAChecksum(l)
 	if !validNMEAcs {
 		log.Printf("GPS error. Invalid NMEA string: %s\n", l_valid) // remove log message once validation complete
@@ -493,6 +890,15 @@ func processNMEALine(l string) (sentenceUsed bool) {
 				return false
 			}
 
+			// set the global GPS type to UBX as soon as we see our first (valid length)
+			// PUBX,01 position message, even if we don't have a fix
+			if globalStatus.GPS_detected_type != GPS_TYPE_UBX {
+				globalStatus.GPS_detected_type = GPS_TYPE_UBX
+				log.Printf("GPS detected: u-blox NMEA position message seen.\n")
+			}
+
+			thisGpsPerf.msgType = x[0] + x[1]
+
 			tmpSituation := mySituation // If we decide to not use the data in this message, then don't make incomplete changes in mySituation.
 
 			// Do the accuracy / quality fields first to prevent invalid position etc. from being sent downstream
@@ -541,6 +947,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
 			}
 
 			tmpSituation.LastFixSinceMidnightUTC = float32(3600*hr+60*min) + float32(sec)
+			thisGpsPerf.nmeaTime = tmpSituation.LastFixSinceMidnightUTC
 
 			// field 3-4 = lat
 			if len(x[3]) < 10 {
@@ -591,6 +998,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
 				alt := float32(hae*3.28084) - tmpSituation.GeoidSep        // convert to feet and offset by geoid separation
 				tmpSituation.HeightAboveEllipsoid = float32(hae * 3.28084) // feet
 				tmpSituation.Alt = alt
+				thisGpsPerf.alt = alt
 			}
 
 			tmpSituation.LastFixLocalTime = stratuxClock.Time
@@ -602,6 +1010,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
 			}
 			groundspeed = groundspeed * 0.540003 // convert to knots
 			tmpSituation.GroundSpeed = uint16(groundspeed)
+			thisGpsPerf.gsf = float32(groundspeed)
 
 			// field 12 = track, deg
 			trueCourse := float32(0.0)
@@ -613,7 +1022,9 @@ func processNMEALine(l string) (sentenceUsed bool) {
 				trueCourse = float32(tc)
 				setTrueCourse(uint16(groundspeed), tc)
 				tmpSituation.TrueCourse = trueCourse
+				thisGpsPerf.coursef = float32(tc)
 			} else {
+				thisGpsPerf.coursef = -999.9 // regression will skip negative values
 				// Negligible movement. Don't update course, but do use the slow speed.
 				// TO-DO: use average course over last n seconds?
 			}
@@ -625,6 +1036,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
 				return false
 			}
 			tmpSituation.GPSVertVel = float32(vv * -3.28084) // convert to ft/sec and positive = up
+			thisGpsPerf.vv = tmpSituation.GPSVertVel
 
 			// field 14 = age of diff corrections
 
@@ -637,6 +1049,19 @@ func processNMEALine(l string) (sentenceUsed bool) {
 
 			// We've made it this far, so that means we've processed "everything" and can now make the change to mySituation.
 			mySituation = tmpSituation
+			updateGPSPerf = true
+			if updateGPSPerf {
+				mySituation.mu_GPSPerf.Lock()
+				myGPSPerfStats = append(myGPSPerfStats, thisGpsPerf)
+				lenGPSPerfStats := len(myGPSPerfStats)
+				//	log.Printf("GPSPerf array has %n elements. Contents are: %v\n",lenGPSPerfStats,myGPSPerfStats)
+				if lenGPSPerfStats > 299 { //30 seconds @ 10 Hz for UBX, 30 seconds @ 5 Hz for MTK or SIRF with 2x messages per 200 ms)
+					myGPSPerfStats = myGPSPerfStats[(lenGPSPerfStats - 299):] // remove the first n entries if more than 300 in the slice
+				}
+
+				mySituation.mu_GPSPerf.Unlock()
+			}
+
 			return true
 		} else if x[1] == "03" { // satellite status message. Only the first 20 satellites will be reported in this message for UBX firmware older than v3.0. Order seems to be GPS, then SBAS, then GLONASS.
 
@@ -869,6 +1294,11 @@ func processNMEALine(l string) (sentenceUsed bool) {
 			return false
 		}
 
+		// use RMC / GGA message detection to sense "NMEA" type.
+		if globalStatus.GPS_detected_type == 0 {
+			globalStatus.GPS_detected_type = GPS_TYPE_NMEA
+		}
+
 		// Quality indicator.
 		q, err1 := strconv.Atoi(x[6])
 		if err1 != nil {
@@ -888,6 +1318,9 @@ func processNMEALine(l string) (sentenceUsed bool) {
 		}
 
 		tmpSituation.LastFixSinceMidnightUTC = float32(3600*hr+60*min) + float32(sec)
+		if globalStatus.GPS_detected_type != GPS_TYPE_UBX {
+			thisGpsPerf.nmeaTime = tmpSituation.LastFixSinceMidnightUTC
+		}
 
 		// Latitude.
 		if len(x[2]) < 4 {
@@ -926,6 +1359,9 @@ func processNMEALine(l string) (sentenceUsed bool) {
 			return false
 		}
 		tmpSituation.Alt = float32(alt * 3.28084) // Convert to feet.
+		if globalStatus.GPS_detected_type != GPS_TYPE_UBX {
+			thisGpsPerf.alt = float32(tmpSituation.Alt)
+		}
 
 		// Geoid separation (Sep = HAE - MSL)
 		// (needed for proper MSL offset on PUBX,00 altitudes)
@@ -940,11 +1376,28 @@ func processNMEALine(l string) (sentenceUsed bool) {
 		// Timestamp.
 		tmpSituation.LastFixLocalTime = stratuxClock.Time
 
+		if globalStatus.GPS_detected_type != GPS_TYPE_UBX {
+			updateGPSPerf = true
+			thisGpsPerf.msgType = x[0]
+		}
+
 		// We've made it this far, so that means we've processed "everything" and can now make the change to mySituation.
 		mySituation = tmpSituation
+
+		if updateGPSPerf {
+			mySituation.mu_GPSPerf.Lock()
+			myGPSPerfStats = append(myGPSPerfStats, thisGpsPerf)
+			lenGPSPerfStats := len(myGPSPerfStats)
+			//	log.Printf("GPSPerf array has %n elements. Contents are: %v\n",lenGPSPerfStats,myGPSPerfStats)
+			if lenGPSPerfStats > 299 { //30 seconds @ 10 Hz for UBX, 30 seconds @ 5 Hz for MTK or SIRF with 2x messages per 200 ms)
+				myGPSPerfStats = myGPSPerfStats[(lenGPSPerfStats - 299):] // remove the first n entries if more than 300 in the slice
+			}
+			mySituation.mu_GPSPerf.Unlock()
+		}
+
 		return true
 
-	} else if (x[0] == "GNRMC") || (x[0] == "GPRMC") { // Recommended Minimum data. FIXME: Is this needed anymore?
+	} else if (x[0] == "GNRMC") || (x[0] == "GPRMC") { // Recommended Minimum data.
 		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
@@ -966,6 +1419,11 @@ func processNMEALine(l string) (sentenceUsed bool) {
 			return false
 		}
 
+		// use RMC / GGA message detection to sense "NMEA" type.
+		if globalStatus.GPS_detected_type == 0 {
+			globalStatus.GPS_detected_type = GPS_TYPE_NMEA
+		}
+
 		if x[2] != "A" { // invalid fix
 			tmpSituation.Quality = 0 // Just a note.
 			return false
@@ -982,6 +1440,9 @@ func processNMEALine(l string) (sentenceUsed bool) {
 			return false
 		}
 		tmpSituation.LastFixSinceMidnightUTC = float32(3600*hr+60*min) + float32(sec)
+		if globalStatus.GPS_detected_type != GPS_TYPE_UBX {
+			thisGpsPerf.nmeaTime = tmpSituation.LastFixSinceMidnightUTC
+		}
 
 		if len(x[9]) == 6 {
 			// Date of Fix, i.e 191115 =  19 November 2015 UTC  field 9
@@ -1038,26 +1499,50 @@ func processNMEALine(l string) (sentenceUsed bool) {
 			return false
 		}
 		tmpSituation.GroundSpeed = uint16(groundspeed)
+		if globalStatus.GPS_detected_type != GPS_TYPE_UBX {
+			thisGpsPerf.gsf = float32(groundspeed)
+		}
 
 		// ground track "True" (field 8)
 		trueCourse := float32(0)
 		tc, err := strconv.ParseFloat(x[8], 32)
-		if err != nil {
+		if err != nil && groundspeed > 3 { // some receivers return null COG at low speeds. Need to ignore this condition.
 			return false
 		}
 		if groundspeed > 3 { // TO-DO: use average groundspeed over last n seconds to avoid random "jumps"
 			trueCourse = float32(tc)
 			setTrueCourse(uint16(groundspeed), tc)
 			tmpSituation.TrueCourse = trueCourse
+			if globalStatus.GPS_detected_type != GPS_TYPE_UBX {
+				thisGpsPerf.coursef = float32(tc)
+			}
 		} else {
+			if globalStatus.GPS_detected_type != GPS_TYPE_UBX {
+				thisGpsPerf.coursef = -999.9
+			}
 			// Negligible movement. Don't update course, but do use the slow speed.
 			// TO-DO: use average course over last n seconds?
 		}
-
+		if globalStatus.GPS_detected_type != GPS_TYPE_UBX {
+			updateGPSPerf = true
+			thisGpsPerf.msgType = x[0]
+		}
 		tmpSituation.LastGroundTrackTime = stratuxClock.Time
 
 		// We've made it this far, so that means we've processed "everything" and can now make the change to mySituation.
 		mySituation = tmpSituation
+
+		if updateGPSPerf {
+			mySituation.mu_GPSPerf.Lock()
+			myGPSPerfStats = append(myGPSPerfStats, thisGpsPerf)
+			lenGPSPerfStats := len(myGPSPerfStats)
+			//	log.Printf("GPSPerf array has %n elements. Contents are: %v\n",lenGPSPerfStats,myGPSPerfStats)
+			if lenGPSPerfStats > 299 { //30 seconds @ 10 Hz for UBX, 30 seconds @ 5 Hz for MTK or SIRF with 2x messages per 200 ms)
+				myGPSPerfStats = myGPSPerfStats[(lenGPSPerfStats - 299):] // remove the first n entries if more than 300 in the slice
+			}
+			mySituation.mu_GPSPerf.Unlock()
+		}
+
 		setDataLogTimeWithGPS(mySituation)
 		return true
 
@@ -1452,6 +1937,43 @@ func makeAHRSGDL90Report() {
 	sendMsg(prepareMessage(msg), NETWORK_AHRS_GDL90, false)
 }
 
+func gpsAttitudeSender() {
+	timer := time.NewTicker(100 * time.Millisecond) // ~10Hz update.
+	for {
+		<-timer.C
+		myGPSPerfStats = make([]gpsPerfStats, 0) // reinitialize statistics on disconnect / reconnect
+		for globalSettings.GPS_Enabled && globalStatus.GPS_connected /*&& globalSettings.GPSAttitude_Enabled*/ && !(globalSettings.AHRS_Enabled) {
+			<-timer.C
+
+			if mySituation.Quality == 0 || !calcGPSAttitude() {
+				if globalSettings.DEBUG {
+					log.Printf("Could'nt calculate GPS-based attitude statistics\n")
+				}
+			} else {
+				mySituation.mu_GPSPerf.Lock()
+				index := len(myGPSPerfStats) - 1
+				if index > 1 {
+					mySituation.Pitch = myGPSPerfStats[index].gpsPitch
+					mySituation.Roll = myGPSPerfStats[index].gpsRoll
+					mySituation.Gyro_heading = float64(mySituation.TrueCourse)
+					mySituation.LastAttitudeTime = stratuxClock.Time
+					//if globalSettings.ForeFlightSimMode == true {
+					//	globalSettings.AHRS_GDL90_Enabled = false // both can't be simultaneously active
+					//	makeFFAHRSSimReport()
+					//} else if globalSettings.AHRS_GDL90_Enabled == true {
+					//globalSettings.ForeFlightSimMode = false // both can't be simultaneoussly active
+					//makeAHRSGDL90Report()
+					//}
+
+					if globalSettings.AHRS_GDL90_Enabled == true {
+						makeAHRSGDL90Report()
+					}
+				}
+				mySituation.mu_GPSPerf.Unlock()
+			}
+		}
+	}
+}
 func attitudeReaderSender() {
 	timer := time.NewTicker(100 * time.Millisecond) // ~10Hz update.
 	for globalStatus.RY835AI_connected && globalSettings.AHRS_Enabled {
@@ -1476,7 +1998,9 @@ func attitudeReaderSender() {
 		//		if isGPSGroundTrackValid(), etc.
 
 		// makeFFAHRSSimReport() // simultaneous use of GDL90 and FFSIM not supported in FF 7.5.1 or later. Function definition will be kept for AHRS debugging and future workarounds.
-		makeAHRSGDL90Report()
+		if globalSettings.AHRS_GDL90_Enabled == true {
+			makeAHRSGDL90Report()
+		}
 
 		mySituation.mu_Attitude.Unlock()
 	}
@@ -1577,6 +2101,7 @@ func initAHRS() error {
 func pollRY835AI() {
 	readyToInitGPS = true //TO-DO: Implement more robust method (channel control) to kill zombie serial readers
 	timer := time.NewTicker(4 * time.Second)
+	go gpsAttitudeSender()
 	for {
 		<-timer.C
 		// GPS enabled, was not connected previously?
@@ -1599,6 +2124,7 @@ func pollRY835AI() {
 
 func initRY835AI() {
 	mySituation.mu_GPS = &sync.Mutex{}
+	mySituation.mu_GPSPerf = &sync.Mutex{}
 	mySituation.mu_Attitude = &sync.Mutex{}
 	satelliteMutex = &sync.Mutex{}
 	Satellites = make(map[string]SatelliteInfo)

web/plates/js/settings.js

@@ -6,7 +6,7 @@ function SettingsCtrl($rootScope, $scope, $state, $location, $window, $http) {
 
 	$scope.$parent.helppage = 'plates/settings-help.html';
 
-	var toggles = ['UAT_Enabled', 'ES_Enabled', 'Ping_Enabled', 'GPS_Enabled', 'AHRS_Enabled', 'DisplayTrafficSource', 'DEBUG', 'ReplayLog']; 
+	var toggles = ['UAT_Enabled', 'ES_Enabled', 'Ping_Enabled', 'GPS_Enabled', 'AHRS_GDL90_Enabled', 'AHRS_Enabled', 'DisplayTrafficSource', 'DEBUG', 'ReplayLog']; 
 	var settings = {};
 	for (i = 0; i < toggles.length; i++) {
 		settings[toggles[i]] = undefined;
@@ -27,6 +27,7 @@ function SettingsCtrl($rootScope, $scope, $state, $location, $window, $http) {
 		$scope.Ping_Enabled = settings.Ping_Enabled;
 		$scope.GPS_Enabled = settings.GPS_Enabled;
 		$scope.AHRS_Enabled = settings.AHRS_Enabled;
+		$scope.AHRS_GDL90_Enabled = settings.AHRS_GDL90_Enabled;
 		$scope.DisplayTrafficSource = settings.DisplayTrafficSource;
 		$scope.DEBUG = settings.DEBUG;
 		$scope.ReplayLog = settings.ReplayLog;

web/plates/settings.html

@@ -29,7 +29,7 @@
 					</div>
 				</div>
 				<div class="form-group">
-					<label class="control-label col-xs-7">AHRS</label>
+					<label class="control-label col-xs-7"><a ui-turn-on="modalAHRSHardwareInfo">AHRS Module</a></label>
 					<div class="col-xs-5">
 						<ui-switch ng-model='AHRS_Enabled' settings-change></ui-switch>
 					</div>
@@ -70,6 +70,12 @@
 		<div class="panel panel-default">
 			<div class="panel-heading">Configuration</div>
 			<div class="panel-body">
+				<div class="form-group">
+					<label class="control-label col-xs-7"><a ui-turn-on="modalAHRSWarning">AHRS Output to EFB</a></label>
+					<div class="col-xs-5">
+						<ui-switch ng-model='AHRS_GDL90_Enabled' settings-change ui-turn-on="modalAHRSWarning"></ui-switch>
+					</div>
+				</div>
 				<div class="form-group reset-flow">
 					<label class="control-label col-xs-5">Mode S Code (Hex)</label>
 					<form name="modeForm" ng-submit="updatemodes()" novalidate>
@@ -214,6 +220,50 @@
     	  </div>
     </div>
   </div> 
+  
+	<div class="modal" ui-if="modalAHRSHardwareInfo" ui-state="modalAHRSHardwareInfo">
+		<div class="modal-overlay "></div>
+		<div class="vertical-alignment-helper center-block">
+			<div class="modal-dialog vertical-align-center">
+				<div class="modal-content">
+					<div class="modal-header">
+						<button class="close" ui-turn-off="modalAHRSHardwareInfo"></button>
+						<h4 class="modal-title">AHRS Module Support: Information Note</h4>
+					</div>
+					<div class="modal-body">
+						<p>Enabling this option will allow Stratux to read a BMP180 pressure sensor and a MPU6050 6-axis gyro / accelerometer connected to the Raspberry Pi GPIO (I2C) pins. The Reyax RY835AI AHRS / GPS module includes both of these sensors.</p>
+						<p><strong>AHRS support is still experimental, and will not correctly indicate roll angle during coordinated turns.</strong></p>
+						<p>Refer to the README / FAQ at <a href="http://stratux.me">stratux.me</a> for current hardware support.</p>
+					</div>
+					<div class="modal-footer">
+						<a ui-turn-off="modalAHRSHardwareInfo" class="btn btn-primary">Acknowledge</a>
+					</div>
+				</div>
+			</div>
+		</div>
+	</div> 
 
+	<div class="modal" ui-if="modalAHRSWarning" ui-state="modalAHRSWarning">
+		<div class="modal-overlay "></div>
+       	<div class="vertical-alignment-helper center-block">
+        	<div class="modal-dialog vertical-align-center">
+   			  <div class="modal-content">
+				<div class="modal-header">
+				  <button class="close" 
+						  ui-turn-off="modalAHRSWarning"></button>
+				  <h4 class="modal-title">AHRS Output to EFB: Compatibility Note</h4>
+				</div>
+				<div class="modal-body">
+				  <p>This option must be enabled to send AHRS messages to compatible EFBs (WingX, iFly, etc.) over the WiFi interface using the GDL90 protocol. If <strong>AHRS Module</strong> is not enabled or AHRS hardware is not present, Stratux will estimate pitch and bank angles from GPS groundspeed, climb rate, and turn rate.</p>
+				  <p><strong>AHRS output is not compatible with ForeFlight 7.6, and must be disabled for ForeFlight 7.6 to work at all. Stratux will automatically disable AHRS output if it detects a connected ForeFlight client.</strong></p>
+				</div>
+				<div class="modal-footer">
+				  <a ui-turn-off="modalAHRSWarning" class="btn btn-primary">Acknowledge</a>
+				</div>
+			  </div>
+    	  </div>
+    </div>
+  </div> 
+	
 </div>
 

web/plates/status.html

@@ -3,16 +3,6 @@
             <a ng-click="VersionClick()" class="btn btn-hidden"<strong>Version: <span>{{Version}} ({{Build}})</span></strong></a>
     </div>
 	<div class="panel panel-default">
-		<div class="panel-heading" ng-class="{'section_invisible': !visible_errors}">
-			<span class="panel_label">Errors</span>
-		</div>
-		<div class="panel-body" ng-class="{'section_invisible': !visible_errors}">
-			<ul>
-				<li class="status-error" ng-repeat="err in Errors">
-					<span class="fa fa-exclamation-triangle icon-red"></span> <span class="icon-red">{{err}}</span>
-				</li>
-			</ul>
-		</div>
 		<div class="panel-heading">
 			<span class="panel_label">Status</span>
 			<span ng-show="ConnectState == 'Connected'" class="label label-success">{{ConnectState}}</span>
@@ -141,6 +131,24 @@
 					</div>
 				</div>
 			</div>
-		</div>
+		</div>	
 	</div>
+
+	<div class="panel panel-default">
+		<div class="panel-heading" ng-class="{'section_invisible': !visible_errors}">
+			<span class="panel_label">Errors</span>
+		</div>
+		<div class="panel-body" ng-class="{'section_invisible': !visible_errors}">
+			<ul>
+				<li class="status-error" ng-repeat="err in Errors">
+					<span class="fa fa-exclamation-triangle icon-red"></span> <span class="icon-red">{{err}}</span>
+				</li>
+				</ul>
+			</div>
+	</div>
+				
+				
+		
+				
+			
 </div>