Pull request changes b3nn0#261

Patch Version 3:
+ If a working device is disconnected the system will try to reconnect every 10s

image/99-uavionix.rules

@@ -4,3 +4,5 @@ ATTRS{idProduct}=="74f0", ATTRS{idVendor}=="0403", RUN+="/sbin/modprobe -q ftdi_
 # uAvionix Ping Raw
 ATTRS{idProduct}=="74f1", ATTRS{idVendor}=="0403", RUN+="/sbin/modprobe -q ftdi_sio" RUN+="/bin/sh -c 'echo 0403 74f1 > /sys/bus/usb-serial/drivers/ftdi_sio/new_id'", OWNER="root", MODE="0666", SYMLINK+="ping"
 
+# uAvionix PingUSB MavLink Device (ID 0403:6015 Future Technology Devices International, Ltd Bridge(I2C/SPI/UART/FIFO))
+SUBSYSTEM=="tty", SUBSYSTEMS=="usb", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", SYMLINK+="pingusb"

main/ping.go

@@ -5,6 +5,7 @@
 	as part of this header.
 
 	ping.go: uAvionix Ping ADS-B monitoring and management.
+    2023 Added PingUSB MavLink support device 0403:6015
 */
 
 package main
@@ -24,6 +25,7 @@ import (
 
 	// Using forked version of tarm/serial to force Linux
 	// instead of posix code, allowing for higher baud rates
+	"github.com/b3nn0/stratux/common"
 	"github.com/uavionix/serial"
 )
 
@@ -33,10 +35,16 @@ var pingSerialPort *serial.Port
 var pingWG *sync.WaitGroup
 var closeCh chan int
 
