tello: properly event the various known notification packets

Signed-off-by: Ron Evans <[email protected]>

examples/tello_video.go

@@ -30,15 +30,19 @@ func main() {
 			return
 		}
 
-		drone.StartVideo()
-		gobot.Every(1*time.Second, func() {
+		drone.On(tello.ConnectedEvent, func(data interface{}) {
+			fmt.Println("Connected")
 			drone.StartVideo()
+			gobot.Every(1*time.Second, func() {
+				drone.StartVideo()
+			})
 		})
 
-		drone.On(tello.EvtVideoFrame, func(data interface{}) {
+		drone.On(tello.VideoFrameEvent, func(data interface{}) {
 			pkt := data.([]byte)
 			if len(pkt) > 6 && pkt[0] == 0x00 && pkt[1] == 0x00 && pkt[2] == 0x00 && pkt[3] == 0x01 {
-				fmt.Println("nal type = ", pkt[6]&0x1f)
+				nalType := pkt[6] & 0x1f
+				fmt.Println("nal type = ", nalType)
 			}
 
 			fmt.Printf("Writing %d bytes\n", len(pkt))

platforms/dji/tello/driver.go

@@ -13,11 +13,17 @@ import (
 )
 
 const (
-	// EvtFlightData event
-	EvtFlightData = "flightdata"
+	// ConnectedEvent event
+	ConnectedEvent = "connected"
 
-	// EvtVideoFrame event
-	EvtVideoFrame = "videoframe"
+	// FlightDataEvent event
+	FlightDataEvent = "flightdata"
+
+	// WifiEvent event
+	WifiEvent = "wifi"
+
+	// VideoFrameEvent event
+	VideoFrameEvent = "videoframe"
 )
 
 // FlightData packet returned by the Tello
@@ -84,8 +90,10 @@ func NewDriver(port string) *Driver {
 		Eventer:   gobot.NewEventer(),
 	}
 
-	d.AddEvent(EvtFlightData)
-	d.AddEvent(EvtVideoFrame)
+	d.AddEvent(ConnectedEvent)
+	d.AddEvent(FlightDataEvent)
+	d.AddEvent(WifiEvent)
+	d.AddEvent(VideoFrameEvent)
 
 	return d
 }
@@ -117,10 +125,6 @@ func (d *Driver) Start() error {
 		return err
 	}
 
-	// video listener
-	videoPort, err := net.ResolveUDPAddr("udp", ":6038")
-	d.videoConn, err = net.ListenUDP("udp", videoPort)
-
 	// handle responses
 	go func() {
 		for {
@@ -131,19 +135,6 @@ func (d *Driver) Start() error {
 		}
 	}()
 
-	// handle video
-	go func() {
-		buf := make([]byte, 2048)
-		for {
-			n, _, err := d.videoConn.ReadFromUDP(buf)
-			d.Publish(d.Event(EvtVideoFrame), buf[2:n])
-
-			if err != nil {
-				fmt.Println("Error: ", err)
-			}
-		}
-	}()
-
 	// starts notifications coming from drone to port 6038 aka 0x9617 when encoded low-endian.
 	d.SendCommand("conn_req:\x96\x17")
 
@@ -174,15 +165,48 @@ func (d *Driver) handleResponse() error {
 		switch buf[5] {
 		case 0x56:
 			fd, _ := d.ParseFlightData(buf[9:])
-			d.Publish(d.Event(EvtFlightData), fd)
+			d.Publish(d.Event(FlightDataEvent), fd)
+		case 26:
+			d.Publish(d.Event(WifiEvent), buf[9:12])
 		default:
-			fmt.Printf("Unknown message: %+v\n", buf)
+			fmt.Printf("Unknown message: %+v\n", buf[0:n])
 		}
 		return nil
 	}
 
-	resp := string(buf[0:n])
-	d.responses <- resp
+	if buf[0] == 0x63 && buf[1] == 0x6f && buf[2] == 0x6e {
+		d.Publish(d.Event(ConnectedEvent), nil)
+		d.processVideo()
+	}
+
+	// resp := string(buf[0:n])
+	// d.responses <- resp
+	return nil
+}
+
+func (d *Driver) processVideo() error {
+	// handle video
+	videoPort, err := net.ResolveUDPAddr("udp", ":6038")
+	if err != nil {
+		return err
+	}
+	d.videoConn, err = net.ListenUDP("udp", videoPort)
+	if err != nil {
+		return err
+	}
+
+	go func() {
+		buf := make([]byte, 2048)
+		for {
+			n, _, err := d.videoConn.ReadFromUDP(buf)
+			d.Publish(d.Event(VideoFrameEvent), buf[2:n])
+
+			if err != nil {
+				fmt.Println("Error: ", err)
+			}
+		}
+	}()
+
 	return nil
 }