kopia lustrzana https://github.com/cyoung/stratux
204 wiersze
3.9 KiB
Go
204 wiersze
3.9 KiB
Go
package main
|
|
|
|
import (
|
|
"bufio"
|
|
"encoding/hex"
|
|
"encoding/json"
|
|
"fmt"
|
|
"io/ioutil"
|
|
"net"
|
|
"net/http"
|
|
"os"
|
|
"strconv"
|
|
"strings"
|
|
"sync"
|
|
"time"
|
|
)
|
|
|
|
const (
|
|
SITUATION_URL = "http://127.0.0.1/getSituation"
|
|
)
|
|
|
|
type MySituation struct {
|
|
AHRSRoll float64
|
|
AHRSPitch float64
|
|
}
|
|
|
|
var Location MySituation
|
|
|
|
var situationMutex *sync.Mutex
|
|
|
|
func chkErr(err error) {
|
|
if err != nil {
|
|
fmt.Printf("error: %s\n", err.Error())
|
|
os.Exit(1)
|
|
}
|
|
}
|
|
|
|
/*
|
|
ffMonitor().
|
|
Watches for "i-want-to-play-ffm-udp", "i-can-play-ffm-udp", and "i-cannot-play-ffm-udp" UDP messages broadcasted on
|
|
port 50113. Tags the client, issues a warning, and disables AHRS GDL90 output.
|
|
|
|
*/
|
|
|
|
var ffPlay bool
|
|
|
|
func ffMonitor() {
|
|
addr := net.UDPAddr{Port: 50113, IP: net.ParseIP("0.0.0.0")}
|
|
conn, err := net.ListenUDP("udp", &addr)
|
|
if err != nil {
|
|
fmt.Printf("ffMonitor(): error listening on port 50113: %s\n", err.Error())
|
|
return
|
|
}
|
|
defer conn.Close()
|
|
for {
|
|
buf := make([]byte, 1024)
|
|
n, _, err := conn.ReadFrom(buf)
|
|
if err != nil {
|
|
fmt.Printf("err: %s\n", err.Error())
|
|
return
|
|
}
|
|
// Got message, check if it's in the correct format.
|
|
if n < 3 || buf[0] != 0xFF || buf[1] != 0xFE {
|
|
continue
|
|
}
|
|
s := string(buf[2:n])
|
|
s = strings.Replace(s, "\x00", "", -1)
|
|
if strings.HasPrefix(s, "i-want-to-play-ffm-udp") || strings.HasPrefix(s, "i-can-play-ffm-udp") {
|
|
// Enable AHRS emulation.
|
|
ffPlay = true
|
|
}
|
|
}
|
|
}
|
|
|
|
func situationUpdater() {
|
|
situationUpdateTicker := time.NewTicker(100 * time.Millisecond)
|
|
for {
|
|
<-situationUpdateTicker.C
|
|
situationMutex.Lock()
|
|
|
|
resp, err := http.Get(SITUATION_URL)
|
|
if err != nil {
|
|
fmt.Printf("HTTP GET error: %s\n", err.Error())
|
|
situationMutex.Unlock()
|
|
continue
|
|
}
|
|
|
|
body, err := ioutil.ReadAll(resp.Body)
|
|
if err != nil {
|
|
fmt.Printf("HTTP GET body error: %s\n", err.Error())
|
|
resp.Body.Close()
|
|
situationMutex.Unlock()
|
|
continue
|
|
}
|
|
|
|
// fmt.Printf("body: %s\n", string(body))
|
|
err = json.Unmarshal(body, &Location)
|
|
|
|
if err != nil {
|
|
fmt.Printf("HTTP JSON unmarshal error: %s\n", err.Error())
|
|
}
|
|
resp.Body.Close()
|
|
situationMutex.Unlock()
|
|
|
|
}
|
|
}
|
|
|
|
type AHRSData struct {
|
|
Roll float64
|
|
Pitch float64
|
|
Trigger []byte
|
|
}
|
|
|
|
func main() {
|
|
situationMutex = &sync.Mutex{}
|
|
|
|
BROADCAST_IPv4 := net.IPv4(255, 255, 255, 255)
|
|
conn, err := net.DialUDP("udp", nil, &net.UDPAddr{
|
|
IP: BROADCAST_IPv4,
|
|
Port: 41504,
|
|
})
|
|
|
|
if err != nil {
|
|
fmt.Printf("err conn: %s\n", err.Error())
|
|
return
|
|
}
|
|
|
|
ahrsTable := make([]AHRSData, 0)
|
|
|
|
f, err := os.Open("/root/ahrs_table.log")
|
|
if err != nil {
|
|
fmt.Printf("error: %s\n", err.Error())
|
|
return
|
|
}
|
|
defer f.Close()
|
|
|
|
scanner := bufio.NewScanner(f)
|
|
for scanner.Scan() {
|
|
s := scanner.Text()
|
|
x := strings.Split(s, ",")
|
|
if len(x) < 3 {
|
|
continue
|
|
}
|
|
|
|
buf := make([]byte, 1024)
|
|
n, err := hex.Decode(buf, []byte(x[2]))
|
|
if err != nil || n == 0 {
|
|
fmt.Printf("error parsing '%s'.\n", x[2])
|
|
continue
|
|
}
|
|
|
|
roll, err := strconv.ParseFloat(x[0], 64)
|
|
if err != nil {
|
|
fmt.Printf("error parsing '%s'.\n", x[0])
|
|
continue
|
|
}
|
|
pitch, err := strconv.ParseFloat(x[1], 64)
|
|
if err != nil {
|
|
fmt.Printf("error parsing '%s'.\n", x[1])
|
|
continue
|
|
}
|
|
|
|
newEntry := AHRSData{
|
|
Roll: roll,
|
|
Pitch: pitch,
|
|
Trigger: buf[:n],
|
|
}
|
|
|
|
ahrsTable = append(ahrsTable, newEntry)
|
|
|
|
}
|
|
|
|
fmt.Printf("loaded %d size ahrs table.\n", len(ahrsTable))
|
|
|
|
go situationUpdater()
|
|
go ffMonitor()
|
|
|
|
tm := time.NewTicker(125 * time.Millisecond)
|
|
for {
|
|
<-tm.C
|
|
situationMutex.Lock()
|
|
myPitch := Location.AHRSPitch
|
|
myRoll := Location.AHRSRoll
|
|
situationMutex.Unlock()
|
|
|
|
mB := make([]byte, 0)
|
|
var mV float64
|
|
for _, v := range ahrsTable {
|
|
roll := v.Roll
|
|
pitch := v.Pitch
|
|
trigger := v.Trigger
|
|
z := ((roll - myRoll) * (roll - myRoll)) + ((pitch - myPitch) * (pitch - myPitch))
|
|
if (z < mV) || ((mV - 0.000) < 0.00001) {
|
|
mV = z
|
|
mB = trigger
|
|
}
|
|
}
|
|
if len(mB) > 0 && ffPlay { // Only send if we have both an AHRS approximation to send and FF was detected.
|
|
conn.Write(mB)
|
|
}
|
|
}
|
|
|
|
}
|