kopia lustrzana https://github.com/cyoung/stratux
Add distance / bearing calculation. Don't log stale traffic.
AvSquirrel
rodzic
d407c513c9
commit
8b9c89542f
2
Makefile
2
Makefile
|
|
@ -14,7 +14,7 @@ all:
|
|||
|
||||
xgen_gdl90:
|
||||
go get -t -d -v ./main ./test ./linux-mpu9150/mpu ./godump978 ./mpu6050 ./uatparse
|
||||
go build $(BUILDINFO) -p 4 main/gen_gdl90.go main/traffic.go main/ry835ai.go main/network.go main/managementinterface.go main/sdr.go main/uibroadcast.go main/monotonic.go main/datalog.go
|
||||
go build $(BUILDINFO) -p 4 main/gen_gdl90.go main/traffic.go main/ry835ai.go main/network.go main/managementinterface.go main/sdr.go main/uibroadcast.go main/monotonic.go main/datalog.go main/equations.go
|
||||
|
||||
xdump1090:
|
||||
git submodule update --init
|
||||
|
|
|
|||
|
|
@ -0,0 +1,324 @@
|
|||
/*
|
||||
Copyright (c) 2016 AvSquirrel (https://github.com/AvSquirrel)
|
||||
Distributable under the terms of the "BSD New" License
|
||||
that can be found in the LICENSE file, herein included
|
||||
as part of this header.
|
||||
|
||||
equations.go: Math and statistics library used to support AHRS
|
||||
and other fuctions of Stratux package
|
||||
*/
|
||||
|
||||
package main
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"math"
|
||||
)
|
||||
|
||||
// linReg calculates slope and intercept for a least squares linear regression of y[] vs x[]
|
||||
// Returns error if fewer than two data points in each series, or if series lengths are different
|
||||
|
||||
func linReg(x, y []float64) (slope, intercept float64, valid bool) {
|
||||
|
||||
n := len(x)
|
||||
nf := float64(n)
|
||||
|
||||
if n != len(y) {
|
||||
fmt.Printf("linReg: Lengths not equal\n")
|
||||
return math.NaN(), math.NaN(), false
|
||||
}
|
||||
|
||||
if n < 2 {
|
||||
fmt.Printf("linReg: Lengths too short\n")
|
||||
return math.NaN(), math.NaN(), false
|
||||
}
|
||||
|
||||
var Sx, Sy, Sxx, Sxy, Syy float64
|
||||
|
||||
for i := range x {
|
||||
Sx += x[i]
|
||||
Sy += y[i]
|
||||
Sxx += x[i] * x[i]
|
||||
Sxy += x[i] * y[i]
|
||||
Syy += y[i] * y[i]
|
||||
}
|
||||
|
||||
if nf*Sxx == Sx*Sx {
|
||||
fmt.Printf("linReg: Infinite slope\n")
|
||||
return math.NaN(), math.NaN(), false
|
||||
}
|
||||
|
||||
// Calculate slope and intercept
|
||||
slope = (nf*Sxy - Sx*Sy) / (nf*Sxx - Sx*Sx)
|
||||
intercept = Sy/nf - slope*Sx/nf
|
||||
valid = true
|
||||
return
|
||||
}
|
||||
|
||||
// linRegWeighted calculates slope and intercept for a weighted least squares
|
||||
// linear regression of y[] vs x[], given weights w[] for each point.
|
||||
// Returns error if fewer than two data points in each series, if series lengths are different,
|
||||
// if weights sum to zero, or if slope is infinite
|
||||
|
||||
func linRegWeighted(x, y, w []float64) (slope, intercept float64, valid bool) {
|
||||
|
||||
n := len(x)
|
||||
|
||||
if n != len(y) || n != len(w) {
|
||||
fmt.Printf("linRegWeighted: Lengths not equal\n")
|
||||
return math.NaN(), math.NaN(), false
|
||||
}
|
||||
|
||||
if n < 2 {
|
||||
fmt.Printf("linRegWeighted: Lengths too short\n")
|
||||
return math.NaN(), math.NaN(), false
|
||||
}
|
||||
|
||||
//var Sx, Sy, Sxx, Sxy, Syy float64
|
||||
var Sw, Swx, Swy, Swxx, Swxy, Swyy float64
|
||||
|
||||
for i := range x {
|
||||
Sw += w[i]
|
||||
Swxy += w[i] * x[i] * y[i]
|
||||
Swx += w[i] * x[i]
|
||||
Swy += w[i] * y[i]
|
||||
Swxx += w[i] * x[i] * x[i]
|
||||
Swyy += w[i] * y[i] * y[i]
|
||||
/*
|
||||
Sx += x[i]
|
||||
Sy += y[i]
|
||||
Sxx += x[i]*x[i]
|
||||
Sxy += x[i]*y[i]
|
||||
Syy += y[i]*y[i]
|
||||
*/
|
||||
}
|
||||
|
||||
if Sw == 0 {
|
||||
fmt.Printf("linRegWeighted: Sum of weights is zero\n")
|
||||
return math.NaN(), math.NaN(), false
|
||||
}
|
||||
|
||||
if Sw*Swxx == Swx*Swx {
|
||||
fmt.Printf("linRegWeighted: Infinite slope\n")
|
||||
return math.NaN(), math.NaN(), false
|
||||
}
|
||||
|
||||
// Calculate slope and intercept
|
||||
slope = (Sw*Swxy - Swx*Swy) / (Sw*Swxx - Swx*Swx)
|
||||
intercept = Swy/Sw - slope*Swx/Sw
|
||||
valid = true
|
||||
return
|
||||
}
|
||||
|
||||
// triCubeWeight returns the value of the tricube weight function
|
||||
// at point x, for the given center and halfwidth.
|
||||
func triCubeWeight(center, halfwidth, x float64) float64 {
|
||||
var weight, x_t float64
|
||||
x_t = math.Abs((x - center) / halfwidth)
|
||||
if x_t < 1 {
|
||||
weight = math.Pow((1 - math.Pow(x_t, 3)), 3)
|
||||
} else {
|
||||
weight = 0
|
||||
}
|
||||
return weight
|
||||
}
|
||||
|
||||
// arrayMin calculates the minimum value in array x
|
||||
func arrayMin(x []float64) (float64, bool) {
|
||||
if len(x) < 1 {
|
||||
fmt.Printf("arrayMin: Length too short\n")
|
||||
return math.NaN(), false
|
||||
}
|
||||
|
||||
min := x[0]
|
||||
for i := range x {
|
||||
if x[i] < min {
|
||||
min = x[i]
|
||||
}
|
||||
}
|
||||
return min, true
|
||||
}
|
||||
|
||||
// arrayMax calculates the maximum value in array x
|
||||
func arrayMax(x []float64) (float64, bool) {
|
||||
if len(x) < 1 {
|
||||
fmt.Printf("arrayMax: Length too short\n")
|
||||
return math.NaN(), false
|
||||
}
|
||||
|
||||
max := x[0]
|
||||
for i := range x {
|
||||
if x[i] > max {
|
||||
max = x[i]
|
||||
}
|
||||
}
|
||||
return max, true
|
||||
}
|
||||
|
||||
// arrayRange calculates the range of values in array x
|
||||
func arrayRange(x []float64) (float64, bool) {
|
||||
max, err1 := arrayMax(x)
|
||||
min, err2 := arrayMin(x)
|
||||
|
||||
if !err1 || !err2 {
|
||||
fmt.Printf("Error calculating range\n")
|
||||
return math.NaN(), false
|
||||
}
|
||||
|
||||
return (max - min), true
|
||||
}
|
||||
|
||||
// mean returns the arithmetic mean of array x
|
||||
func mean(x []float64) (float64, bool) {
|
||||
if len(x) < 1 {
|
||||
fmt.Printf("mean: Length too short\n")
|
||||
return math.NaN(), false
|
||||
}
|
||||
|
||||
sum := 0.0
|
||||
nf := float64(len(x))
|
||||
|
||||
for i := range x {
|
||||
sum += x[i]
|
||||
}
|
||||
|
||||
return sum / nf, true
|
||||
}
|
||||
|
||||
// stdev estimates the sample standard deviation of array x
|
||||
func stdev(x []float64) (float64, bool) {
|
||||
if len(x) < 2 {
|
||||
fmt.Printf("stdev: Length too short\n")
|
||||
return math.NaN(), false
|
||||
}
|
||||
|
||||
nf := float64(len(x))
|
||||
xbar, xbarValid := mean(x)
|
||||
|
||||
if !xbarValid {
|
||||
fmt.Printf("stdev: Error calculating xbar\n")
|
||||
return math.NaN(), false
|
||||
}
|
||||
|
||||
sumsq := 0.0
|
||||
|
||||
for i := range x {
|
||||
sumsq += (x[i] - xbar) * (x[i] - xbar)
|
||||
}
|
||||
|
||||
return math.Pow(sumsq/(nf-1), 0.5), true
|
||||
}
|
||||
|
||||
// radians converts angle from degrees, and returns its value in radians
|
||||
func radians(angle float64) float64 {
|
||||
return angle * math.Pi / 180.0
|
||||
}
|
||||
|
||||
// degrees converts angle from radians, and returns its value in degrees
|
||||
func degrees(angle float64) float64 {
|
||||
return angle * 180.0 / math.Pi
|
||||
}
|
||||
|
||||
// radiansRel converts angle from degrees, and returns its value in radians in the range -Pi to + Pi
|
||||
func radiansRel(angle float64) float64 {
|
||||
for angle > 180 {
|
||||
angle -= 360
|
||||
}
|
||||
for angle < -180 {
|
||||
angle += 360
|
||||
}
|
||||
return angle * math.Pi / 180.0
|
||||
}
|
||||
|
||||
// degreesRel converts angle from radians, and returns its value in the range of -180 to +180 degrees
|
||||
func degreesRel(angle float64) float64 {
|
||||
for angle > math.Pi {
|
||||
angle -= 2 * math.Pi
|
||||
}
|
||||
for angle < -math.Pi {
|
||||
angle += 2 * math.Pi
|
||||
}
|
||||
return angle * 180.0 / math.Pi
|
||||
}
|
||||
|
||||
// degreesHdg converts angle from radians, and returns its value in the range of 0+ to 360 degrees
|
||||
func degreesHdg(angle float64) float64 {
|
||||
for angle < 0 {
|
||||
angle += 2 * math.Pi
|
||||
}
|
||||
return angle * 180.0 / math.Pi
|
||||
}
|
||||
|
||||
/*
|
||||
Distance functions based on rectangular coordinate systems
|
||||
Simple calculations and "good enough" on small scale (± 1° of lat / lon)
|
||||
suitable for relative distance to nearby traffic
|
||||
*/
|
||||
|
||||
// distRect returns distance and bearing to target #2 (e.g. traffic) from target #1 (e.g. ownship)
|
||||
// Inputs are lat / lon of both points in decimal degrees
|
||||
// Outputs are distance in meters and bearing in degrees (0° = north, 90° = east)
|
||||
// Secondary outputs are north and east components of distance in meters (north, east positive)
|
||||
|
||||
func distRect(lat1, lon1, lat2, lon2 float64) (dist, bearing, distN, distE float64) {
|
||||
radius_earth := 6371008.8 // meters; mean radius
|
||||
dLat := radiansRel(lat2 - lat1)
|
||||
avgLat := radiansRel((lat2 + lat1) / 2)
|
||||
dLon := radiansRel(lon2 - lon1)
|
||||
distN = dLat * radius_earth
|
||||
distE = dLon * radius_earth * math.Abs(math.Cos(avgLat))
|
||||
dist = math.Pow(distN*distN+distE*distE, 0.5)
|
||||
bearing = math.Atan2(distE, distN)
|
||||
bearing = degreesHdg(bearing)
|
||||
return
|
||||
}
|
||||
|
||||
// distRectNorth returns north-south distance from point 1 to point 2.
|
||||
// Inputs are lat in decimal degrees. Output is distance in meters (east positive)
|
||||
func distRectNorth(lat1, lat2 float64) float64 {
|
||||
var dist float64
|
||||
radius_earth := 6371008.8 // meters; mean radius
|
||||
dLat := radiansRel(lat2 - lat1)
|
||||
dist = dLat * radius_earth
|
||||
return dist
|
||||
}
|
||||
|
||||
// distRectEast returns east-west distance from point 1 to point 2.
|
||||
// Inputs are lat/lon in decimal degrees. Output is distance in meters (north positive)
|
||||
func distRectEast(lat1, lon1, lat2, lon2 float64) float64 {
|
||||
var dist float64
|
||||
radius_earth := 6371008.8 // meters; mean radius
|
||||
//dLat := radiansRel(lat2 - lat1) // unused
|
||||
avgLat := radiansRel((lat2 + lat1) / 2)
|
||||
dLon := radiansRel(lon2 - lon1)
|
||||
dist = dLon * radius_earth * math.Abs(math.Cos(avgLat))
|
||||
return dist
|
||||
}
|
||||
|
||||
/*
|
||||
Distance functions: Polar coordinate systems
|
||||
More accurate over longer distances
|
||||
*/
|
||||
|
||||
// distance calculates distance between two points using the law of cosines.
|
||||
// Inputs are lat / lon of both points in decimal degrees
|
||||
// Outputs are distance in meters and bearing to the target from origin in degrees (0° = north, 90° = east)
|
||||
func distance(lat1, lon1, lat2, lon2 float64) (dist, bearing float64) {
|
||||
radius_earth := 6371008.8 // meters; mean radius
|
||||
|
||||
lat1 = radians(lat1)
|
||||
lon1 = radians(lon1)
|
||||
lat2 = radians(lat2)
|
||||
lon2 = radians(lon2)
|
||||
|
||||
dist = math.Acos(math.Sin(lat1)*math.Sin(lat2)+math.Cos(lat1)*math.Cos(lat2)*math.Cos(lon2-lon1)) * radius_earth
|
||||
|
||||
var x, y float64
|
||||
|
||||
x = math.Cos(lat1)*math.Sin(lat2) - math.Sin(lat1)*math.Cos(lat2)*math.Cos(lon2-lon1)
|
||||
y = math.Sin(lon2-lon1) * math.Cos(lat2)
|
||||
|
||||
bearing = degreesHdg(math.Atan2(y, x))
|
||||
|
||||
return
|
||||
}
|
||||
|
|
@ -160,17 +160,14 @@ func sendTrafficUpdates() {
|
|||
log.Printf("==================================================================\n")
|
||||
}
|
||||
for icao, ti := range traffic { // TO-DO: Limit number of aircraft in traffic message. 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))
|
||||
ti.Distance = dist
|
||||
ti.Bearing = bearing
|
||||
}
|
||||
*/
|
||||
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))
|
||||
ti.Distance = dist
|
||||
ti.Bearing = bearing
|
||||
}
|
||||
ti.Age = stratuxClock.Since(ti.Last_seen).Seconds()
|
||||
ti.AgeLastAlt = stratuxClock.Since(ti.Last_alt).Seconds()
|
||||
logTraffic(ti)
|
||||
|
||||
// DEBUG: Print the list of all tracked targets (with data) to the log every 15 seconds if "DEBUG" option is enabled
|
||||
if globalSettings.DEBUG && (stratuxClock.Time.Second()%15) == 0 {
|
||||
|
|
@ -189,6 +186,7 @@ func sendTrafficUpdates() {
|
|||
trafficUpdate.Send(tiJSON)
|
||||
}
|
||||
if ti.Position_valid && ti.Age < 6 { // ... but don't pass stale data to the EFB. TO-DO: Coast old traffic? Need to determine how FF, WingX, etc deal with stale targets.
|
||||
logTraffic(ti) // only add to the SQLite log if it's not stale
|
||||
msg = append(msg, makeTrafficReportMsg(ti)...)
|
||||
}
|
||||
}
|
||||
|
|
@ -200,7 +198,7 @@ func sendTrafficUpdates() {
|
|||
|
||||
// Send update to attached JSON client.
|
||||
func registerTrafficUpdate(ti TrafficInfo) {
|
||||
//logTraffic(ti)
|
||||
//logTraffic(ti) // moved to sendTrafficUpdates() to reduce SQLite log size
|
||||
/*
|
||||
if !ti.Position_valid { // Don't send unless a valid position exists.
|
||||
return
|
||||
|
|
@ -438,11 +436,9 @@ func parseDownlinkReport(s string, signalLevel int) {
|
|||
if ti.Position_valid {
|
||||
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))
|
||||
}
|
||||
*/ // to-do
|
||||
if isGPSValid() {
|
||||
ti.Distance, ti.Bearing = distance(float64(mySituation.Lat), float64(mySituation.Lng), float64(ti.Lat), float64(ti.Lng))
|
||||
}
|
||||
ti.Last_seen = stratuxClock.Time
|
||||
ti.ExtrapolatedPosition = false
|
||||
}
|
||||
|
|
@ -707,11 +703,9 @@ func esListen() {
|
|||
if valid_position {
|
||||
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))
|
||||
}
|
||||
*/ // todo
|
||||
if isGPSValid() {
|
||||
ti.Distance, ti.Bearing = distance(float64(mySituation.Lat), float64(mySituation.Lng), float64(ti.Lat), float64(ti.Lng))
|
||||
}
|
||||
ti.Position_valid = true
|
||||
ti.ExtrapolatedPosition = false
|
||||
ti.Last_seen = stratuxClock.Time // only update "last seen" data on position updates
|
||||
|
|
@ -944,9 +938,8 @@ func updateDemoTraffic(icao uint32, tail string, relAlt float32, gs float64, off
|
|||
ti.Lat = float32(lat + traffRelLat)
|
||||
ti.Lng = float32(lng + traffRelLng)
|
||||
|
||||
/*
|
||||
ti.Distance, ti.Bearing = distance(float64(lat), float64(lng), float64(ti.Lat), float64(ti.Lng))
|
||||
*/ // todo
|
||||
ti.Distance, ti.Bearing = distance(float64(lat), float64(lng), float64(ti.Lat), float64(ti.Lng))
|
||||
|
||||
ti.Position_valid = true
|
||||
ti.ExtrapolatedPosition = false
|
||||
ti.Alt = int32(mySituation.Alt + relAlt)
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue