Mostly working driver

platforms/holystone/hs200/README.md

@@ -1,14 +1,19 @@
 ## How to Use
+- Connect to the drone's Wi-Fi network and identify the drone/gateway IP address.
+- Use that IP address when you create a new driver.
+- Some drones appear to use a different TCP port (8080 vs. 8888?).  If the example doesn't work scan the drone for open ports or modify the driver not to use TCP.
 
+Here is a sample of how you initialize and use the driver:
 ```go
 package main
 
 import (
 	"log"
 	"time"
 
-	"gobot.io/x/gobot/platforms/holystone/hs200"
 	"fmt"
+
+	"gobot.io/x/gobot/platforms/holystone/hs200"
 )
 
 func main() {
@@ -22,11 +27,36 @@ func main() {
 	fmt.Println("Take off!")
 	drone.TakeOff()
 	time.Sleep(5 * time.Second)
+	fmt.Println("Full steam ahead!")
+	drone.Forward(1.0)
+	time.Sleep(3 * time.Second)
+	fmt.Println("Full steam back!")
+	drone.Forward(-1.0)
+	time.Sleep(3 * time.Second)
+	fmt.Println("Hover!")
+	drone.Forward(0)
+	time.Sleep(3 * time.Second)
 	fmt.Println("Land!")
 	drone.Land()
 	time.Sleep(5 * time.Second)
 	fmt.Println("Disable!")
 	drone.Disable()
-	time.Sleep(5*time.Second)
+	time.Sleep(5 * time.Second)
 }
-```
+```
+
+## References
+https://hackaday.io/project/19356/logs
+
+https://github.com/lancecaraccioli/holystone-hs110w
+
+## Random notes
+- The hs200 sends out an RTSP video feed from its own board camera.  Not clear how this is turned on.  The data is apparently streamed over UDP. (Reference mentions rtsp://192.168.0.1/0 in VLC, I didn't try it!)
+- The Android control app seems to be sending out the following TCP bytes for an unknown purpose:
+`00 01 02 03 04 05 06 07 08 09 25 25` but the drone flies without a TCP connection.
+- The drone apparently always replies "noact\r\n" over TCP.
+- The app occasionally sends out 29 bytes long UDP packets besides the 11 byte control packet for an unknown purpose:
+`26 e1 07 00 00 07 00 00 00 10 00 00 00 00 00 00 00 14 00 00 00 0e 00 00 00 03 00 00 00`
+- The doesn't seem to be any telemetry coming out of the drone besides the video feed.
+- The drone can sometimes be a little flaky.  Ensure you've got a fully charged battery, minimal Wi-Fi interference, various connectors on the drone all well seated.
+- It's not clear whether the drone's remote uses Wi-Fi or not, possibly Wi-Fi is only for the mobile app.

platforms/holystone/hs200/hs200-driver.go

@@ -1,65 +1,76 @@
 package hs200
 
 import (
-	"fmt"
 	"net"
 	"sync"
 	"time"
 )
 
+// Driver reperesents the control information for the hs200 drone
 type Driver struct {
-	mutex   sync.RWMutex
-	stop    chan struct{}
-	cmd     []byte
-	enabled bool
-	udpconn net.Conn
-	tcpconn net.Conn
+	mutex   sync.RWMutex  // Protect the command from concurrent access
+	stopc   chan struct{} // Stop the flight loop goroutine
+	cmd     []byte        // the UDP command packet we keep sending the drone
+	enabled bool          // Are we in an enabled state
+	udpconn net.Conn      // UDP connection to the drone
+	tcpconn net.Conn      // TCP connection to the drone
 }
 
+// NewDriver creates a driver for the HolyStone hs200
 func NewDriver(tcpaddress string, udpaddress string) (*Driver, error) {
-//	tc, terr := net.Dial("tcp", tcpaddress)
-//	if terr != nil {
-//		return nil, terr
-//	}
+	tc, terr := net.Dial("tcp", tcpaddress)
+	if terr != nil {
+		return nil, terr
+	}
 	uc, uerr := net.Dial("udp4", udpaddress)
 	if uerr != nil {
 		return nil, uerr
 	}
+
 	command := []byte{
-		0xff, //unknown header sent with apparently constant value
-		0x04, //unknown header sent with apparently constant value
-		0x3f, //vertical lift up/down
-		0x3f, //rotation rate left/right
-		0xc0, //advance forward / backward
-		0x3f, //strafe left / right
-		0x90, //yaw (used as a setting to trim the yaw of the uav)
-		0x10, //pitch (used as a setting to trim the pitch of the uav)
-		0x10, //roll (used as a setting to trim the roll of the uav)
-		0x40, //throttle
-		0x00, //this is a sanity check; 255 - ((sum of flight controls from index 1 to 9) % 256)
+		0xff, // 2 byte header
+		0x04,
+
+		// Left joystick
+		0x7e, // throttle 0x00 - 0xff(?)
+		0x3f, // rotate left/right
+
+		// Right joystick
+		0xc0, // forward / backward 0x80 - 0xfe(?)
+		0x3f, // left / right 0x00 - 0x7e(?)
+
+		// Trim
+		0x90, // ? yaw (used as a setting to trim the yaw of the uav)
+		0x10, // ? pitch (used as a setting to trim the pitch of the uav)
+		0x10, // ? roll (used as a setting to trim the roll of the uav)
+
+		0x00, // flags/buttons
+		0x00, // checksum; 255 - ((sum of flight controls from index 1 to 9) % 256)
 	}
 	command[10] = checksum(command)
 
-	return &Driver{stop: make(chan struct{}), cmd: command, udpconn: uc, tcpconn: nil}, nil
+	return &Driver{stopc: make(chan struct{}), cmd: command, udpconn: uc, tcpconn: tc}, nil
 }
 
-func (d *Driver) flightLoop(stop chan struct{}) {
+func (d *Driver) stop() {
+	d.mutex.Lock()
+	defer d.mutex.Unlock()
+	d.enabled = false
+}
+
+func (d *Driver) flightLoop(stopc chan struct{}) {
 	udpTick := time.NewTicker(50 * time.Millisecond)
 	defer udpTick.Stop()
 	tcpTick := time.NewTicker(1000 * time.Millisecond)
 	defer tcpTick.Stop()
 	for {
 		select {
 		case <-udpTick.C:
-			d.mutex.RLock()
-			defer d.mutex.RUnlock()
 			d.sendUDP()
 		case <-tcpTick.C:
-			//d.tcpconn.Write([]byte("1\r\n"))
-		case <-stop:
-			d.mutex.Lock()
-			defer d.mutex.Unlock()
-			d.enabled = false
+			// Send TCP commands from here once we figure out what they do...
+		case <-stopc:
+			d.stop()
 			return
 		}
 	}
@@ -72,15 +83,17 @@ func checksum(c []byte) byte {
 	}
 	return 255 - sum
 }
-func (d Driver) sendUDP() {
+func (d *Driver) sendUDP() {
+	d.mutex.RLock()
+	defer d.mutex.RUnlock()
 	d.udpconn.Write(d.cmd)
 }
 
 func (d Driver) Enable() {
 	d.mutex.Lock()
 	defer d.mutex.Unlock()
 	if !d.enabled {
-		go d.flightLoop(d.stop)
+		go d.flightLoop(d.stopc)
 		d.enabled = true
 	}
 }
@@ -89,35 +102,76 @@ func (d Driver) Disable() {
 	d.mutex.Lock()
 	defer d.mutex.Unlock()
 	if d.enabled {
-		d.stop <- struct{}{}
+		d.stopc <- struct{}{}
 	}
 }
 
 func (d Driver) TakeOff() {
 	d.mutex.Lock()
-	defer d.mutex.Unlock()
-	d.cmd[2] = 0x7e
+	d.cmd[9] = 0x40
+	d.cmd[10] = checksum(d.cmd)
+	d.mutex.Unlock()
+	time.Sleep(500 * time.Millisecond)
+	d.mutex.Lock()
+	d.cmd[9] = 0x04
+	d.cmd[10] = checksum(d.cmd)
+	d.mutex.Unlock()
+}
+
+func (d Driver) Land() {
+	d.mutex.Lock()
+	d.cmd[9] = 0x80
+	d.cmd[10] = checksum(d.cmd)
+	d.mutex.Unlock()
+	time.Sleep(500 * time.Millisecond)
+	d.mutex.Lock()
+	d.cmd[9] = 0x04
 	d.cmd[10] = checksum(d.cmd)
+	d.mutex.Unlock()
 }
 
-func (d Driver) VerticalControl(delta int) {
-	current := int(d.cmd[2])
-	current += delta
-	if current > 255 {
-		current = 255
+// floatToCmdByte converts a float in the range of -1 to +1 to an integer command
+func floatToCmdByte(cmd float32, mid byte, maxv byte) byte {
+	if cmd > 1.0 {
+		cmd = 1.0
 	}
-	if current < 0 {
-		current = 0
+	if cmd < -1.0 {
+		cmd = -1.0
 	}
-	fmt.Printf("Setting to %v", byte(current))
-	d.cmd[2] = byte(delta)
+	cmd = cmd * float32(maxv)
+	bval := byte(cmd + float32(mid) + 0.5)
+	return bval
+}
+
+// Throttle sends the drone up from a hover (or down if speed is negative)
+func (d *Driver) Throttle(speed float32) {
+	d.mutex.Lock()
+	defer d.mutex.Unlock()
+	d.cmd[2] = floatToCmdByte(speed, 0x7e, 0x7e)
 	d.cmd[10] = checksum(d.cmd)
+}
 
+// Rotate rotates the drone (yaw)
+func (d *Driver) Rotate(speed float32) {
+	d.mutex.Lock()
+	defer d.mutex.Unlock()
+	d.cmd[3] = floatToCmdByte(speed, 0x3f, 0x3f)
+	d.cmd[10] = checksum(d.cmd)
 }
 
-func (d Driver) Land() {
+// Forward sends the drone forward (or backwards if speed is negative, pitch the drone)
+func (d *Driver) Forward(speed float32) {
+	speed = -speed
+	d.mutex.Lock()
+	defer d.mutex.Unlock()
+	d.cmd[4] = floatToCmdByte(speed, 0xc0, 0x3f)
+	d.cmd[10] = checksum(d.cmd)
+}
+
+// Right moves the drone to the right (or left if speed is negative, rolls the drone)
+func (d *Driver) Right(speed float32) {
 	d.mutex.Lock()
 	defer d.mutex.Unlock()
-	d.cmd[2] = 0
+	d.cmd[5] = floatToCmdByte(speed, 0x3f, 0x3f)
 	d.cmd[10] = checksum(d.cmd)
 }