kopia lustrzana https://github.com/cyoung/stratux
Remove old AHRS replay tools. Add protocol tools.
Christopher Young
rodzic
46233e704c
commit
3f72b1e18b
5
Makefile
5
Makefile
|
|
@ -11,7 +11,7 @@ $(if $(GOROOT),,$(error GOROOT is not set!))
|
|||
endif
|
||||
|
||||
all:
|
||||
make xdump978 xdump1090 xgen_gdl90 $(PLATFORMDEPENDENT)
|
||||
make ahrs_approx xdump978 xdump1090 xgen_gdl90 $(PLATFORMDEPENDENT)
|
||||
|
||||
xgen_gdl90:
|
||||
go get -t -d -v ./main ./test ./godump978 ./uatparse ./sensors
|
||||
|
|
@ -29,6 +29,9 @@ xdump978:
|
|||
cd dump978 && make lib
|
||||
sudo cp -f ./libdump978.so /usr/lib/libdump978.so
|
||||
|
||||
ahrs_approx:
|
||||
go build $(BUILDINFO) -p 4 test-data/ahrs/ahrs_approx
|
||||
|
||||
.PHONY: test
|
||||
test:
|
||||
make -C test
|
||||
|
|
|
|||
17311
test-data/ahrs/ahrs.log
17311
test-data/ahrs/ahrs.log
Plik diff jest za duży
Load Diff
|
|
@ -0,0 +1,165 @@
|
|||
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)
|
||||
}
|
||||
}
|
||||
|
||||
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("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()
|
||||
|
||||
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 {
|
||||
conn.Write(mB)
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -1,444 +0,0 @@
|
|||
//https://www.youtube.com/watch?v=sQxJkSFmy_M&t=12m14s
|
||||
//https://www.youtube.com/watch?v=sQxJkSFmy_M&t=14m12s
|
||||
//815
|
||||
|
||||
package main
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"log"
|
||||
"os"
|
||||
"bufio"
|
||||
"strconv"
|
||||
"encoding/json"
|
||||
"errors"
|
||||
"strings"
|
||||
"net"
|
||||
"math"
|
||||
"time"
|
||||
)
|
||||
|
||||
var Crc16Table [256]uint16
|
||||
|
||||
|
||||
const (
|
||||
IPAD_ADDR = "192.168.1.133"
|
||||
LON_LAT_RESOLUTION = float64(180.0 / 8388608.0)
|
||||
TRACK_RESOLUTION = float32(360.0 / 256.0)
|
||||
)
|
||||
|
||||
type GPSData struct {
|
||||
Timestamp int64
|
||||
Lat float64
|
||||
Lng float64
|
||||
Alt float64
|
||||
Speed float64
|
||||
Course float64
|
||||
}
|
||||
|
||||
type AHRSData struct {
|
||||
Timestamp int64
|
||||
RawGyro []int64
|
||||
RawAccel []int64
|
||||
RawQuat []int64
|
||||
DmpTimestamp int64
|
||||
RawMag []int64
|
||||
MagTimestamp int64
|
||||
CalibratedAccel []int64
|
||||
CalibratedMag []int64
|
||||
FusedQuat []float64
|
||||
FusedEuler []float64
|
||||
LastDMPYaw float64
|
||||
LastYaw float64
|
||||
}
|
||||
|
||||
|
||||
// Construct the CRC table. Adapted from FAA ref above.
|
||||
func crcInit() {
|
||||
var i uint16
|
||||
var bitctr uint16
|
||||
var crc uint16
|
||||
for i = 0; i < 256; i++ {
|
||||
crc = (i << 8)
|
||||
for bitctr = 0; bitctr < 8; bitctr++ {
|
||||
z := uint16(0)
|
||||
if (crc & 0x8000) != 0 {
|
||||
z = 0x1021
|
||||
}
|
||||
crc = (crc << 1) ^ z
|
||||
}
|
||||
Crc16Table[i] = crc
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
func cleanStr(str string) string {
|
||||
str = strings.Trim(str, "\r\n ")
|
||||
str = strings.Replace(str, "\x00", "", -1)
|
||||
return str
|
||||
}
|
||||
|
||||
func seekTimestampInAHRS(fn string, startnsec, tol int64) (*os.File, int64, error) {
|
||||
fp, err := os.OpenFile(fn, os.O_RDONLY, 0)
|
||||
if err != nil {
|
||||
return nil, 0, err
|
||||
}
|
||||
rdr := bufio.NewReader(fp)
|
||||
for {
|
||||
buf, err := rdr.ReadString('\n')
|
||||
if err != nil {
|
||||
break
|
||||
}
|
||||
buf = cleanStr(buf)
|
||||
if len(buf) == 0 {
|
||||
continue
|
||||
}
|
||||
|
||||
var ahrs AHRSData
|
||||
err = json.Unmarshal([]byte(buf), &ahrs)
|
||||
if err != nil {
|
||||
continue
|
||||
}
|
||||
|
||||
i := ahrs.Timestamp
|
||||
if ((i > startnsec) && (i - startnsec) <= tol) || ((i < startnsec) && (startnsec - i) <= tol) { // Found it.
|
||||
return fp, i, nil
|
||||
}
|
||||
}
|
||||
return nil, 0, errors.New("can't find start.")
|
||||
}
|
||||
|
||||
func getLine(rdr *bufio.Reader) ([]string, int64) {
|
||||
ret := make([]string, 0)
|
||||
retIdx := int64(0)
|
||||
for len(ret) == 0 {
|
||||
buf, err := rdr.ReadString('\n')
|
||||
if err != nil {
|
||||
fmt.Printf("quitting. err: %s\n", err.Error())
|
||||
os.Exit(0)
|
||||
}
|
||||
buf = cleanStr(buf)
|
||||
ln := strings.Split(buf, ",")
|
||||
if len(ln) < 2 {
|
||||
continue
|
||||
}
|
||||
idx, err := strconv.ParseInt(ln[0], 10, 64)
|
||||
if err != nil {
|
||||
continue
|
||||
}
|
||||
ret = ln
|
||||
retIdx = idx
|
||||
}
|
||||
return ret, retIdx
|
||||
}
|
||||
|
||||
func getAHRS (rdr *bufio.Reader) (AHRSData, int64) {
|
||||
var ahrs AHRSData
|
||||
for ahrs.Timestamp == 0 {
|
||||
buf, err := rdr.ReadString('\n')
|
||||
if err != nil {
|
||||
fmt.Printf("quitting. err: %s\n", err.Error())
|
||||
os.Exit(0)
|
||||
}
|
||||
buf = cleanStr(buf)
|
||||
err = json.Unmarshal([]byte(buf), &ahrs)
|
||||
if err != nil {
|
||||
continue
|
||||
}
|
||||
}
|
||||
return ahrs, ahrs.Timestamp
|
||||
}
|
||||
|
||||
func getGPS (rdr *bufio.Reader) (GPSData, int64) {
|
||||
var gps GPSData
|
||||
for gps.Timestamp == 0 {
|
||||
buf, err := rdr.ReadString('\n')
|
||||
if err != nil {
|
||||
fmt.Printf("quitting. err: %s\n", err.Error())
|
||||
os.Exit(0)
|
||||
}
|
||||
buf = cleanStr(buf)
|
||||
err = json.Unmarshal([]byte(buf), &gps)
|
||||
if err != nil {
|
||||
continue
|
||||
}
|
||||
}
|
||||
return gps, gps.Timestamp
|
||||
}
|
||||
|
||||
|
||||
// Compute CRC. Adapted from FAA ref above.
|
||||
func crcCompute(data []byte) uint16 {
|
||||
ret := uint16(0)
|
||||
for i := 0; i < len(data); i++ {
|
||||
ret = Crc16Table[ret>>8] ^ (ret << 8) ^ uint16(data[i])
|
||||
}
|
||||
return ret
|
||||
}
|
||||
|
||||
func prepareMessage(data []byte) []byte {
|
||||
// Compute CRC before modifying the message.
|
||||
crc := crcCompute(data)
|
||||
// Add the two CRC16 bytes before replacing control characters.
|
||||
data = append(data, byte(crc&0xFF))
|
||||
data = append(data, byte(crc>>8))
|
||||
|
||||
tmp := []byte{0x7E} // Flag start.
|
||||
|
||||
// Copy the message over, escaping 0x7E (Flag Byte) and 0x7D (Control-Escape).
|
||||
for i := 0; i < len(data); i++ {
|
||||
mv := data[i]
|
||||
if (mv == 0x7E) || (mv == 0x7D) {
|
||||
mv = mv ^ 0x20
|
||||
tmp = append(tmp, 0x7D)
|
||||
}
|
||||
tmp = append(tmp, mv)
|
||||
}
|
||||
|
||||
tmp = append(tmp, 0x7E) // Flag end.
|
||||
|
||||
return tmp
|
||||
}
|
||||
|
||||
|
||||
func makeHeartbeat() []byte {
|
||||
msg := make([]byte, 7)
|
||||
// See p.10.
|
||||
msg[0] = 0x00 // Message type "Heartbeat".
|
||||
msg[1] = 0x01 // "UAT Initialized".
|
||||
msg[1] = msg[1] | 0x80
|
||||
msg[1] = msg[1] | 0x10 //FIXME: Addr talkback.
|
||||
|
||||
nowUTC := time.Now().UTC()
|
||||
// Seconds since 0000Z.
|
||||
midnightUTC := time.Date(nowUTC.Year(), nowUTC.Month(), nowUTC.Day(), 0, 0, 0, 0, time.UTC)
|
||||
secondsSinceMidnightUTC := uint32(nowUTC.Sub(midnightUTC).Seconds())
|
||||
|
||||
msg[2] = byte(((secondsSinceMidnightUTC >> 16) << 7) | 0x1) // UTC OK.
|
||||
msg[3] = byte((secondsSinceMidnightUTC & 0xFF))
|
||||
msg[4] = byte((secondsSinceMidnightUTC & 0xFFFF) >> 8)
|
||||
|
||||
// TODO. Number of uplink messages. See p.12.
|
||||
// msg[5]
|
||||
// msg[6]
|
||||
|
||||
return prepareMessage(msg)
|
||||
}
|
||||
|
||||
func makeLatLng(v float64) []byte {
|
||||
ret := make([]byte, 3)
|
||||
|
||||
v = v / LON_LAT_RESOLUTION
|
||||
wk := int32(v)
|
||||
|
||||
ret[0] = byte((wk & 0xFF0000) >> 16)
|
||||
ret[1] = byte((wk & 0x00FF00) >> 8)
|
||||
ret[2] = byte((wk & 0x0000FF))
|
||||
|
||||
return ret
|
||||
}
|
||||
|
||||
|
||||
func makeOwnshipReport(gps GPSData) []byte {
|
||||
msg := make([]byte, 28)
|
||||
// See p.16.
|
||||
msg[0] = 0x0A // Message type "Ownship".
|
||||
|
||||
msg[1] = 0x01 // Alert status, address type.
|
||||
|
||||
msg[2] = 1 // Address.
|
||||
msg[3] = 1 // Address.
|
||||
msg[4] = 1 // Address.
|
||||
|
||||
tmp := makeLatLng(gps.Lat)
|
||||
msg[5] = tmp[0] // Latitude.
|
||||
msg[6] = tmp[1] // Latitude.
|
||||
msg[7] = tmp[2] // Latitude.
|
||||
|
||||
tmp = makeLatLng(gps.Lng)
|
||||
msg[8] = tmp[0] // Longitude.
|
||||
msg[9] = tmp[1] // Longitude.
|
||||
msg[10] = tmp[2] // Longitude.
|
||||
|
||||
// This is **PRESSURE ALTITUDE**
|
||||
//FIXME: Temporarily removing "invalid altitude" when pressure altitude not available - using GPS altitude instead.
|
||||
// alt := uint16(0xFFF) // 0xFFF "invalid altitude."
|
||||
|
||||
alt := uint16(gps.Alt) //FIXME: This should not be here.
|
||||
alt = (alt + 1000) / 25
|
||||
|
||||
alt = alt & 0xFFF // Should fit in 12 bits.
|
||||
|
||||
msg[11] = byte((alt & 0xFF0) >> 4) // Altitude.
|
||||
msg[12] = byte((alt & 0x00F) << 4)
|
||||
|
||||
msg[12] = byte(((alt & 0x00F) << 4) | 0xB) // "Airborne" + "True Heading"
|
||||
|
||||
msg[13] = 0xBB // NIC and NACp.
|
||||
|
||||
|
||||
gdSpeed := uint16(gps.Speed)
|
||||
gdSpeed = gdSpeed & 0x0FFF // Should fit in 12 bits.
|
||||
|
||||
msg[14] = byte((gdSpeed & 0xFF0) >> 4)
|
||||
msg[15] = byte((gdSpeed & 0x00F) << 4)
|
||||
|
||||
verticalVelocity := int16(1000 / 64) // ft/min. 64 ft/min resolution.
|
||||
//TODO: 0x800 = no information available.
|
||||
verticalVelocity = verticalVelocity & 0x0FFF // Should fit in 12 bits.
|
||||
msg[15] = msg[15] | byte((verticalVelocity&0x0F00)>>8)
|
||||
msg[16] = byte(verticalVelocity & 0xFF)
|
||||
|
||||
// Showing magnetic (corrected) on ForeFlight. Needs to be True Heading.
|
||||
groundTrack := uint16(gps.Course)
|
||||
trk := uint8(float32(groundTrack) / TRACK_RESOLUTION) // Resolution is ~1.4 degrees.
|
||||
|
||||
msg[17] = byte(trk)
|
||||
|
||||
msg[18] = 0x01 // "Light (ICAO) < 15,500 lbs"
|
||||
|
||||
return prepareMessage(msg)
|
||||
}
|
||||
|
||||
//TODO
|
||||
func makeOwnshipGeometricAltitudeReport(gps GPSData) []byte {
|
||||
msg := make([]byte, 5)
|
||||
// See p.28.
|
||||
msg[0] = 0x0B // Message type "Ownship Geo Alt".
|
||||
alt := int16(gps.Alt) // GPS Altitude.
|
||||
alt = alt / 5
|
||||
msg[1] = byte(alt >> 8) // Altitude.
|
||||
msg[2] = byte(alt & 0x00FF) // Altitude.
|
||||
|
||||
//TODO: "Figure of Merit". 0x7FFF "Not available".
|
||||
msg[3] = 0x00
|
||||
msg[4] = 0x0A
|
||||
|
||||
return prepareMessage(msg)
|
||||
}
|
||||
|
||||
var myGPS GPSData
|
||||
|
||||
func heartBeatSender() {
|
||||
addr, err := net.ResolveUDPAddr("udp", IPAD_ADDR + ":4000")
|
||||
if err != nil {
|
||||
log.Printf("ResolveUDPAddr(%s): %s\n", IPAD_ADDR, err.Error())
|
||||
return
|
||||
}
|
||||
gdlConn, err := net.DialUDP("udp", nil, addr)
|
||||
if err != nil {
|
||||
log.Printf("DialUDP(%s): %s\n", IPAD_ADDR, err.Error())
|
||||
return
|
||||
}
|
||||
|
||||
timer := time.NewTicker(1 * time.Second)
|
||||
for {
|
||||
<-timer.C
|
||||
gdlConn.Write(makeHeartbeat())
|
||||
gdlConn.Write(makeOwnshipReport(myGPS))
|
||||
gdlConn.Write(makeOwnshipGeometricAltitudeReport(myGPS))
|
||||
}
|
||||
}
|
||||
|
||||
var cal_pitch float64
|
||||
var cal_roll float64
|
||||
|
||||
var cal_num int
|
||||
|
||||
func main() {
|
||||
crcInit()
|
||||
if len(os.Args) < 5 {
|
||||
fmt.Printf("%s <start second> <ahrs file> <gps file> <replay speed>\n", os.Args[0])
|
||||
return
|
||||
}
|
||||
startsec, err := strconv.Atoi(os.Args[1])
|
||||
if err != nil {
|
||||
fmt.Printf("invalid: %s\n", os.Args[1])
|
||||
return
|
||||
}
|
||||
|
||||
replayspeed, err := strconv.Atoi(os.Args[4])
|
||||
if err != nil {
|
||||
fmt.Printf("invalid: %s\n", os.Args[4])
|
||||
return
|
||||
}
|
||||
|
||||
startnsec := int64(startsec) * 1000000000
|
||||
|
||||
ahrsfp, ahrsIdx, err := seekTimestampInAHRS(os.Args[2], startnsec, 1000000000) // Find the index with 1.00s tolerance.
|
||||
if err != nil {
|
||||
panic(err)
|
||||
}
|
||||
defer ahrsfp.Close()
|
||||
gpsfp, err := os.OpenFile(os.Args[3], os.O_RDONLY, 0)
|
||||
if err != nil {
|
||||
panic(err)
|
||||
}
|
||||
defer gpsfp.Close()
|
||||
|
||||
addr, err := net.ResolveUDPAddr("udp", IPAD_ADDR + ":49002")
|
||||
if err != nil {
|
||||
log.Printf("ResolveUDPAddr(%s): %s\n", IPAD_ADDR, err.Error())
|
||||
return
|
||||
}
|
||||
outConn, err := net.DialUDP("udp", nil, addr)
|
||||
if err != nil {
|
||||
log.Printf("DialUDP(%s): %s\n", IPAD_ADDR, err.Error())
|
||||
return
|
||||
}
|
||||
go heartBeatSender()
|
||||
|
||||
ahrsReader := bufio.NewReader(ahrsfp)
|
||||
gpsReader := bufio.NewReader(gpsfp)
|
||||
|
||||
lastTs := ahrsIdx
|
||||
for {
|
||||
gps, gpsIdx := getGPS(gpsReader)
|
||||
ahrs, ahrsIdx := getAHRS(ahrsReader)
|
||||
|
||||
// Correct for drift between the samples.
|
||||
drift := int64(math.Abs(float64(ahrsIdx - gpsIdx)))
|
||||
// fmt.Printf("drift: %d\n", drift)
|
||||
if drift >= 200000000 {
|
||||
// There's a problem. One of the files is ahead of the other by more than 0.10s.
|
||||
if gpsIdx > ahrsIdx { // GPS got ahead of AHRS? When does this happen?
|
||||
// fmt.Printf("GPS sample ahead of AHRS - correcting\n")
|
||||
for gpsIdx - ahrsIdx >= 200000000 {
|
||||
ln, nidx := getAHRS(ahrsReader)
|
||||
ahrsIdx = nidx
|
||||
ahrs = ln
|
||||
}
|
||||
} else { // AHRS got ahead of GPS? This usually happens.
|
||||
for ahrsIdx - gpsIdx >= 200000000 {
|
||||
// fmt.Printf("AHRS sample ahead of GPS - correcting\n")
|
||||
ln, nidx := getGPS(gpsReader)
|
||||
gpsIdx = nidx
|
||||
gps = ln
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
myGPS = gps
|
||||
fmt.Printf("matchy: %d, %d\n", ahrs.Timestamp, gps.Timestamp)
|
||||
pitch := ahrs.FusedEuler[0] * (180.0 / math.Pi)
|
||||
roll := -ahrs.FusedEuler[1] * (180.0 / math.Pi)
|
||||
|
||||
if cal_num < 20 { // Average the first 5 measurements and call this "level".
|
||||
cal_pitch = ((cal_pitch * float64(cal_num)) + pitch) / float64(cal_num + 1)
|
||||
cal_roll = ((cal_roll * float64(cal_num)) + roll) / float64(cal_num + 1)
|
||||
cal_num++
|
||||
}
|
||||
|
||||
// Apply the calibration values.
|
||||
pitch -= cal_pitch
|
||||
roll -= cal_roll
|
||||
|
||||
fmt.Printf("%f %f\n", pitch, roll)
|
||||
s := fmt.Sprintf("XATTStratux,%f,%f,%f", gps.Course, pitch, roll)
|
||||
outConn.Write([]byte(s))
|
||||
time.Sleep(time.Duration((ahrs.Timestamp - lastTs)/int64(replayspeed)))
|
||||
lastTs = ahrs.Timestamp
|
||||
|
||||
// Now we're working with synced samples.
|
||||
}
|
||||
}
|
||||
Plik diff jest za duży
Load Diff
Plik diff jest za duży
Load Diff
Plik binarny nie jest wyświetlany.
|
|
@ -1 +0,0 @@
|
|||
START,Wed Sep 23 22:08:32 +0000 UTC 2015
|
||||
Plik binarny nie jest wyświetlany.
Plik diff jest za duży
Load Diff
Plik binarny nie jest wyświetlany.
|
|
@ -0,0 +1,87 @@
|
|||
package main
|
||||
|
||||
import (
|
||||
"bufio"
|
||||
"fmt"
|
||||
"github.com/gonum/plot"
|
||||
"github.com/gonum/plot/plotter"
|
||||
"github.com/gonum/plot/plotutil"
|
||||
"github.com/gonum/plot/vg"
|
||||
"net/http"
|
||||
"os"
|
||||
"strconv"
|
||||
"strings"
|
||||
"time"
|
||||
)
|
||||
|
||||
type XY struct {
|
||||
X float64
|
||||
Y float64
|
||||
}
|
||||
|
||||
func imageWriter() {
|
||||
for {
|
||||
p, err := plot.New()
|
||||
if err != nil {
|
||||
panic(err)
|
||||
}
|
||||
p.Title.Text = "AHRS Plot"
|
||||
p.X.Label.Text = "Roll"
|
||||
p.Y.Label.Text = "Pitch"
|
||||
|
||||
file, err := os.Open("ahrs_table.log")
|
||||
if err != nil {
|
||||
panic(err)
|
||||
}
|
||||
defer file.Close()
|
||||
|
||||
scanner := bufio.NewScanner(file)
|
||||
|
||||
vals := make([]XY, 0)
|
||||
for scanner.Scan() {
|
||||
l := scanner.Text()
|
||||
x := strings.Split(l, ",")
|
||||
if len(x) < 3 {
|
||||
continue
|
||||
}
|
||||
roll, err := strconv.ParseFloat(x[0], 64)
|
||||
if err != nil {
|
||||
continue
|
||||
}
|
||||
pitch, err := strconv.ParseFloat(x[1], 64)
|
||||
if err != nil {
|
||||
continue
|
||||
}
|
||||
v := XY{X: roll, Y: pitch}
|
||||
vals = append(vals, v)
|
||||
}
|
||||
vals_XY := make(plotter.XYs, len(vals))
|
||||
for i := 0; i < len(vals); i++ {
|
||||
vals_XY[i].X = vals[i].X
|
||||
vals_XY[i].Y = vals[i].Y
|
||||
}
|
||||
err = plotutil.AddScatters(p, "First", vals_XY)
|
||||
if err != nil {
|
||||
panic(err)
|
||||
}
|
||||
if err := p.Save(8*vg.Inch, 8*vg.Inch, "out.png"); err != nil {
|
||||
panic(err)
|
||||
}
|
||||
time.Sleep(1000 * time.Millisecond)
|
||||
}
|
||||
}
|
||||
|
||||
func main() {
|
||||
go imageWriter()
|
||||
http.Handle("/", http.FileServer(http.Dir(".")))
|
||||
err := http.ListenAndServe(":8080", nil)
|
||||
|
||||
if err != nil {
|
||||
fmt.Printf("managementInterface ListenAndServe: %s\n", err.Error())
|
||||
}
|
||||
|
||||
for {
|
||||
time.Sleep(1 * time.Second)
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,109 @@
|
|||
package main
|
||||
|
||||
import (
|
||||
"encoding/hex"
|
||||
"encoding/json"
|
||||
"fmt"
|
||||
"io/ioutil"
|
||||
"net"
|
||||
"net/http"
|
||||
"os"
|
||||
"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)
|
||||
}
|
||||
}
|
||||
|
||||
var currentAHRSString string
|
||||
|
||||
func listener() {
|
||||
t := time.Now()
|
||||
addr := net.UDPAddr{Port: 41504, IP: net.ParseIP("0.0.0.0")}
|
||||
conn, err := net.ListenUDP("udp", &addr)
|
||||
if err != nil {
|
||||
fmt.Printf("error listening: %s\n", err.Error())
|
||||
return
|
||||
}
|
||||
defer conn.Close()
|
||||
for {
|
||||
buf := make([]byte, 1024)
|
||||
n, _, err := conn.ReadFrom(buf)
|
||||
if err != nil {
|
||||
fmt.Printf("Err receive: %s\n", err.Error())
|
||||
continue
|
||||
}
|
||||
buf_encoded := make([]byte, hex.EncodedLen(n))
|
||||
hex.Encode(buf_encoded, buf[:n])
|
||||
t2 := time.Now()
|
||||
time_diff := t2.Sub(t)
|
||||
t = t2
|
||||
|
||||
fmt.Sprintf("%d,%s\n", time_diff/time.Millisecond, buf_encoded)
|
||||
currentAHRSString = string(buf_encoded)
|
||||
}
|
||||
}
|
||||
|
||||
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()
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
func main() {
|
||||
situationMutex = &sync.Mutex{}
|
||||
|
||||
go listener()
|
||||
go situationUpdater()
|
||||
|
||||
tm := time.NewTicker(125 * time.Millisecond)
|
||||
for {
|
||||
<-tm.C
|
||||
fmt.Printf("%f,%f,%s\n", Location.AHRSRoll, Location.AHRSPitch, currentAHRSString)
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
package main
|
||||
|
||||
import (
|
||||
"encoding/hex"
|
||||
"fmt"
|
||||
"net"
|
||||
"time"
|
||||
)
|
||||
|
||||
func main() {
|
||||
t := time.Now()
|
||||
addr := net.UDPAddr{Port: 41504, IP: net.ParseIP("0.0.0.0")}
|
||||
conn, err := net.ListenUDP("udp", &addr)
|
||||
if err != nil {
|
||||
fmt.Printf("ffMonitor(): error listening: %s\n", err.Error())
|
||||
return
|
||||
}
|
||||
defer conn.Close()
|
||||
for {
|
||||
buf := make([]byte, 1024)
|
||||
n, _, err := conn.ReadFrom(buf)
|
||||
if err != nil {
|
||||
fmt.Printf("Err receive: %s\n", err.Error())
|
||||
continue
|
||||
}
|
||||
buf_encoded := make([]byte, hex.EncodedLen(n))
|
||||
hex.Encode(buf_encoded, buf[:n])
|
||||
t2 := time.Now()
|
||||
time_diff := t2.Sub(t)
|
||||
t = t2
|
||||
|
||||
fmt.Printf("%d,%s\n", time_diff/time.Millisecond, buf_encoded)
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,67 @@
|
|||
package main
|
||||
|
||||
import (
|
||||
"bufio"
|
||||
"encoding/hex"
|
||||
"fmt"
|
||||
"net"
|
||||
"os"
|
||||
"strconv"
|
||||
"strings"
|
||||
"time"
|
||||
)
|
||||
|
||||
func main() {
|
||||
if len(os.Args) < 2 {
|
||||
fmt.Printf("%s: <file>\n", os.Args[0])
|
||||
return
|
||||
}
|
||||
|
||||
f, err := os.Open(os.Args[1])
|
||||
if err != nil {
|
||||
fmt.Printf("error: %s\n", err.Error())
|
||||
return
|
||||
}
|
||||
defer f.Close()
|
||||
|
||||
scanner := bufio.NewScanner(f)
|
||||
|
||||
// Open a socket.
|
||||
BROADCAST_IPv4 := net.IPv4(255, 255, 255, 255)
|
||||
|
||||
// addr, _ := net.ResolveUDPAddr("udp", "192.168.10.10:41501")
|
||||
// laddr, _ := net.ResolveUDPAddr("udp", "192.168.10.1:58814")
|
||||
// conn, err := net.DialUDP("udp", laddr, addr)
|
||||
conn, err := net.DialUDP("udp", nil, &net.UDPAddr{
|
||||
IP: BROADCAST_IPv4,
|
||||
Port: 41504,
|
||||
})
|
||||
|
||||
if err != nil {
|
||||
fmt.Printf("DialUDP(): %s\n", err.Error())
|
||||
return
|
||||
}
|
||||
|
||||
for scanner.Scan() {
|
||||
s := scanner.Text()
|
||||
x := strings.Split(s, ",")
|
||||
if len(x) < 2 {
|
||||
continue
|
||||
}
|
||||
i, err := strconv.ParseInt(x[0], 10, 32)
|
||||
if err != nil {
|
||||
fmt.Printf("error parsing '%s': %s.\n", x[0], err.Error())
|
||||
continue
|
||||
}
|
||||
buf := make([]byte, hex.DecodedLen(len(x[1])))
|
||||
n, err := hex.Decode(buf, []byte(x[1]))
|
||||
if err != nil {
|
||||
fmt.Printf("error parsing '%s': %s.\n", x[1], err.Error())
|
||||
continue
|
||||
}
|
||||
|
||||
fmt.Printf("sleeping %dms, sending %s\n", i, x[1])
|
||||
time.Sleep(time.Duration(i) * time.Millisecond)
|
||||
conn.Write(buf[:n])
|
||||
}
|
||||
}
|
||||
Ładowanie…
Reference in New Issue