tello: ability to send real-time stick commands

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

examples/tello.go

@@ -13,6 +13,7 @@ package main
 
 import (
 	"os"
+	"time"
 
 	"gobot.io/x/gobot"
 	"gobot.io/x/gobot/platforms/dji/tello"
@@ -22,20 +23,11 @@ func main() {
 	drone := tello.NewDriver(os.Args[1])
 
 	work := func() {
-		// battery, _ := drone.Battery()
-		// fmt.Println("battery:", battery)
+		drone.TakeOff()
 
-		// speed, _ := drone.Speed()
-		// fmt.Println("speed:", speed)
-
-		//		fmt.Println("Flying")
-		// drone.TakeOff()
-
-		// gobot.After(5*time.Second, func() {
-		// 	drone.Land()
-		// 	ft, _ := drone.FlightTime()
-		// 	fmt.Println("flight time:", ft)
-		// })
+		gobot.After(5*time.Second, func() {
+			drone.Land()
+		})
 	}
 
 	robot := gobot.NewRobot("tello",

examples/tello_ps3.go

@@ -0,0 +1,164 @@
+// +build example
+//
+// Do not build by default.
+
+/*
+ How to setup
+ You must be using a PS3 or compatible controller, along with
+ the DJI Tello drones to run this example.
+
+ You run the Go program on your computer and communicate
+ wirelessly with the Tello.
+
+ How to run
+ Pass the Bluetooth name or address as first param:
+
+	go run examples/tello_ps3.go "Travis_1234"
+
+ NOTE: sudo is required to use BLE in Linux
+*/
+
+package main
+
+import (
+	"os"
+	"sync/atomic"
+	"time"
+
+	"gobot.io/x/gobot"
+	"gobot.io/x/gobot/platforms/dji/tello"
+	"gobot.io/x/gobot/platforms/joystick"
+)
+
+type pair struct {
+	x float64
+	y float64
+}
+
+var leftX, leftY, rightX, rightY atomic.Value
+
+const offset = 32767.0
+
+func main() {
+	joystickAdaptor := joystick.NewAdaptor()
+	stick := joystick.NewDriver(joystickAdaptor,
+		"./platforms/joystick/configs/dualshock3.json",
+	)
+
+	drone := tello.NewDriver(os.Args[1])
+
+	work := func() {
+		leftX.Store(float64(0.0))
+		leftY.Store(float64(0.0))
+		rightX.Store(float64(0.0))
+		rightY.Store(float64(0.0))
+
+		recording := false
+
+		stick.On(joystick.CirclePress, func(data interface{}) {
+			// if recording {
+			// 	drone.StopRecording()
+			// } else {
+			// 	drone.StartRecording()
+			// }
+			recording = !recording
+		})
+
+		stick.On(joystick.SquarePress, func(data interface{}) {
+			//drone.Stop()
+		})
+
+		stick.On(joystick.TrianglePress, func(data interface{}) {
+			drone.TakeOff()
+		})
+
+		stick.On(joystick.XPress, func(data interface{}) {
+			drone.Land()
+		})
+
+		stick.On(joystick.LeftX, func(data interface{}) {
+			val := float64(data.(int16))
+			leftX.Store(val)
+		})
+
+		stick.On(joystick.LeftY, func(data interface{}) {
+			val := float64(data.(int16))
+			leftY.Store(val)
+		})
+
+		stick.On(joystick.RightX, func(data interface{}) {
+			val := float64(data.(int16))
+			rightX.Store(val)
+		})
+
+		stick.On(joystick.RightY, func(data interface{}) {
+			val := float64(data.(int16))
+			rightY.Store(val)
+		})
+
+		gobot.Every(10*time.Millisecond, func() {
+			rightStick := getRightStick()
+
+			switch {
+			case rightStick.y < -10:
+				drone.Forward(tello.ValidatePitch(rightStick.y, offset))
+			case rightStick.y > 10:
+				drone.Backward(tello.ValidatePitch(rightStick.y, offset))
+			default:
+				drone.Forward(0)
+			}
+
+			switch {
+			case rightStick.x > 10:
+				drone.Right(tello.ValidatePitch(rightStick.x, offset))
+			case rightStick.x < -10:
+				drone.Left(tello.ValidatePitch(rightStick.x, offset))
+			default:
+				drone.Right(0)
+			}
+		})
+
+		gobot.Every(10*time.Millisecond, func() {
+			leftStick := getLeftStick()
+			switch {
+			case leftStick.y < -10:
+				drone.Up(tello.ValidatePitch(leftStick.y, offset))
+			case leftStick.y > 10:
+				drone.Down(tello.ValidatePitch(leftStick.y, offset))
+			default:
+				drone.Up(0)
+			}
+
+			switch {
+			case leftStick.x > 20:
+				drone.Clockwise(tello.ValidatePitch(leftStick.x, offset))
+			case leftStick.x < -20:
+				drone.CounterClockwise(tello.ValidatePitch(leftStick.x, offset))
+			default:
+				drone.Clockwise(0)
+			}
+		})
+	}
+
+	robot := gobot.NewRobot("tello",
+		[]gobot.Connection{joystickAdaptor},
+		[]gobot.Device{stick, drone},
+		work,
+	)
+
+	robot.Start()
+}
+
+func getLeftStick() pair {
+	s := pair{x: 0, y: 0}
+	s.x = leftX.Load().(float64)
+	s.y = leftY.Load().(float64)
+	return s
+}
+
+func getRightStick() pair {
+	s := pair{x: 0, y: 0}
+	s.x = rightX.Load().(float64)
+	s.y = rightY.Load().(float64)
+	return s
+}

platforms/dji/tello/README.md

@@ -52,4 +52,10 @@ func main() {
 
 ## References
 
-Thanks to https://github.com/microlinux/tello for serving as an example for the Tello community with his Python library.
+This driver could not exist without the awesome members of the unofficial Tello forum:
+
+https://tellopilots.com/forums/tello-development.8/
+
+Special thanks to [@Kragrathea](https://github.com/Kragrathea) who figured out a LOT of the packets and code as implemented in C#: [https://github.com/Kragrathea/TelloPC](https://github.com/Kragrathea/TelloPC)
+
+Also thanks to [@microlinux](https://github.com/microlinux) with the Python library which served as the first example for the Tello community: [https://github.com/microlinux/tello](https://github.com/microlinux/tello)

platforms/dji/tello/crc.go

@@ -0,0 +1,55 @@
+package tello
+
+const (
+	poly int = 13970
+)
+
+func fcstab() []int {
+	return []int{0, 4489, 8978, 12955, 17956, 22445, 25910, 29887, 35912, 40385, 44890, 48851, 51820, 56293, 59774, 63735, 4225, 264, 13203, 8730, 22181, 18220, 30135, 25662, 40137, 36160, 49115, 44626, 56045, 52068, 63999, 59510, 8450, 12427, 528, 5017, 26406, 30383, 17460, 21949, 44362, 48323, 36440, 40913, 60270, 64231, 51324, 55797, 12675, 8202, 4753, 792, 30631, 26158, 21685, 17724, 48587, 44098, 40665, 36688, 64495, 60006, 55549, 51572, 16900, 21389, 24854, 28831, 1056, 5545, 10034, 14011, 52812, 57285, 60766, 64727, 34920, 39393, 43898, 47859, 21125, 17164, 29079, 24606, 5281, 1320, 14259, 9786, 57037, 53060, 64991, 60502, 39145, 35168, 48123, 43634, 25350, 29327, 16404, 20893, 9506, 13483, 1584, 6073, 61262, 65223, 52316, 56789, 43370, 47331, 35448, 39921, 29575, 25102, 20629, 16668, 13731, 9258, 5809, 1848, 65487, 60998, 56541, 52564, 47595, 43106, 39673, 35696, 33800, 38273, 42778, 46739, 49708, 54181, 57662, 61623, 2112, 6601, 11090, 15067, 20068, 24557, 28022, 31999, 38025, 34048, 47003, 42514, 53933, 49956, 61887, 57398, 6337, 2376, 15315, 10842, 24293, 20332, 32247, 27774, 42250, 46211, 34328, 38801, 58158, 62119, 49212, 53685, 10562, 14539, 2640, 7129, 28518, 32495, 19572, 24061, 46475, 41986, 38553, 34576, 62383, 57894, 53437, 49460, 14787, 10314, 6865, 2904, 32743, 28270, 23797, 19836, 50700, 55173, 58654, 62615, 32808, 37281, 41786, 45747, 19012, 23501, 26966, 30943, 3168, 7657, 12146, 16123, 54925, 50948, 62879, 58390, 37033, 33056, 46011, 41522, 23237, 19276, 31191, 26718, 7393, 3432, 16371, 11898, 59150, 63111, 50204, 54677, 41258, 45219, 33336, 37809, 27462, 31439, 18516, 23005, 11618, 15595, 3696, 8185, 63375, 58886, 54429, 50452, 45483, 40994, 37561, 33584, 31687, 27214, 22741, 18780, 15843, 11370, 7921, 3960}
+}
+
+func fsc16(bytes []byte, length int, poly int) int {
+	fs := fcstab()
+	if len(bytes) == 0 {
+		return 65535
+	}
+	i := 0
+	j := poly
+	poly = length
+	length = j
+	for {
+		j = length
+		if poly == 0 {
+			break
+		}
+		j = int(bytes[i])
+		length = fs[((length^j)&0xFF)] ^ length>>8
+		i++
+		poly--
+	}
+	return j
+}
+
+// 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 {
+	fs := uCRCTable()
+
+	j := 0
+	i := poly
+	poly = j
+	for length != 0 {
+		j = int(bytes[poly]) ^ i
+		i = j
+		if j < 0 {
+			i = j + 256
+		}
+		i = fs[i]
+		poly++
+		length--
+	}
+	return i
+}