From e386e0f860abff0f3d5c0702056ed6ecd43ff84f Mon Sep 17 00:00:00 2001 From: Datong Sun Date: Thu, 27 Apr 2017 11:52:28 -0700 Subject: [PATCH 1/7] fixed a bug that NEO-M8 will use 1 Hz update rate when Galileo is enabled. Slightly reduced Galileo's maximum tracking channel. --- main/gps.go | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/main/gps.go b/main/gps.go index 45987e7a..c22da9dd 100644 --- a/main/gps.go +++ b/main/gps.go @@ -292,8 +292,8 @@ 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 - updatespeed = []byte{0x06, 0x00, 0xF4, 0x01, 0x01, 0x00} // Nav speed 2Hz + galileo = []byte{0x02, 0x04, 0x05, 0x00, 0x01, 0x00, 0x01, 0x01} // this enables Galileo with 4-5 tracking channels + updatespeed = []byte{0xF4, 0x01, 0x01, 0x00, 0x01, 0x00} // Nav speed 2 Hz for enabling Galileo } cfgGnss = append(cfgGnss, gps...) cfgGnss = append(cfgGnss, sbas...) From bd45ced50ca80ee084ef8bde718685f40481af43 Mon Sep 17 00:00:00 2001 From: Datong Sun Date: Sat, 13 May 2017 22:09:27 -0700 Subject: [PATCH 2/7] Removed 2Hz configuration for enabling Galileo. --- main/gps.go | 1 - 1 file changed, 1 deletion(-) diff --git a/main/gps.go b/main/gps.go index c22da9dd..469e4c43 100644 --- a/main/gps.go +++ b/main/gps.go @@ -293,7 +293,6 @@ func initGPSSerial() bool { //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, 0x05, 0x00, 0x01, 0x00, 0x01, 0x01} // this enables Galileo with 4-5 tracking channels - updatespeed = []byte{0xF4, 0x01, 0x01, 0x00, 0x01, 0x00} // Nav speed 2 Hz for enabling Galileo } cfgGnss = append(cfgGnss, gps...) cfgGnss = append(cfgGnss, sbas...) From d801686afd72e3830c28e35545d27a61a08d7e22 Mon Sep 17 00:00:00 2001 From: Datong Sun Date: Sun, 14 May 2017 00:02:06 -0700 Subject: [PATCH 3/7] Added GPS Nav rate calculation when IMU is enabled. --- main/gps.go | 77 +++++++++++++++++++++++++++++++++-------------------- 1 file changed, 48 insertions(+), 29 deletions(-) diff --git a/main/gps.go b/main/gps.go index 469e4c43..a0c82feb 100644 --- a/main/gps.go +++ b/main/gps.go @@ -542,35 +542,7 @@ func calcGPSAttitude() bool { center := float64(myGPSPerfStats[index].nmeaTime) // current time for calculating regression weights - // 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 = 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 - } + halfwidth = calculateNavRate() 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. @@ -831,6 +803,41 @@ 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)) + } + } + + 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. @@ -1046,6 +1053,10 @@ func processNMEALine(l string) (sentenceUsed bool) { mySituation.muGPSPerformance.Unlock() } + if globalSettings.IMU_Sensor_Enabled && globalStatus.IMUConnected { + calculateNavRate() + } + 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. @@ -1380,6 +1391,10 @@ func processNMEALine(l string) (sentenceUsed bool) { mySituation.muGPSPerformance.Unlock() } + if globalSettings.IMU_Sensor_Enabled && globalStatus.IMUConnected { + calculateNavRate() + } + return true } else if (x[0] == "GNRMC") || (x[0] == "GPRMC") { // Recommended Minimum data. @@ -1528,6 +1543,10 @@ func processNMEALine(l string) (sentenceUsed bool) { mySituation.muGPSPerformance.Unlock() } + if globalSettings.IMU_Sensor_Enabled && globalStatus.IMUConnected { + calculateNavRate() + } + setDataLogTimeWithGPS(mySituation) return true From 3c0e0f4f6aca88e33ec8ae683b86ba74e9953703 Mon Sep 17 00:00:00 2001 From: Datong Sun Date: Sun, 14 May 2017 00:07:05 -0700 Subject: [PATCH 4/7] Added sample rate display on gps.html. --- web/plates/gps.html | 2 +- web/plates/js/gps.js | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/web/plates/gps.html b/web/plates/gps.html index 5315db07..c429fc3b 100644 --- a/web/plates/gps.html +++ b/web/plates/gps.html @@ -110,7 +110,7 @@
- {{SolutionText}} + {{SolutionText}} @ {{ GPS_PositionSampleRate }} Hz
diff --git a/web/plates/js/gps.js b/web/plates/js/gps.js index 5a1d2eef..8f10e527 100644 --- a/web/plates/js/gps.js +++ b/web/plates/js/gps.js @@ -105,6 +105,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) { From 0a12f3447362e5b65d6445426a962933380b50f9 Mon Sep 17 00:00:00 2001 From: Datong Sun Date: Sun, 14 May 2017 00:12:09 -0700 Subject: [PATCH 5/7] Added missing variables. --- main/gps.go | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/main/gps.go b/main/gps.go index a0c82feb..a612f1fc 100644 --- a/main/gps.go +++ b/main/gps.go @@ -534,7 +534,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 @@ -814,7 +814,8 @@ func calculateNavRate() float64 { } } - dt_avg, valid = mean(tempSpeedTime) + 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) From 1326450eb36007a52f1810504dfe7ba9d712b634 Mon Sep 17 00:00:00 2001 From: Datong Sun Date: Sun, 14 May 2017 00:43:16 -0700 Subject: [PATCH 6/7] Do not reset myGPSPerfStats when IMU is connected. --- main/gps.go | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/main/gps.go b/main/gps.go index a612f1fc..086975b6 100644 --- a/main/gps.go +++ b/main/gps.go @@ -1928,7 +1928,10 @@ 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 + } + for !(globalSettings.IMU_Sensor_Enabled && globalStatus.IMUConnected) && (globalSettings.GPS_Enabled && globalStatus.GPS_connected) { <-timer.C From 30d08af3002959adcd7d6aa8d4a6db91f7bcee3a Mon Sep 17 00:00:00 2001 From: Datong Sun Date: Sun, 14 May 2017 01:02:58 -0700 Subject: [PATCH 7/7] Calculate Nav rate less frequently to reduce CPU usage. --- main/gps.go | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) diff --git a/main/gps.go b/main/gps.go index 086975b6..3c7b2639 100644 --- a/main/gps.go +++ b/main/gps.go @@ -1054,10 +1054,6 @@ func processNMEALine(l string) (sentenceUsed bool) { mySituation.muGPSPerformance.Unlock() } - if globalSettings.IMU_Sensor_Enabled && globalStatus.IMUConnected { - calculateNavRate() - } - 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. @@ -1392,10 +1388,6 @@ func processNMEALine(l string) (sentenceUsed bool) { mySituation.muGPSPerformance.Unlock() } - if globalSettings.IMU_Sensor_Enabled && globalStatus.IMUConnected { - calculateNavRate() - } - return true } else if (x[0] == "GNRMC") || (x[0] == "GPRMC") { // Recommended Minimum data. @@ -1544,10 +1536,6 @@ func processNMEALine(l string) (sentenceUsed bool) { mySituation.muGPSPerformance.Unlock() } - if globalSettings.IMU_Sensor_Enabled && globalStatus.IMUConnected { - calculateNavRate() - } - setDataLogTimeWithGPS(mySituation) return true @@ -1930,6 +1918,8 @@ func gpsAttitudeSender() { <-timer.C 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) {