minidrone: adds Emergency() and TakePicture() commands

Signed-off-by: deadprogram <[email protected]>
KISSMonX · Dec 27, 2016 · e6de2a8 · e6de2a8
1 parent 244b849
commit e6de2a8
Show file tree

Hide file tree

Showing 3 changed files with 86 additions and 34 deletions.
diff --git a/examples/minidrone.go b/examples/minidrone.go
@@ -1,7 +1,6 @@
 package main
 
 import (
-	"fmt"
 	"os"
 	"time"
 
@@ -15,36 +14,11 @@ func main() {
 	drone := minidrone.NewDriver(bleAdaptor)
 
 	work := func() {
-		drone.On(minidrone.Battery, func(data interface{}) {
-			fmt.Printf("battery: %d\n", data)
-		})
-
-		drone.On(minidrone.FlightStatus, func(data interface{}) {
-			fmt.Printf("flight status: %d\n", data)
-		})
-
-		drone.On(minidrone.Takeoff, func(data interface{}) {
-			fmt.Println("taking off...")
-		})
-
-		drone.On(minidrone.Hovering, func(data interface{}) {
-			fmt.Println("hovering!")
-			gobot.After(5*time.Second, func() {
-				drone.Land()
-				drone.Land()
-			})
-		})
-
-		drone.On(minidrone.Landing, func(data interface{}) {
-			fmt.Println("landing...")
-		})
+		drone.TakeOff()
 
-		drone.On(minidrone.Landed, func(data interface{}) {
-			fmt.Println("landed.")
+		gobot.After(5*time.Second, func() {
+			drone.Land()
 		})
-
-		time.Sleep(1000 * time.Millisecond)
-		drone.TakeOff()
 	}
 
 	robot := gobot.NewRobot("minidrone",

diff --git a/examples/minidrone_events.go b/examples/minidrone_events.go
@@ -0,0 +1,57 @@
+package main
+
+import (
+	"fmt"
+	"os"
+	"time"
+
+	"gobot.io/x/gobot"
+	"gobot.io/x/gobot/platforms/ble"
+	"gobot.io/x/gobot/platforms/parrot/minidrone"
+)
+
+func main() {
+	bleAdaptor := ble.NewClientAdaptor(os.Args[1])
+	drone := minidrone.NewDriver(bleAdaptor)
+
+	work := func() {
+		drone.On(minidrone.Battery, func(data interface{}) {
+			fmt.Printf("battery: %d\n", data)
+		})
+
+		drone.On(minidrone.FlightStatus, func(data interface{}) {
+			fmt.Printf("flight status: %d\n", data)
+		})
+
+		drone.On(minidrone.Takeoff, func(data interface{}) {
+			fmt.Println("taking off...")
+		})
+
+		drone.On(minidrone.Hovering, func(data interface{}) {
+			fmt.Println("hovering!")
+			gobot.After(5*time.Second, func() {
+				drone.Land()
+				drone.Land()
+			})
+		})
+
+		drone.On(minidrone.Landing, func(data interface{}) {
+			fmt.Println("landing...")
+		})
+
+		drone.On(minidrone.Landed, func(data interface{}) {
+			fmt.Println("landed.")
+		})
+
+		time.Sleep(1000 * time.Millisecond)
+		drone.TakeOff()
+	}
+
+	robot := gobot.NewRobot("minidrone",
+		[]gobot.Connection{bleAdaptor},
+		[]gobot.Device{drone},
+		work,
+	)
+
+	robot.Start()
+}
diff --git a/platforms/parrot/minidrone/minidrone_driver.go b/platforms/parrot/minidrone/minidrone_driver.go
@@ -26,9 +26,12 @@ const (
 	droneCommandService      = "9a66fa000800919111e4012d1540cb8e"
 	droneNotificationService = "9a66fb000800919111e4012d1540cb8e"
 
-	// BLE characteristics
-	pcmdCharacteristic         = "9a66fa0a0800919111e4012d1540cb8e"
-	commandCharacteristic      = "9a66fa0b0800919111e4012d1540cb8e"
+	// send characteristics
+	pcmdCharacteristic     = "9a66fa0a0800919111e4012d1540cb8e"
+	commandCharacteristic  = "9a66fa0b0800919111e4012d1540cb8e"
+	priorityCharacteristic = "9a66fa0c0800919111e4012d1540cb8e"
+
+	// receive characteristics
 	flightStatusCharacteristic = "9a66fb0e0800919111e4012d1540cb8e"
 	batteryCharacteristic      = "9a66fb0f0800919111e4012d1540cb8e"
 
@@ -164,7 +167,7 @@ func (b *Driver) Init() (err error) {
 			return
 		}
 
-		b.Publish(b.Event(FlightStatus), data[4])
+		b.Publish(FlightStatus, data[4])
 
 		if data[4] == flatTrimChanged {
 			b.Publish(FlatTrimChange, true)
@@ -230,7 +233,7 @@ func (b *Driver) TakeOff() (err error) {
 // Land tells the Minidrone to land
 func (b *Driver) Land() (err error) {
 	b.stepsfa0b++
-	buf := []byte{0x02, byte(b.stepsfa0b), 0x02, 0x00, 0x03, 0x00}
+	buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x00, 0x03, 0x00}
 	err = b.adaptor().WriteCharacteristic(droneCommandService, commandCharacteristic, buf)
 
 	return err
@@ -245,6 +248,24 @@ func (b *Driver) FlatTrim() (err error) {
 	return err
 }
 
+// Emergency sets the Minidrone into emergency mode
+func (b *Driver) Emergency() (err error) {
+	b.stepsfa0b++
+	buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x00, 0x04, 0x00}
+	err = b.adaptor().WriteCharacteristic(droneCommandService, priorityCharacteristic, buf)
+
+	return err
+}
+
+// TakePicture tells the Minidrone to take a picture
+func (b *Driver) TakePicture() (err error) {
+	b.stepsfa0b++
+	buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x06, 0x01, 0x00}
+	err = b.adaptor().WriteCharacteristic(droneCommandService, commandCharacteristic, buf)
+
+	return err
+}
+
 // StartPcmd starts the continuous Pcmd communication with the Minidrone
 func (b *Driver) StartPcmd() {
 	go func() {