Merge pull request #598 from dndx/ahrs_dev

Added nav rate display and Galileo nav rate change for AHRS branch.

main/gps.go

@@ -306,8 +306,7 @@ func initGPSSerial() bool {
 		if (globalStatus.GPS_detected_type == GPS_TYPE_UBX8) || (globalStatus.GPS_detected_type == GPS_TYPE_UART) { // assume that any GPS connected to serial GPIO is ublox8 (RY835/6AI)
 			//log.Printf("UBX8 device detected on USB, or GPS serial connection in use. Attempting GLONASS and Galelio configuration.\n")
 			glonass = []byte{0x06, 0x08, 0x0E, 0x00, 0x01, 0x00, 0x01, 0x01} // this enables GLONASS with 8-14 tracking channels
-			galileo = []byte{0x02, 0x04, 0x08, 0x00, 0x01, 0x00, 0x01, 0x01} // this enables Galileo with 4-8 tracking channels
+			galileo = []byte{0x02, 0x04, 0x05, 0x00, 0x01, 0x00, 0x01, 0x01} // this enables Galileo with 4-5 tracking channels
-			updatespeed = []byte{0x06, 0x00, 0xF4, 0x01, 0x01, 0x00}         // Nav speed 2Hz
 		}
 		cfgGnss = append(cfgGnss, gps...)
 		cfgGnss = append(cfgGnss, sbas...)
@@ -549,7 +548,7 @@ func calcGPSAttitude() bool {
 	// 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 headingAvg, dh, v_x, v_z, a_c, omega, slope, intercept float64
 	var tempHdg, tempHdgUnwrapped, tempHdgTime, tempSpeed, tempVV, tempSpeedTime, tempRegWeights []float64 // temporary arrays for regression calculation
 	var valid bool
 	var lengthHeading, lengthSpeed int
@@ -557,35 +556,7 @@ func calcGPSAttitude() bool {
 
 	center := float64(myGPSPerfStats[index].nmeaTime) // current time for calculating regression weights
 
-	// frequency detection
+	halfwidth = calculateNavRate()
-	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 = 9 * dt_avg
-		mySituation.GPSPositionSampleRate = 1 / dt_avg
-	} else {
-		if globalSettings.DEBUG {
-			log.Printf("GPS attitude: Couldn't determine sample rate\n")
-		}
-		halfwidth = 3.5
-		mySituation.GPSPositionSampleRate = 0
-	}
-
-	if halfwidth > 3.5 {
-		halfwidth = 3.5 // limit calculation window to 3.5 seconds of data for 1 Hz or slower samples
-	} else if halfwidth < 1.5 {
-		halfwidth = 1.5 // use minimum of 1.5 seconds for sample rates faster than 5 Hz
-	}
 
 	if (globalStatus.GPS_detected_type & 0xf0) == GPS_PROTOCOL_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.
@@ -846,6 +817,42 @@ func registerSituationUpdate() {
 	situationUpdate.SendJSON(mySituation)
 }
 
+func calculateNavRate() float64 {
+	length := len(myGPSPerfStats)
+	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))
+		}
+	}
+
+	var halfwidth float64
+	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 = 9 * dt_avg
+		mySituation.GPSPositionSampleRate = 1 / dt_avg
+	} else {
+		if globalSettings.DEBUG {
+			log.Printf("GPS attitude: Couldn't determine sample rate\n")
+		}
+		halfwidth = 3.5
+		mySituation.GPSPositionSampleRate = 0
+	}
+
+	if halfwidth > 3.5 {
+		halfwidth = 3.5 // limit calculation window to 3.5 seconds of data for 1 Hz or slower samples
+	} else if halfwidth < 1.5 {
+		halfwidth = 1.5 // use minimum of 1.5 seconds for sample rates faster than 5 Hz
+	}
+
+	return halfwidth
+}
+
 /*
 processNMEALine parses NMEA-0183 formatted strings against several message types.
 
@@ -1940,7 +1947,12 @@ func gpsAttitudeSender() {
 	timer := time.NewTicker(100 * time.Millisecond) // ~10Hz update.
 	for {
 		<-timer.C
-		myGPSPerfStats = make([]gpsPerfStats, 0) // reinitialize statistics on disconnect / reconnect
+		if !(globalStatus.GPS_connected || globalStatus.IMUConnected) {
+			myGPSPerfStats = make([]gpsPerfStats, 0) // reinitialize statistics on disconnect / reconnect
+		} else {
+			calculateNavRate()
+		}
+
 		for !(globalSettings.IMU_Sensor_Enabled && globalStatus.IMUConnected) && (globalSettings.GPS_Enabled && globalStatus.GPS_connected) {
 			<-timer.C
 

web/plates/gps.html

@@ -110,7 +110,7 @@
 			<div class="panel-footer">
 				<div class="row">
 					<label class="col-xs-6">GPS solution:</label>
-					<span class="col-xs-6">{{SolutionText}}</span>
+					<span class="col-xs-6">{{SolutionText}} @ {{ GPS_PositionSampleRate }} Hz</span>
 				</div>
 				<div class="row">
 					<label class="col-xs-6">Summary:</label>

web/plates/js/gps.js

@@ -109,6 +109,7 @@ function GPSCtrl($rootScope, $scope, $state, $http, $interval) {
         $scope.GPS_satellites_tracked = situation.GPSSatellitesTracked;
         $scope.GPS_satellites_seen = situation.GPSSatellitesSeen;
         $scope.Quality = situation.GPSFixQuality;
+        $scope.GPS_PositionSampleRate = situation.GPSPositionSampleRate.toFixed(1);
 
         var solutionText = "No Fix";
         if (situation.GPSFixQuality === 2) {