tello: WIP on adding support for video streaming from drone

Signed-off-by: Ron Evans <ron@hybridgroup.com>

examples/tello_video.go

@@ -0,0 +1,59 @@
+// +build example
+//
+// Do not build by default.
+
+/*
+ How to run
+ Pass the file name to use to save the raw H264 video from the drone as first param:
+
+	go run examples/tello_video.go "/tmp/tello.h264"
+*/
+
+package main
+
+import (
+	"fmt"
+	"os"
+	"time"
+
+	"gobot.io/x/gobot"
+	"gobot.io/x/gobot/platforms/dji/tello"
+)
+
+func main() {
+	drone := tello.NewDriver("8890")
+
+	work := func() {
+		f, err := os.Create(os.Args[1])
+		if err != nil {
+			fmt.Println(err)
+			return
+		}
+
+		drone.StartVideo()
+		gobot.Every(1*time.Second, func() {
+			drone.StartVideo()
+		})
+
+		drone.On(tello.EvtVideoFrame, 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)
+			}
+
+			fmt.Printf("Writing %d bytes\n", len(pkt))
+			_, err := f.Write(pkt)
+			if err != nil {
+				fmt.Println(err)
+			}
+		})
+	}
+
+	robot := gobot.NewRobot("tello",
+		[]gobot.Connection{},
+		[]gobot.Device{drone},
+		work,
+	)
+
+	robot.Start()
+}

platforms/dji/tello/driver.go

@@ -12,6 +12,14 @@ import (
 	"gobot.io/x/gobot"
 )
 
+const (
+	// EvtFlightData event
+	EvtFlightData = "flightdata"
+
+	// EvtVideoFrame event
+	EvtVideoFrame = "videoframe"
+)
+
 // FlightData packet returned by the Tello
 type FlightData struct {
 	batteryLow               int16
@@ -58,19 +66,28 @@ type Driver struct {
 	name                     string
 	reqAddr                  string
 	reqConn                  *net.UDPConn // UDP connection to send/receive drone commands
+	videoConn                *net.UDPConn // UDP connection for drone video
 	respPort                 string
 	responses                chan string
 	cmdMutex                 sync.Mutex
 	rx, ry, lx, ly, throttle float32
+	gobot.Eventer
 }
 
 // NewDriver creates a driver for the Tello drone. Pass in the UDP port to use for the responses
 // from the drone.
 func NewDriver(port string) *Driver {
-	return &Driver{name: gobot.DefaultName("Tello"),
+	d := &Driver{name: gobot.DefaultName("Tello"),
 		reqAddr:   "192.168.10.1:8889",
 		respPort:  port,
-		responses: make(chan string)}
+		responses: make(chan string),
+		Eventer:   gobot.NewEventer(),
+	}
+
+	d.AddEvent(EvtFlightData)
+	d.AddEvent(EvtVideoFrame)
+
+	return d
 }
 
 // Name returns the name of the device.
@@ -100,6 +117,10 @@ 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 {
@@ -110,12 +131,25 @@ func (d *Driver) Start() error {
 		}
 	}()
 
-	// starts notifications coming from drone to port aka 0x9617 when encoded low-endian.
+	// 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")
 
 	// send stick commands
 	go func() {
-		time.Sleep(500 * time.Millisecond)
+		time.Sleep(100 * time.Millisecond)
 		for {
 			err := d.SendStickCommand()
 			if err != nil {
@@ -140,7 +174,7 @@ func (d *Driver) handleResponse() error {
 		switch buf[5] {
 		case 0x56:
 			fd, _ := d.ParseFlightData(buf[9:])
-			fmt.Printf("Flight data: %+v\n", fd)
+			d.Publish(d.Event(EvtFlightData), fd)
 		default:
 			fmt.Printf("Unknown message: %+v\n", buf)
 		}
@@ -172,6 +206,13 @@ func (d *Driver) Land() (err error) {
 	return
 }
 
+// StartVideo tells to start video stream.
+func (d *Driver) StartVideo() (err error) {
+	pkt := []byte{0xcc, 0x58, 0x00, 0x7c, 0x60, 0x25, 0x00, 0x00, 0x00, 0x6c, 0x95}
+	_, err = d.reqConn.Write(pkt)
+	return
+}
+
 // Up tells the drone to ascend. Pass in an int from 0-100.
 func (d *Driver) Up(val int) error {
 	d.cmdMutex.Lock()