diff --git a/LICENSE b/LICENSE new file mode 100644 index 00000000..2ca453c6 --- /dev/null +++ b/LICENSE @@ -0,0 +1,26 @@ +Copyright (c) 2015 Christopher Young. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Joseph D Poirier nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + diff --git a/Makefile b/Makefile new file mode 100644 index 00000000..c6ddb7ed --- /dev/null +++ b/Makefile @@ -0,0 +1,4 @@ +all: + GOARCH=6 go-linux-arm build gen_gdl90.go traffic.go ry835ai.go +clean: + rm -f gen_gdl90 \ No newline at end of file diff --git a/gen_gdl90.go b/gen_gdl90.go index be07373d..a5d258f0 100644 --- a/gen_gdl90.go +++ b/gen_gdl90.go @@ -15,7 +15,7 @@ import ( const ( stratuxVersion = "v0.1" - configLocation = "stratux.conf" + configLocation = "/etc/stratux.conf" ipadAddr = "192.168.10.255:4000" // Port 4000 for FreeFlight RANGR. maxDatagramSize = 8192 UPLINK_BLOCK_DATA_BITS = 576 @@ -39,11 +39,16 @@ const ( MSGCLASS_UAT = 0 MSGCLASS_ES = 1 + + LON_LAT_RESOLUTION = float32(180.0 / 8388608.0) + TRACK_RESOLUTION = float32(360.0 / 256.0) ) var Crc16Table [256]uint16 var outConn *net.UDPConn +var myGPS GPSData + type msg struct { MessageClass uint TimeReceived time.Time @@ -103,18 +108,143 @@ func prepareMessage(data []byte) []byte { return tmp } +func makeLatLng(v float32) []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 isGPSValid() bool { + return time.Since(myGPS.lastFixLocalTime).Seconds() < 15 +} + +func isGPSGroundTrackValid() bool { + return time.Since(myGPS.lastGroundTrackTime).Seconds() < 15 +} + +//TODO +func makeOwnshipReport() bool { + if !isGPSValid() { + return false + } + 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(myGPS.lat) + msg[5] = tmp[0] // Latitude. + msg[6] = tmp[1] // Latitude. + msg[7] = tmp[2] // Latitude. + + tmp = makeLatLng(myGPS.lng) + msg[8] = tmp[0] // Longitude. + msg[9] = tmp[1] // Longitude. + msg[10] = tmp[2] // Longitude. + + +//TODO: 0xFFF "invalid altitude." +//FIXME: This is **PRESSURE ALTITUDE** + + alt := uint16(myGPS.alt) + 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) + + if isGPSGroundTrackValid() { + msg[12] = byte(((alt & 0x00F) << 4) | 0xB) // "Airborne" + "True Heading" + } else { + msg[12] = byte((alt & 0x00F) << 4) + } + msg[13] = 0xBB // NIC and NACp. + + gdSpeed := uint16(0) // 1kt resolution. + if isGPSGroundTrackValid() { + gdSpeed = myGPS.groundSpeed + } + 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(0) + if isGPSGroundTrackValid() { + groundTrack = myGPS.trueCourse + } + trk := uint8(float32(groundTrack) / TRACK_RESOLUTION) // Resolution is ~1.4 degrees. + + msg[17] = byte(trk) + + msg[18] = 0x01 // "Light (ICAO) < 15,500 lbs" + + outConn.Write(prepareMessage(msg)) + return true +} + +//TODO +func makeOwnshipGeometricAltitudeReport() bool { + if !isGPSValid() { + return false + } + msg := make([]byte, 5) + // See p.28. + msg[0] = 0x0B // Message type "Ownship Geo Alt". + alt := int16(myGPS.alt) + 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 + + outConn.Write(prepareMessage(msg)) + return true +} + +func makeInitializationMessage() []byte { + msg := make([]byte, 3) + // See p.13. + msg[0] = 0x02 // Message type "Initialization". + msg[1] = 0x00 //TODO + msg[2] = 0x00 //TODO + return prepareMessage(msg) +} func makeHeartbeat() []byte { msg := make([]byte, 7) // See p.10. msg[0] = 0x00 // Message type "Heartbeat". - msg[1] = 0x01 // "UAT Initialized". + msg[1] = 0x01 // "UAT Initialized". //FIXME + msg[1] = 0x91 //FIXME: GPS valid. 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) + msg[2] = byte(((secondsSinceMidnightUTC >> 16) << 7) | 0x1) // UTC OK. msg[3] = byte((secondsSinceMidnightUTC & 0xFF)) msg[4] = byte((secondsSinceMidnightUTC & 0xFFFF) >> 8) @@ -143,6 +273,12 @@ func relayMessage(msgtype uint16, msg []byte) { func heartBeatSender() { for { outConn.Write(makeHeartbeat()) +// outConn.Write(makeTrafficReport()) + makeOwnshipReport() + makeOwnshipGeometricAltitudeReport() + outConn.Write(makeInitializationMessage()) + sendTrafficUpdates() + updateStatus() time.Sleep(1 * time.Second) } } @@ -165,8 +301,13 @@ func updateStatus() { MsgLog = t globalStatus.UAT_messages_last_minute = UAT_messages_last_minute globalStatus.ES_messages_last_minute = ES_messages_last_minute + + if isGPSValid() { + globalStatus.GPS_satellites_locked = myGPS.satellites + } } + func parseInput(buf string) ([]byte, uint16) { x := strings.Split(buf, ";") // Discard everything after the first ';'. if len(x) == 0 { @@ -178,6 +319,10 @@ func parseInput(buf string) ([]byte, uint16) { } msgtype := uint16(0) + if s[0] == '-' { + parseDownlinkReport(s) + } + s = s[1:] msglen := len(s) / 2 @@ -208,7 +353,6 @@ func parseInput(buf string) ([]byte, uint16) { thisMsg.TimeReceived = time.Now() thisMsg.Data = frame MsgLog = append(MsgLog, thisMsg) - updateStatus() return frame, msgtype } @@ -216,6 +360,7 @@ func parseInput(buf string) ([]byte, uint16) { type settings struct { UAT_Enabled bool ES_Enabled bool + GPS_Enabled bool } type status struct { @@ -225,7 +370,7 @@ type status struct { UAT_messages_max uint ES_messages_last_minute uint ES_messages_max uint - GPS_satellites_locked uint + GPS_satellites_locked uint16 } var globalSettings settings @@ -257,6 +402,9 @@ func handleManagementConnection(conn net.Conn) { fmt.Printf("%s - error: %s\n", s, err.Error()) } else { fmt.Printf("new settings: %s\n", s) + if !globalSettings.GPS_Enabled && newSettings.GPS_Enabled { // GPS was enabled, restart the reader thread. + go gpsReader() + } globalSettings = newSettings saveSettings() } @@ -284,6 +432,7 @@ func managementInterface() { func defaultSettings() { globalSettings.UAT_Enabled = true //TODO globalSettings.ES_Enabled = false //TODO + globalSettings.GPS_Enabled = false //TODO } func readSettings() { @@ -326,6 +475,10 @@ func saveSettings() { func main() { MsgLog = make([]msg, 0) + + crcInit() // Initialize CRC16 table. + initTraffic() + globalStatus.Version = stratuxVersion globalStatus.Devices = 123 //TODO globalStatus.UAT_messages_last_minute = 567 //TODO @@ -333,7 +486,9 @@ func main() { readSettings() - crcInit() // Initialize CRC16 table. + if globalSettings.GPS_Enabled { + go gpsReader() + } // Open UDP port to send out the messages. addr, err := net.ResolveUDPAddr("udp", ipadAddr) diff --git a/image/spindle/wheezy-stage4 b/image/spindle/wheezy-stage4 index d368bf1f..85cbaa32 100755 --- a/image/spindle/wheezy-stage4 +++ b/image/spindle/wheezy-stage4 @@ -67,10 +67,6 @@ echo blacklist rtl2832 >>/etc/modprobe.d/rtl-sdr-blacklist.conf echo "# prevent power down of wireless when idle" >>/etc/modprobe.d/8192cu.conf echo "options 8192cu rtw_power_mgnt=0 rtw_enusbss=0" >>/etc/modprobe.d/8192cu.conf -#usb hub power -echo "max_usb_current=1" >>/boot/config.txt - - EOF echo "*** STRATUX COMPILE/PACKAGE INSTALL ***" @@ -103,14 +99,12 @@ cp dump1090 /usr/bin/ EOF echo " - Stratux" + +scp_in_to_qemu /root/spindle/gen_gdl90 /tmp/gen_gdl90 + ssh_in_to_qemu chroot /mnt sh -l -ex - <<\EOF -apt-get install -y golang-go cd /root -mkdir /root/go -export GOPATH=/root/go - - rm -rf stratux git clone https://github.com/cyoung/stratux cd stratux @@ -118,12 +112,9 @@ cd dump978 make cp dump978 /usr/bin/ cd .. -go build gen_gdl90.go -go get github.com/sevlyar/go-daemon -go build 1090es_relay.go - -cp gen_gdl90 /usr/bin/ +mv /tmp/gen_gdl90 /usr/bin/gen_gdl90 +chmod +x /usr/bin/gen_gdl90 cp start_uat.sh /usr/bin/start_uat cp init.d-stratux /etc/init.d/stratux cp start_stratux.sh /usr/sbin/stratux @@ -132,7 +123,15 @@ chmod 755 /usr/sbin/stratux chmod 755 /etc/init.d/stratux ln -s /etc/init.d/stratux /etc/rc2.d/S01stratux ln -s /etc/init.d/stratux /etc/rc6.d/K01stratux + +echo -n '{"UAT_Enabled":false,"ES_Enabled":true,"GPS_Enabled":true}' >/etc/stratux.conf + update-rc.d stratux enable + +#usb hub power +echo "max_usb_current=1" >>/boot/config.txt + + EOF echo "**** END STRATUX SETUP *****" diff --git a/ry835ai.go b/ry835ai.go new file mode 100644 index 00000000..8379fb2f --- /dev/null +++ b/ry835ai.go @@ -0,0 +1,186 @@ +package main + + +import ( + "fmt" + "github.com/tarm/serial" + "time" + "strings" + "strconv" +) + +type GPSData struct { + lastFixSinceMidnightUTC uint32 + lat float32 + lng float32 + quality uint8 + satellites uint16 + accuracy float32 // Meters. + alt float32 // Feet. + alt_accuracy float32 + lastFixLocalTime time.Time + trueCourse uint16 + groundSpeed uint16 + lastGroundTrackTime time.Time +} + +var serialConfig *serial.Config +var serialPort *serial.Port + +func initGPSSerialReader() bool { + serialConfig = &serial.Config{Name: "/dev/ttyACM0", Baud: 9600} + p, err := serial.OpenPort(serialConfig) + if err != nil { + fmt.Printf("serial port err: %s\n", err.Error()) + return false + } + serialPort = p + return true +} + +func processNMEALine(l string) bool { + x := strings.Split(l, ",") + if x[0] == "$GNVTG" { // Ground track information. + if len(x) < 10 { + return false + } + trueCourse := uint16(0) + if len(x[1]) > 0 { + tc, err := strconv.ParseFloat(x[1], 32) + if err != nil { + return false + } + trueCourse = uint16(tc) + } else { + // No movement. + myGPS.trueCourse = 0 + myGPS.groundSpeed = 0 + myGPS.lastGroundTrackTime = time.Time{} + return true + } + groundSpeed, err := strconv.ParseFloat(x[5], 32) // Knots. + if err != nil { + return false + } + + myGPS.trueCourse = uint16(trueCourse) + myGPS.groundSpeed = uint16(groundSpeed) + myGPS.lastGroundTrackTime = time.Now() + } else if x[0] == "$GNGGA" { // GPS fix. + if len(x) < 15 { + return false + } + var fix GPSData + + fix = myGPS + + // Timestamp. + if len(x[1]) < 9 { + return false + } + hr, err1 := strconv.Atoi(x[1][0:2]) + min, err2 := strconv.Atoi(x[1][2:4]) + sec, err3 := strconv.Atoi(x[1][4:6]) + if err1 != nil || err2 != nil || err3 != nil { + return false + } + + fix.lastFixSinceMidnightUTC = uint32((hr * 60 * 60) + (min * 60) + sec) + + // Latitude. + if len(x[2]) < 10 { + return false + } + hr, err1 = strconv.Atoi(x[2][0:2]) + minf, err2 := strconv.ParseFloat(x[2][2:10], 32) + if err1 != nil || err2 != nil { + return false + } + + fix.lat = float32(hr) + float32(minf / 60.0) + if x[3] == "S" { // South = negative. + fix.lat = -fix.lat + } + + // Longitude. + if len(x[4]) < 11 { + return false + } + hr, err1 = strconv.Atoi(x[4][0:3]) + minf, err2 = strconv.ParseFloat(x[4][3:11], 32) + if err1 != nil || err2 != nil { + return false + } + + fix.lng = float32(hr) + float32(minf / 60.0) + if x[5] == "W" { // West = negative. + fix.lng = -fix.lng + } + + // Quality indicator. + q, err1 := strconv.Atoi(x[6]) + if err1 != nil { + return false + } + fix.quality = uint8(q) + + // Satellites. + sat, err1 := strconv.Atoi(x[7]) + if err1 != nil { + return false + } + fix.satellites = uint16(sat) + + // Accuracy. + hdop, err1 := strconv.ParseFloat(x[8], 32) + if err1 != nil { + return false + } + fix.accuracy = float32(hdop * 5.0) //FIXME: 5 meters ~ 1.0 HDOP? + + // Altitude. + alt, err1 := strconv.ParseFloat(x[9], 32) + if err1 != nil { + return false + } + fix.alt = float32(alt * 3.28084) // Covnert to feet. + + //TODO: Altitude accuracy. + fix.alt_accuracy = 0 + + // Timestamp. + fix.lastFixLocalTime = time.Now() + + myGPS = fix + + } + return true +} + +func gpsSerialReader() { + defer serialPort.Close() + for { + if !globalSettings.GPS_Enabled { // GPS was turned off. Shut down. + break + } + buf := make([]byte, 1024) + n, err := serialPort.Read(buf) + if err != nil { + fmt.Printf("gps unit read error: %s\n", err.Error()) + return + } + s := string(buf[:n]) + x := strings.Split(s, "\n") + for _, l := range x { + processNMEALine(l) + } + } +} + +func gpsReader() { + if initGPSSerialReader() { + gpsSerialReader() + } else { + globalSettings.GPS_Enabled = false + } +} \ No newline at end of file diff --git a/traffic.go b/traffic.go new file mode 100644 index 00000000..2a374bce --- /dev/null +++ b/traffic.go @@ -0,0 +1,286 @@ +package main + +import ( + "encoding/hex" + "math" + "time" +) + +//-0b2b48fe3aef1f88621a0856110a31c01105c4e6c4e6c40a9a820300000000000000;rs=7; + + +/* + +HDR: + MDB Type: 1 + Address: 2B48FE (TIS-B track file address) +SV: + NIC: 6 + Latitude: +41.4380 + Longitude: -84.1056 + Altitude: 2300 ft (barometric) + N/S velocity: -65 kt + E/W velocity: -98 kt + Track: 236 + Speed: 117 kt + Vertical rate: 0 ft/min (from barometric altitude) + UTC coupling: no + TIS-B site ID: 1 +MS: + Emitter category: No information + Callsign: unavailable + Emergency status: No emergency + UAT version: 2 + SIL: 2 + Transmit MSO: 38 + NACp: 8 + NACv: 1 + NICbaro: 0 + Capabilities: + Active modes: + Target track type: true heading +AUXSV: + Sec. altitude: unavailable + +*/ + +type TrafficInfo struct { + icao_addr uint32 + addr_type uint8 + + lat float32 + lng float32 + + position_valid bool + + alt uint32 + + track uint16 + speed uint16 + speed_valid bool + + vvel int16 + + tail string + + last_seen time.Time +} + +var traffic map[uint32]TrafficInfo + +func cleanupOldEntries() { + for icao_addr, ti := range traffic { + if time.Since(ti.last_seen).Seconds() > float64(60) { //FIXME: 60 seconds with no update on this address - stop displaying. + delete(traffic, icao_addr) + } + } +} + +func sendTrafficUpdates() { + cleanupOldEntries() + for _, ti := range traffic { + makeTrafficReport(ti) + } +} + +func initTraffic() { + traffic = make(map[uint32]TrafficInfo) +} + +func makeTrafficReport(ti TrafficInfo) { + msg := make([]byte, 28) + // See p.16. + msg[0] = 0x14 // Message type "Traffic Report". + + msg[1] = 0x10 | ti.addr_type // Alert status, address type. + + // ICAO Address. + msg[2] = byte((ti.icao_addr & 0x00FF0000) >> 16) + msg[3] = byte((ti.icao_addr & 0x0000FF00) >> 8) + msg[4] = byte((ti.icao_addr & 0x000000FF)) + + lat := float32(ti.lat) + tmp := makeLatLng(lat) + + msg[5] = tmp[0] // Latitude. + msg[6] = tmp[1] // Latitude. + msg[7] = tmp[2] // Latitude. + + lng := float32(ti.lng) + tmp = makeLatLng(lng) + + msg[8] = tmp[0] // Longitude. + msg[9] = tmp[1] // Longitude. + msg[10] = tmp[2] // Longitude. + + +//Altitude: OK +//TODO: 0xFFF "invalid altitude." + alt := uint16(ti.alt) + 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) | 0x8) //FIXME. "Airborne". + + msg[13] = 0x11 //FIXME. + + // Horizontal velocity (speed). + + msg[14] = byte((ti.speed & 0x0FF0) >> 4) + msg[15] = byte((ti.speed & 0x000F) << 4) + + // Vertical velocity. + vvel := ti.vvel / 64 // 64fpm resolution. + msg[15] = msg[15] | byte((vvel & 0x0F00) >> 8) + msg[16] = byte(vvel & 0x00FF) + + // Track. + trk := uint8(float32(ti.track) / TRACK_RESOLUTION) // Resolution is ~1.4 degrees. + msg[17] = byte(trk) + + msg[18] = 0x01 // "light" + + outConn.Write(prepareMessage(msg)) +} + + +func parseDownlinkReport(s string) { + var ti TrafficInfo + s = s[1:] + frame := make([]byte, len(s)/2) + hex.Decode(frame, []byte(s)) + + // Header. +// msg_type := (uint8(frame[0]) >> 3) & 0x1f + ti.addr_type = uint8(frame[0]) & 0x07 + ti.icao_addr = (uint32(frame[1]) << 16) | (uint32(frame[2]) << 8) | uint32(frame[3]) + + // OK. +// fmt.Printf("%d, %d, %06X\n", msg_type, ti.addr_type, ti.icao_addr) + + nic := uint8(frame[11]) & 15 //TODO: Meaning? + + raw_lat := (uint32(frame[4]) << 15) | (uint32(frame[5]) << 7) | (uint32(frame[6]) >> 1) + raw_lon := ((uint32(frame[6]) & 0x01) << 23) | (uint32(frame[7]) << 15) | (uint32(frame[8]) << 7) | (uint32(frame[9]) >> 1) + + lat := float32(0.0) + lng := float32(0.0) + + position_valid := false + if nic != 0 || raw_lat != 0 || raw_lon != 0 { + position_valid = true + lat = float32(raw_lat) * 360.0 / 16777216.0 + if lat > 90 { + lat = lat - 180 + } + lng = float32(raw_lon) * 360.0 / 16777216.0 + if lng > 180 { + lng = lng - 360 + } + } + ti.lat = lat + ti.lng = lng + ti.position_valid = position_valid + + raw_alt := (uint32(frame[10]) << 4) | ((uint32(frame[11]) & 0xf0) >> 4) +// alt_geo := false // Barometric if not geometric. + alt := uint32(0) + if raw_alt != 0 { +// alt_geo = (uint8(frame[9]) & 1) != 0 + alt = ((raw_alt - 1) * 25) - 1000 + } + ti.alt = alt + + //OK. +// fmt.Printf("%d, %t, %f, %f, %t, %d\n", nic, position_valid, lat, lng, alt_geo, alt) + + airground_state := (uint8(frame[12]) >> 6) & 0x03 + //OK. +// fmt.Printf("%d\n", airground_state) + + ns_vel := int16(0) + ew_vel := int16(0) + track := uint16(0) + speed_valid := false + speed := uint16(0) + vvel := int16(0) +// vvel_geo := false + if airground_state == 0 || airground_state == 1 { // Subsonic. Supersonic. + // N/S velocity. + ns_vel_valid := false + ew_vel_valid := false + raw_ns := ((int16(frame[12]) & 0x1f) << 6) | ((int16(frame[13]) & 0xfc) >> 2) + if (raw_ns & 0x3ff) != 0 { + ns_vel_valid = true + ns_vel = ((raw_ns & 0x3ff) - 1) + if (raw_ns & 0x400) != 0 { + ns_vel = 0 - ns_vel + } + if airground_state == 1 { // Supersonic. + ns_vel = ns_vel * 4 + } + } + // E/W velocity. + raw_ew := ((int16(frame[13]) & 0x03) << 9) | (int16(frame[14]) << 1) | ((int16(frame[15] & 0x80)) >> 7) + if (raw_ew & 0x3ff) != 0 { + ew_vel_valid = true + ew_vel = (raw_ew & 0x3ff) - 1 + if (raw_ew & 0x400) != 0 { + ew_vel = 0 - ew_vel + } + if airground_state == 1 { // Supersonic + ew_vel = ew_vel * 4 + } + } + if ns_vel_valid && ew_vel_valid { + if ns_vel != 0 && ew_vel != 0 { + //TODO: Track type + track = (360 + 90 - uint16(math.Atan2(float64(ns_vel), float64(ew_vel)) * 180 / math.Pi)) % 360 + } + speed_valid = true + speed = uint16(math.Sqrt(float64((ns_vel * ns_vel) + (ew_vel * ew_vel)))) + } + + // Vertical velocity. + raw_vvel := ((int16(frame[15]) & 0x7f) << 4) | ((int16(frame[16]) & 0xf0) >> 4) + if (raw_vvel & 0x1ff) != 0 { +// vvel_geo = (raw_vvel & 0x400) == 0 + vvel = ((raw_vvel & 0x1ff) - 1) * 64 + if (raw_vvel & 0x200) != 0 { + vvel = 0 - vvel + } + } + } else if airground_state == 2 { // Ground vehicle. + //TODO. + return + } + + ti.track = track + ti.speed = speed + ti.vvel = vvel + ti.speed_valid = speed_valid + + //OK. +// fmt.Printf("ns_vel %d, ew_vel %d, track %d, speed_valid %t, speed %d, vvel_geo %t, vvel %d\n", ns_vel, ew_vel, track, speed_valid, speed, vvel_geo, vvel) + +/* + utc_coupled := false + tisb_site_id := uint8(0) + + if (uint8(frame[0]) & 7) == 2 || (uint8(frame[0]) & 7) == 3 { //TODO: Meaning? + tisb_site_id = uint8(frame[16]) & 0x0f + } else { + utc_coupled = (uint8(frame[16]) & 0x08) != 0 + } +*/ + + //OK. +// fmt.Printf("tisb_site_id %d, utc_coupled %t\n", tisb_site_id, utc_coupled) + + ti.last_seen = time.Now() + + traffic[ti.icao_addr] = ti +} \ No newline at end of file