kopia lustrzana https://github.com/cyoung/stratux
Merge pull request #598 from dndx/ahrs_dev
Added nav rate display and Galileo nav rate change for AHRS branch.ahrs_dev
commit
84e5165292
78
main/gps.go
78
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
|
||||
|
||||
|
|
|
@ -110,7 +110,7 @@
|
|||
<div class="panel-footer">
|
||||
<div class="row">
|
||||
<label class="col-xs-6">GPS solution:</label>
|
||||
<span class="col-xs-6">{{SolutionText}}</span>
|
||||
<span class="col-xs-6">{{SolutionText}} @ {{ GPS_PositionSampleRate }} Hz</span>
|
||||
</div>
|
||||
<div class="row">
|
||||
<label class="col-xs-6">Summary:</label>
|
||||
|
|
|
@ -109,6 +109,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) {
|
||||
|
|
Ładowanie…
Reference in New Issue