tello: add SetVideoEncoderRate command

Signed-off-by: Ron Evans <[email protected]>
thomasbhatia · Apr 19, 2018 · 9c798b8 · 9c798b8
1 parent 8097533
commit 9c798b8
Show file tree

Hide file tree

Showing 3 changed files with 49 additions and 10 deletions.
diff --git a/examples/tello_opencv.go b/examples/tello_opencv.go
@@ -3,8 +3,8 @@
 // Do not build by default.
 
 /*
-You must have ffmpeg and ffplay installed in order to run this code. it will connect to the Tello
-and then open a window using ffplay showing the streaming video.
+You must have ffmpeg and OpenCV installed in order to run this code. It will connect to the Tello
+and then open a window using OpenCV showing the streaming video.
 
 How to run
 
@@ -35,7 +35,7 @@ func main() {
 
 	work := func() {
 		ffmpeg := exec.Command("ffmpeg", "-i", "pipe:0", "-pix_fmt", "bgr24", "-vcodec", "rawvideo",
-			"-an", "-sn", "-r", "25", "-s", "960x720", "-f", "rawvideo", "pipe:1")
+			"-an", "-sn", "-s", "960x720", "-f", "rawvideo", "pipe:1")
 		ffmpegIn, _ := ffmpeg.StdinPipe()
 		ffmpegOut, _ := ffmpeg.StdoutPipe()
 		if err := ffmpeg.Start(); err != nil {
@@ -63,7 +63,9 @@ func main() {
 		drone.On(tello.ConnectedEvent, func(data interface{}) {
 			fmt.Println("Connected")
 			drone.StartVideo()
-			gobot.Every(1*time.Second, func() {
+			drone.SetExposure(1)
+			drone.SetVideoEncoderRate(3)
+			gobot.Every(250*time.Millisecond, func() {
 				drone.StartVideo()
 			})
 		})

diff --git a/examples/tello_video.go b/examples/tello_video.go
@@ -36,7 +36,8 @@ func main() {
 		drone.On(tello.ConnectedEvent, func(data interface{}) {
 			fmt.Println("Connected")
 			drone.StartVideo()
-			gobot.Every(1*time.Second, func() {
+			drone.SetExposure(0)
+			gobot.Every(250*time.Millisecond, func() {
 				drone.StartVideo()
 			})
 		})

diff --git a/platforms/dji/tello/driver.go b/platforms/dji/tello/driver.go
@@ -40,6 +40,9 @@ const (
 	// LightStrengthEvent event
 	LightStrengthEvent = "lightstrength"
 
+	// SetExposureEvent event
+	SetExposureEvent = "setexposure"
+
 	// VideoFrameEvent event
 	VideoFrameEvent = "videoframe"
 )
@@ -53,10 +56,13 @@ const (
 
 	logMessage = 0x50
 
-	videoStartCommand = 0x25
-	takeoffCommand    = 0x54
-	landCommand       = 0x55
-	flipCommand       = 0x5c
+	videoEncoderRateCommand = 0x20
+	videoStartCommand       = 0x25
+	exposureCommand         = 0x34
+	stickCommand            = 0x50
+	takeoffCommand          = 0x54
+	landCommand             = 0x55
+	flipCommand             = 0x5c
 
 	flipFront = 0
 	flipLeft  = 1
@@ -137,6 +143,7 @@ func NewDriver(port string) *Driver {
 	d.AddEvent(WifiDataEvent)
 	d.AddEvent(LightStrengthEvent)
 	d.AddEvent(VideoFrameEvent)
+	d.AddEvent(SetExposureEvent)
 
 	return d
 }
@@ -241,6 +248,8 @@ func (d *Driver) handleResponse() error {
 		case flightMessage:
 			fd, _ := d.ParseFlightData(buf[9:])
 			d.Publish(d.Event(FlightDataEvent), fd)
+		case exposureCommand:
+			d.Publish(d.Event(SetExposureEvent), buf[7:8])
 		default:
 			fmt.Printf("Unknown message: %+v\n", buf[0:n])
 		}
@@ -309,6 +318,33 @@ func (d *Driver) StartVideo() (err error) {
 	return
 }
 
+// SetExposure sets the drone camera exposure level. Valid levels are 0, 1, and 2.
+func (d *Driver) SetExposure(level int) (err error) {
+	if level < 0 || level > 2 {
+		return errors.New("Invalid exposure level")
+	}
+	pkt := []byte{messageStart, 0x60, 0x00, 0x27, 0x48, exposureCommand, 0x00, 0xe6, 0x01, byte(level), 0x00, 0x00}
+
+	// sets ending crc bytes for packet
+	l := len(pkt)
+	pkt[(l - 2)], pkt[(l - 1)] = CalculateCRC(pkt)
+
+	_, err = d.reqConn.Write(pkt)
+	return
+}
+
+// SetVideoEncoderRate sets the drone video encoder rate.
+func (d *Driver) SetVideoEncoderRate(rate int) (err error) {
+	pkt := []byte{messageStart, 0x60, 0x00, 0x27, 0x68, videoEncoderRateCommand, 0x00, 0xe6, 0x01, byte(rate), 0x00, 0x00}
+
+	// sets ending crc bytes for packet
+	l := len(pkt)
+	pkt[(l - 2)], pkt[(l - 1)] = CalculateCRC(pkt)
+
+	_, 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()
@@ -479,7 +515,7 @@ func (d *Driver) SendStickCommand() (err error) {
 	d.cmdMutex.Lock()
 	defer d.cmdMutex.Unlock()
 
-	pkt := []byte{messageStart, 0xb0, 0x00, 0x7f, 0x60, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x12, 0x16, 0x01, 0x0e, 0x00, 0x25, 0x54}
+	pkt := []byte{messageStart, 0xb0, 0x00, 0x7f, 0x60, stickCommand, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x12, 0x16, 0x01, 0x0e, 0x00, 0x25, 0x54}
 
 	// RightX center=1024 left =364 right =-364
 	axis1 := int16(660.0*d.rx + 1024.0)