mirror
/
stratux
kopia lustrzana https://github.com/cyoung/stratux
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność

Merge branch 'ahrs_dev'

pull/592/head
Eric Westphal 2017-04-01 08:35:34 -04:00
rodzic c12aa3e728 18af334813
commit a998ede17b
6 zmienionych plików z 297 dodań i 293 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

70
main/gen_gdl90.go
Wyświetl plik

@ -247,11 +247,11 @@ func makeOwnshipReport() bool {
msg[9] = tmp[1] // Longitude.
msg[10] = tmp[2] // Longitude.
} else {
tmp = makeLatLng(mySituation.Lat)
tmp = makeLatLng(mySituation.GPSLatitude)
msg[5] = tmp[0] // Latitude.
msg[6] = tmp[1] // Latitude.
msg[7] = tmp[2] // Latitude.
tmp = makeLatLng(mySituation.Lng)
tmp = makeLatLng(mySituation.GPSLongitude)
msg[8] = tmp[0] // Longitude.
msg[9] = tmp[1] // Longitude.
msg[10] = tmp[2] // Longitude.
@ -267,9 +267,9 @@ func makeOwnshipReport() bool {
if selfOwnshipValid {
altf = float64(curOwnship.Alt)
} else if isTempPressValid() {
altf = float64(mySituation.Pressure_alt)
altf = float64(mySituation.BaroPressureAltitude)
} else {
altf = float64(mySituation.Alt) //FIXME: Pass GPS altitude if PA not available. **WORKAROUND FOR FF**
altf = float64(mySituation.GPSAltitudeMSL) //FIXME: Pass GPS altitude if PA not available. **WORKAROUND FOR FF**
}
altf = (altf + 1000) / 25
@ -282,13 +282,13 @@ func makeOwnshipReport() bool {
msg[12] = msg[12] | 0x09 // "Airborne" + "True Track"
}
msg[13] = byte(0x80 | (mySituation.NACp & 0x0F)) //Set NIC = 8 and use NACp from gps.go.
msg[13] = byte(0x80 | (mySituation.GPSNACp & 0x0F)) //Set NIC = 8 and use NACp from gps.go.
gdSpeed := uint16(0) // 1kt resolution.
if selfOwnshipValid && curOwnship.Speed_valid {
gdSpeed = curOwnship.Speed
} else if isGPSGroundTrackValid() {
gdSpeed = uint16(mySituation.GroundSpeed + 0.5)
gdSpeed = uint16(mySituation.GPSGroundSpeed + 0.5)
}
// gdSpeed should fit in 12 bits.
@ -306,7 +306,7 @@ func makeOwnshipReport() bool {
if selfOwnshipValid {
groundTrack = float32(curOwnship.Track)
} else if isGPSGroundTrackValid() {
groundTrack = mySituation.TrueCourse
groundTrack = mySituation.GPSTrueCourse
}
tempTrack := groundTrack + TRACK_RESOLUTION/2 // offset by half the 8-bit resolution to minimize binning error
@ -356,10 +356,10 @@ func makeOwnshipGeometricAltitudeReport() bool {
}
msg := make([]byte, 5)
// See p.28.
msg[0] = 0x0B // Message type "Ownship Geo Alt".
alt := int16(mySituation.Alt / 5) // GPS Altitude, encoded to 16-bit int using 5-foot resolution
msg[1] = byte(alt >> 8) // Altitude.
msg[2] = byte(alt & 0x00FF) // Altitude.
msg[0] = 0x0B // Message type "Ownship Geo Alt".
alt := int16(mySituation.GPSAltitudeMSL / 5) // GPS Altitude, encoded to 16-bit int using 5-foot resolution
msg[1] = byte(alt >> 8) // Altitude.
msg[2] = byte(alt & 0x00FF) // Altitude.
//TODO: "Figure of Merit". 0x7FFF "Not available".
msg[3] = 0x00
@ -425,7 +425,7 @@ func makeStratuxStatus() []byte {
// Valid and enabled flags.
// Valid/Enabled: GPS portion.
if isGPSValid() {
switch mySituation.Quality {
switch mySituation.GPSFixQuality {
case 1: // 1 = 3D GPS.
msg[13] = 1
case 2: // 2 = DGPS (SBAS /WAAS).
@ -753,13 +753,13 @@ func cpuMonitor() {
}
func updateStatus() {
if mySituation.Quality == 2 {
if mySituation.GPSFixQuality == 2 {
globalStatus.GPS_solution = "GPS + SBAS (WAAS)"
} else if mySituation.Quality == 1 {
} else if mySituation.GPSFixQuality == 1 {
globalStatus.GPS_solution = "3D GPS"
} else if mySituation.Quality == 6 {
} else if mySituation.GPSFixQuality == 6 {
globalStatus.GPS_solution = "Dead Reckoning"
} else if mySituation.Quality == 0 {
} else if mySituation.GPSFixQuality == 0 {
globalStatus.GPS_solution = "No Fix"
} else {
globalStatus.GPS_solution = "Unknown"
@ -767,22 +767,22 @@ func updateStatus() {
if !(globalStatus.GPS_connected) || !(isGPSConnected()) { // isGPSConnected looks for valid NMEA messages. GPS_connected is set by gpsSerialReader and will immediately fail on disconnected USB devices, or in a few seconds after "blocked" comms on ttyAMA0.
mySituation.mu_Satellite.Lock()
mySituation.muSatellite.Lock()
Satellites = make(map[string]SatelliteInfo)
mySituation.mu_Satellite.Unlock()
mySituation.muSatellite.Unlock()
mySituation.Satellites = 0
mySituation.SatellitesSeen = 0
mySituation.SatellitesTracked = 0
mySituation.Quality = 0
mySituation.GPSSatellites = 0
mySituation.GPSSatellitesSeen = 0
mySituation.GPSSatellitesTracked = 0
mySituation.GPSFixQuality = 0
globalStatus.GPS_solution = "Disconnected"
globalStatus.GPS_connected = false
}
globalStatus.GPS_satellites_locked = mySituation.Satellites
globalStatus.GPS_satellites_seen = mySituation.SatellitesSeen
globalStatus.GPS_satellites_tracked = mySituation.SatellitesTracked
globalStatus.GPS_position_accuracy = mySituation.Accuracy
globalStatus.GPS_satellites_locked = mySituation.GPSSatellites
globalStatus.GPS_satellites_seen = mySituation.GPSSatellitesSeen
globalStatus.GPS_satellites_tracked = mySituation.GPSSatellitesTracked
globalStatus.GPS_position_accuracy = mySituation.GPSHorizontalAccuracy
// Update Uptime value
globalStatus.Uptime = int64(stratuxClock.Milliseconds)
@ -1214,14 +1214,14 @@ func printStats() {
log.Printf(" - UAT/min %s/%s [maxSS=%.02f%%], ES/min %s/%s, 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))))
log.Printf(" - Network data messages sent: %d total, %d nonqueueable. Network data bytes sent: %d total, %d nonqueueable.\n", globalStatus.NetworkDataMessagesSent, globalStatus.NetworkDataMessagesSentNonqueueable, globalStatus.NetworkDataBytesSent, globalStatus.NetworkDataBytesSentNonqueueable)
if globalSettings.GPS_Enabled {
log.Printf(" - Last GPS fix: %s, GPS solution type: %d using %d satellites (%d/%d seen/tracked), NACp: %d, est accuracy %.02f m\n", stratuxClock.HumanizeTime(mySituation.LastFixLocalTime), mySituation.Quality, mySituation.Satellites, mySituation.SatellitesSeen, mySituation.SatellitesTracked, mySituation.NACp, mySituation.Accuracy)
log.Printf(" - GPS vertical velocity: %.02f ft/sec; GPS vertical accuracy: %v m\n", mySituation.GPSVertVel, mySituation.AccuracyVert)
log.Printf(" - Last GPS fix: %s, GPS solution type: %d using %d satellites (%d/%d seen/tracked), NACp: %d, est accuracy %.02f m\n", stratuxClock.HumanizeTime(mySituation.GPSLastFixLocalTime), mySituation.GPSFixQuality, mySituation.GPSSatellites, mySituation.GPSSatellitesSeen, mySituation.GPSSatellitesTracked, mySituation.GPSNACp, mySituation.GPSHorizontalAccuracy)
log.Printf(" - GPS vertical velocity: %.02f ft/sec; GPS vertical accuracy: %v m\n", mySituation.GPSVerticalSpeed, mySituation.GPSVerticalAccuracy)
}
if globalSettings.IMU_Sensor_Enabled {
log.Printf(" - Last IMU read: %s\n", stratuxClock.HumanizeTime(mySituation.LastAttitudeTime))
log.Printf(" - Last IMU read: %s\n", stratuxClock.HumanizeTime(mySituation.AHRSLastAttitudeTime))
}
if globalSettings.BMP_Sensor_Enabled {
log.Printf(" - Last BMP read: %s\n", stratuxClock.HumanizeTime(mySituation.LastTempPressTime))
log.Printf(" - Last BMP read: %s\n", stratuxClock.HumanizeTime(mySituation.BaroLastMeasurementTime))
}
// Check if we're using more than 95% of the free space. If so, throw a warning (only once).
if !diskUsageWarning && usage.Usage() > 0.95 {
@ -1334,11 +1334,11 @@ func main() {
stratuxClock = NewMonotonic() // Start our "stratux clock".
// Set up mySituation, do it here so logging JSON doesn't panic
mySituation.mu_GPS = &sync.Mutex{}
mySituation.mu_GPSPerf = &sync.Mutex{}
mySituation.mu_Attitude = &sync.Mutex{}
mySituation.mu_Pressure = &sync.Mutex{}
mySituation.mu_Satellite = &sync.Mutex{}
mySituation.muGPS = &sync.Mutex{}
mySituation.muGPSPerformance = &sync.Mutex{}
mySituation.muAttitude = &sync.Mutex{}
mySituation.muBaro = &sync.Mutex{}
mySituation.muSatellite = &sync.Mutex{}
// Set up status.
globalStatus.Version = stratuxVersion

316
main/gps.go
Wyświetl plik

@ -50,51 +50,51 @@ type SatelliteInfo struct {
type SituationData struct {
// From GPS.
mu_GPS *sync.Mutex
mu_GPSPerf *sync.Mutex
mu_Satellite *sync.Mutex
LastFixSinceMidnightUTC float32
Lat float32
Lng float32
Quality uint8
HeightAboveEllipsoid float32 // GPS height above WGS84 ellipsoid, ft. This is specified by the GDL90 protocol, but most EFBs use MSL altitude instead. HAE is about 70-100 ft below GPS MSL altitude over most of the US.
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 MSL
AccuracyVert float32 // 95% confidence for vertical position, meters
GPSVertVel float32 // GPS vertical velocity, feet per second
LastFixLocalTime time.Time
TrueCourse float32
GPSTurnRate float64 // calculated GPS rate of turn, degrees per second
GroundSpeed float64
LastGroundTrackTime time.Time
GPSTime time.Time
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.
muGPS *sync.Mutex
muGPSPerformance *sync.Mutex
muSatellite *sync.Mutex
GPSLastFixSinceMidnightUTC float32
GPSLatitude float32
GPSLongitude float32
GPSFixQuality uint8
GPSHeightAboveEllipsoid float32 // GPS height above WGS84 ellipsoid, ft. This is specified by the GDL90 protocol, but most EFBs use MSL altitude instead. HAE is about 70-100 ft below GPS MSL altitude over most of the US.
GPSGeoidSep float32 // geoid separation, ft, MSL minus HAE (used in altitude calculation)
GPSSatellites uint16 // satellites used in solution
GPSSatellitesTracked uint16 // satellites tracked (almanac data received)
GPSSatellitesSeen uint16 // satellites seen (signal received)
GPSHorizontalAccuracy float32 // 95% confidence for horizontal position, meters.
GPSNACp uint8 // NACp categories are defined in AC 20-165A
GPSAltitudeMSL float32 // Feet MSL
GPSVerticalAccuracy float32 // 95% confidence for vertical position, meters
GPSVerticalSpeed float32 // GPS vertical velocity, feet per second
GPSLastFixLocalTime time.Time
GPSTrueCourse float32
GPSTurnRate float64 // calculated GPS rate of turn, degrees per second
GPSGroundSpeed float64
GPSLastGroundTrackTime time.Time
GPSTime time.Time
GPSLastGPSTimeStratuxTime time.Time // stratuxClock time since last GPS time received.
GPSLastValidNMEAMessageTime time.Time // time valid NMEA message last seen
GPSLastValidNMEAMessage string // last NMEA message processed.
// From pressure sensor.
mu_Pressure *sync.Mutex
Temp float64
Pressure_alt float64
RateOfClimb float64
LastTempPressTime time.Time
muBaro *sync.Mutex
BaroTemperature float64
BaroPressureAltitude float64
BaroVerticalSpeed float64
BaroLastMeasurementTime time.Time
// From AHRS source.
mu_Attitude *sync.Mutex
Pitch float64
Roll float64
Gyro_heading float64
Mag_heading float64
SlipSkid float64
RateOfTurn float64
GLoad float64
LastAttitudeTime time.Time
AHRSStatus uint8
muAttitude *sync.Mutex
AHRSPitch float64
AHRSRoll float64
AHRSGyroHeading float64
AHRSMagHeading float64
AHRSSlipSkid float64
AHRSTurnRate float64
AHRSGLoad float64
AHRSLastAttitudeTime time.Time
AHRSStatus uint8
}
/*
@ -417,7 +417,7 @@ func validateNMEAChecksum(s string) (string, bool) {
//TODO: Some more robust checking above current and last speed.
//TODO: Dynamic adjust for gain based on groundspeed
func setTrueCourse(groundSpeed uint16, trueCourse float64) {
if mySituation.GroundSpeed >= 7 && groundSpeed >= 7 {
if mySituation.GPSGroundSpeed >= 7 && groundSpeed >= 7 {
// This was previously used to filter small ground speed spikes caused by GPS position drift.
// It was passed to the previous AHRS heading calculator. Currently unused, maybe in the future it will be.
_ = trueCourse
@ -439,8 +439,8 @@ the LICENSE file, herein included as part of this header.
func calcGPSAttitude() bool {
// check slice length. Return error if empty set or set zero values
mySituation.mu_GPSPerf.Lock()
defer mySituation.mu_GPSPerf.Unlock()
mySituation.muGPSPerformance.Lock()
defer mySituation.muGPSPerformance.Unlock()
length := len(myGPSPerfStats)
index := length - 1
@ -822,13 +822,13 @@ return is true if parse occurs correctly and position is valid.
*/
func processNMEALine(l string) (sentenceUsed bool) {
mySituation.mu_GPS.Lock()
mySituation.muGPS.Lock()
defer func() {
if sentenceUsed || globalSettings.DEBUG {
registerSituationUpdate()
}
mySituation.mu_GPS.Unlock()
mySituation.muGPS.Unlock()
}()
// Local variables for GPS attitude estimation
@ -844,8 +844,8 @@ func processNMEALine(l string) (sentenceUsed bool) {
}
x := strings.Split(l_valid, ",")
mySituation.LastValidNMEAMessageTime = stratuxClock.Time
mySituation.LastValidNMEAMessage = l
mySituation.GPSLastValidNMEAMessageTime = stratuxClock.Time
mySituation.GPSLastValidNMEAMessage = l
if x[0] == "PUBX" { // UBX proprietary message
if x[1] == "00" { // Position fix.
@ -868,16 +868,16 @@ func processNMEALine(l string) (sentenceUsed bool) {
// 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" {
tmpSituation.Quality = 2
tmpSituation.GPSFixQuality = 2
} else if x[8] == "G2" || x[8] == "G3" {
tmpSituation.Quality = 1
tmpSituation.GPSFixQuality = 1
} else if x[8] == "DR" || x[8] == "RK" {
tmpSituation.Quality = 6
tmpSituation.GPSFixQuality = 6
} else if x[8] == "NF" {
tmpSituation.Quality = 0 // Just a note.
tmpSituation.GPSFixQuality = 0 // Just a note.
return false
} else {
tmpSituation.Quality = 0 // Just a note.
tmpSituation.GPSFixQuality = 0 // Just a note.
return false
}
@ -886,17 +886,17 @@ func processNMEALine(l string) (sentenceUsed bool) {
if err != nil {
return false
}
tmpSituation.Accuracy = float32(hAcc * 2) // UBX reports 1-sigma variation; NACp is 95% confidence (2-sigma)
tmpSituation.GPSHorizontalAccuracy = float32(hAcc * 2) // UBX reports 1-sigma variation; NACp is 95% confidence (2-sigma)
// NACp estimate.
tmpSituation.NACp = calculateNACp(tmpSituation.Accuracy)
tmpSituation.GPSNACp = calculateNACp(tmpSituation.GPSHorizontalAccuracy)
// field 10 = vertical accuracy, m
vAcc, err := strconv.ParseFloat(x[10], 32)
if err != nil {
return false
}
tmpSituation.AccuracyVert = float32(vAcc * 2) // UBX reports 1-sigma variation; we want 95% confidence
tmpSituation.GPSVerticalAccuracy = float32(vAcc * 2) // UBX reports 1-sigma variation; we want 95% confidence
// field 2 = time
if len(x[2]) < 8 {
@ -909,8 +909,8 @@ func processNMEALine(l string) (sentenceUsed bool) {
return false
}
tmpSituation.LastFixSinceMidnightUTC = float32(3600*hr+60*min) + float32(sec)
thisGpsPerf.nmeaTime = tmpSituation.LastFixSinceMidnightUTC
tmpSituation.GPSLastFixSinceMidnightUTC = float32(3600*hr+60*min) + float32(sec)
thisGpsPerf.nmeaTime = tmpSituation.GPSLastFixSinceMidnightUTC
// field 3-4 = lat
if len(x[3]) < 10 {
@ -923,9 +923,9 @@ func processNMEALine(l string) (sentenceUsed bool) {
return false
}
tmpSituation.Lat = float32(hr) + float32(minf/60.0)
tmpSituation.GPSLatitude = float32(hr) + float32(minf/60.0)
if x[4] == "S" { // South = negative.
tmpSituation.Lat = -tmpSituation.Lat
tmpSituation.GPSLatitude = -tmpSituation.GPSLatitude
}
// field 5-6 = lon
@ -938,9 +938,9 @@ func processNMEALine(l string) (sentenceUsed bool) {
return false
}
tmpSituation.Lng = float32(hr) + float32(minf/60.0)
tmpSituation.GPSLongitude = float32(hr) + float32(minf/60.0)
if x[6] == "W" { // West = negative.
tmpSituation.Lng = -tmpSituation.Lng
tmpSituation.GPSLongitude = -tmpSituation.GPSLongitude
}
// field 7 = height above ellipsoid, m
@ -951,20 +951,20 @@ func processNMEALine(l string) (sentenceUsed bool) {
}
// the next 'if' statement is a workaround for a ubx7 firmware bug:
// PUBX,00 reports HAE with a floor of zero (i.e. negative altitudes are set to zero). This causes GPS altitude to never read lower than -GeoidSep,
// PUBX,00 reports HAE with a floor of zero (i.e. negative altitudes are set to zero). This causes GPS altitude to never read lower than -GPSGeoidSep,
// placing minimum GPS altitude at about +80' MSL over most of North America.
//
// This does not affect GGA messages, so we can just rely on GGA to set altitude in these cases. It's a slower (1 Hz vs 5 Hz / 10 Hz), less precise
// (0.1 vs 0.001 mm resolution) report, but in practice the accuracy never gets anywhere near this resolution, so this should be an acceptable tradeoff
if hae != 0 {
alt := float32(hae*3.28084) - tmpSituation.GeoidSep // convert to feet and offset by geoid separation
tmpSituation.HeightAboveEllipsoid = float32(hae * 3.28084) // feet
tmpSituation.Alt = alt
alt := float32(hae*3.28084) - tmpSituation.GPSGeoidSep // convert to feet and offset by geoid separation
tmpSituation.GPSHeightAboveEllipsoid = float32(hae * 3.28084) // feet
tmpSituation.GPSAltitudeMSL = alt
thisGpsPerf.alt = alt
}
tmpSituation.LastFixLocalTime = stratuxClock.Time
tmpSituation.GPSLastFixLocalTime = stratuxClock.Time
// field 11 = groundspeed, km/h
groundspeed, err := strconv.ParseFloat(x[11], 32)
@ -972,7 +972,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
return false
}
groundspeed = groundspeed * 0.540003 // convert to knots
tmpSituation.GroundSpeed = groundspeed
tmpSituation.GPSGroundSpeed = groundspeed
thisGpsPerf.gsf = float32(groundspeed)
// field 12 = track, deg
@ -984,22 +984,22 @@ func processNMEALine(l string) (sentenceUsed bool) {
if groundspeed > 3 { //TODO: use average groundspeed over last n seconds to avoid random "jumps"
trueCourse = float32(tc)
setTrueCourse(uint16(groundspeed), tc)
tmpSituation.TrueCourse = trueCourse
tmpSituation.GPSTrueCourse = trueCourse
thisGpsPerf.coursef = float32(tc)
} else {
thisGpsPerf.coursef = -999.9 // regression will skip negative values
// Negligible movement. Don't update course, but do use the slow speed.
//TODO: use average course over last n seconds?
}
tmpSituation.LastGroundTrackTime = stratuxClock.Time
tmpSituation.GPSLastGroundTrackTime = stratuxClock.Time
// field 13 = vertical velocity, m/s
vv, err := strconv.ParseFloat(x[13], 32)
if err != nil {
return false
}
tmpSituation.GPSVertVel = float32(vv * -3.28084) // convert to ft/sec and positive = up
thisGpsPerf.vv = tmpSituation.GPSVertVel
tmpSituation.GPSVerticalSpeed = float32(vv * -3.28084) // convert to ft/sec and positive = up
thisGpsPerf.vv = tmpSituation.GPSVerticalSpeed
// field 14 = age of diff corrections
@ -1008,13 +1008,13 @@ func processNMEALine(l string) (sentenceUsed bool) {
if err1 != nil {
return false
}
tmpSituation.Satellites = uint16(sat) // this seems to be reliable. UBX,03 handles >12 satellites solutions correctly.
tmpSituation.GPSSatellites = uint16(sat) // this seems to be reliable. UBX,03 handles >12 satellites solutions correctly.
// We've made it this far, so that means we've processed "everything" and can now make the change to mySituation.
mySituation = tmpSituation
updateGPSPerf = true
if updateGPSPerf {
mySituation.mu_GPSPerf.Lock()
mySituation.muGPSPerformance.Lock()
myGPSPerfStats = append(myGPSPerfStats, thisGpsPerf)
lenGPSPerfStats := len(myGPSPerfStats)
// log.Printf("GPSPerf array has %n elements. Contents are: %v\n",lenGPSPerfStats,myGPSPerfStats)
@ -1022,7 +1022,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
myGPSPerfStats = myGPSPerfStats[(lenGPSPerfStats - 299):] // remove the first n entries if more than 300 in the slice
}
mySituation.mu_GPSPerf.Unlock()
mySituation.muGPSPerformance.Unlock()
}
return true
@ -1050,7 +1050,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
return false
}
mySituation.SatellitesTracked = uint16(satTracked) // requires UBX M8N firmware v3.01 or later to report > 20 satellites
mySituation.GPSSatellitesTracked = uint16(satTracked) // requires UBX M8N firmware v3.01 or later to report > 20 satellites
// fields 3-8 are repeated block
@ -1096,7 +1096,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
var thisSatellite SatelliteInfo
// START OF PROTECTED BLOCK
mySituation.mu_Satellite.Lock()
mySituation.muSatellite.Lock()
// Retrieve previous information on this satellite code.
if val, ok := Satellites[svStr]; ok { // if we've already seen this satellite identifier, copy it in to do updates
@ -1156,7 +1156,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
Satellites[thisSatellite.SatelliteID] = thisSatellite // Update constellation with this satellite
updateConstellation()
mySituation.mu_Satellite.Unlock()
mySituation.muSatellite.Unlock()
// END OF PROTECTED BLOCK
// end of satellite iteration loop
@ -1196,10 +1196,10 @@ func processNMEALine(l string) (sentenceUsed bool) {
gpsTime, err := time.Parse("020106 15:04:05.000", gpsTimeStr)
if err == nil {
// We only update ANY of the times if all of the time parsing is complete.
mySituation.LastGPSTimeTime = stratuxClock.Time
mySituation.GPSLastGPSTimeStratuxTime = stratuxClock.Time
mySituation.GPSTime = gpsTime
stratuxClock.SetRealTimeReference(gpsTime)
mySituation.LastFixSinceMidnightUTC = float32(3600*hr+60*min) + float32(sec)
mySituation.GPSLastFixSinceMidnightUTC = float32(3600*hr+60*min) + float32(sec)
// log.Printf("GPS time is: %s\n", gpsTime) //debug
if time.Since(gpsTime) > 3*time.Second || time.Since(gpsTime) < -3*time.Second {
setStr := gpsTime.Format("20060102 15:04:05.000") + " UTC"
@ -1227,7 +1227,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
if err != nil {
return false
}
tmpSituation.GroundSpeed = groundspeed
tmpSituation.GPSGroundSpeed = groundspeed
trueCourse := float32(0)
tc, err := strconv.ParseFloat(x[1], 32)
@ -1237,12 +1237,12 @@ func processNMEALine(l string) (sentenceUsed bool) {
if groundspeed > 3 { //TODO: use average groundspeed over last n seconds to avoid random "jumps"
trueCourse = float32(tc)
setTrueCourse(uint16(groundspeed), tc)
tmpSituation.TrueCourse = trueCourse
tmpSituation.GPSTrueCourse = trueCourse
} else {
// Negligible movement. Don't update course, but do use the slow speed.
//TODO: use average course over last n seconds?
}
tmpSituation.LastGroundTrackTime = stratuxClock.Time
tmpSituation.GPSLastGroundTrackTime = stratuxClock.Time
// We've made it this far, so that means we've processed "everything" and can now make the change to mySituation.
mySituation = tmpSituation
@ -1260,12 +1260,12 @@ func processNMEALine(l string) (sentenceUsed bool) {
globalStatus.GPS_detected_type = GPS_TYPE_NMEA
}
// Quality indicator.
// GPSFixQuality indicator.
q, err1 := strconv.Atoi(x[6])
if err1 != nil {
return false
}
tmpSituation.Quality = uint8(q) // 1 = 3D GPS; 2 = DGPS (SBAS /WAAS)
tmpSituation.GPSFixQuality = uint8(q) // 1 = 3D GPS; 2 = DGPS (SBAS /WAAS)
// Timestamp.
if len(x[1]) < 7 {
@ -1278,9 +1278,9 @@ func processNMEALine(l string) (sentenceUsed bool) {
return false
}
tmpSituation.LastFixSinceMidnightUTC = float32(3600*hr+60*min) + float32(sec)
tmpSituation.GPSLastFixSinceMidnightUTC = float32(3600*hr+60*min) + float32(sec)
if globalStatus.GPS_detected_type != GPS_TYPE_UBX {
thisGpsPerf.nmeaTime = tmpSituation.LastFixSinceMidnightUTC
thisGpsPerf.nmeaTime = tmpSituation.GPSLastFixSinceMidnightUTC
}
// Latitude.
@ -1294,9 +1294,9 @@ func processNMEALine(l string) (sentenceUsed bool) {
return false
}
tmpSituation.Lat = float32(hr) + float32(minf/60.0)
tmpSituation.GPSLatitude = float32(hr) + float32(minf/60.0)
if x[3] == "S" { // South = negative.
tmpSituation.Lat = -tmpSituation.Lat
tmpSituation.GPSLatitude = -tmpSituation.GPSLatitude
}
// Longitude.
@ -1309,9 +1309,9 @@ func processNMEALine(l string) (sentenceUsed bool) {
return false
}
tmpSituation.Lng = float32(hr) + float32(minf/60.0)
tmpSituation.GPSLongitude = float32(hr) + float32(minf/60.0)
if x[5] == "W" { // West = negative.
tmpSituation.Lng = -tmpSituation.Lng
tmpSituation.GPSLongitude = -tmpSituation.GPSLongitude
}
// Altitude.
@ -1319,9 +1319,9 @@ func processNMEALine(l string) (sentenceUsed bool) {
if err1 != nil {
return false
}
tmpSituation.Alt = float32(alt * 3.28084) // Convert to feet.
tmpSituation.GPSAltitudeMSL = float32(alt * 3.28084) // Convert to feet.
if globalStatus.GPS_detected_type != GPS_TYPE_UBX {
thisGpsPerf.alt = float32(tmpSituation.Alt)
thisGpsPerf.alt = float32(tmpSituation.GPSAltitudeMSL)
}
// Geoid separation (Sep = HAE - MSL)
@ -1331,11 +1331,11 @@ func processNMEALine(l string) (sentenceUsed bool) {
if err1 != nil {
return false
}
tmpSituation.GeoidSep = float32(geoidSep * 3.28084) // Convert to feet.
tmpSituation.HeightAboveEllipsoid = tmpSituation.GeoidSep + tmpSituation.Alt
tmpSituation.GPSGeoidSep = float32(geoidSep * 3.28084) // Convert to feet.
tmpSituation.GPSHeightAboveEllipsoid = tmpSituation.GPSGeoidSep + tmpSituation.GPSAltitudeMSL
// Timestamp.
tmpSituation.LastFixLocalTime = stratuxClock.Time
tmpSituation.GPSLastFixLocalTime = stratuxClock.Time
if globalStatus.GPS_detected_type != GPS_TYPE_UBX {
updateGPSPerf = true
@ -1346,14 +1346,14 @@ func processNMEALine(l string) (sentenceUsed bool) {
mySituation = tmpSituation
if updateGPSPerf {
mySituation.mu_GPSPerf.Lock()
mySituation.muGPSPerformance.Lock()
myGPSPerfStats = append(myGPSPerfStats, thisGpsPerf)
lenGPSPerfStats := len(myGPSPerfStats)
// log.Printf("GPSPerf array has %n elements. Contents are: %v\n",lenGPSPerfStats,myGPSPerfStats)
if lenGPSPerfStats > 299 { //30 seconds @ 10 Hz for UBX, 30 seconds @ 5 Hz for MTK or SIRF with 2x messages per 200 ms)
myGPSPerfStats = myGPSPerfStats[(lenGPSPerfStats - 299):] // remove the first n entries if more than 300 in the slice
}
mySituation.mu_GPSPerf.Unlock()
mySituation.muGPSPerformance.Unlock()
}
return true
@ -1386,7 +1386,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
}
if x[2] != "A" { // invalid fix
tmpSituation.Quality = 0 // Just a note.
tmpSituation.GPSFixQuality = 0 // Just a note.
return false
}
@ -1400,9 +1400,9 @@ func processNMEALine(l string) (sentenceUsed bool) {
if err1 != nil || err2 != nil || err3 != nil {
return false
}
tmpSituation.LastFixSinceMidnightUTC = float32(3600*hr+60*min) + float32(sec)
tmpSituation.GPSLastFixSinceMidnightUTC = float32(3600*hr+60*min) + float32(sec)
if globalStatus.GPS_detected_type != GPS_TYPE_UBX {
thisGpsPerf.nmeaTime = tmpSituation.LastFixSinceMidnightUTC
thisGpsPerf.nmeaTime = tmpSituation.GPSLastFixSinceMidnightUTC
}
if len(x[9]) == 6 {
@ -1410,7 +1410,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
gpsTimeStr := fmt.Sprintf("%s %02d:%02d:%06.3f", x[9], hr, min, sec)
gpsTime, err := time.Parse("020106 15:04:05.000", gpsTimeStr)
if err == nil && gpsTime.After(time.Date(2016, time.January, 0, 0, 0, 0, 0, time.UTC)) { // Ignore dates before 2016-JAN-01.
tmpSituation.LastGPSTimeTime = stratuxClock.Time
tmpSituation.GPSLastGPSTimeStratuxTime = stratuxClock.Time
tmpSituation.GPSTime = gpsTime
stratuxClock.SetRealTimeReference(gpsTime)
if time.Since(gpsTime) > 3*time.Second || time.Since(gpsTime) < -3*time.Second {
@ -1434,9 +1434,9 @@ func processNMEALine(l string) (sentenceUsed bool) {
if err1 != nil || err2 != nil {
return false
}
tmpSituation.Lat = float32(hr) + float32(minf/60.0)
tmpSituation.GPSLatitude = float32(hr) + float32(minf/60.0)
if x[4] == "S" { // South = negative.
tmpSituation.Lat = -tmpSituation.Lat
tmpSituation.GPSLatitude = -tmpSituation.GPSLatitude
}
// Longitude.
if len(x[5]) < 5 {
@ -1447,19 +1447,19 @@ func processNMEALine(l string) (sentenceUsed bool) {
if err1 != nil || err2 != nil {
return false
}
tmpSituation.Lng = float32(hr) + float32(minf/60.0)
tmpSituation.GPSLongitude = float32(hr) + float32(minf/60.0)
if x[6] == "W" { // West = negative.
tmpSituation.Lng = -tmpSituation.Lng
tmpSituation.GPSLongitude = -tmpSituation.GPSLongitude
}
tmpSituation.LastFixLocalTime = stratuxClock.Time
tmpSituation.GPSLastFixLocalTime = stratuxClock.Time
// ground speed in kts (field 7)
groundspeed, err := strconv.ParseFloat(x[7], 32)
if err != nil {
return false
}
tmpSituation.GroundSpeed = groundspeed
tmpSituation.GPSGroundSpeed = groundspeed
if globalStatus.GPS_detected_type != GPS_TYPE_UBX {
thisGpsPerf.gsf = float32(groundspeed)
}
@ -1473,7 +1473,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
if groundspeed > 3 { //TODO: use average groundspeed over last n seconds to avoid random "jumps"
trueCourse = float32(tc)
setTrueCourse(uint16(groundspeed), tc)
tmpSituation.TrueCourse = trueCourse
tmpSituation.GPSTrueCourse = trueCourse
if globalStatus.GPS_detected_type != GPS_TYPE_UBX {
thisGpsPerf.coursef = float32(tc)
}
@ -1488,20 +1488,20 @@ func processNMEALine(l string) (sentenceUsed bool) {
updateGPSPerf = true
thisGpsPerf.msgType = x[0]
}
tmpSituation.LastGroundTrackTime = stratuxClock.Time
tmpSituation.GPSLastGroundTrackTime = stratuxClock.Time
// We've made it this far, so that means we've processed "everything" and can now make the change to mySituation.
mySituation = tmpSituation
if updateGPSPerf {
mySituation.mu_GPSPerf.Lock()
mySituation.muGPSPerformance.Lock()
myGPSPerfStats = append(myGPSPerfStats, thisGpsPerf)
lenGPSPerfStats := len(myGPSPerfStats)
// log.Printf("GPSPerf array has %n elements. Contents are: %v\n",lenGPSPerfStats,myGPSPerfStats)
if lenGPSPerfStats > 299 { //30 seconds @ 10 Hz for UBX, 30 seconds @ 5 Hz for MTK or SIRF with 2x messages per 200 ms)
myGPSPerfStats = myGPSPerfStats[(lenGPSPerfStats - 299):] // remove the first n entries if more than 300 in the slice
}
mySituation.mu_GPSPerf.Unlock()
mySituation.muGPSPerformance.Unlock()
}
setDataLogTimeWithGPS(mySituation)
@ -1520,7 +1520,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
/*
if (x[1] != "A") && (x[1] != "M") { // invalid fix ... but x[2] is a better indicator of fix quality. Deprecating this.
tmpSituation.Quality = 0 // Just a note.
tmpSituation.GPSFixQuality = 0 // Just a note.
return false
}
*/
@ -1528,7 +1528,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
// field 2: solution type
// 1 = no solution; 2 = 2D fix, 3 = 3D fix. WAAS status is parsed from GGA message, so no need to get here
if (x[2] == "") || (x[2] == "1") { // missing or no solution
tmpSituation.Quality = 0 // Just a note.
tmpSituation.GPSFixQuality = 0 // Just a note.
return false
}
@ -1563,7 +1563,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
var thisSatellite SatelliteInfo
// START OF PROTECTED BLOCK
mySituation.mu_Satellite.Lock()
mySituation.muSatellite.Lock()
// Retrieve previous information on this satellite code.
if val, ok := Satellites[svStr]; ok { // if we've already seen this satellite identifier, copy it in to do updates
@ -1582,15 +1582,15 @@ func processNMEALine(l string) (sentenceUsed bool) {
Satellites[thisSatellite.SatelliteID] = thisSatellite // Update constellation with this satellite
updateConstellation()
mySituation.mu_Satellite.Unlock()
mySituation.muSatellite.Unlock()
// END OF PROTECTED BLOCK
}
}
if sat < 12 || tmpSituation.Satellites < 13 { // GSA only reports up to 12 satellites in solution, so we don't want to overwrite higher counts based on updateConstellation().
tmpSituation.Satellites = uint16(sat)
if (tmpSituation.Quality == 2) && !svSBAS && !svGLONASS { // add one to the satellite count if we have a SBAS solution, but the GSA message doesn't track a SBAS satellite
tmpSituation.Satellites++
if sat < 12 || tmpSituation.GPSSatellites < 13 { // GSA only reports up to 12 satellites in solution, so we don't want to overwrite higher counts based on updateConstellation().
tmpSituation.GPSSatellites = uint16(sat)
if (tmpSituation.GPSFixQuality == 2) && !svSBAS && !svGLONASS { // add one to the satellite count if we have a SBAS solution, but the GSA message doesn't track a SBAS satellite
tmpSituation.GPSSatellites++
}
}
@ -1600,14 +1600,14 @@ func processNMEALine(l string) (sentenceUsed bool) {
if err1 != nil {
return false
}
if tmpSituation.Quality == 2 {
tmpSituation.Accuracy = float32(hdop * 4.0) // Rough 95% confidence estimate for WAAS / DGPS solution
if tmpSituation.GPSFixQuality == 2 {
tmpSituation.GPSHorizontalAccuracy = float32(hdop * 4.0) // Rough 95% confidence estimate for WAAS / DGPS solution
} else {
tmpSituation.Accuracy = float32(hdop * 8.0) // Rough 95% confidence estimate for 3D non-WAAS solution
tmpSituation.GPSHorizontalAccuracy = float32(hdop * 8.0) // Rough 95% confidence estimate for 3D non-WAAS solution
}
// NACp estimate.
tmpSituation.NACp = calculateNACp(tmpSituation.Accuracy)
tmpSituation.GPSNACp = calculateNACp(tmpSituation.GPSHorizontalAccuracy)
// field 17: VDOP
// accuracy estimate
@ -1615,7 +1615,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
if err1 != nil {
return false
}
tmpSituation.AccuracyVert = float32(vdop * 5) // rough estimate for 95% confidence
tmpSituation.GPSVerticalAccuracy = float32(vdop * 5) // rough estimate for 95% confidence
// We've made it this far, so that means we've processed "everything" and can now make the change to mySituation.
mySituation = tmpSituation
@ -1649,7 +1649,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
}
*/
//mySituation.SatellitesTracked = uint16(satTracked) // Replaced with parsing of 'Satellites' data structure
//mySituation.GPSSatellitesTracked = uint16(satTracked) // Replaced with parsing of 'Satellites' data structure
// field 4-7 = repeating block with satellite id, elevation, azimuth, and signal strengh (Cno)
@ -1687,7 +1687,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
var thisSatellite SatelliteInfo
// START OF PROTECTED BLOCK
mySituation.mu_Satellite.Lock()
mySituation.muSatellite.Lock()
// Retrieve previous information on this satellite code.
if val, ok := Satellites[svStr]; ok { // if we've already seen this satellite identifier, copy it in to do updates
@ -1729,7 +1729,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
// hack workaround for GSA 12-sv limitation... if this is a SBAS satellite, we have a SBAS solution, and signal is greater than some arbitrary threshold, set InSolution
// drawback is this will show all tracked SBAS satellites as being in solution.
if thisSatellite.Type == SAT_TYPE_SBAS {
if mySituation.Quality == 2 {
if mySituation.GPSFixQuality == 2 {
if thisSatellite.Signal > 16 {
thisSatellite.InSolution = true
thisSatellite.TimeLastSolution = stratuxClock.Time
@ -1750,7 +1750,7 @@ func processNMEALine(l string) (sentenceUsed bool) {
Satellites[thisSatellite.SatelliteID] = thisSatellite // Update constellation with this satellite
updateConstellation()
mySituation.mu_Satellite.Unlock()
mySituation.muSatellite.Unlock()
// END OF PROTECTED BLOCK
}
@ -1794,7 +1794,7 @@ func gpsSerialReader() {
}
func makeFFAHRSSimReport() {
s := fmt.Sprintf("XATTStratux,%f,%f,%f", mySituation.Gyro_heading, mySituation.Pitch, mySituation.Roll)
s := fmt.Sprintf("XATTStratux,%f,%f,%f", mySituation.AHRSGyroHeading, mySituation.AHRSPitch, mySituation.AHRSRoll)
sendMsg([]byte(s), NETWORK_AHRS_FFSIM, false)
}
@ -1817,16 +1817,16 @@ func makeAHRSGDL90Report() {
palt := uint16(0xFFFF)
vs := int16(0x7FFF)
if isAHRSValid() {
pitch = roundToInt16(mySituation.Pitch * 10)
roll = roundToInt16(mySituation.Roll * 10)
hdg = roundToInt16(mySituation.Gyro_heading * 10) // TODO westphae: switch to Mag_heading?
slip_skid = roundToInt16(mySituation.SlipSkid * 10)
yaw_rate = roundToInt16(mySituation.RateOfTurn * 10)
g = roundToInt16(mySituation.GLoad * 10)
pitch = roundToInt16(mySituation.AHRSPitch * 10)
roll = roundToInt16(mySituation.AHRSRoll * 10)
hdg = roundToInt16(mySituation.AHRSGyroHeading * 10) // TODO westphae: switch to AHRSMagHeading?
slip_skid = roundToInt16(mySituation.AHRSSlipSkid * 10)
yaw_rate = roundToInt16(mySituation.AHRSTurnRate * 10)
g = roundToInt16(mySituation.AHRSGLoad * 10)
}
if isTempPressValid() {
palt = uint16(mySituation.Pressure_alt + 5000.5)
vs = roundToInt16(mySituation.RateOfClimb)
palt = uint16(mySituation.BaroPressureAltitude + 5000.5)
vs = roundToInt16(mySituation.BaroVerticalSpeed)
}
// Roll.
@ -1880,22 +1880,22 @@ func gpsAttitudeSender() {
for !(globalSettings.IMU_Sensor_Enabled && globalStatus.IMUConnected) && (globalSettings.GPS_Enabled && globalStatus.GPS_connected) {
<-timer.C
if mySituation.Quality == 0 || !calcGPSAttitude() {
if mySituation.GPSFixQuality == 0 || !calcGPSAttitude() {
if globalSettings.DEBUG {
log.Printf("Couldn't calculate GPS-based attitude statistics\n")
}
} else {
mySituation.mu_GPSPerf.Lock()
mySituation.muGPSPerformance.Lock()
index := len(myGPSPerfStats) - 1
if index > 1 {
mySituation.Pitch = myGPSPerfStats[index].gpsPitch
mySituation.Roll = myGPSPerfStats[index].gpsRoll
mySituation.Gyro_heading = float64(mySituation.TrueCourse)
mySituation.LastAttitudeTime = stratuxClock.Time
mySituation.AHRSPitch = myGPSPerfStats[index].gpsPitch
mySituation.AHRSRoll = myGPSPerfStats[index].gpsRoll
mySituation.AHRSGyroHeading = float64(mySituation.GPSTrueCourse)
mySituation.AHRSLastAttitudeTime = stratuxClock.Time
makeAHRSGDL90Report()
}
mySituation.mu_GPSPerf.Unlock()
mySituation.muGPSPerformance.Unlock()
}
}
}
@ -1903,7 +1903,7 @@ func gpsAttitudeSender() {
/*
updateConstellation(): Periodic cleanup and statistics calculation for 'Satellites'
data structure. Calling functions must protect this in a mySituation.mu_Satellite.
data structure. Calling functions must protect this in a mySituation.muSatellite.
*/
@ -1928,47 +1928,47 @@ func updateConstellation() {
}
}
mySituation.Satellites = uint16(sats)
mySituation.SatellitesTracked = uint16(tracked)
mySituation.SatellitesSeen = uint16(seen)
mySituation.GPSSatellites = uint16(sats)
mySituation.GPSSatellitesTracked = uint16(tracked)
mySituation.GPSSatellitesSeen = uint16(seen)
}
func isGPSConnected() bool {
return stratuxClock.Since(mySituation.LastValidNMEAMessageTime) < 5*time.Second
return stratuxClock.Since(mySituation.GPSLastValidNMEAMessageTime) < 5*time.Second
}
/*
isGPSValid returns true only if a valid position fix has been seen in the last 15 seconds,
and if the GPS subsystem has recently detected a GPS device.
If false, 'Quality` is set to 0 ("No fix"), as is the number of satellites in solution.
If false, 'GPSFixQuality` is set to 0 ("No fix"), as is the number of satellites in solution.
*/
func isGPSValid() bool {
isValid := false
if (stratuxClock.Since(mySituation.LastFixLocalTime) < 15*time.Second) && globalStatus.GPS_connected && mySituation.Quality > 0 {
if (stratuxClock.Since(mySituation.GPSLastFixLocalTime) < 15*time.Second) && globalStatus.GPS_connected && mySituation.GPSFixQuality > 0 {
isValid = true
} else {
mySituation.Quality = 0
mySituation.Satellites = 0
mySituation.GPSFixQuality = 0
mySituation.GPSSatellites = 0
}
return isValid
}
func isGPSGroundTrackValid() bool {
return stratuxClock.Since(mySituation.LastGroundTrackTime) < 15*time.Second
return stratuxClock.Since(mySituation.GPSLastGroundTrackTime) < 15*time.Second
}
func isGPSClockValid() bool {
return stratuxClock.Since(mySituation.LastGPSTimeTime) < 15*time.Second
return stratuxClock.Since(mySituation.GPSLastGPSTimeStratuxTime) < 15*time.Second
}
func isAHRSValid() bool {
return stratuxClock.Since(mySituation.LastAttitudeTime) < 1*time.Second // If attitude information gets to be over 1 second old, declare invalid.
return stratuxClock.Since(mySituation.AHRSLastAttitudeTime) < 1*time.Second // If attitude information gets to be over 1 second old, declare invalid.
}
func isTempPressValid() bool {
return stratuxClock.Since(mySituation.LastTempPressTime) < 15*time.Second
return stratuxClock.Since(mySituation.BaroLastMeasurementTime) < 15*time.Second
}
func pollGPS() {

4
main/managementinterface.go
Wyświetl plik

@ -222,13 +222,13 @@ func handleTowersRequest(w http.ResponseWriter, r *http.Request) {
func handleSatellitesRequest(w http.ResponseWriter, r *http.Request) {
setNoCache(w)
setJSONHeaders(w)
mySituation.mu_Satellite.Lock()
mySituation.muSatellite.Lock()
satellitesJSON, err := json.Marshal(&Satellites)
if err != nil {
log.Printf("Error sending GNSS satellite JSON data: %s\n", err.Error())
}
fmt.Fprintf(w, "%s\n", satellitesJSON)
mySituation.mu_Satellite.Unlock()
mySituation.muSatellite.Unlock()
}
// AJAX call - /getSettings. Responds with all stratux.conf data.

112
main/sensors.go
Wyświetl plik

@ -105,7 +105,7 @@ func tempAndPressureSender() {
press, err = myPressureReader.Pressure()
if err != nil {
log.Printf("AHRS Error: Couldn't read pressure from sensor: %s", err)
failnum += 1
failnum++
if failnum > numRetries {
log.Printf("AHRS Error: Couldn't read pressure from sensor %d times, closing BMP: %s", failnum, err)
myPressureReader.Close()
@ -115,17 +115,17 @@ func tempAndPressureSender() {
}
// Update the Situation data.
mySituation.mu_Pressure.Lock()
mySituation.LastTempPressTime = stratuxClock.Time
mySituation.Temp = temp
mySituation.muBaro.Lock()
mySituation.BaroLastMeasurementTime = stratuxClock.Time
mySituation.BaroTemperature = temp
altitude = CalcAltitude(press)
mySituation.Pressure_alt = altitude
mySituation.BaroPressureAltitude = altitude
if altLast < -2000 {
altLast = altitude // Initialize
}
// Assuming timer is reasonably accurate, use a regular ewma
mySituation.RateOfClimb = u*mySituation.RateOfClimb + (1-u)*(altitude-altLast)/(float64(dt)/60)
mySituation.mu_Pressure.Unlock()
mySituation.BaroVerticalSpeed = u*mySituation.BaroVerticalSpeed + (1-u)*(altitude-altLast)/(float64(dt)/60)
mySituation.muBaro.Unlock()
altLast = altitude
}
}
@ -142,11 +142,10 @@ func initIMU() (ok bool) {
log.Println("AHRS Info: Successfully calibrated MPU9250")
ahrsCalibrating = false
return true
} else {
log.Println("AHRS Info: couldn't calibrate MPU9250")
ahrsCalibrating = false
return false
}
log.Println("AHRS Info: couldn't calibrate MPU9250")
ahrsCalibrating = false
return false
}
// TODO westphae: try to connect to MPU9150
@ -182,28 +181,7 @@ func sensorAttitudeSender() {
// Need a sampling freq faster than 10Hz
timer := time.NewTicker(50 * time.Millisecond) // ~20Hz update.
for {
if globalSettings.IMUMapping[0] == 0 { // if unset, default to RY836AI
globalSettings.IMUMapping[0] = -1 // +2
globalSettings.IMUMapping[1] = -3 // +3
saveSettings()
}
f := globalSettings.IMUMapping
// Set up orientation matrix; a bit ugly for now
ff = new([3][3]float64)
if f[0] < 0 {
ff[0][-f[0]-1] = -1
} else {
ff[0][+f[0]-1] = +1
}
if f[1] < 0 {
ff[2][-f[1]-1] = -1
} else {
ff[2][+f[1]-1] = +1
}
ff[1][0] = ff[2][1]*ff[0][2] - ff[2][2]*ff[0][1]
ff[1][1] = ff[2][2]*ff[0][0] - ff[2][0]*ff[0][2]
ff[1][2] = ff[2][0]*ff[0][1] - ff[2][1]*ff[0][0]
ff = makeSensorRotationMatrix(m)
failnum = 0
<-timer.C
@ -214,6 +192,8 @@ func sensorAttitudeSender() {
ahrsCalibrating = true
if err := myIMUReader.Calibrate(1, 1); err == nil {
log.Println("AHRS Info: Successfully recalibrated MPU9250")
ff = makeSensorRotationMatrix(m)
} else {
log.Println("AHRS Info: couldn't recalibrate MPU9250")
}
@ -247,7 +227,7 @@ func sensorAttitudeSender() {
m.MValid = magError == nil
if mpuError != nil {
log.Printf("AHRS Gyro/Accel Error: %s\n", mpuError)
failnum += 1
failnum++
if failnum > numRetries {
log.Printf("AHRS Gyro/Accel Error: failed to read %d times, restarting: %s\n",
failnum-1, mpuError)
@ -263,15 +243,15 @@ func sensorAttitudeSender() {
// Don't necessarily disconnect here, unless AHRSProvider deeply depends on magnetometer
}
m.TW = float64(mySituation.LastGroundTrackTime.UnixNano()/1000) / 1e6
m.WValid = t.Sub(mySituation.LastGroundTrackTime) < 3000*time.Millisecond
m.TW = float64(mySituation.GPSLastGroundTrackTime.UnixNano()/1000) / 1e6
m.WValid = t.Sub(mySituation.GPSLastGroundTrackTime) < 3000*time.Millisecond
if m.WValid {
m.W1 = mySituation.GroundSpeed * math.Sin(float64(mySituation.TrueCourse)*ahrs.Deg)
m.W2 = mySituation.GroundSpeed * math.Cos(float64(mySituation.TrueCourse)*ahrs.Deg)
m.W1 = mySituation.GPSGroundSpeed * math.Sin(float64(mySituation.GPSTrueCourse)*ahrs.Deg)
m.W2 = mySituation.GPSGroundSpeed * math.Cos(float64(mySituation.GPSTrueCourse)*ahrs.Deg)
if globalSettings.BMP_Sensor_Enabled && globalStatus.BMPConnected {
m.W3 = mySituation.RateOfClimb * 60 / 6076.12
m.W3 = mySituation.BaroVerticalSpeed * 60 / 6076.12
} else {
m.W3 = float64(mySituation.GPSVertVel) * 3600 / 6076.12
m.W3 = float64(mySituation.GPSVerticalSpeed) * 3600 / 6076.12
}
}
@ -282,20 +262,20 @@ func sensorAttitudeSender() {
// If we have valid AHRS info, then update mySituation
if s.Valid() {
mySituation.mu_Attitude.Lock()
mySituation.muAttitude.Lock()
roll, pitch, heading = s.RollPitchHeading()
mySituation.Roll = roll / ahrs.Deg
mySituation.Pitch = pitch / ahrs.Deg
mySituation.Gyro_heading = heading / ahrs.Deg
mySituation.AHRSRoll = roll / ahrs.Deg
mySituation.AHRSPitch = pitch / ahrs.Deg
mySituation.AHRSGyroHeading = heading / ahrs.Deg
mySituation.Mag_heading = s.MagHeading()
mySituation.SlipSkid = s.SlipSkid()
mySituation.RateOfTurn = s.RateOfTurn()
mySituation.GLoad = s.GLoad()
mySituation.AHRSMagHeading = s.MagHeading()
mySituation.AHRSSlipSkid = s.SlipSkid()
mySituation.AHRSTurnRate = s.RateOfTurn()
mySituation.AHRSGLoad = s.GLoad()
mySituation.LastAttitudeTime = t
mySituation.mu_Attitude.Unlock()
mySituation.AHRSLastAttitudeTime = t
mySituation.muAttitude.Unlock()
// makeFFAHRSSimReport() // simultaneous use of GDL90 and FFSIM not supported in FF 7.5.1 or later. Function definition will be kept for AHRS debugging and future workarounds.
} else {
@ -328,6 +308,34 @@ func sensorAttitudeSender() {
}
}
func makeSensorRotationMatrix(mCal *ahrs.Measurement) (ff *[3][3]float64) {
if globalSettings.IMUMapping[0] == 0 { // if unset, default to RY836AI in standard orientation
globalSettings.IMUMapping[0] = -1 // +2
globalSettings.IMUMapping[1] = -3 // +3
saveSettings()
}
f := globalSettings.IMUMapping
ff = new([3][3]float64)
// TODO westphae: remove the projection on the measured gravity vector so it's orthogonal.
if f[0] < 0 { // This is the "forward direction" chosen for the sensor.
ff[0][-f[0]-1] = -1
} else {
ff[0][+f[0]-1] = +1
}
//TODO westphae: replace "up direction" with opposite of measured gravity.
if f[1] < 0 { // This is the "up direction" chosen for the sensor.
ff[2][-f[1]-1] = -1
} else {
ff[2][+f[1]-1] = +1
}
// This specifies the "left wing" direction for a right-handed coordinate system.
ff[1][0] = ff[2][1]*ff[0][2] - ff[2][2]*ff[0][1]
ff[1][1] = ff[2][2]*ff[0][0] - ff[2][0]*ff[0][2]
ff[1][2] = ff[2][0]*ff[0][1] - ff[2][1]*ff[0][0]
return ff
}
// This is used in the orientation process where the user specifies the forward and up directions.
func getMinAccelDirection() (i int, err error) {
_, _, _, _, a1, a2, a3, _, _, _, err, _ := myIMUReader.Read()
if err != nil {
@ -379,8 +387,8 @@ func updateAHRSStatus() {
msg = 0
// GPS valid
if stratuxClock.Time.Sub(mySituation.LastGroundTrackTime) < 3000*time.Millisecond {
msg += 1
if stratuxClock.Time.Sub(mySituation.GPSLastGroundTrackTime) < 3000*time.Millisecond {
msg++
}
// IMU is being used
imu = globalSettings.IMU_Sensor_Enabled && globalStatus.IMUConnected

12
main/traffic.go
Wyświetl plik

@ -187,7 +187,7 @@ func sendTrafficUpdates() {
for icao, ti := range traffic { // ForeFlight 7.5 chokes at ~1000-2000 messages depending on iDevice RAM. Practical limit likely around ~500 aircraft without filtering.
if isGPSValid() {
// func distRect(lat1, lon1, lat2, lon2 float64) (dist, bearing, distN, distE float64) {
dist, bearing := distance(float64(mySituation.Lat), float64(mySituation.Lng), float64(ti.Lat), float64(ti.Lng))
dist, bearing := distance(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng))
ti.Distance = dist
ti.Bearing = bearing
}
@ -489,7 +489,7 @@ func parseDownlinkReport(s string, signalLevel int) {
ti.Lat = lat
ti.Lng = lng
if isGPSValid() {
ti.Distance, ti.Bearing = distance(float64(mySituation.Lat), float64(mySituation.Lng), float64(ti.Lat), float64(ti.Lng))
ti.Distance, ti.Bearing = distance(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng))
}
ti.Last_seen = stratuxClock.Time
ti.ExtrapolatedPosition = false
@ -772,7 +772,7 @@ func esListen() {
ti.Lat = lat
ti.Lng = lng
if isGPSValid() {
ti.Distance, ti.Bearing = distance(float64(mySituation.Lat), float64(mySituation.Lng), float64(ti.Lat), float64(ti.Lng))
ti.Distance, ti.Bearing = distance(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng))
}
ti.Position_valid = true
ti.ExtrapolatedPosition = false
@ -976,8 +976,8 @@ func updateDemoTraffic(icao uint32, tail string, relAlt float32, gs float64, off
lat := 43.99
lng := -88.56
if isGPSValid() {
lat = float64(mySituation.Lat)
lng = float64(mySituation.Lng)
lat = float64(mySituation.GPSLatitude)
lng = float64(mySituation.GPSLongitude)
}
traffRelLat := y / 60
traffRelLng := -x / (60 * math.Cos(lat*math.Pi/180.0))
@ -1010,7 +1010,7 @@ func updateDemoTraffic(icao uint32, tail string, relAlt float32, gs float64, off
ti.Position_valid = true
ti.ExtrapolatedPosition = false
ti.Alt = int32(mySituation.Alt + relAlt)
ti.Alt = int32(mySituation.GPSAltitudeMSL + relAlt)
ti.Track = uint16(hdg)
ti.Speed = uint16(gs)
if hdg >= 240 && hdg < 270 {

76
web/plates/js/gps.js
Wyświetl plik

@ -101,62 +101,61 @@ function GPSCtrl($rootScope, $scope, $state, $http, $interval) {
// consider using angular.extend()
$scope.raw_data = angular.toJson(data, true); // makes it pretty
$scope.Satellites = situation.Satellites;
$scope.GPS_satellites_tracked = situation.SatellitesTracked;
$scope.GPS_satellites_seen = situation.SatellitesSeen;
$scope.Quality = situation.Quality;
$scope.Satellites = situation.GPSSatellites;
$scope.GPS_satellites_tracked = situation.GPSSatellitesTracked;
$scope.GPS_satellites_seen = situation.GPSSatellitesSeen;
$scope.Quality = situation.GPSFixQuality;
var solutionText = "No Fix";
if (situation.Quality === 2) {
if (situation.GPSFixQuality === 2) {
solutionText = "GPS + SBAS (WAAS / EGNOS)";
} else if (situation.Quality === 1) {
} else if (situation.GPSFixQuality === 1) {
solutionText = "3D GPS"
}
$scope.SolutionText = solutionText;
$scope.gps_accuracy = situation.Accuracy.toFixed(1);
$scope.gps_vert_accuracy = (situation.AccuracyVert*3.2808).toFixed(1); // accuracy is in meters, need to display in ft
$scope.gps_horizontal_accuracy = situation.GPSHorizontalAccuracy.toFixed(1);
$scope.gps_vertical_accuracy = (situation.GPSVerticalAccuracy*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];
// $scope.gps_horizontal_accuracy = accuracies[status.NACp];
// "LastFixLocalTime":"2015-10-11T16:47:03.523085162Z"
$scope.gps_lat = situation.Lat.toFixed(5); // result is string
$scope.gps_lon = situation.Lng.toFixed(5); // result is string
$scope.gps_alt = situation.Alt.toFixed(1);
$scope.gps_track = situation.TrueCourse.toFixed(1);
$scope.gps_speed = situation.GroundSpeed.toFixed(1);
$scope.gps_vert_speed = situation.GPSVertVel.toFixed(1);
$scope.gps_lat = situation.GPSLatitude.toFixed(5); // result is string
$scope.gps_lon = situation.GPSLongitude.toFixed(5); // result is string
$scope.gps_alt = situation.GPSAltitudeMSL.toFixed(1);
$scope.gps_track = situation.GPSTrueCourse.toFixed(1);
$scope.gps_speed = situation.GPSGroundSpeed.toFixed(1);
$scope.gps_vert_speed = situation.GPSVerticalSpeed.toFixed(1);
// "LastGroundTrackTime":"0001-01-01T00:00:00Z"
/* not currently used
$scope.ahrs_temp = status.Temp;
*/
$scope.press_time = Date.parse(situation.LastTempPressTime);
$scope.gps_time = Date.parse(situation.LastGPSTimeTime);
$scope.ahrs_temp = status.Temp;
*/
$scope.press_time = Date.parse(situation.BaroLastMeasurementTime);
$scope.gps_time = Date.parse(situation.GPSLastGPSTimeStratuxTime);
if ($scope.gps_time - $scope.press_time < 1000) {
$scope.ahrs_alt = Math.round(situation.Pressure_alt);
$scope.ahrs_alt = Math.round(situation.BaroPressureAltitude);
} else {
$scope.ahrs_alt = "---";
}
$scope.ahrs_heading = Math.round(situation.Gyro_heading);
$scope.ahrs_heading = Math.round(situation.AHRSGyroHeading);
// pitch and roll are in degrees
$scope.ahrs_pitch = Math.round(situation.Pitch*10)/10;
$scope.ahrs_roll = Math.round(situation.Roll*10)/10;
$scope.ahrs_slip_skid = Math.round(situation.SlipSkid*10)/10;
$scope.ahrs_pitch = Math.round(situation.AHRSPitch*10)/10;
$scope.ahrs_roll = Math.round(situation.AHRSRoll*10)/10;
$scope.ahrs_slip_skid = Math.round(situation.AHRSSlipSkid*10)/10;
ahrs.update($scope.ahrs_pitch, $scope.ahrs_roll, $scope.ahrs_heading, $scope.ahrs_slip_skid);
$scope.ahrs_heading_mag = Math.round(situation.Mag_heading);
$scope.ahrs_gload = Math.round(situation.GLoad*100)/100;
$scope.ahrs_heading_mag = Math.round(situation.AHRSMagHeading);
$scope.ahrs_gload = Math.round(situation.AHRSGLoad*100)/100;
gMeter.update($scope.ahrs_gload);
if (situation.RateOfTurn > 1) {
$scope.ahrs_turn_rate = Math.round(360/situation.RateOfTurn/60*10)/10; // minutes/turn
if (situation.AHRSTurnRate> 0.25) {
$scope.ahrs_turn_rate = Math.round(360/situation.AHRSTurnRate/60*10)/10; // minutes/turn
} else {
$scope.ahrs_turn_rate = '---'
}
@ -202,7 +201,7 @@ function GPSCtrl($rootScope, $scope, $state, $http, $interval) {
}
// "LastAttitudeTime":"2015-10-11T16:47:03.534615187Z"
setGeoReferenceMap(situation.Lat, situation.Lng);
setGeoReferenceMap(situation.GPSLatitude, situation.GPSLongitude);
}
function resetSituation() { // mySituation
@ -246,12 +245,11 @@ function GPSCtrl($rootScope, $scope, $state, $http, $interval) {
new_satellite.InSolution = obj.InSolution; // is this satellite in the position solution
}
function loadSatellites(data) {
function loadSatellites(satellites) {
if (($scope === undefined) || ($scope === null))
return; // we are getting called once after clicking away from the status page
var satellites = data; // it seems the json was already converted to an object list by the http request
$scope.raw_data = angular.toJson(data, true);
$scope.raw_data = angular.toJson(satellites, true);
$scope.data_list.length = 0; // clear array
// we need to use an array so AngularJS can perform sorting; it also means we need to loop to find a tower in the towers set
@ -264,11 +262,9 @@ function GPSCtrl($rootScope, $scope, $state, $http, $interval) {
}
// $scope.$apply();
}
var updateSatellites = $interval(function () {
// refresh satellite info once each second (aka polling)
getSatellites();
}, 1000, 0, false);
// refresh satellite info once each second (aka polling)
var updateSatellites = $interval(getSatellites, 1000, 0, false);
$state.get('gps').onEnter = function () {
// everything gets handled correctly by the controller