Skip to content

Commit

Permalink
tello: flip commands works, a bunch of events, and video streaming ex…
Browse files Browse the repository at this point in the history 
…ample using ffplay

Signed-off-by: Ron Evans <[email protected]>
  • Loading branch information
@deadprogram
deadprogram committed Apr 18, 2018
1 parent 3c4c15b commit 4fe09d8
Show file tree
Hide file tree
Showing 4 changed files with 175 additions and 38 deletions.
21 changes: 21 additions & 0 deletions examples/tello_ps3.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ How to run
package main

import (
"fmt"
"sync/atomic"
"time"

Expand Down Expand Up @@ -53,6 +54,26 @@ func main() {
drone.Land()
})

stick.On(joystick.UpPress, func(data interface{}) {
fmt.Println("FrontFlip")
drone.FrontFlip()
})

stick.On(joystick.DownPress, func(data interface{}) {
fmt.Println("BackFlip")
drone.BackFlip()
})

stick.On(joystick.RightPress, func(data interface{}) {
fmt.Println("RightFlip")
drone.RightFlip()
})

stick.On(joystick.LeftPress, func(data interface{}) {
fmt.Println("LeftFlip")
drone.LeftFlip()
})

stick.On(joystick.LeftX, func(data interface{}) {
val := float64(data.(int16))
leftX.Store(val)
Expand Down
28 changes: 15 additions & 13 deletions examples/tello_video.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,17 +3,18 @@
// 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:
You must have ffmpeg and ffplay installed in order to run this code.
go run examples/tello_video.go "/tmp/tello.h264"
How to run
go run examples/tello_video.go
*/

package main

import (
"fmt"
"os"
"os/exec"
"time"

"gobot.io/x/gobot"
Expand All @@ -24,8 +25,10 @@ func main() {
drone := tello.NewDriver("8890")

work := func() {
f, err := os.Create(os.Args[1])
if err != nil {
// launch ffplay and get it ready to display video
ffplay := exec.Command("ffplay", "-i", "pipe:0")
ffplayIn, _ := ffplay.StdinPipe()
if err := ffplay.Start(); err != nil {
fmt.Println(err)
return
}
Expand All @@ -40,14 +43,13 @@ func main() {

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 {
nalType := pkt[6] & 0x1f
fmt.Println("nal type = ", nalType)
}
// if len(pkt) > 6 && pkt[0] == 0x00 && pkt[1] == 0x00 && pkt[2] == 0x00 && pkt[3] == 0x01 {
// nalType := pkt[6] & 0x1f
// //fmt.Println("nal type = ", nalType)
// }

fmt.Printf("Writing %d bytes\n", len(pkt))
_, err := f.Write(pkt)
if err != nil {
//fmt.Printf("Writing %d bytes\n", len(pkt))
if _, err := ffplayIn.Write(pkt); err != nil {
fmt.Println(err)
}
})
Expand Down
12 changes: 11 additions & 1 deletion platforms/dji/tello/crc.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,12 +30,22 @@ func fsc16(bytes []byte, length int, poly int) int {
return j
}

// CalculateCRC calculates the ending CRC bytes for packet.
func CalculateCRC(pkt []byte) (bLow, bHigh byte) {
l := len(pkt)
i := fsc16(pkt, l-2, poly)
bLow = ((byte)(i & 0xFF))
bHigh = ((byte)(i >> 8 & 0xFF))
return
}

// uCRC
func uCRCTable() []int {
return []int{0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65, 157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220, 35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98, 190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255, 70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7, 219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154, 101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36, 248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185, 140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205, 17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80, 175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238, 50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115, 202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139, 87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22, 233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168, 116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53}
}

func uCRC(bytes []byte, length int, poly int) int {
// CalculateUCRC calculates the starting uCRC byte for packet.
func CalculateUCRC(bytes []byte, length int, poly int) int {
fs := uCRCTable()

j := 0
Expand Down
152 changes: 128 additions & 24 deletions platforms/dji/tello/driver.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,13 +19,51 @@ const (
// FlightDataEvent event
FlightDataEvent = "flightdata"

// WifiEvent event
WifiEvent = "wifi"
// TakeoffEvent event
TakeoffEvent = "takeoff"

// LandingEvent event
LandingEvent = "landing"

// FlipEvent event
FlipEvent = "flip"

// TimeEvent event
TimeEvent = "time"

// LogEvent event
LogEvent = "log"

// WifiDataEvent event
WifiDataEvent = "wifidata"

// LightStrengthEvent event
LightStrengthEvent = "lightstrength"

// VideoFrameEvent event
VideoFrameEvent = "videoframe"
)

const (
messageStart = 0xcc
wifiMessage = 26
lightMessage = 53
timeMessage = 70
flightMessage = 86

logMessage = 0x50

videoStartCommand = 0x25
takeoffCommand = 0x54
landCommand = 0x55
flipCommand = 0x5c

flipFront = 0
flipLeft = 1
flipBack = 2
flipRight = 3
)

// FlightData packet returned by the Tello
type FlightData struct {
batteryLow int16
Expand Down Expand Up @@ -67,14 +105,19 @@ type FlightData struct {
windState int16
}

// WifiData packet returned by the Tello
type WifiData struct {
Disturb int16
Strength int16
}

// Driver represents the DJI Tello drone
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
Expand All @@ -84,15 +127,15 @@ type Driver struct {
// from the drone.
func NewDriver(port string) *Driver {
d := &Driver{name: gobot.DefaultName("Tello"),
reqAddr: "192.168.10.1:8889",
respPort: port,
responses: make(chan string),
Eventer: gobot.NewEventer(),
reqAddr: "192.168.10.1:8889",
respPort: port,
Eventer: gobot.NewEventer(),
}

d.AddEvent(ConnectedEvent)
d.AddEvent(FlightDataEvent)
d.AddEvent(WifiEvent)
d.AddEvent(WifiDataEvent)
d.AddEvent(LightStrengthEvent)
d.AddEvent(VideoFrameEvent)

return d
Expand Down Expand Up @@ -136,6 +179,7 @@ func (d *Driver) Start() error {
}()

// starts notifications coming from drone to port 6038 aka 0x9617 when encoded low-endian.
// TODO: allow setting a specific video port.
d.SendCommand("conn_req:\x96\x17")

// send stick commands
Expand All @@ -160,32 +204,59 @@ func (d *Driver) handleResponse() error {
return err
}

if buf[0] == 0xcc {
// parse binary packet
// parse binary packet
if buf[0] == messageStart {
if buf[6] == 0x10 {
switch buf[5] {
case logMessage:
d.Publish(d.Event(LogEvent), buf[9:])
default:
fmt.Printf("Unknown message: %+v\n", buf[0:n])
}
return nil
}

switch buf[5] {
case 0x56:
case wifiMessage:
buf := bytes.NewReader(buf[9:12])
wd := &WifiData{}

err = binary.Read(buf, binary.LittleEndian, &wd.Disturb)
err = binary.Read(buf, binary.LittleEndian, &wd.Strength)
d.Publish(d.Event(WifiDataEvent), wd)
case lightMessage:
buf := bytes.NewReader(buf[9:10])
var ld int16

err = binary.Read(buf, binary.LittleEndian, &ld)
d.Publish(d.Event(LightStrengthEvent), ld)
case timeMessage:
d.Publish(d.Event(TimeEvent), buf[7:8])
case takeoffCommand:
d.Publish(d.Event(TakeoffEvent), buf[7:8])
case landCommand:
d.Publish(d.Event(LandingEvent), buf[7:8])
case flipCommand:
d.Publish(d.Event(FlipEvent), buf[7:8])
case flightMessage:
fd, _ := d.ParseFlightData(buf[9:])
d.Publish(d.Event(FlightDataEvent), fd)
case 26:
d.Publish(d.Event(WifiEvent), buf[9:12])
default:
fmt.Printf("Unknown message: %+v\n", buf[0:n])
}
return nil
}

// parse text packet
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
Expand Down Expand Up @@ -213,26 +284,27 @@ func (d *Driver) processVideo() error {
// Halt stops the driver.
func (d *Driver) Halt() (err error) {
d.reqConn.Close()
d.videoConn.Close()
return
}

// TakeOff tells drones to liftoff and start flying.
func (d *Driver) TakeOff() (err error) {
takeOffPacket := []byte{0xcc, 0x58, 0x00, 0x7c, 0x68, 0x54, 0x00, 0xe4, 0x01, 0xc2, 0x16}
takeOffPacket := []byte{messageStart, 0x58, 0x00, 0x7c, 0x68, takeoffCommand, 0x00, 0xe4, 0x01, 0xc2, 0x16}
_, err = d.reqConn.Write(takeOffPacket)
return
}

// Land tells drone to come in for landing.
func (d *Driver) Land() (err error) {
landPacket := []byte{0xcc, 0x60, 0x00, 0x27, 0x68, 0x55, 0x00, 0xe5, 0x01, 0x00, 0xba, 0xc7}
landPacket := []byte{messageStart, 0x60, 0x00, 0x27, 0x68, landCommand, 0x00, 0xe5, 0x01, 0x00, 0xba, 0xc7}
_, err = d.reqConn.Write(landPacket)
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}
pkt := []byte{messageStart, 0x58, 0x00, 0x7c, 0x60, videoStartCommand, 0x00, 0x00, 0x00, 0x6c, 0x95}
_, err = d.reqConn.Write(pkt)
return
}
Expand Down Expand Up @@ -310,6 +382,38 @@ func (d *Driver) CounterClockwise(val int) error {
return nil
}

// Flip tells drone to flip
func (d *Driver) Flip(direction int) (err error) {
pkt := []byte{messageStart, 0x60, 0x00, 0x27, 0x70, flipCommand, 0x00, 0xe6, 0x01, byte(direction), 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
}

// FrontFlip tells the drone to perform a front flip.
func (d *Driver) FrontFlip() (err error) {
return d.Flip(flipFront)
}

// BackFlip tells the drone to perform a back flip.
func (d *Driver) BackFlip() (err error) {
return d.Flip(flipBack)
}

// RightFlip tells the drone to perform a flip to the right.
func (d *Driver) RightFlip() (err error) {
return d.Flip(flipRight)
}

// LeftFlip tells the drone to perform a flip to the left.
func (d *Driver) LeftFlip() (err error) {
return d.Flip(flipLeft)
}

// ParseFlightData from drone
func (d *Driver) ParseFlightData(b []byte) (fd *FlightData, err error) {
buf := bytes.NewReader(b)
Expand Down Expand Up @@ -370,11 +474,12 @@ func (d *Driver) ParseFlightData(b []byte) (fd *FlightData, err error) {
return
}

// SendStickCommand sends the joystick command packet to the drone.
func (d *Driver) SendStickCommand() (err error) {
d.cmdMutex.Lock()
defer d.cmdMutex.Unlock()

pkt := []byte{0xcc, 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, 0x50, 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)
Expand Down Expand Up @@ -406,16 +511,15 @@ func (d *Driver) SendStickCommand() (err error) {
pkt[18] = byte(now.UnixNano() / int64(time.Millisecond) & 0xff)
pkt[19] = byte(now.UnixNano() / int64(time.Millisecond) >> 8)

// sets crc for packet
// sets ending crc for packet
l := len(pkt)
i := fsc16(pkt, l-2, poly)
pkt[(l - 2)] = ((byte)(i & 0xFF))
pkt[(l - 1)] = ((byte)(i >> 8 & 0xFF))
pkt[(l - 2)], pkt[(l - 1)] = CalculateCRC(pkt)

_, err = d.reqConn.Write(pkt)
return
}

// SendCommand is used to send a text command such as the initial connection request to the drone.
func (d *Driver) SendCommand(cmd string) (err error) {
_, err = d.reqConn.Write([]byte(cmd))
return
Expand Down

0 comments on commit 4fe09d8

Please sign in to comment.