From ee97b68b67d3dd1e4772c7c5a8860636ba626596 Mon Sep 17 00:00:00 2001 From: Adrian Batzill Date: Sat, 27 Mar 2021 12:11:34 +0000 Subject: [PATCH] split out fancontrol and gen_gdl90 into different modules, and a common module that shares code between the two --- .vscode/settings.json | 8 +-- Makefile | 11 ++-- Makefile.x86 | 6 +-- {main => common}/cputemp.go | 12 ++--- {main => common}/equations.go | 72 ++++++++++++------------- {main => fancontrol_main}/fancontrol.go | 16 +++--- main/flarm-nmea.go | 14 ++--- main/gen_gdl90.go | 13 ++--- main/gps.go | 46 ++++++++-------- main/ogn.go | 19 +++---- main/sensors.go | 3 +- main/traffic.go | 32 +++++------ 12 files changed, 129 insertions(+), 123 deletions(-) rename {main => common}/cputemp.go (79%) rename {main => common}/equations.go (84%) rename {main => fancontrol_main}/fancontrol.go (95%) diff --git a/.vscode/settings.json b/.vscode/settings.json index 92bd8ef2..8471c328 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -13,8 +13,8 @@ // Use this to make it possible to hide the gen_gdl90 vs fancontrol redeclaration errors.. - //"files.exclude": { - // "**/.git": true, - // "main/fancontrol.go": true - //} + "files.exclude": { + "**/.git": true, + "main/fancontrol.go": true + } } \ No newline at end of file diff --git a/Makefile b/Makefile index a92f5882..310425eb 100644 --- a/Makefile +++ b/Makefile @@ -24,18 +24,15 @@ else OGN_RX_BINARY=ogn/ogn-rx-eu_arm endif -STRATUX_SRC=main/gen_gdl90.go main/traffic.go main/gps.go main/network.go main/managementinterface.go main/sdr.go main/ping.go main/uibroadcast.go main/monotonic.go main/datalog.go main/equations.go main/sensors.go main/cputemp.go main/lowpower_uat.go main/ogn.go main/flarm-nmea.go main/networksettings.go main/xplane.go -FANCONTROL_SRC=main/fancontrol.go main/equations.go main/cputemp.go - all: make xdump978 xdump1090 gen_gdl90 $(PLATFORMDEPENDENT) -gen_gdl90: $(STRATUX_SRC) - export CGO_CFLAGS_ALLOW="-L/root/stratux" && go build $(BUILDINFO) -o gen_gdl90 -p 4 $(STRATUX_SRC) +gen_gdl90: main/*.go common/*.go + export CGO_CFLAGS_ALLOW="-L/root/stratux" && go build $(BUILDINFO) -o gen_gdl90 -p 4 ./main/ -fancontrol: $(FANCONTROL_SRC) - go build $(BUILDINFO) -o fancontrol -p 4 $(FANCONTROL_SRC) +fancontrol: fancontrol_main/*.go common/*.go + go build $(BUILDINFO) -o fancontrol -p 4 ./fancontrol_main/ xdump1090: git submodule update --init diff --git a/Makefile.x86 b/Makefile.x86 index 3545618c..40a26963 100644 --- a/Makefile.x86 +++ b/Makefile.x86 @@ -15,11 +15,9 @@ FANCONTROL_SRC=main/fancontrol.go main/equations.go main/cputemp.go all: xdump978 xdump1090 gen_gdl90 $(PLATFORMDEPENDENT) -gen_gdl90: $(STRATUX_SRC) - CGO_CFLAGS_ALLOW="-L$(CURDIR)" go build $(BUILDINFO) -gcflags '-N -l' -p 4 $(STRATUX_SRC) +gen_gdl90: main/*.go common/*.go + CGO_CFLAGS_ALLOW="-L$(CURDIR)" go build $(BUILDINFO) -gcflags '-N -l' -o gen_gdl90 -p 4 ./main/ -fancontrol: - go build $(BUILDINFO) -p 4 $(FANCONTROL_SRC) xdump1090: git submodule update --init diff --git a/main/cputemp.go b/common/cputemp.go similarity index 79% rename from main/cputemp.go rename to common/cputemp.go index 0e2df678..8e7bac65 100644 --- a/main/cputemp.go +++ b/common/cputemp.go @@ -1,4 +1,4 @@ -package main +package common import ( "io/ioutil" @@ -7,7 +7,7 @@ import ( "time" ) -const invalidCpuTemp = float32(-99.0) +const InvalidCpuTemp = float32(-99.0) type CpuTempUpdateFunc func(cpuTemp float32) @@ -16,13 +16,13 @@ calls a callback. This is broken out into its own function (run as its own goroutine) because the RPi temperature monitor code is buggy, and often times reading this file hangs quite some time. */ -func cpuTempMonitor(updater CpuTempUpdateFunc) { +func CpuTempMonitor(updater CpuTempUpdateFunc) { timer := time.NewTicker(1 * time.Second) for { // Update CPUTemp. temp, err := ioutil.ReadFile("/sys/class/thermal/thermal_zone0/temp") tempStr := strings.Trim(string(temp), "\n") - t := invalidCpuTemp + t := InvalidCpuTemp if err == nil { tInt, err := strconv.Atoi(tempStr) if err == nil { @@ -33,7 +33,7 @@ func cpuTempMonitor(updater CpuTempUpdateFunc) { } } } - if t >= invalidCpuTemp { // Only update if valid value was obtained. + if t >= InvalidCpuTemp { // Only update if valid value was obtained. updater(t) } <-timer.C @@ -41,6 +41,6 @@ func cpuTempMonitor(updater CpuTempUpdateFunc) { } // Check if CPU temperature is valid. Assume <= 0 is invalid. -func isCPUTempValid(cpuTemp float32) bool { +func IsCPUTempValid(cpuTemp float32) bool { return cpuTemp > 0 } diff --git a/main/equations.go b/common/equations.go similarity index 84% rename from main/equations.go rename to common/equations.go index 5448939a..d101b7b4 100644 --- a/main/equations.go +++ b/common/equations.go @@ -8,7 +8,7 @@ and other fuctions of Stratux package */ -package main +package common import ( "fmt" @@ -18,7 +18,7 @@ import ( // 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) { +func LinReg(x, y []float64) (slope, intercept float64, valid bool) { n := len(x) nf := float64(n) @@ -60,7 +60,7 @@ func linReg(x, y []float64) (slope, intercept float64, valid bool) { // 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) { +func LinRegWeighted(x, y, w []float64) (slope, intercept float64, valid bool) { n := len(x) @@ -112,7 +112,7 @@ func linRegWeighted(x, y, w []float64) (slope, intercept float64, valid bool) { // 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 { +func TriCubeWeight(center, halfwidth, x float64) float64 { var weight, x_t float64 x_t = math.Abs((x - center) / halfwidth) if x_t < 1 { @@ -124,7 +124,7 @@ func triCubeWeight(center, halfwidth, x float64) float64 { } // arrayMin calculates the minimum value in array x -func arrayMin(x []float64) (float64, bool) { +func ArrayMin(x []float64) (float64, bool) { if len(x) < 1 { fmt.Printf("arrayMin: Length too short\n") return math.NaN(), false @@ -140,7 +140,7 @@ func arrayMin(x []float64) (float64, bool) { } // arrayMax calculates the maximum value in array x -func arrayMax(x []float64) (float64, bool) { +func ArrayMax(x []float64) (float64, bool) { if len(x) < 1 { fmt.Printf("arrayMax: Length too short\n") return math.NaN(), false @@ -156,9 +156,9 @@ func arrayMax(x []float64) (float64, bool) { } // arrayRange calculates the range of values in array x -func arrayRange(x []float64) (float64, bool) { - max, err1 := arrayMax(x) - min, err2 := arrayMin(x) +func ArrayRange(x []float64) (float64, bool) { + max, err1 := ArrayMax(x) + min, err2 := ArrayMin(x) if !err1 || !err2 { fmt.Printf("Error calculating range\n") @@ -169,7 +169,7 @@ func arrayRange(x []float64) (float64, bool) { } // mean returns the arithmetic mean of array x -func mean(x []float64) (float64, bool) { +func Mean(x []float64) (float64, bool) { if len(x) < 1 { fmt.Printf("mean: Length too short\n") return math.NaN(), false @@ -186,14 +186,14 @@ func mean(x []float64) (float64, bool) { } // stdev estimates the sample standard deviation of array x -func stdev(x []float64) (float64, bool) { +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) + xbar, xbarValid := Mean(x) if !xbarValid { fmt.Printf("stdev: Error calculating xbar\n") @@ -210,17 +210,17 @@ func stdev(x []float64) (float64, bool) { } // radians converts angle from degrees, and returns its value in radians -func radians(angle float64) float64 { +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 { +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 { +func RadiansRel(angle float64) float64 { for angle > 180 { angle -= 360 } @@ -231,7 +231,7 @@ func radiansRel(angle float64) float64 { } // degreesRel converts angle from radians, and returns its value in the range of -180 to +180 degrees -func degreesRel(angle float64) float64 { +func DegreesRel(angle float64) float64 { for angle > math.Pi { angle -= 2 * math.Pi } @@ -242,7 +242,7 @@ func degreesRel(angle float64) float64 { } // degreesHdg converts angle from radians, and returns its value in the range of 0+ to 360 degrees -func degreesHdg(angle float64) float64 { +func DegreesHdg(angle float64) float64 { for angle < 0 { angle += 2 * math.Pi } @@ -250,7 +250,7 @@ func degreesHdg(angle float64) float64 { } // roundToInt16 cheaply rounds a float64 to an int16, rather than truncating -func roundToInt16(in float64) (out int16) { +func RoundToInt16(in float64) (out int16) { if in >= 0 { out = int16(in + 0.5) } else { @@ -270,37 +270,37 @@ suitable for relative distance to nearby traffic // 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) { +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) + 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) + 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 { +func DistRectNorth(lat1, lat2 float64) float64 { var dist float64 radius_earth := 6371008.8 // meters; mean radius - dLat := radiansRel(lat2 - lat1) + 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 { +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) + avgLat := RadiansRel((lat2 + lat1) / 2) + dLon := RadiansRel(lon2 - lon1) dist = dLon * radius_earth * math.Abs(math.Cos(avgLat)) return dist } @@ -313,13 +313,13 @@ 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) { +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) + 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 @@ -328,7 +328,7 @@ func distance(lat1, lon1, lat2, lon2 float64) (dist, bearing 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)) + bearing = DegreesHdg(math.Atan2(y, x)) return } @@ -340,14 +340,14 @@ func CalcAltitude(press float64, altoffset int) (altitude float64) { } // golang only defines min/max for float64. Really. -func iMin(x, y int) int { +func IMin(x, y int) int { if x < y { return x } return y } -func iMax(x, y int) int { +func IMax(x, y int) int { if x > y { return x } diff --git a/main/fancontrol.go b/fancontrol_main/fancontrol.go similarity index 95% rename from main/fancontrol.go rename to fancontrol_main/fancontrol.go index 26767dcb..69551fbb 100644 --- a/main/fancontrol.go +++ b/fancontrol_main/fancontrol.go @@ -4,15 +4,17 @@ import ( "encoding/json" "flag" "fmt" - "github.com/prometheus/client_golang/prometheus" - "github.com/prometheus/client_golang/prometheus/promhttp" - "github.com/takama/daemon" "log" "net/http" "os" "os/signal" "syscall" "time" + + "github.com/b3nn0/stratux/common" + "github.com/prometheus/client_golang/prometheus" + "github.com/prometheus/client_golang/prometheus/promhttp" + "github.com/takama/daemon" ) // #include @@ -121,8 +123,8 @@ func fanControl() { myFanControl.TempCurrent = 0 - go cpuTempMonitor(func(cpuTemp float32) { - if isCPUTempValid(cpuTemp) { + go common.CpuTempMonitor(func(cpuTemp float32) { + if common.IsCPUTempValid(cpuTemp) { myFanControl.TempCurrent = cpuTemp } }) @@ -139,9 +141,9 @@ func fanControl() { } if myFanControl.TempCurrent > (myFanControl.TempTarget + hysteresis) { - myFanControl.PWMDutyCurrent = iMax(iMin(myFanControl.PWMDutyMax, myFanControl.PWMDutyCurrent+pwmDutyStep), myFanControl.PWMDutyMin) + myFanControl.PWMDutyCurrent = common.IMax(common.IMin(myFanControl.PWMDutyMax, myFanControl.PWMDutyCurrent+pwmDutyStep), myFanControl.PWMDutyMin) } else if myFanControl.TempCurrent < (myFanControl.TempTarget - hysteresis) { - myFanControl.PWMDutyCurrent = iMax(myFanControl.PWMDutyCurrent-pwmDutyStep, 0) + myFanControl.PWMDutyCurrent = common.IMax(myFanControl.PWMDutyCurrent-pwmDutyStep, 0) if myFanControl.PWMDutyCurrent < myFanControl.PWMDutyMin { myFanControl.PWMDutyCurrent = myFanControl.PWMDutyMin } diff --git a/main/flarm-nmea.go b/main/flarm-nmea.go index 7e4bc31c..89d4977a 100644 --- a/main/flarm-nmea.go +++ b/main/flarm-nmea.go @@ -12,17 +12,19 @@ package main import ( + "bufio" "encoding/binary" "encoding/hex" "fmt" - "bufio" "io" "log" "math" "net" - "time" "strconv" "strings" + "time" + + "github.com/b3nn0/stratux/common" ) /* @@ -59,7 +61,7 @@ func makeFlarmPFLAUString(ti TrafficInfo) (msg string) { gpsStatus = 2 } - dist, bearing, _, _ := distRect(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng)) + dist, bearing, _, _ := common.DistRect(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng)) relativeVertical := computeRelativeVertical(ti) alarmLevel := computeAlarmLevel(dist, relativeVertical) @@ -213,7 +215,7 @@ func makeFlarmPFLAAString(ti TrafficInfo) (msg string, valid bool, alarmLevel ui } // determine distance and bearing to target - dist, bearing, distN, distE := distRect(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng)) + dist, bearing, distN, distE := common.DistRect(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng)) if !ti.Position_valid { dist = ti.DistanceEstimated distN = ti.DistanceEstimated @@ -799,11 +801,11 @@ func parseFlarmPFLAA(message []string) { // lat dist = 60nm = 111,12km ti.Lat = mySituation.GPSLatitude + (relNorth / 111120.0) avgLat := ti.Lat / 2.0 + mySituation.GPSLatitude / 2.0 - lngFactor := float32(111120.0 * math.Cos(radians(float64(avgLat)))) + lngFactor := float32(111120.0 * math.Cos(common.Radians(float64(avgLat)))) ti.Lng = mySituation.GPSLongitude + (relEast / lngFactor) if isGPSValid() { - ti.Distance, ti.Bearing = distance(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng)) + ti.Distance, ti.Bearing = common.Distance(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng)) ti.BearingDist_valid = true } diff --git a/main/gen_gdl90.go b/main/gen_gdl90.go index bdc593e3..2c1ceaab 100644 --- a/main/gen_gdl90.go +++ b/main/gen_gdl90.go @@ -32,6 +32,7 @@ import ( "syscall" "time" + "github.com/b3nn0/stratux/common" "github.com/b3nn0/stratux/uatparse" humanize "github.com/dustin/go-humanize" "github.com/ricochet2200/go-disk-usage/du" @@ -552,7 +553,7 @@ func makeStratuxStatus() []byte { } // Valid/Enabled: CPU temperature portion. - if isCPUTempValid(globalStatus.CPUTemp) { + if common.IsCPUTempValid(globalStatus.CPUTemp) { msg[13] = msg[13] | (1 << 4) } @@ -1726,14 +1727,14 @@ func main() { go baroAltGuesser() // Monitor RPi CPU temp. - globalStatus.CPUTempMin = invalidCpuTemp - globalStatus.CPUTempMax = invalidCpuTemp - go cpuTempMonitor(func(cpuTemp float32) { + globalStatus.CPUTempMin = common.InvalidCpuTemp + globalStatus.CPUTempMax = common.InvalidCpuTemp + go common.CpuTempMonitor(func(cpuTemp float32) { globalStatus.CPUTemp = cpuTemp - if isCPUTempValid(cpuTemp) && ((cpuTemp < globalStatus.CPUTempMin) || !isCPUTempValid(globalStatus.CPUTempMin)) { + if common.IsCPUTempValid(cpuTemp) && ((cpuTemp < globalStatus.CPUTempMin) || !common.IsCPUTempValid(globalStatus.CPUTempMin)) { globalStatus.CPUTempMin = cpuTemp } - if isCPUTempValid(cpuTemp) && ((cpuTemp > globalStatus.CPUTempMax) || !isCPUTempValid(globalStatus.CPUTempMax)) { + if common.IsCPUTempValid(cpuTemp) && ((cpuTemp > globalStatus.CPUTempMax) || !common.IsCPUTempValid(globalStatus.CPUTempMax)) { globalStatus.CPUTempMax = cpuTemp } }) diff --git a/main/gps.go b/main/gps.go index 0330a022..85877890 100755 --- a/main/gps.go +++ b/main/gps.go @@ -10,10 +10,10 @@ package main import ( + "errors" "fmt" "log" "math" - "errors" "strconv" "strings" "sync" @@ -25,6 +25,8 @@ import ( "os" "os/exec" + + "github.com/b3nn0/stratux/common" ) const ( @@ -681,7 +683,7 @@ func calcGPSAttitude() bool { for i := 0; i < length; i++ { tempTime[i] = float64(myGPSPerfStats[i].nmeaTime) } - minTime, _ := arrayMin(tempTime) + minTime, _ := common.ArrayMin(tempTime) if minTime > 86401.0 { log.Printf("GPS attitude: Rebasing GPS time since midnight to current day.\n") for i := 0; i < length; i++ { @@ -757,7 +759,7 @@ func calcGPSAttitude() bool { if myGPSPerfStats[i].msgType == "GPRMC" || myGPSPerfStats[i].msgType == "GNRMC" { tempSpeed = append(tempSpeed, float64(myGPSPerfStats[i].gsf)) tempSpeedTime = append(tempSpeedTime, float64(myGPSPerfStats[i].nmeaTime)) - tempRegWeights = append(tempRegWeights, triCubeWeight(center, halfwidth, float64(myGPSPerfStats[i].nmeaTime))) + tempRegWeights = append(tempRegWeights, common.TriCubeWeight(center, halfwidth, float64(myGPSPerfStats[i].nmeaTime))) } } lengthSpeed = len(tempSpeed) @@ -767,7 +769,7 @@ func calcGPSAttitude() bool { } else if lengthSpeed == 1 { v_x = tempSpeed[0] * 1.687810 } else { - slope, intercept, valid = linRegWeighted(tempSpeedTime, tempSpeed, tempRegWeights) + slope, intercept, valid = common.LinRegWeighted(tempSpeedTime, tempSpeed, tempRegWeights) if !valid { log.Printf("GPS attitude: Error calculating speed regression from NMEA RMC position messages") return false @@ -786,7 +788,7 @@ func calcGPSAttitude() bool { if myGPSPerfStats[i].msgType == "GPGGA" || myGPSPerfStats[i].msgType == "GNGGA" { tempVV = append(tempVV, float64(myGPSPerfStats[i].alt)) tempSpeedTime = append(tempSpeedTime, float64(myGPSPerfStats[i].nmeaTime)) - tempRegWeights = append(tempRegWeights, triCubeWeight(center, halfwidth, float64(myGPSPerfStats[i].nmeaTime))) + tempRegWeights = append(tempRegWeights, common.TriCubeWeight(center, halfwidth, float64(myGPSPerfStats[i].nmeaTime))) } } lengthSpeed = len(tempVV) @@ -794,7 +796,7 @@ func calcGPSAttitude() bool { log.Printf("GPS Attitude: Not enough points to calculate vertical speed from NMEA GGA messages\n") return false } else { - slope, _, valid = linRegWeighted(tempSpeedTime, tempVV, tempRegWeights) + slope, _, valid = common.LinRegWeighted(tempSpeedTime, tempVV, tempRegWeights) if !valid { log.Printf("GPS attitude: Error calculating vertical speed regression from NMEA GGA messages") return false @@ -846,9 +848,9 @@ func calcGPSAttitude() bool { if lengthHeading > 1 { tempHdgUnwrapped[0] = tempHdg[0] - tempRegWeights[0] = triCubeWeight(center, halfwidth, tempHdgTime[0]) + tempRegWeights[0] = common.TriCubeWeight(center, halfwidth, tempHdgTime[0]) for i := 1; i < lengthHeading; i++ { - tempRegWeights[i] = triCubeWeight(center, halfwidth, tempHdgTime[i]) + tempRegWeights[i] = common.TriCubeWeight(center, halfwidth, tempHdgTime[i]) if math.Abs(tempHdg[i]-tempHdg[i-1]) < 180 { // case 1: if angle change is less than 180 degrees, use the same reference system tempHdgUnwrapped[i] = tempHdgUnwrapped[i-1] + tempHdg[i] - tempHdg[i-1] } else if tempHdg[i] > tempHdg[i-1] { // case 2: heading has wrapped around from NE to NW. Subtract 360 to keep consistent with previous data. @@ -865,7 +867,7 @@ func calcGPSAttitude() bool { } // Finally, calculate turn rate as the slope of the weighted linear regression of unwrapped heading. - slope, intercept, valid = linRegWeighted(tempHdgTime, tempHdgUnwrapped, tempRegWeights) + slope, intercept, valid = common.LinRegWeighted(tempHdgTime, tempHdgUnwrapped, tempRegWeights) if !valid { log.Printf("GPS attitude: Regression error calculating turn rate") @@ -918,7 +920,7 @@ func calcGPSAttitude() bool { */ g := 32.174 // ft/(s^2) - omega = radians(myGPSPerfStats[index].gpsTurnRate) // need radians/sec + omega = common.Radians(myGPSPerfStats[index].gpsTurnRate) // need radians/sec a_c = v_x * omega myGPSPerfStats[index].gpsRoll = math.Atan2(a_c, g) * 180 / math.Pi // output is degrees myGPSPerfStats[index].gpsLoadFactor = math.Sqrt(a_c*a_c+g*g) / g @@ -980,7 +982,7 @@ func calculateNavRate() float64 { } var halfwidth float64 - dt_avg, valid := mean(tempSpeedTime) + dt_avg, valid := common.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) @@ -1755,7 +1757,7 @@ func baroAltGuesser() { alts = append(alts, float64(k*100)) diffs = append(diffs, float64(v)) } - slope, intercept, valid := linReg(alts, diffs) + slope, intercept, valid := common.LinReg(alts, diffs) //fmt.Printf("General: %f * x + %f \n", slope, intercept) trafficMutex.Lock() @@ -1819,7 +1821,7 @@ func baroAltGuesser() { // X-axis is altitude, Y axis is reported GnssBaroDiff } if len(gnssBaroAltDiffs) >= 2 { - slope, intercept, valid := linRegWeighted(alts, diffs, weights) + slope, intercept, valid := common.LinRegWeighted(alts, diffs, weights) if valid { gnssBaroDiff := float64(myAlt) * slope + intercept mySituation.muBaro.Lock() @@ -1897,10 +1899,10 @@ func makeFFAHRSMessage() { if isAHRSValid() { if !isAHRSInvalidValue(mySituation.AHRSPitch) { - pitch = roundToInt16(mySituation.AHRSPitch * 10) + pitch = common.RoundToInt16(mySituation.AHRSPitch * 10) } if !isAHRSInvalidValue(mySituation.AHRSRoll) { - roll = roundToInt16(mySituation.AHRSRoll * 10) + roll = common.RoundToInt16(mySituation.AHRSRoll * 10) } } @@ -1959,27 +1961,27 @@ func makeAHRSGDL90Report() { vs := int16(0x7FFF) if isAHRSValid() { if !isAHRSInvalidValue(mySituation.AHRSPitch) { - pitch = roundToInt16(mySituation.AHRSPitch * 10) + pitch = common.RoundToInt16(mySituation.AHRSPitch * 10) } if !isAHRSInvalidValue(mySituation.AHRSRoll) { - roll = roundToInt16(mySituation.AHRSRoll * 10) + roll = common.RoundToInt16(mySituation.AHRSRoll * 10) } if !isAHRSInvalidValue(mySituation.AHRSGyroHeading) { - hdg = roundToInt16(mySituation.AHRSGyroHeading * 10) + hdg = common.RoundToInt16(mySituation.AHRSGyroHeading * 10) } if !isAHRSInvalidValue(mySituation.AHRSSlipSkid) { - slip_skid = roundToInt16(-mySituation.AHRSSlipSkid * 10) + slip_skid = common.RoundToInt16(-mySituation.AHRSSlipSkid * 10) } if !isAHRSInvalidValue(mySituation.AHRSTurnRate) { - yaw_rate = roundToInt16(mySituation.AHRSTurnRate * 10) + yaw_rate = common.RoundToInt16(mySituation.AHRSTurnRate * 10) } if !isAHRSInvalidValue(mySituation.AHRSGLoad) { - g = roundToInt16(mySituation.AHRSGLoad * 10) + g = common.RoundToInt16(mySituation.AHRSGLoad * 10) } } if isTempPressValid() { palt = uint16(mySituation.BaroPressureAltitude + 5000.5) - vs = roundToInt16(float64(mySituation.BaroVerticalSpeed)) + vs = common.RoundToInt16(float64(mySituation.BaroVerticalSpeed)) } // Roll. diff --git a/main/ogn.go b/main/ogn.go index f07b275b..bc56d8ea 100644 --- a/main/ogn.go +++ b/main/ogn.go @@ -1,4 +1,3 @@ - /* Copyright (c) 2020 Adrian Batzill Distributable under the terms of The "BSD New" License @@ -11,16 +10,18 @@ package main import ( - "encoding/json" - "encoding/hex" - "encoding/binary" - "net" "bufio" + "encoding/binary" + "encoding/hex" + "encoding/json" + "io/ioutil" + "log" + "net" "strconv" "strings" "time" - "log" - "io/ioutil" + + "github.com/b3nn0/stratux/common" ) // {"sys":"OGN","addr":"395F39","addr_type":3,"acft_type":"1","lat_deg":51.7657533,"lon_deg":-1.1918533,"alt_msl_m":124,"alt_std_m":63,"track_deg":0.0,"speed_mps":0.3,"climb_mps":-0.5,"turn_dps":0.0,"DOP":1.5} @@ -211,7 +212,7 @@ func importOgnTrafficMessage(msg OgnMessage, data string) { } // Basic plausibility check: - dist, _, _, _ := distRect(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(msg.Lat_deg), float64(msg.Lon_deg)) + dist, _, _, _ := common.DistRect(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(msg.Lat_deg), float64(msg.Lon_deg)) if (isGPSValid() && dist >= 50000) || (msg.Lat_deg == 0 && msg.Lon_deg == 0) { // more than 50km away? Ignore. Most likely invalid data return @@ -282,7 +283,7 @@ func importOgnTrafficMessage(msg OgnMessage, data string) { ti.SignalLevel = msg.SNR_dB if isGPSValid() { - ti.Distance, ti.Bearing = distance(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng)) + ti.Distance, ti.Bearing = common.Distance(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng)) ti.BearingDist_valid = true } ti.Position_valid = true diff --git a/main/sensors.go b/main/sensors.go index eb07f960..50b4c9d6 100644 --- a/main/sensors.go +++ b/main/sensors.go @@ -10,6 +10,7 @@ import ( "github.com/b3nn0/goflying/ahrs" "github.com/b3nn0/goflying/ahrsweb" + "github.com/b3nn0/stratux/common" "github.com/b3nn0/stratux/sensors" "github.com/kidoman/embd" _ "github.com/kidoman/embd/host/all" @@ -123,7 +124,7 @@ func tempAndPressureSender() { mySituation.muBaro.Lock() mySituation.BaroLastMeasurementTime = stratuxClock.Time mySituation.BaroTemperature = float32(temp) - altitude = CalcAltitude(press, globalSettings.AltitudeOffset) + altitude = common.CalcAltitude(press, globalSettings.AltitudeOffset) mySituation.BaroPressureAltitude = float32(altitude) if altLast < -2000 { altLast = altitude // Initialize diff --git a/main/traffic.go b/main/traffic.go index b6a6fd3e..01089ab0 100644 --- a/main/traffic.go +++ b/main/traffic.go @@ -21,6 +21,8 @@ import ( "strings" "sync" "time" + + "github.com/b3nn0/stratux/common" ) //-0b2b48fe3aef1f88621a0856110a31c01105c4e6c4e6c40a9a820300000000000000;rs=7; @@ -86,14 +88,14 @@ type TrafficInfo struct { SignalLevel float64 // Signal level, dB RSSI. Squawk int // Squawk code Position_valid bool //TODO: set when position report received. Unset after n seconds? - Lat float32 // decimal degrees, north positive - Lng float32 // decimal degrees, east positive + Lat float32 // decimal common.Degrees, north positive + Lng float32 // decimal common.Degrees, east positive Alt int32 // Pressure altitude, feet GnssDiffFromBaroAlt int32 // GNSS altitude above WGS84 datum. Reported in TC 20-22 messages AltIsGNSS bool // Pressure alt = 0; GNSS alt = 1 NIC int // Navigation Integrity Category. NACp int // Navigation Accuracy Category for Position. - Track float32 // degrees true + Track float32 // common.Degrees true TurnRate float32 // Turn rate in deg/sec (negative = turning left, positive = right) Speed uint16 // knots Speed_valid bool // set when speed report received. @@ -120,7 +122,7 @@ type TrafficInfo struct { Alt_fix int32 // Last real, non-extrapolated altitude BearingDist_valid bool // set when bearing and distance information is valid - Bearing float64 // Bearing in degrees true to traffic from ownship, if it can be calculated. Units: degrees. + Bearing float64 // Bearing in common.Degrees true to traffic from ownship, if it can be calculated. Units: common.Degrees. Distance float64 // Distance to traffic from ownship, if it can be calculated. Units: meters. DistanceEstimated float64 // Estimated distance of the target if real distance can't be calculated, Estimated from signal strength with exponential smoothing. DistanceEstimatedLastTs time.Time // Used to compute moving average @@ -225,7 +227,7 @@ func isOwnshipTrafficInfo(ti TrafficInfo) (isOwnshipInfo bool, shouldIgnore bool } trafficDist := 0.0 if isGPSValid() { - trafficDist, _, _, _ = distRect(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng)) + trafficDist, _, _, _ = common.DistRect(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng)) } altDiff := 99999.0 if ti.AltIsGNSS && ti.Alt != 0 { @@ -306,7 +308,7 @@ func sendTrafficUpdates() { for key, 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() && ti.Position_valid { // func distRect(lat1, lon1, lat2, lon2 float64) (dist, bearing, distN, distE float64) { - dist, bearing := distance(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng)) + dist, bearing := common.Distance(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng)) ti.Distance = dist ti.Bearing = bearing ti.BearingDist_valid = true @@ -477,17 +479,17 @@ func estimateDistance(ti *TrafficInfo) { // calculates coordinates of a point defined by a location, a bearing, and a distance, thanks to 0x74-0x62 func calcLocationForBearingDistance(lat1, lon1, bearingDeg, distanceNm float64) (lat2, lon2 float64) { - lat1Rad := radians(lat1) - lon1Rad := radians(lon1) - bearingRad := radians(bearingDeg) + lat1Rad := common.Radians(lat1) + lon1Rad := common.Radians(lon1) + bearingRad := common.Radians(bearingDeg) distanceRad := distanceNm / (180 * 60 / math.Pi) lat2Rad := math.Asin(math.Sin(lat1Rad)*math.Cos(distanceRad) + math.Cos(lat1Rad)*math.Sin(distanceRad)*math.Cos(bearingRad)) distanceLon := -math.Atan2(math.Sin(bearingRad)*math.Sin(distanceRad)*math.Cos(lat1Rad), math.Cos(distanceRad)-math.Sin(lat1Rad)*math.Sin(lat2Rad)) lon2Rad := math.Mod(lon1Rad-distanceLon+math.Pi, 2.0*math.Pi) - math.Pi - lat2 = degrees(lat2Rad) - lon2 = degrees(lon2Rad) + lat2 = common.Degrees(lat2Rad) + lon2 = common.Degrees(lon2Rad) return } @@ -639,7 +641,7 @@ func makeTrafficReportMsg(ti TrafficInfo) []byte { msg[16] = byte(vvel & 0x00FF) // Track. - trk := uint8(ti.Track / TRACK_RESOLUTION) // Resolution is ~1.4 degrees. + trk := uint8(ti.Track / TRACK_RESOLUTION) // Resolution is ~1.4 common.Degrees. msg[17] = byte(trk) msg[18] = ti.Emitter_category @@ -871,7 +873,7 @@ func parseDownlinkReport(s string, signalLevel int) { ti.Lat = lat ti.Lng = lng if isGPSValid() { - ti.Distance, ti.Bearing = distance(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng)) + ti.Distance, ti.Bearing = common.Distance(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng)) } ti.Last_seen = stratuxClock.Time ti.ExtrapolatedPosition = false @@ -1172,7 +1174,7 @@ func esListen() { ti.Lat = lat ti.Lng = lng if isGPSValid() { - ti.Distance, ti.Bearing = distance(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng)) + ti.Distance, ti.Bearing = common.Distance(float64(mySituation.GPSLatitude), float64(mySituation.GPSLongitude), float64(ti.Lat), float64(ti.Lng)) ti.BearingDist_valid = true } ti.Position_valid = true @@ -1467,7 +1469,7 @@ 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)) + ti.Distance, ti.Bearing = common.Distance(float64(lat), float64(lng), float64(ti.Lat), float64(ti.Lng)) ti.BearingDist_valid = true ti.Position_valid = true