tello: able to fetch speed, battery, and flight time data from drone

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

examples/tello.go

@@ -24,11 +24,19 @@ func main() {
 	drone := tello.NewDriver(os.Args[1])
 
 	work := func() {
+		battery, _ := drone.Battery()
+		fmt.Println("battery:", battery)
+
+		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)
 		})
 	}
 

platforms/dji/tello/driver.go

@@ -4,6 +4,8 @@ import (
 	"errors"
 	"fmt"
 	"net"
+	"strconv"
+	"strings"
 	"time"
 
 	"gobot.io/x/gobot"
@@ -56,7 +58,10 @@ func (d *Driver) Start() error {
 
 	go func() {
 		for {
-			d.handleResponse()
+			err := d.handleResponse()
+			if err != nil {
+				return
+			}
 		}
 	}()
 
@@ -70,16 +75,17 @@ func (d *Driver) Start() error {
 	return nil
 }
 
-func (d *Driver) handleResponse() {
+func (d *Driver) handleResponse() error {
 	var buf [256]byte
 	n, err := d.reqConn.Read(buf[0:])
 	if err != nil {
-		fmt.Println("Error on response")
-		return
+		fmt.Println(err)
+		return err
 	}
 
 	resp := string(buf[0:n])
 	d.responses <- resp
+	return nil
 }
 
 // Halt stops the driver.
@@ -96,11 +102,32 @@ func (d *Driver) sendCommand(cmd string) error {
 
 	select {
 	case res := <-d.responses:
-		fmt.Println(res)
+		switch res {
+		case "OK":
+			return nil
+		case "FALSE":
+			return errors.New("Command returned false: " + cmd)
+		default:
+			fmt.Println("Unknown response:", res)
+			return nil
+		}
 	case <-time.After(5 * time.Second):
 		return errors.New("Command timeout: " + cmd)
 	}
-	return nil
+}
+
+func (d *Driver) sendFunction(cmd string) (string, error) {
+	_, err := d.reqConn.Write([]byte(cmd))
+	if err != nil {
+		return "", err
+	}
+
+	select {
+	case res := <-d.responses:
+		return strings.Replace(res, "\r\n", "", -1), nil
+	case <-time.After(5 * time.Second):
+		return "", errors.New("Command timeout: " + cmd)
+	}
 }
 
 // TakeOff tells drones to liftoff and start flying.
@@ -158,3 +185,61 @@ func (d *Driver) Clockwise(deg int) error {
 func (d *Driver) CounterClockwise(deg int) error {
 	return d.Move("ccw", deg)
 }
+
+// FrontFlip tells the drone to perform a front flip
+func (d *Driver) FrontFlip() (err error) {
+	return d.sendCommand("flip f")
+}
+
+// BackFlip tells the drone to perform a backflip
+func (d *Driver) BackFlip() (err error) {
+	return d.sendCommand("flip b")
+}
+
+// RightFlip tells the drone to perform a flip to the right
+func (d *Driver) RightFlip() (err error) {
+	return d.sendCommand("flip r")
+}
+
+// LeftFlip tells the drone to perform a flip to the left
+func (d *Driver) LeftFlip() (err error) {
+	return d.sendCommand("flip l")
+}
+
+// StartRecording is not yet supported.
+func (d *Driver) StartRecording() error {
+	return nil
+}
+
+// StopRecording is not yet supported.
+func (d *Driver) StopRecording() error {
+	return nil
+}
+
+// Battery returns the current battery level in the drone as a percentage.
+func (d *Driver) Battery() (int, error) {
+	res, err := d.sendFunction("battery?")
+	if err != nil {
+		return 0, err
+	}
+	return strconv.Atoi(res)
+}
+
+// FlightTime returns the current elapsed flight time for the drone in seconds.
+func (d *Driver) FlightTime() (string, error) {
+	return d.sendFunction("time?")
+}
+
+// Speed returns the current speed for the drone in cm per second.
+func (d *Driver) Speed() (float64, error) {
+	res, err := d.sendFunction("speed?")
+	if err != nil {
+		return 0, err
+	}
+	return strconv.ParseFloat(res, 32)
+}
+
+// SetSpeed sets the drone speed from 1-100 cm per second.
+func (d *Driver) SetSpeed(speed int) error {
+	return d.sendCommand(fmt.Sprintf("speed %d", speed))
+}