diff --git a/main/gps.go b/main/gps.go index 438debbb..7e7fbebe 100644 --- a/main/gps.go +++ b/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 - 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 } 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 - 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. @@ -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 diff --git a/web/plates/gps.html b/web/plates/gps.html index 8dc94a49..093bf1bf 100644 --- a/web/plates/gps.html +++ b/web/plates/gps.html @@ -110,7 +110,7 @@