From d2855bb43c714481bf24b6d94bad901076bf6c06 Mon Sep 17 00:00:00 2001 From: Guy Sirton Date: Sat, 15 Jul 2017 14:47:06 -0700 Subject: [PATCH 1/4] WIP: Take off and land --- platforms/holystone/hs200/hs200-driver.go | 92 +++++++++++++++++++++++ 1 file changed, 92 insertions(+) create mode 100644 platforms/holystone/hs200/hs200-driver.go diff --git a/platforms/holystone/hs200/hs200-driver.go b/platforms/holystone/hs200/hs200-driver.go new file mode 100644 index 00000000..056e228c --- /dev/null +++ b/platforms/holystone/hs200/hs200-driver.go @@ -0,0 +1,92 @@ +package hs200 + +import ( + "fmt" + "net" + "time" +) + +type Driver struct { + cmd []byte + udpconn net.Conn + tcpconn net.Conn +} + +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, //unknown header sent with apparently constant value + 0x04, //unknown header sent with apparently constant value + 0x7e, // 0x7f,//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) + } + command[10] = checksum(command) + + return &Driver{command, uc, tc}, nil +} + +func (d Driver) flightloop() { + i := 0 + for { + time.Sleep(20 * time.Millisecond) + if i%5 == 0 { + d.tcpconn.Write([]byte("remote\r\n")) + } + d.sendUDP() + i++ + } +} + +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.udpconn.Write(d.cmd) +} + +func (d Driver) Enable() { + go d.flightloop() +} + +func (d Driver) TakeOff() { + d.cmd[2] = 0x7e + d.cmd[10] = checksum(d.cmd) +} + +func (d Driver) VerticalControl(delta int) { + current := int(d.cmd[2]) + current += delta + if current > 255 { + current = 255 + } + if current < 0 { + current = 0 + } + fmt.Printf("Setting to %v", byte(current)) + d.cmd[2] = byte(delta) + d.cmd[10] = checksum(d.cmd) + +} + +func (d Driver) Land() { + d.cmd[2] = 0x3f + d.cmd[10] = checksum(d.cmd) +} From 0d25e83441d0f21114d990e181a6d5c4f4c40437 Mon Sep 17 00:00:00 2001 From: Guy Sirton Date: Sat, 15 Jul 2017 14:50:57 -0700 Subject: [PATCH 2/4] WIP: Small demo code --- platforms/holystone/hs200/demo.txt | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 platforms/holystone/hs200/demo.txt diff --git a/platforms/holystone/hs200/demo.txt b/platforms/holystone/hs200/demo.txt new file mode 100644 index 00000000..2ec20710 --- /dev/null +++ b/platforms/holystone/hs200/demo.txt @@ -0,0 +1,25 @@ +package main + +import ( + "fmt" + "gobot.io/x/gobot/platforms/holystone/hs200" + "log" + "time" +) + +func main() { + drone, err := hs200.NewDriver("172.16.10.1:8888", "172.16.10.1:8080") + if err != nil { + log.Fatal(err) + } + drone.Enable() + drone.TakeOff() + time.Sleep(5 * time.Second) + for lift := 256; lift >= 0; lift -= 16 { + fmt.Printf("Lift is %d\n", lift) + drone.VerticalControl(lift) + time.Sleep(250*time.Millisecond) + } + drone.Land() + time.Sleep(10 * time.Second) +} From dba28a12b53f121ab366425188104335464ebf1a Mon Sep 17 00:00:00 2001 From: Guy Sirton Date: Tue, 18 Jul 2017 22:03:05 -0700 Subject: [PATCH 3/4] WIP: Introduce a mutex, a stop channel, and use tickers for periodic updates --- platforms/holystone/hs200/README.md | 32 ++++++++++++ platforms/holystone/hs200/hs200-driver.go | 61 +++++++++++++++++------ 2 files changed, 78 insertions(+), 15 deletions(-) create mode 100644 platforms/holystone/hs200/README.md diff --git a/platforms/holystone/hs200/README.md b/platforms/holystone/hs200/README.md new file mode 100644 index 00000000..e8120a9a --- /dev/null +++ b/platforms/holystone/hs200/README.md @@ -0,0 +1,32 @@ +## How to Use + +```go +package main + +import ( + "log" + "time" + + "gobot.io/x/gobot/platforms/holystone/hs200" + "fmt" +) + +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("Land!") + drone.Land() + time.Sleep(5 * time.Second) + fmt.Println("Disable!") + drone.Disable() + time.Sleep(5*time.Second) +} +``` \ No newline at end of file diff --git a/platforms/holystone/hs200/hs200-driver.go b/platforms/holystone/hs200/hs200-driver.go index 056e228c..0bed5f6f 100644 --- a/platforms/holystone/hs200/hs200-driver.go +++ b/platforms/holystone/hs200/hs200-driver.go @@ -3,20 +3,24 @@ package hs200 import ( "fmt" "net" + "sync" "time" ) type Driver struct { + mutex sync.RWMutex + stop chan struct{} cmd []byte + enabled bool udpconn net.Conn tcpconn net.Conn } 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 @@ -24,7 +28,7 @@ func NewDriver(tcpaddress string, udpaddress string) (*Driver, error) { command := []byte{ 0xff, //unknown header sent with apparently constant value 0x04, //unknown header sent with apparently constant value - 0x7e, // 0x7f,//vertical lift up/down + 0x3f, //vertical lift up/down 0x3f, //rotation rate left/right 0xc0, //advance forward / backward 0x3f, //strafe left / right @@ -36,18 +40,28 @@ func NewDriver(tcpaddress string, udpaddress string) (*Driver, error) { } command[10] = checksum(command) - return &Driver{command, uc, tc}, nil + return &Driver{stop: make(chan struct{}), cmd: command, udpconn: uc, tcpconn: nil}, nil } -func (d Driver) flightloop() { - i := 0 +func (d *Driver) flightLoop(stop chan struct{}) { + udpTick := time.NewTicker(50 * time.Millisecond) + defer udpTick.Stop() + tcpTick := time.NewTicker(1000 * time.Millisecond) + defer tcpTick.Stop() for { - time.Sleep(20 * time.Millisecond) - if i%5 == 0 { - d.tcpconn.Write([]byte("remote\r\n")) + 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 + return } - d.sendUDP() - i++ } } @@ -63,10 +77,25 @@ func (d Driver) sendUDP() { } func (d Driver) Enable() { - go d.flightloop() + d.mutex.Lock() + defer d.mutex.Unlock() + if !d.enabled { + go d.flightLoop(d.stop) + d.enabled = true + } +} + +func (d Driver) Disable() { + d.mutex.Lock() + defer d.mutex.Unlock() + if d.enabled { + d.stop <- struct{}{} + } } func (d Driver) TakeOff() { + d.mutex.Lock() + defer d.mutex.Unlock() d.cmd[2] = 0x7e d.cmd[10] = checksum(d.cmd) } @@ -87,6 +116,8 @@ func (d Driver) VerticalControl(delta int) { } func (d Driver) Land() { - d.cmd[2] = 0x3f + d.mutex.Lock() + defer d.mutex.Unlock() + d.cmd[2] = 0 d.cmd[10] = checksum(d.cmd) } From 483e1fbeee5646ac78f0a92c947e210c2d7f758f Mon Sep 17 00:00:00 2001 From: Guy Sirton Date: Sun, 23 Jul 2017 18:22:45 -0700 Subject: [PATCH 4/4] Mostly working driver --- platforms/holystone/hs200/README.md | 36 ++++- platforms/holystone/hs200/demo.txt | 25 ---- platforms/holystone/hs200/hs200-driver.go | 156 +++++++++++++++------- 3 files changed, 138 insertions(+), 79 deletions(-) delete mode 100644 platforms/holystone/hs200/demo.txt diff --git a/platforms/holystone/hs200/README.md b/platforms/holystone/hs200/README.md index e8120a9a..8c941b39 100644 --- a/platforms/holystone/hs200/README.md +++ b/platforms/holystone/hs200/README.md @@ -1,5 +1,9 @@ ## 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 @@ -7,8 +11,9 @@ 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) } -``` \ No newline at end of file +``` + +## 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/demo.txt b/platforms/holystone/hs200/demo.txt deleted file mode 100644 index 2ec20710..00000000 --- a/platforms/holystone/hs200/demo.txt +++ /dev/null @@ -1,25 +0,0 @@ -package main - -import ( - "fmt" - "gobot.io/x/gobot/platforms/holystone/hs200" - "log" - "time" -) - -func main() { - drone, err := hs200.NewDriver("172.16.10.1:8888", "172.16.10.1:8080") - if err != nil { - log.Fatal(err) - } - drone.Enable() - drone.TakeOff() - time.Sleep(5 * time.Second) - for lift := 256; lift >= 0; lift -= 16 { - fmt.Printf("Lift is %d\n", lift) - drone.VerticalControl(lift) - time.Sleep(250*time.Millisecond) - } - drone.Land() - time.Sleep(10 * time.Second) -} diff --git a/platforms/holystone/hs200/hs200-driver.go b/platforms/holystone/hs200/hs200-driver.go index 0bed5f6f..76d3e905 100644 --- a/platforms/holystone/hs200/hs200-driver.go +++ b/platforms/holystone/hs200/hs200-driver.go @@ -1,49 +1,64 @@ 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) @@ -51,15 +66,11 @@ func (d *Driver) flightLoop(stop chan struct{}) { 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,7 +83,9 @@ 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) } @@ -80,7 +93,7 @@ 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) -} - -func (d Driver) VerticalControl(delta int) { - current := int(d.cmd[2]) - current += delta - if current > 255 { - current = 255 - } - if current < 0 { - current = 0 - } - fmt.Printf("Setting to %v", byte(current)) - d.cmd[2] = byte(delta) + 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] = 0 + 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) }