diff --git a/main/gps.go b/main/gps.go index d64ef22d..798d9c55 100644 --- a/main/gps.go +++ b/main/gps.go @@ -78,6 +78,7 @@ type SituationData struct { LastGPSTimeTime time.Time // stratuxClock time since last GPS time received. LastValidNMEAMessageTime time.Time // time valid NMEA message last seen LastValidNMEAMessage string // last NMEA message processed. + PositionSampleRate float64 // calculated sample rate of GPS positions // From BMP180 pressure sensor. Temp float64 @@ -252,7 +253,7 @@ func initGPSSerial() bool { } else { // Byte order for UBX configuration is little endian. - // Set 10 Hz update to make gpsattitude more responsive. + // Set 10 Hz update to make gpsattitude more responsive for ublox7/8. p.Write(makeUBXCFG(0x06, 0x08, 6, []byte{0x64, 0x00, 0x01, 0x00, 0x01, 0x00})) // 10 Hz //p.Write(makeUBXCFG(0x06, 0x08, 6, []byte{0xc8, 0x00, 0x01, 0x00, 0x01, 0x00})) // 5 Hz @@ -282,7 +283,7 @@ func initGPSSerial() bool { qzss := []byte{0x05, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x01} glonass := []byte{0x06, 0x04, 0x0E, 0x00, 0x00, 0x00, 0x01, 0x01} // this disables GLONASS - if (device == "/dev/ublox8") || (device == "/dev/ttyAMA0") { // assume that any GPS connected to serial GPIO is ublox8 (RY835/6AI) + 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 configuration.\n") glonass = []byte{0x06, 0x08, 0x0E, 0x00, 0x01, 0x00, 0x01, 0x01} // this enables GLONASS with 8-14 tracking channels } @@ -303,9 +304,7 @@ func initGPSSerial() bool { p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x01})) // GGA enabled every 5th message p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01})) // GLL disabled p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01})) // GSA disabled - //p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x02, 0x00, 0x05, 0x00, 0x05, 0x00, 0x01})) // GSA enabled disabled every 5th position (used for testing only) p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01})) // GSV disabled - //p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x03, 0x00, 0x05, 0x00, 0x05, 0x00, 0x01})) // GSV enabled for every 5th position (used for testing only) p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01})) // RMC p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01})) // VGT p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00})) // GRS @@ -316,9 +315,9 @@ func initGPSSerial() bool { p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00})) // GNS p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x0E, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00})) // ??? p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00})) // VLW - p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF1, 0x00, 0x01, 0x01, 0x01, 0x01, 0x01, 0x00})) // Ublox,0 - p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF1, 0x03, 0x05, 0x05, 0x05, 0x05, 0x05, 0x00})) // Ublox,3 - p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF1, 0x04, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x00})) // Ublox,4 + p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF1, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00})) // Ublox,0 + p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF1, 0x03, 0x00, 0x05, 0x00, 0x05, 0x00, 0x00})) // Ublox,3 + p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF1, 0x04, 0x00, 0x0A, 0x00, 0x0A, 0x00, 0x00})) // Ublox,4 // Reconfigure serial port. cfg := make([]byte, 20) @@ -531,7 +530,7 @@ func calcGPSAttitude() 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 + halfwidth := float64(1.4) // width of regression evaluation window. Default of 1.4 seconds for 5 Hz sampling; will increase up to 3.5 sec @ 1 Hz // frequency detection tempSpeedTime = make([]float64, 0) @@ -547,12 +546,14 @@ func calcGPSAttitude() bool { if globalSettings.DEBUG { log.Printf("GPS attitude: Average delta time is %.2f s (%.1f Hz)\n", dt_avg, 1/dt_avg) } - halfwidth = 12 * dt_avg + halfwidth = 14 * dt_avg + mySituation.PositionSampleRate = 1 / dt_avg } else { if globalSettings.DEBUG { log.Printf("GPS attitude: Couldn't determine sample rate\n") } - halfwidth = 3.0 + halfwidth = 3.5 + mySituation.PositionSampleRate = 0 } if halfwidth > 3.5 {