+// 0 => pingEFB - 1090ES
+// 1 => pingUSB - MavLink
+var pingDeviceModel int
+var pingDeviceSuccessfullyWorking bool
+
 func initPingSerial() bool {
 	var device string
 	baudrate := int(2000000)
 
+	pingDeviceModel = 0
 	log.Printf("Configuring Ping ADS-B\n")
 
 	if _, err := os.Stat("/dev/ping"); err == nil {
@@ -44,6 +52,11 @@ func initPingSerial() bool {
 	} else if _, err := os.Stat("/dev/softrf"); err == nil {
 		device = "/dev/softrf"
 		baudrate = int(38400)
+	} else if _, err := os.Stat("/dev/pingusb"); err == nil {
+		// 99-uavionix.rules 0403:6015
+		device = "/dev/pingusb"
+		baudrate = int(57600)
+		pingDeviceModel = 1
 	} else {
 		log.Printf("No suitable Ping device found.\n")
 		return false
@@ -149,6 +162,7 @@ func pingSerialReader() {
 
 	scanner := bufio.NewScanner(pingSerialPort)
 	for scanner.Scan() && globalStatus.Ping_connected && globalSettings.Ping_Enabled {
+		pingDeviceSuccessfullyWorking = true
 		s := scanner.Text()
 		// Trimspace removes newlines as well as whitespace
 		s = strings.TrimSpace(s)
@@ -211,8 +225,11 @@ func pingShutdown() {
 	//log.Println("Ping shutdown(): calling pingWG.Wait() ...")
 	//pingWG.Wait() // Wait for the goroutine to shutdown
 	//log.Println("Ping shutdown(): pingWG.Wait() returned...")
-	// RCB TODO FINISH
-	globalStatus.Ping_connected = false
+	// Serial Port Gracefully Close and Read() returns
+	//globalStatus.Ping_connected = false
+	if globalStatus.Ping_connected == true {
+		pingSerialPort.Close()
+	}
 }
 
 func pingKill() {
@@ -232,6 +249,7 @@ var shutdownPing bool
 // Watch for config/device changes.
 func pingWatcher() {
 	prevPingEnabled := false
+	pingDeviceSuccessfullyWorking = false
 
 	for {
 		time.Sleep(1 * time.Second)
@@ -240,6 +258,12 @@ func pingWatcher() {
 		if shutdownPing {
 			pingShutdown()
 			shutdownPing = false
+			// Shutdown this reconnection loop
+			break
+		}
+		// Autoreconnect the device
+		if pingDeviceSuccessfullyWorking == true && globalSettings.Ping_Enabled && !globalStatus.Ping_connected {
+			prevPingEnabled = false
 		}
 
 		if prevPingEnabled == globalSettings.Ping_Enabled {
@@ -249,8 +273,15 @@ func pingWatcher() {
 		// Global settings have changed, reconfig
 		if globalSettings.Ping_Enabled && !globalStatus.Ping_connected {
 			globalStatus.Ping_connected = initPingSerial()
+			// This will retry next loop to connect again to the device
+			if globalStatus.Ping_connected == false {
+				// Relaxed polling to wait the device to be discovered
+				time.Sleep(10 * time.Second)
+				continue
+			}
 			//count := 0
-			if globalStatus.Ping_connected {
+			// pingEFB - 1090
+			if globalStatus.Ping_connected && pingDeviceModel == 0 {
 				//pingWG.Add(1)
 				go pingNetworkRepeater()
 				//pingNetworkConnection()
@@ -258,6 +289,10 @@ func pingWatcher() {
 				// Emulate SDR count
 				//count = 2
 			}
+			// pingUSB - MavLink
+			if globalStatus.Ping_connected && pingDeviceModel == 1 {
+				go pingUSBSerialReader()
+			}
 			//atomic.StoreUint32(&globalStatus.Devices, uint32(count))
 		} else if !globalSettings.Ping_Enabled {
 			pingShutdown()
@@ -270,3 +305,213 @@ func pingWatcher() {
 func pingInit() {
 	go pingWatcher()
 }
+
+type MavlinkTrafficMessageFormat struct {
+	ICAO_address  uint32
+	lat           int32
+	lon           int32
+	altitude      int32
+	heading       uint16
+	hor_velocity  uint16
+	ver_velocity  int16
+	validFlags    uint16
+	squawk        uint16
+	altitude_type uint8
+	callsign      [9]byte
+	emitter_type  uint8
+	tslc          uint8
+}
+
+func mavLinkFormat(x []byte) {
+	msglenMavLink := x[1]
+	msgtypeMavLink := x[5]
+	if msgtypeMavLink == 246 && msglenMavLink >= 38 {
+		mavLink := MavlinkTrafficMessageFormat{}
+		pingUsbHeaderLen := 6
+		pingUsbHeaderCursor := 0
+		mavLink.ICAO_address = uint32(x[3+pingUsbHeaderLen+pingUsbHeaderCursor])<<24 | uint32(x[2+pingUsbHeaderLen+pingUsbHeaderCursor])<<16 | uint32(x[1+pingUsbHeaderLen+pingUsbHeaderCursor])<<8 | uint32(x[0+pingUsbHeaderLen+pingUsbHeaderCursor])
+		pingUsbHeaderCursor = 4
+		mavLink.lat = int32(uint32(x[3+pingUsbHeaderLen+pingUsbHeaderCursor])<<24 | uint32(x[2+pingUsbHeaderLen+pingUsbHeaderCursor])<<16 | uint32(x[1+pingUsbHeaderLen+pingUsbHeaderCursor])<<8 | uint32(x[0+pingUsbHeaderLen+pingUsbHeaderCursor]))
+		pingUsbHeaderCursor = 8
+		mavLink.lon = int32(uint32(x[3+pingUsbHeaderLen+pingUsbHeaderCursor])<<24 | uint32(x[2+pingUsbHeaderLen+pingUsbHeaderCursor])<<16 | uint32(x[1+pingUsbHeaderLen+pingUsbHeaderCursor])<<8 | uint32(x[0+pingUsbHeaderLen+pingUsbHeaderCursor]))
+		pingUsbHeaderCursor = 12
+		mavLink.altitude = int32(uint32(x[3+pingUsbHeaderLen+pingUsbHeaderCursor])<<24 | uint32(x[2+pingUsbHeaderLen+pingUsbHeaderCursor])<<16 | uint32(x[1+pingUsbHeaderLen+pingUsbHeaderCursor])<<8 | uint32(x[0+pingUsbHeaderLen+pingUsbHeaderCursor]))
+		pingUsbHeaderCursor = 16
+		mavLink.heading = (uint16(x[1+pingUsbHeaderLen+pingUsbHeaderCursor])<<8 | uint16(x[0+pingUsbHeaderLen+pingUsbHeaderCursor]))
+		pingUsbHeaderCursor = 18
+		mavLink.hor_velocity = (uint16(x[1+pingUsbHeaderLen+pingUsbHeaderCursor])<<8 | uint16(x[0+pingUsbHeaderLen+pingUsbHeaderCursor]))
+		pingUsbHeaderCursor = 20
+		mavLink.ver_velocity = int16(uint16(x[1+pingUsbHeaderLen+pingUsbHeaderCursor])<<8 | uint16(x[0+pingUsbHeaderLen+pingUsbHeaderCursor]))
+		pingUsbHeaderCursor = 22
+		mavLink.validFlags = (uint16(x[1+pingUsbHeaderLen+pingUsbHeaderCursor])<<8 | uint16(x[0+pingUsbHeaderLen+pingUsbHeaderCursor]))
+		pingUsbHeaderCursor = 24
+		mavLink.squawk = (uint16(x[1+pingUsbHeaderLen+pingUsbHeaderCursor])<<8 | uint16(x[0+pingUsbHeaderLen+pingUsbHeaderCursor]))
+		pingUsbHeaderCursor = 26
+		mavLink.altitude_type = x[0+pingUsbHeaderLen+pingUsbHeaderCursor]
+		pingUsbHeaderCursor = 27
+
+		for j := 0; j < 9; j++ {
+			mavLink.callsign[j] = 0
+		}
+		for a, b := range x[0+pingUsbHeaderLen+pingUsbHeaderCursor:] {
+			if a > 8 || b == ' ' {
+				break
+			}
+			mavLink.callsign[a] = b
+		}
+		pingUsbHeaderCursor = 36
+		mavLink.emitter_type = x[0+pingUsbHeaderLen+pingUsbHeaderCursor]
+		pingUsbHeaderCursor = 37
+		mavLink.tslc = x[0+pingUsbHeaderLen+pingUsbHeaderCursor]
+		if globalSettings.DEBUG {
+			log.Printf("ICAO_address %06X lat: %d lon: %d alt: %d head: %d call: %s vspeed: %d speed: %d", mavLink.ICAO_address, mavLink.lat, mavLink.lon, mavLink.altitude, mavLink.heading, mavLink.callsign, mavLink.ver_velocity, mavLink.hor_velocity)
+		}
+		var ti TrafficInfo
+		trafficMutex.Lock()
+		signalLevelSimulated := -1
+		if val, ok := traffic[mavLink.ICAO_address]; ok { // if we've already seen it, copy it in to do updates as it may contain some useful information like "tail" from 1090ES.
+			ti = val
+		} else {
+			// New
+			ti.Last_seen = stratuxClock.Time
+		}
+		if mavLink.heading != 0 {
+			ti.Track = float32(mavLink.heading / 100)
+		} else {
+			signalLevelSimulated -= 5
+		}
+		ti.ReceivedMsgs += 1
+		ti.Icao_addr = mavLink.ICAO_address
+		ti.OnGround = false
+		ti.Addr_type = 0
+		if mavLink.squawk != 0 {
+			ti.Squawk = int(mavLink.squawk)
+		}
+		ti.TargetType = TARGET_TYPE_ADSB
+		if mavLink.emitter_type != 0 {
+			ti.Emitter_category = uint8(mavLink.emitter_type)
+		}
+		if mavLink.lat != 0 && mavLink.lon != 0 {
+			lat := float32(mavLink.lat) / 10000000.0
+			lng := float32(mavLink.lon) / 10000000.0
+			// Low signal may involve into a freeze location, update only if it really changes
+			if lng != ti.Lng && lat != ti.Lat {
+				ti.Lat = lat
+				ti.Lng = lng
+				if isGPSValid() {
+					lat := float64(mySituation.GPSLatitude)
+					lng := float64(mySituation.GPSLongitude)
+					ti.Distance, ti.Bearing = common.Distance(float64(lat), float64(lng), float64(ti.Lat), float64(ti.Lng))
+					ti.BearingDist_valid = true
+				} else {
+					ti.BearingDist_valid = false
+				}
+				ti.Position_valid = true
+				ti.ExtrapolatedPosition = false
+				ti.Last_seen = stratuxClock.Time
+				ti.Timestamp = time.Now().UTC()
+			}
+		} else {
+			ti.Position_valid = false
+			signalLevelSimulated -= 5
+		}
+		altitudeFoot := int32(float32(mavLink.altitude) / 304.8)
+		if altitudeFoot > 0 && altitudeFoot < 70000 {
+			// Low signal may involve into a freeze location, update only if it really changes
+			if ti.Alt != altitudeFoot {
+				ti.Alt = altitudeFoot
+				ti.Last_alt = stratuxClock.Time
+				ti.Last_seen = stratuxClock.Time
+				ti.Timestamp = time.Now().UTC()
+			}
+		} else {
+			signalLevelSimulated -= 5
+		}
+		if mavLink.altitude_type > 0 {
+			ti.AltIsGNSS = true
+		}
+		if mavLink.hor_velocity > 0 {
+			ti.Speed = uint16(float32(mavLink.hor_velocity) * 36 / 1852)
+			ti.Speed_valid = true
+			ti.Last_speed = stratuxClock.Time
+		} else {
+			ti.Speed_valid = false
+			signalLevelSimulated -= 5
+		}
+		ti.Vvel = int16(float32(mavLink.ver_velocity) * 60.0 / 10.0 / 3.048)
+		var callsignLen = 0
+		for a, b := range mavLink.callsign {
+			if b == ' ' || b == '\u0000' {
+				break
+			}
+			callsignLen = a + 1
+		}
+		thisReg, validReg := icao2reg(ti.Icao_addr)
+		if validReg {
+			ti.Reg = thisReg
+		}
+		if callsignLen > 0 {
+			ti.Tail = string(mavLink.callsign[:callsignLen])
+		} else {
+			signalLevelSimulated -= 5
+			if validReg {
+				ti.Tail = thisReg
+			}
+		}
+		// Timestamp and Last_seen are updated only if Location or Altitude changes
+		ti.NACp = 8
+		ti.NIC = 8
+		ti.Last_source = TRAFFIC_SOURCE_1090ES
+		ti.SignalLevel = float64(signalLevelSimulated)
+		postProcessTraffic(&ti)
+		traffic[ti.Icao_addr] = ti
+		registerTrafficUpdate(ti)
+		seenTraffic[ti.Icao_addr] = true
+		trafficMutex.Unlock()
+	}
+}
+
+func mavLinkParse(mavLinkFrame []byte) bool {
+	if mavLinkFrame[0] != 0xfe || len(mavLinkFrame) > 1024 {
+		return true
+	}
+	if len(mavLinkFrame) < 9 || mavLinkFrame[0] != 0xfe || int(mavLinkFrame[1]+8) != len(mavLinkFrame) {
+		return false
+	}
+	mavLinkFormat(mavLinkFrame)
+	pingDeviceSuccessfullyWorking = true
+	return true
+}
+
+func pingUSBSerialReader() {
+	defer pingSerialPort.Close()
+	// RCB TODO channel control for terminate
+	log.Printf("Starting PingUSB serial reader")
+	data := make([]byte, 4096)
+	mavLinkFrame := make([]byte, 4096)
+	mavLinkFrameLastIndex := 0
+	for globalStatus.Ping_connected && globalSettings.Ping_Enabled {
+		n, err := pingSerialPort.Read(data)
+		if err != nil {
+			break
+		}
+		for _, b := range data[:n] {
+			if b == 0xfe && mavLinkFrameLastIndex > 0 {
+				if mavLinkFrameLastIndex > 0 {
+					if mavLinkParse(mavLinkFrame[:mavLinkFrameLastIndex]) {
+						mavLinkFrameLastIndex = 0
+					}
+					mavLinkFrame[mavLinkFrameLastIndex] = b
+					mavLinkFrameLastIndex += 1
+				}
+			} else {
+				mavLinkFrame[mavLinkFrameLastIndex] = b
+				mavLinkFrameLastIndex += 1
+			}
+		}
+		continue
+	}
+	globalStatus.Ping_connected = false
+	log.Printf("Exiting PingUSB serial reader")
+	return
+}