Merge pull request #442 from GuySirton/dev
Initial support for HOLY STONE HS200W drone
This commit is contained in:
Ron Evans
GitHub
commit
89ba30f2a9
|
|
@ -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.
|
||||
|
|
@ -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)
|
||||
}
|
||||
Loading…
Reference in New Issue