diff --git a/platforms/holystone/hs200/README.md b/platforms/holystone/hs200/README.md new file mode 100644 index 00000000..8c941b39 --- /dev/null +++ b/platforms/holystone/hs200/README.md @@ -0,0 +1,62 @@ +## 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" + + "fmt" + + "gobot.io/x/gobot/platforms/holystone/hs200" +) + +func main() { + drone, err := hs200.NewDriver("172.16.10.1:8888", "172.16.10.1:8080") + if err != nil { + log.Fatal(err) + } + fmt.Println("Enable!") + drone.Enable() + time.Sleep(5 * time.Second) + 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) +} +``` + +## 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. \ No newline at end of file diff --git a/platforms/holystone/hs200/hs200-driver.go b/platforms/holystone/hs200/hs200-driver.go new file mode 100644 index 00000000..76d3e905 --- /dev/null +++ b/platforms/holystone/hs200/hs200-driver.go @@ -0,0 +1,177 @@ +package hs200 + +import ( + "net" + "sync" + "time" +) + +// Driver reperesents the control information for the hs200 drone +type Driver struct { + 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 + } + uc, uerr := net.Dial("udp4", udpaddress) + if uerr != nil { + return nil, uerr + } + + command := []byte{ + 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{stopc: make(chan struct{}), cmd: command, udpconn: uc, tcpconn: tc}, nil +} + +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.sendUDP() + case <-tcpTick.C: + // Send TCP commands from here once we figure out what they do... + case <-stopc: + d.stop() + return + } + } +} + +func checksum(c []byte) byte { + var sum byte + for i := 1; i < 10; i++ { + sum += c[i] + } + return 255 - sum +} +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.stopc) + d.enabled = true + } +} + +func (d Driver) Disable() { + d.mutex.Lock() + defer d.mutex.Unlock() + if d.enabled { + d.stopc <- struct{}{} + } +} + +func (d Driver) TakeOff() { + d.mutex.Lock() + 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() +} + +// 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 cmd < -1.0 { + cmd = -1.0 + } + 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) +} + +// 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[5] = floatToCmdByte(speed, 0x3f, 0x3f) + d.cmd[10] = checksum(d.cmd) +}