kopia lustrzana https://github.com/cyoung/stratux
Merge pull request #169 from AvSquirrel/GPS-UBX2
Updated GPS initialization and parsingpull/156/merge
cyoung
commit
502084c20f
|
|
@ -459,8 +459,8 @@ func makeSXHeartbeat() []byte {
|
|||
// Number of GPS satellites locked.
|
||||
msg[16] = byte(globalStatus.GPS_satellites_locked)
|
||||
|
||||
//FIXME: Number of satellites connected. ??
|
||||
msg[17] = 0xFF
|
||||
// Number of satellites tracked
|
||||
msg[17] = byte(globalStatus.GPS_satellites_tracked)
|
||||
|
||||
// Summarize number of UAT and 1090ES traffic targets for reports that follow.
|
||||
var uat_traffic_targets uint16
|
||||
|
|
@ -703,12 +703,16 @@ func cpuTempMonitor() {
|
|||
func updateStatus() {
|
||||
if isGPSValid() {
|
||||
globalStatus.GPS_satellites_locked = mySituation.Satellites
|
||||
globalStatus.GPS_satellites_seen = mySituation.SatellitesSeen
|
||||
globalStatus.GPS_satellites_tracked = mySituation.SatellitesTracked
|
||||
if mySituation.quality == 2 {
|
||||
globalStatus.GPS_solution = "DGPS (WAAS)"
|
||||
globalStatus.GPS_solution = "DGPS (SBAS / WAAS)"
|
||||
} else if mySituation.quality == 1 {
|
||||
globalStatus.GPS_solution = "3D GPS"
|
||||
} else if mySituation.quality == 6 {
|
||||
globalStatus.GPS_solution = "Dead Reckoning"
|
||||
} else {
|
||||
globalStatus.GPS_solution = "N/A"
|
||||
globalStatus.GPS_solution = "No Fix"
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -985,6 +989,8 @@ type status struct {
|
|||
ES_messages_last_minute uint
|
||||
ES_messages_max uint
|
||||
GPS_satellites_locked uint16
|
||||
GPS_satellites_seen uint16
|
||||
GPS_satellites_tracked uint16
|
||||
GPS_connected bool
|
||||
GPS_solution string
|
||||
RY835AI_connected bool
|
||||
|
|
@ -1107,7 +1113,8 @@ func printStats() {
|
|||
log.Printf(" - CPUTemp=%.02f deg C, MemStats.Alloc=%s, MemStats.Sys=%s, totalNetworkMessagesSent=%s\n", globalStatus.CPUTemp, humanize.Bytes(uint64(memstats.Alloc)), humanize.Bytes(uint64(memstats.Sys)), humanize.Comma(int64(totalNetworkMessagesSent)))
|
||||
log.Printf(" - UAT/min %s/%s [maxSS=%.02f%%], ES/min %s/%s\n, Total traffic targets tracked=%s", humanize.Comma(int64(globalStatus.UAT_messages_last_minute)), humanize.Comma(int64(globalStatus.UAT_messages_max)), float64(maxSignalStrength)/10.0, humanize.Comma(int64(globalStatus.ES_messages_last_minute)), humanize.Comma(int64(globalStatus.ES_messages_max)), humanize.Comma(int64(len(seenTraffic))))
|
||||
if globalSettings.GPS_Enabled {
|
||||
log.Printf(" - Last GPS fix: %s, GPS solution type: %d, NACp: %d, est accuracy %.02f m\n", humanize.Time(mySituation.LastFixLocalTime), mySituation.quality, mySituation.NACp, mySituation.Accuracy)
|
||||
log.Printf(" - Last GPS fix: %s, GPS solution type: %d using %v satellites (%v tracked), NACp: %d, est accuracy %.02f m\n", humanize.Time(mySituation.LastFixLocalTime), mySituation.quality, mySituation.Satellites, mySituation.SatellitesTracked, mySituation.NACp, mySituation.Accuracy)
|
||||
log.Printf(" - GPS vertical velocity: %.02f ft/sec; GPS vertical accuracy: %v m\n", mySituation.GPSVertVel, mySituation.AccuracyVert)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
463
main/ry835ai.go
463
main/ry835ai.go
|
|
@ -29,11 +29,15 @@ type SituationData struct {
|
|||
Lat float32
|
||||
Lng float32
|
||||
quality uint8
|
||||
Satellites uint16
|
||||
Accuracy float32 // Meters.
|
||||
GeoidSep float32 // geoid separation, ft, MSL minus HAE (used in altitude calculation)
|
||||
Satellites uint16 // satellites used in solution
|
||||
SatellitesTracked uint16 // satellites tracked (almanac data received)
|
||||
SatellitesSeen uint16 // satellites seen (signal received)
|
||||
Accuracy float32 // 95% confidence for horizontal position, meters.
|
||||
NACp uint8 // NACp categories are defined in AC 20-165A
|
||||
Alt float32 // Feet.
|
||||
alt_accuracy float32
|
||||
Alt float32 // Feet MSL
|
||||
AccuracyVert float32 // 95% confidence for vertical position, meters
|
||||
GPSVertVel float32 // GPS vertical velocity, feet per second
|
||||
LastFixLocalTime time.Time
|
||||
TrueCourse uint16
|
||||
GroundSpeed uint16
|
||||
|
|
@ -105,24 +109,66 @@ func initGPSSerial() bool {
|
|||
device = "/dev/ttyAMA0"
|
||||
}
|
||||
log.Printf("Using %s for GPS\n", device)
|
||||
serialConfig = &serial.Config{Name: device, Baud: 115200}
|
||||
|
||||
/* Developer option -- uncomment to allow "hot" configuration of GPS (assuming 38.4 kpbs on warm start)
|
||||
serialConfig = &serial.Config{Name: device, Baud: 38400}
|
||||
p, err := serial.OpenPort(serialConfig)
|
||||
if err != nil {
|
||||
log.Printf("serial port err: %s\n", err.Error())
|
||||
return false
|
||||
} else { // reset port to 9600 baud for configuration
|
||||
cfg1 := make([]byte, 20)
|
||||
cfg1[0] = 0x01 // portID.
|
||||
cfg1[1] = 0x00 // res0.
|
||||
cfg1[2] = 0x00 // res1.
|
||||
cfg1[3] = 0x00 // res1.
|
||||
|
||||
// [ 7 ] [ 6 ] [ 5 ] [ 4 ]
|
||||
// 0000 0000 0000 0000 1000 0000 1100 0000
|
||||
// UART mode. 0 stop bits, no parity, 8 data bits. Little endian order.
|
||||
cfg1[4] = 0xC0
|
||||
cfg1[5] = 0x08
|
||||
cfg1[6] = 0x00
|
||||
cfg1[7] = 0x00
|
||||
|
||||
// Baud rate. Little endian order.
|
||||
bdrt1 := uint32(9600)
|
||||
cfg1[11] = byte((bdrt1 >> 24) & 0xFF)
|
||||
cfg1[10] = byte((bdrt1 >> 16) & 0xFF)
|
||||
cfg1[9] = byte((bdrt1 >> 8) & 0xFF)
|
||||
cfg1[8] = byte(bdrt1 & 0xFF)
|
||||
|
||||
// inProtoMask. NMEA and UBX. Little endian.
|
||||
cfg1[12] = 0x03
|
||||
cfg1[13] = 0x00
|
||||
|
||||
// outProtoMask. NMEA. Little endian.
|
||||
cfg1[14] = 0x02
|
||||
cfg1[15] = 0x00
|
||||
|
||||
cfg1[16] = 0x00 // flags.
|
||||
cfg1[17] = 0x00 // flags.
|
||||
|
||||
cfg1[18] = 0x00 //pad.
|
||||
cfg1[19] = 0x00 //pad.
|
||||
|
||||
p.Write(makeUBXCFG(0x06, 0x00, 20, cfg1))
|
||||
p.Close()
|
||||
}
|
||||
|
||||
-- End developer option */
|
||||
|
||||
// Open port at 9600 baud for config.
|
||||
serialConfig = &serial.Config{Name: device, Baud: 9600}
|
||||
p, err := serial.OpenPort(serialConfig)
|
||||
if err != nil {
|
||||
log.Printf("serial port err: %s\n", err.Error())
|
||||
return false
|
||||
}
|
||||
|
||||
serialPort = p
|
||||
// Open port at 9600 baud for config.
|
||||
serialConfig = &serial.Config{Name: device, Baud: 9600}
|
||||
p, err = serial.OpenPort(serialConfig)
|
||||
if err != nil {
|
||||
log.Printf("serial port err: %s\n", err.Error())
|
||||
return false
|
||||
}
|
||||
// Set 10Hz update. Little endian order.
|
||||
p.Write(makeUBXCFG(0x06, 0x08, 6, []byte{0x64, 0x00, 0x01, 0x00, 0x01, 0x00}))
|
||||
|
||||
// Set 10Hz update.
|
||||
p.Write(makeUBXCFG(0x06, 0x08, 6, []byte{0x64, 0x00, 0x00, 0x01, 0x00, 0x01}))
|
||||
|
||||
// Set navigation settings.
|
||||
nav := make([]byte, 36)
|
||||
|
|
@ -134,6 +180,50 @@ func initGPSSerial() bool {
|
|||
|
||||
p.Write(makeUBXCFG(0x06, 0x24, 36, nav))
|
||||
|
||||
|
||||
// GNSS configuration CFG-GNSS for ublox 7 higher, p. 125 (v8)
|
||||
//
|
||||
// NOTE: Max position rate = 5 Hz if GPS+GLONASS used.
|
||||
// Disable GLONASS to enable 10 Hz solution rate. GLONASS is not used
|
||||
// for SBAS (WAAS), so little real-world impact.
|
||||
|
||||
cfgGnss := []byte{0x00, 0x20, 0x20, 0x05}
|
||||
gps := []byte{0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01}
|
||||
sbas := []byte{0x01, 0x02, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01}
|
||||
beidou := []byte{0x03, 0x00, 0x10, 0x00, 0x00, 0x00, 0x01, 0x01}
|
||||
qzss := []byte{0x05, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x01}
|
||||
glonass := []byte{0x06, 0x04, 0x0E, 0x00, 0x00, 0x00, 0x01, 0x01}
|
||||
cfgGnss = append(cfgGnss, gps...)
|
||||
cfgGnss = append(cfgGnss, sbas...)
|
||||
cfgGnss = append(cfgGnss, beidou...)
|
||||
cfgGnss = append(cfgGnss, qzss...)
|
||||
cfgGnss = append(cfgGnss, glonass...)
|
||||
p.Write(makeUBXCFG(0x06, 0x3E, uint16(len(cfgGnss)), cfgGnss))
|
||||
|
||||
// SBAS configuration for ublox 6 and higher
|
||||
p.Write(makeUBXCFG(0x06, 0x16, 8, []byte{0x01, 0x07, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00}))
|
||||
|
||||
// Message output configuration -- disable standard NMEA messages except 1Hz GGA
|
||||
// Msg DDC UART1 UART2 USB I2C Res
|
||||
p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x00, 0x00, 0x0A, 0x00, 0x0A, 0x00, 0x01})) // GGA
|
||||
p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01})) // GLL
|
||||
p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01})) // GSA
|
||||
p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01})) // GSV
|
||||
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
|
||||
p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00})) // GST
|
||||
p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00})) // ZDA
|
||||
p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00})) // GBS
|
||||
p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x0A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00})) // DTM
|
||||
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, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x00})) // Ublox,3
|
||||
p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF1, 0x04, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x00})) // Ublox,4
|
||||
|
||||
// Reconfigure serial port.
|
||||
cfg := make([]byte, 20)
|
||||
cfg[0] = 0x01 // portID.
|
||||
|
|
@ -141,27 +231,28 @@ func initGPSSerial() bool {
|
|||
cfg[2] = 0x00 // res1.
|
||||
cfg[3] = 0x00 // res1.
|
||||
|
||||
// 0000 0000 0000 0010 0011 0000 0000 0000
|
||||
// UART mode. 0 stop bits, no parity, 8 data bits.
|
||||
cfg[4] = 0x00
|
||||
cfg[5] = 0x20
|
||||
cfg[6] = 0x30
|
||||
// [ 7 ] [ 6 ] [ 5 ] [ 4 ]
|
||||
// 0000 0000 0000 0000 1000 0000 1100 0000
|
||||
// UART mode. 0 stop bits, no parity, 8 data bits. Little endian order.
|
||||
cfg[4] = 0xC0
|
||||
cfg[5] = 0x08
|
||||
cfg[6] = 0x00
|
||||
cfg[7] = 0x00
|
||||
|
||||
// Baud rate.
|
||||
bdrt := uint32(115200)
|
||||
cfg[8] = byte((bdrt >> 24) & 0xFF)
|
||||
cfg[9] = byte((bdrt >> 16) & 0xFF)
|
||||
cfg[10] = byte((bdrt >> 8) & 0xFF)
|
||||
cfg[11] = byte(bdrt & 0xFF)
|
||||
// Baud rate. Little endian order.
|
||||
bdrt := uint32(38400)
|
||||
cfg[11] = byte((bdrt >> 24) & 0xFF)
|
||||
cfg[10] = byte((bdrt >> 16) & 0xFF)
|
||||
cfg[9] = byte((bdrt >> 8) & 0xFF)
|
||||
cfg[8] = byte(bdrt & 0xFF)
|
||||
|
||||
// inProtoMask. NMEA and UBX.
|
||||
cfg[12] = 0x00
|
||||
cfg[13] = 0x03
|
||||
// inProtoMask. NMEA and UBX. Little endian.
|
||||
cfg[12] = 0x03
|
||||
cfg[13] = 0x00
|
||||
|
||||
// outProtoMask. NMEA.
|
||||
cfg[14] = 0x00
|
||||
cfg[15] = 0x02
|
||||
// outProtoMask. NMEA. Little endian.
|
||||
cfg[14] = 0x02
|
||||
cfg[15] = 0x00
|
||||
|
||||
cfg[16] = 0x00 // flags.
|
||||
cfg[17] = 0x00 // flags.
|
||||
|
|
@ -170,16 +261,302 @@ func initGPSSerial() bool {
|
|||
cfg[19] = 0x00 //pad.
|
||||
|
||||
p.Write(makeUBXCFG(0x06, 0x00, 20, cfg))
|
||||
|
||||
p.Close()
|
||||
|
||||
// Re-open port at 38400 baud so we can read messages
|
||||
serialConfig = &serial.Config{Name: device, Baud: 38400}
|
||||
p, err = serial.OpenPort(serialConfig)
|
||||
if err != nil {
|
||||
log.Printf("serial port err: %s\n", err.Error())
|
||||
return false
|
||||
}
|
||||
|
||||
serialPort = p
|
||||
log.Printf("GPS configuration complete\n")
|
||||
return true
|
||||
}
|
||||
|
||||
// func validateNMEAChecksum determines if a string is a properly formatted NMEA sentence with a valid checksum.
|
||||
//
|
||||
// If the input string is valid, output is the input stripped of the "$" token and checksum, along with a boolean 'true'
|
||||
// If the input string is the incorrect format, the checksum is missing/invalid, or checksum calculation fails, an error string and
|
||||
// boolean 'false' are returned
|
||||
//
|
||||
// Checksum is calculated as XOR of all bytes between "$" and "*"
|
||||
|
||||
func validateNMEAChecksum(s string) (string, bool) {
|
||||
//validate format. NMEA sentences start with "$" and end in "*xx" where xx is the XOR value of all bytes between
|
||||
if !(strings.HasPrefix(s, "$") && strings.Contains(s, "*")) {
|
||||
return "Invalid NMEA message", false
|
||||
}
|
||||
|
||||
// strip leading "$" and split at "*"
|
||||
s_split := strings.Split(strings.TrimPrefix(s, "$"), "*")
|
||||
s_out := s_split[0]
|
||||
s_cs := s_split[1]
|
||||
|
||||
if (len(s_cs) < 2) {
|
||||
return "Missing checksum. Fewer than two bytes after asterisk", false
|
||||
}
|
||||
|
||||
cs, err := strconv.ParseUint(s_cs[:2], 16, 8)
|
||||
if err != nil {
|
||||
return "Invalid checksum", false
|
||||
}
|
||||
|
||||
cs_calc := byte(0)
|
||||
for i := range s_out {
|
||||
cs_calc = cs_calc ^ byte(s_out[i])
|
||||
}
|
||||
|
||||
if (cs_calc != byte(cs)) {
|
||||
return fmt.Sprintf("Checksum failed. Calculated %#X; expected %#X", cs_calc, cs), false
|
||||
}
|
||||
|
||||
return s_out, true
|
||||
}
|
||||
|
||||
|
||||
|
||||
func processNMEALine(l string) bool {
|
||||
replayLog(l, MSGCLASS_GPS)
|
||||
x := strings.Split(l, ",")
|
||||
if (x[0] == "$GNVTG") || (x[0] == "$GPVTG") { // Ground track information.
|
||||
l_valid, validNMEAcs := validateNMEAChecksum(l)
|
||||
if (!validNMEAcs) {
|
||||
log.Printf("GPS error. Invalid NMEA string: %s\n", l_valid) // remove log message once validation complete
|
||||
return false
|
||||
}
|
||||
x := strings.Split(l_valid, ",")
|
||||
|
||||
if (x[0] == "PUBX") { // UBX proprietary message
|
||||
if (x[1] == "00") { // position message
|
||||
if len (x) < 20 {
|
||||
return false
|
||||
}
|
||||
|
||||
mySituation.mu_GPS.Lock()
|
||||
defer mySituation.mu_GPS.Unlock()
|
||||
|
||||
// field 2 = time
|
||||
if len(x[2]) < 9 {
|
||||
return false
|
||||
}
|
||||
hr, err1 := strconv.Atoi(x[2][0:2])
|
||||
min, err2 := strconv.Atoi(x[2][2:4])
|
||||
sec, err3 := strconv.Atoi(x[2][4:6])
|
||||
if err1 != nil || err2 != nil || err3 != nil {
|
||||
return false
|
||||
}
|
||||
|
||||
mySituation.lastFixSinceMidnightUTC = uint32((hr * 60 * 60) + (min * 60) + sec)
|
||||
|
||||
// field 3-4 = lat
|
||||
|
||||
if len(x[3]) < 10 {
|
||||
return false
|
||||
}
|
||||
|
||||
hr, err1 = strconv.Atoi(x[3][0:2])
|
||||
minf, err2 := strconv.ParseFloat(x[3][2:10], 32)
|
||||
if err1 != nil || err2 != nil {
|
||||
return false
|
||||
}
|
||||
|
||||
mySituation.Lat = float32(hr) + float32(minf/60.0)
|
||||
if x[4] == "S" { // South = negative.
|
||||
mySituation.Lat = -mySituation.Lat
|
||||
}
|
||||
|
||||
// field 5-6 = lon
|
||||
if len(x[5]) < 11 {
|
||||
return false
|
||||
}
|
||||
hr, err1 = strconv.Atoi(x[5][0:3])
|
||||
minf, err2 = strconv.ParseFloat(x[5][3:11], 32)
|
||||
if err1 != nil || err2 != nil {
|
||||
return false
|
||||
}
|
||||
|
||||
mySituation.Lng = float32(hr) + float32(minf/60.0)
|
||||
if x[6] == "W" { // West = negative.
|
||||
mySituation.Lng = -mySituation.Lng
|
||||
}
|
||||
|
||||
// field 7 = height above ellipsoid, m
|
||||
|
||||
hae, err1 := strconv.ParseFloat(x[7], 32)
|
||||
if err1 != nil {
|
||||
return false
|
||||
}
|
||||
alt := float32(hae*3.28084) - mySituation.GeoidSep // convert to feet and offset by geoid separation
|
||||
mySituation.Alt = alt
|
||||
|
||||
// field 8 = nav status
|
||||
// DR = dead reckoning, G2= 2D GPS, G3 = 3D GPS, D2= 2D diff, D3 = 3D diff, RK = GPS+DR, TT = time only
|
||||
|
||||
if (x[8] == "D2" || x[8] == "D3") {
|
||||
mySituation.quality = 2
|
||||
} else if (x[8] == "G2" || x[8] == "G3") {
|
||||
mySituation.quality = 1
|
||||
} else if (x[8] == "DR" || x[8] == "RK") {
|
||||
mySituation.quality = 6
|
||||
} else if (x[8] == "NF") {
|
||||
mySituation.quality = 0
|
||||
return false // return false if no valid fix.
|
||||
} else {
|
||||
mySituation.quality = 0
|
||||
}
|
||||
|
||||
// field 9 = horizontal accuracy, m
|
||||
hAcc, err := strconv.ParseFloat(x[9], 32)
|
||||
if err != nil {
|
||||
return false
|
||||
}
|
||||
mySituation.Accuracy = float32(hAcc*2) // UBX reports 1-sigma variation; NACp is 95% confidence (2-sigma)
|
||||
|
||||
// NACp estimate.
|
||||
if mySituation.Accuracy < 3 {
|
||||
mySituation.NACp = 11
|
||||
} else if mySituation.Accuracy < 10 {
|
||||
mySituation.NACp = 10
|
||||
} else if mySituation.Accuracy < 30 {
|
||||
mySituation.NACp = 9
|
||||
} else if mySituation.Accuracy < 92.6 {
|
||||
mySituation.NACp = 8
|
||||
} else if mySituation.Accuracy < 185.2 {
|
||||
mySituation.NACp = 7
|
||||
} else if mySituation.Accuracy < 555.6 {
|
||||
mySituation.NACp = 6
|
||||
} else {
|
||||
mySituation.NACp = 0
|
||||
}
|
||||
|
||||
|
||||
// field 10 = vertical accuracy, m
|
||||
vAcc, err := strconv.ParseFloat(x[10], 32)
|
||||
if err != nil {
|
||||
return false
|
||||
}
|
||||
mySituation.AccuracyVert = float32(vAcc*2) // UBX reports 1-sigma variation; we want 95% confidence
|
||||
|
||||
|
||||
// field 11 = groundspeed, km/h
|
||||
groundspeed, err := strconv.ParseFloat(x[11], 32)
|
||||
if err != nil {
|
||||
return false
|
||||
}
|
||||
groundspeed = groundspeed * 0.540003 // convert to knots
|
||||
|
||||
// field 12 = track, deg
|
||||
trueCourse := uint16(0)
|
||||
if len(x[12]) > 0 && groundspeed > 3 {
|
||||
tc, err := strconv.ParseFloat(x[12], 32)
|
||||
if err != nil {
|
||||
return false
|
||||
}
|
||||
trueCourse = uint16(tc)
|
||||
//FIXME: Experimental. Set heading to true heading on the MPU6050 reader.
|
||||
if myMPU6050 != nil && globalStatus.RY835AI_connected && globalSettings.AHRS_Enabled {
|
||||
myMPU6050.ResetHeading(float64(tc))
|
||||
}
|
||||
} else {
|
||||
// No movement.
|
||||
mySituation.TrueCourse = 0
|
||||
mySituation.GroundSpeed = 0
|
||||
mySituation.LastGroundTrackTime = time.Time{}
|
||||
}
|
||||
|
||||
mySituation.TrueCourse = uint16(trueCourse)
|
||||
mySituation.GroundSpeed = uint16(groundspeed)
|
||||
mySituation.LastGroundTrackTime = time.Now()
|
||||
|
||||
|
||||
|
||||
// field 13 = vertical velocity, m/s
|
||||
vv, err := strconv.ParseFloat(x[13], 32)
|
||||
if err != nil {
|
||||
return false
|
||||
}
|
||||
mySituation.GPSVertVel = float32(vv*-3.28084) // convert to ft/sec and positive = up
|
||||
|
||||
|
||||
|
||||
// field 14 = age of diff corrections
|
||||
|
||||
|
||||
// field 18 = number of satellites
|
||||
sat, err1 := strconv.Atoi(x[18])
|
||||
if err1 != nil {
|
||||
return false
|
||||
}
|
||||
mySituation.Satellites = uint16(sat)
|
||||
|
||||
mySituation.LastFixLocalTime = time.Now()
|
||||
|
||||
} else if (x[1] == "03") { // satellite status message
|
||||
|
||||
// field 2 = number of satellites tracked
|
||||
satSeen := 0 // satellites seen (signal present)
|
||||
satTracked, err := strconv.Atoi(x[2])
|
||||
if err != nil {
|
||||
return false
|
||||
}
|
||||
mySituation.SatellitesTracked = uint16(satTracked)
|
||||
|
||||
// fields 3-8 are repeated block
|
||||
for i:= 0; i < satTracked; i++ {
|
||||
if (x[7+6*i] != "") {
|
||||
satSeen++
|
||||
}
|
||||
}
|
||||
|
||||
mySituation.SatellitesSeen = uint16(satSeen)
|
||||
// log.Printf("Satellites with signal: %v\n",mySituation.SatellitesSeen)
|
||||
|
||||
|
||||
/* Reference for future constellation tracking
|
||||
for i:= 0; i < satTracked; i++ {
|
||||
x[3+6*i] // sv number
|
||||
x[4+6*i] // status [ U | e | - ] for used / ephemeris / not used
|
||||
x[5+6*i] // azimuth, deg, 0-359
|
||||
x[6+6*i] // elevation, deg, 0-90
|
||||
x[7+6*i] // signal strength dB-Hz
|
||||
x[8+6*i] // lock time, sec, 0-64
|
||||
*/
|
||||
|
||||
} else if (x[1] == "04") { // clock message
|
||||
|
||||
// field 2 is UTC time
|
||||
if len(x[2]) < 9 {
|
||||
return false
|
||||
}
|
||||
hr, err1 := strconv.Atoi(x[2][0:2])
|
||||
min, err2 := strconv.Atoi(x[2][2:4])
|
||||
sec, err3 := strconv.Atoi(x[2][4:6])
|
||||
if err1 != nil || err2 != nil || err3 != nil {
|
||||
return false
|
||||
}
|
||||
mySituation.lastFixSinceMidnightUTC = uint32((hr * 60 * 60) + (min * 60) + sec)
|
||||
|
||||
// field 3 is date
|
||||
|
||||
if len(x[3]) == 6 {
|
||||
// Date of Fix, i.e 191115 = 19 November 2015 UTC field 9
|
||||
gpsTimeStr := fmt.Sprintf("%s %d:%d:%d", x[3], hr, min, sec)
|
||||
gpsTime, err := time.Parse("020106 15:04:05", gpsTimeStr)
|
||||
if err == nil {
|
||||
if time.Since(gpsTime) > 10*time.Minute {
|
||||
log.Printf("setting system time to: %s\n", gpsTime)
|
||||
setStr := gpsTime.Format("20060102 15:04:05")
|
||||
if err := exec.Command("date", "-s", setStr).Run(); err != nil {
|
||||
log.Printf("Set Date failure: %s error\n", err)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// otherwise look for NMEA standard messages and process them
|
||||
} else if (x[0] == "GNVTG") || (x[0] == "GPVTG") { // Ground track information.
|
||||
mySituation.mu_GPS.Lock()
|
||||
defer mySituation.mu_GPS.Unlock()
|
||||
if len(x) < 10 {
|
||||
|
|
@ -218,7 +595,7 @@ func processNMEALine(l string) bool {
|
|||
mySituation.GroundSpeed = uint16(groundSpeed)
|
||||
mySituation.LastGroundTrackTime = time.Now()
|
||||
|
||||
} else if (x[0] == "$GNGGA") || (x[0] == "$GPGGA") { // GPS fix.
|
||||
} else if (x[0] == "GNGGA") || (x[0] == "GPGGA") { // GPS fix.
|
||||
if len(x) < 15 {
|
||||
return false
|
||||
}
|
||||
|
|
@ -275,6 +652,7 @@ func processNMEALine(l string) bool {
|
|||
}
|
||||
mySituation.quality = uint8(q) // 1 = 3D GPS; 2 = DGPS (SBAS /WAAS)
|
||||
|
||||
/* Satellite count and horizontal accuracy deprecated. Use GSA/GST or PUBX,00 message.
|
||||
// Satellites.
|
||||
sat, err1 := strconv.Atoi(x[7])
|
||||
if err1 != nil {
|
||||
|
|
@ -309,6 +687,7 @@ func processNMEALine(l string) bool {
|
|||
} else {
|
||||
mySituation.NACp = 0
|
||||
}
|
||||
*/
|
||||
|
||||
// Altitude.
|
||||
alt, err1 := strconv.ParseFloat(x[9], 32)
|
||||
|
|
@ -317,13 +696,19 @@ func processNMEALine(l string) bool {
|
|||
}
|
||||
mySituation.Alt = float32(alt * 3.28084) // Convert to feet.
|
||||
|
||||
//TODO: Altitude accuracy.
|
||||
mySituation.alt_accuracy = 0
|
||||
// Geoid separation (Sep = HAE - MSL)
|
||||
// (needed for proper MSL offset on PUBX,00 altitudes)
|
||||
|
||||
geoidSep, err1 := strconv.ParseFloat(x[11], 32)
|
||||
if err1 != nil {
|
||||
return false
|
||||
}
|
||||
mySituation.GeoidSep = float32(geoidSep * 3.28084) // Convert to feet.
|
||||
|
||||
// Timestamp.
|
||||
mySituation.LastFixLocalTime = time.Now()
|
||||
|
||||
} else if (x[0] == "$GNRMC") || (x[0] == "$GPRMC") {
|
||||
} else if (x[0] == "GNRMC") || (x[0] == "GPRMC") {
|
||||
//$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A
|
||||
/* check RY835 man for NMEA version, if >2.2, add mode field
|
||||
Where:
|
||||
|
|
|
|||
|
|
@ -18,10 +18,10 @@
|
|||
<div class="row">
|
||||
<strong class="col-xs-6 text-center">Location:</strong>
|
||||
<strong class="col-xs-6 text-center">Track:</strong>
|
||||
</div>
|
||||
</div>
|
||||
<div class="row">
|
||||
<span class="col-xs-6 text-center">{{gps_lat}}, {{gps_lon}} ± {{gps_accuracy}} m</span>
|
||||
<span class="col-xs-6 text-center">{{gps_track}}° @ {{gps_speed}}KTS @ {{gps_alt}} ft</span>
|
||||
<span class="col-xs-6 text-center">{{gps_lat}}, {{gps_lon}} ± {{gps_accuracy}} m <br> {{gps_alt}} ± {{gps_vert_accuracy}} ft @ {{gps_vert_speed}} ft/min</span>
|
||||
<span class="col-xs-6 text-center">{{gps_track}}° @ {{gps_speed}} KTS</span>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
|
|
|||
|
|
@ -60,18 +60,22 @@ function GPSCtrl($rootScope, $scope, $state, $http, $interval) {
|
|||
/* not currently used
|
||||
$scope.gps_satellites = status.Satellites;
|
||||
*/
|
||||
$scope.gps_accuracy = Math.round(status.Accuracy);
|
||||
$scope.gps_accuracy = status.Accuracy.toFixed(1);
|
||||
$scope.gps_vert_accuracy = (status.AccuracyVert*3.2808).toFixed(1); // accuracy is in meters, need to display in ft
|
||||
|
||||
|
||||
// NACp should be an integer value in the range of 0 .. 11
|
||||
// var accuracies = ["≥ 10 NM", "< 10 NM", "< 4 NM", "< 2 NM", "< 1 NM", "< 0.5 NM", "< 0.3 NM", "< 0.1 NM", "< 100 m", "< 30 m", "< 10 m", "< 3 m"];
|
||||
// $scope.gps_accuracy = accuracies[status.NACp];
|
||||
// "LastFixLocalTime":"2015-10-11T16:47:03.523085162Z"
|
||||
|
||||
$scope.gps_lat = status.Lat.toPrecision(6); // result is string
|
||||
$scope.gps_lon = status.Lng.toPrecision(6); // result is string
|
||||
$scope.gps_lat = status.Lat.toFixed(5); // result is string
|
||||
$scope.gps_lon = status.Lng.toFixed(5); // result is string
|
||||
$scope.gps_alt = Math.round(status.Alt);
|
||||
$scope.gps_track = status.TrueCourse;
|
||||
$scope.gps_speed = status.GroundSpeed;
|
||||
$scope.gps_vert_speed = status.GPSVertVel.toFixed(1);
|
||||
|
||||
// "LastGroundTrackTime":"0001-01-01T00:00:00Z"
|
||||
|
||||
/* not currently used
|
||||
|
|
@ -103,9 +107,9 @@ function GPSCtrl($rootScope, $scope, $state, $http, $interval) {
|
|||
};
|
||||
|
||||
var updateStatus = $interval(function () {
|
||||
// refresh GPS/AHRS status once each second (aka polling)
|
||||
// refresh GPS/AHRS status once each half second (aka polling)
|
||||
getStatus();
|
||||
}, (1 * 1000), 0, false);
|
||||
}, (1 * 500), 0, false);
|
||||
|
||||
$state.get('gps').onEnter = function () {
|
||||
// everything gets handled correctly by the controller
|
||||
|
|
|
|||
|
|
@ -50,6 +50,8 @@ function StatusCtrl($rootScope, $scope, $state, $http, $interval) {
|
|||
$scope.ES_messages_last_minute = status.ES_messages_last_minute;
|
||||
$scope.ES_messages_max = status.ES_messages_max;
|
||||
$scope.GPS_satellites_locked = status.GPS_satellites_locked;
|
||||
$scope.GPS_satellites_tracked = status.GPS_satellites_tracked;
|
||||
$scope.GPS_satellites_seen = status.GPS_satellites_seen;
|
||||
$scope.GPS_solution = status.GPS_solution;
|
||||
$scope.RY835AI_connected = status.RY835AI_connected;
|
||||
|
||||
|
|
|
|||
|
|
@ -62,7 +62,7 @@
|
|||
</div>
|
||||
<div class="row" ng-class="{'section_invisible': !visible_gps}">
|
||||
<label class="col-xs-6">GPS satellites:</label>
|
||||
<span class="col-xs-6">{{GPS_satellites_locked}}</span>
|
||||
<span class="col-xs-6">{{GPS_satellites_locked}} in solution; {{GPS_satellites_seen}} seen; {{GPS_satellites_tracked}} tracked</span>
|
||||
</div>
|
||||
<div class="row" ng-class="{'section_invisible': !visible_ahrs}">
|
||||
<label class="col-xs-6">AHRS:</label>
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue