Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
515 lines (421 sloc) 12.2 KB
package minidrone
import (
"bytes"
"encoding/binary"
"fmt"
"sync"
"time"
"gobot.io/x/gobot"
"gobot.io/x/gobot/platforms/ble"
)
// Driver is the Gobot interface to the Parrot Minidrone
type Driver struct {
name string
connection gobot.Connection
stepsfa0a uint16
stepsfa0b uint16
pcmdMutex sync.Mutex
flying bool
Pcmd Pcmd
gobot.Eventer
}
const (
// BLE services
droneCommandService = "9a66fa000800919111e4012d1540cb8e"
droneNotificationService = "9a66fb000800919111e4012d1540cb8e"
// send characteristics
pcmdCharacteristic = "9a66fa0a0800919111e4012d1540cb8e"
commandCharacteristic = "9a66fa0b0800919111e4012d1540cb8e"
priorityCharacteristic = "9a66fa0c0800919111e4012d1540cb8e"
// receive characteristics
flightStatusCharacteristic = "9a66fb0e0800919111e4012d1540cb8e"
batteryCharacteristic = "9a66fb0f0800919111e4012d1540cb8e"
// piloting states
flatTrimChanged = 0
flyingStateChanged = 1
// flying states
flyingStateLanded = 0
flyingStateTakeoff = 1
flyingStateHovering = 2
flyingStateFlying = 3
flyingStateLanding = 4
flyingStateEmergency = 5
flyingStateRolling = 6
// Battery event
Battery = "battery"
// FlightStatus event
FlightStatus = "flightstatus"
// Takeoff event
Takeoff = "takeoff"
// Hovering event
Hovering = "hovering"
// Flying event
Flying = "flying"
// Landing event
Landing = "landing"
// Landed event
Landed = "landed"
// Emergency event
Emergency = "emergency"
// Rolling event
Rolling = "rolling"
// FlatTrimChange event
FlatTrimChange = "flattrimchange"
// LightFixed mode for LightControl
LightFixed = 0
// LightBlinked mode for LightControl
LightBlinked = 1
// LightOscillated mode for LightControl
LightOscillated = 3
// ClawOpen mode for ClawControl
ClawOpen = 0
// ClawClosed mode for ClawControl
ClawClosed = 1
)
// Pcmd is the Parrot Command structure for flight control
type Pcmd struct {
Flag int
Roll int
Pitch int
Yaw int
Gaz int
Psi float32
}
// NewDriver creates a Parrot Minidrone Driver
func NewDriver(a ble.BLEConnector) *Driver {
n := &Driver{
name: gobot.DefaultName("Minidrone"),
connection: a,
Pcmd: Pcmd{
Flag: 0,
Roll: 0,
Pitch: 0,
Yaw: 0,
Gaz: 0,
Psi: 0,
},
Eventer: gobot.NewEventer(),
}
n.AddEvent(Battery)
n.AddEvent(FlightStatus)
n.AddEvent(Takeoff)
n.AddEvent(Flying)
n.AddEvent(Hovering)
n.AddEvent(Landing)
n.AddEvent(Landed)
n.AddEvent(Emergency)
n.AddEvent(Rolling)
return n
}
// Connection returns the BLE connection
func (b *Driver) Connection() gobot.Connection { return b.connection }
// Name returns the Driver Name
func (b *Driver) Name() string { return b.name }
// SetName sets the Driver Name
func (b *Driver) SetName(n string) { b.name = n }
// adaptor returns BLE adaptor
func (b *Driver) adaptor() ble.BLEConnector {
return b.Connection().(ble.BLEConnector)
}
// Start tells driver to get ready to do work
func (b *Driver) Start() (err error) {
b.adaptor().WithoutResponses(true)
b.Init()
b.FlatTrim()
b.StartPcmd()
b.FlatTrim()
return
}
// Halt stops minidrone driver (void)
func (b *Driver) Halt() (err error) {
b.Land()
time.Sleep(500 * time.Millisecond)
return
}
// Init initializes the BLE insterfaces used by the Minidrone
func (b *Driver) Init() (err error) {
b.GenerateAllStates()
// subscribe to battery notifications
b.adaptor().Subscribe(batteryCharacteristic, func(data []byte, e error) {
b.Publish(b.Event(Battery), data[len(data)-1])
})
// subscribe to flying status notifications
b.adaptor().Subscribe(flightStatusCharacteristic, func(data []byte, e error) {
b.processFlightStatus(data)
})
return
}
// GenerateAllStates sets up all the default states aka settings on the drone
func (b *Driver) GenerateAllStates() (err error) {
b.stepsfa0b++
buf := []byte{0x04, byte(b.stepsfa0b), 0x00, 0x04, 0x01, 0x00, 0x32, 0x30, 0x31, 0x34, 0x2D, 0x31, 0x30, 0x2D, 0x32, 0x38, 0x00}
err = b.adaptor().WriteCharacteristic(commandCharacteristic, buf)
return
}
// TakeOff tells the Minidrone to takeoff
func (b *Driver) TakeOff() (err error) {
b.stepsfa0b++
buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x00, 0x01, 0x00}
err = b.adaptor().WriteCharacteristic(commandCharacteristic, buf)
return
}
// Land tells the Minidrone to land
func (b *Driver) Land() (err error) {
b.stepsfa0b++
buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x00, 0x03, 0x00}
err = b.adaptor().WriteCharacteristic(commandCharacteristic, buf)
return err
}
// FlatTrim calibrates the Minidrone to use its current position as being level
func (b *Driver) FlatTrim() (err error) {
b.stepsfa0b++
buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x00, 0x00, 0x00}
err = b.adaptor().WriteCharacteristic(commandCharacteristic, buf)
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(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(commandCharacteristic, buf)
return err
}
// StartPcmd starts the continuous Pcmd communication with the Minidrone
func (b *Driver) StartPcmd() {
go func() {
// wait a little bit so that there is enough time to get some ACKs
time.Sleep(500 * time.Millisecond)
for {
err := b.adaptor().WriteCharacteristic(pcmdCharacteristic, b.generatePcmd().Bytes())
if err != nil {
fmt.Println("pcmd write error:", err)
}
time.Sleep(50 * time.Millisecond)
}
}()
}
// Up tells the drone to ascend. Pass in an int from 0-100.
func (b *Driver) Up(val int) error {
b.pcmdMutex.Lock()
defer b.pcmdMutex.Unlock()
b.Pcmd.Flag = 1
b.Pcmd.Gaz = validatePitch(val)
return nil
}
// Down tells the drone to descend. Pass in an int from 0-100.
func (b *Driver) Down(val int) error {
b.pcmdMutex.Lock()
defer b.pcmdMutex.Unlock()
b.Pcmd.Flag = 1
b.Pcmd.Gaz = validatePitch(val) * -1
return nil
}
// Forward tells the drone to go forward. Pass in an int from 0-100.
func (b *Driver) Forward(val int) error {
b.pcmdMutex.Lock()
defer b.pcmdMutex.Unlock()
b.Pcmd.Flag = 1
b.Pcmd.Pitch = validatePitch(val)
return nil
}
// Backward tells drone to go in reverse. Pass in an int from 0-100.
func (b *Driver) Backward(val int) error {
b.pcmdMutex.Lock()
defer b.pcmdMutex.Unlock()
b.Pcmd.Flag = 1
b.Pcmd.Pitch = validatePitch(val) * -1
return nil
}
// Right tells drone to go right. Pass in an int from 0-100.
func (b *Driver) Right(val int) error {
b.pcmdMutex.Lock()
defer b.pcmdMutex.Unlock()
b.Pcmd.Flag = 1
b.Pcmd.Roll = validatePitch(val)
return nil
}
// Left tells drone to go left. Pass in an int from 0-100.
func (b *Driver) Left(val int) error {
b.pcmdMutex.Lock()
defer b.pcmdMutex.Unlock()
b.Pcmd.Flag = 1
b.Pcmd.Roll = validatePitch(val) * -1
return nil
}
// Clockwise tells drone to rotate in a clockwise direction. Pass in an int from 0-100.
func (b *Driver) Clockwise(val int) error {
b.pcmdMutex.Lock()
defer b.pcmdMutex.Unlock()
b.Pcmd.Flag = 1
b.Pcmd.Yaw = validatePitch(val)
return nil
}
// CounterClockwise tells drone to rotate in a counter-clockwise direction.
// Pass in an int from 0-100.
func (b *Driver) CounterClockwise(val int) error {
b.pcmdMutex.Lock()
defer b.pcmdMutex.Unlock()
b.Pcmd.Flag = 1
b.Pcmd.Yaw = validatePitch(val) * -1
return nil
}
// Stop tells the drone to stop moving in any direction and simply hover in place
func (b *Driver) Stop() error {
b.pcmdMutex.Lock()
defer b.pcmdMutex.Unlock()
b.Pcmd = Pcmd{
Flag: 0,
Roll: 0,
Pitch: 0,
Yaw: 0,
Gaz: 0,
Psi: 0,
}
return nil
}
// StartRecording is not supported by the Parrot Minidrone
func (b *Driver) StartRecording() error {
return nil
}
// StopRecording is not supported by the Parrot Minidrone
func (b *Driver) StopRecording() error {
return nil
}
// HullProtection is not supported by the Parrot Minidrone
func (b *Driver) HullProtection(protect bool) error {
return nil
}
// Outdoor mode is not supported by the Parrot Minidrone
func (b *Driver) Outdoor(outdoor bool) error {
return nil
}
// FrontFlip tells the drone to perform a front flip
func (b *Driver) FrontFlip() (err error) {
return b.adaptor().WriteCharacteristic(commandCharacteristic, b.generateAnimation(0).Bytes())
}
// BackFlip tells the drone to perform a backflip
func (b *Driver) BackFlip() (err error) {
return b.adaptor().WriteCharacteristic(commandCharacteristic, b.generateAnimation(1).Bytes())
}
// RightFlip tells the drone to perform a flip to the right
func (b *Driver) RightFlip() (err error) {
return b.adaptor().WriteCharacteristic(commandCharacteristic, b.generateAnimation(2).Bytes())
}
// LeftFlip tells the drone to perform a flip to the left
func (b *Driver) LeftFlip() (err error) {
return b.adaptor().WriteCharacteristic(commandCharacteristic, b.generateAnimation(3).Bytes())
}
// LightControl controls lights on those Minidrone models which
// have the correct hardware, such as the Maclane, Blaze, & Swat.
// Params:
// id - always 0
// mode - either LightFixed, LightBlinked, or LightOscillated
// intensity - Light intensity from 0 (OFF) to 100 (Max intensity).
// Only used in LightFixed mode.
//
func (b *Driver) LightControl(id uint8, mode uint8, intensity uint8) (err error) {
b.stepsfa0b++
buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x10, 0x00, id, mode, intensity, 0x00}
err = b.adaptor().WriteCharacteristic(commandCharacteristic, buf)
return
}
// ClawControl controls the claw on the Parrot Mambo
// Params:
// id - always 0
// mode - either ClawOpen or ClawClosed
//
func (b *Driver) ClawControl(id uint8, mode uint8) (err error) {
b.stepsfa0b++
buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x10, 0x01, id, mode, 0x00}
err = b.adaptor().WriteCharacteristic(commandCharacteristic, buf)
return
}
// GunControl fires the gun on the Parrot Mambo
// Params:
// id - always 0
//
func (b *Driver) GunControl(id uint8) (err error) {
b.stepsfa0b++
buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x10, 0x02, id, 0x00}
err = b.adaptor().WriteCharacteristic(commandCharacteristic, buf)
return
}
func (b *Driver) generateAnimation(direction int8) *bytes.Buffer {
b.stepsfa0b++
buf := []byte{0x02, byte(b.stepsfa0b) & 0xff, 0x02, 0x04, 0x00, 0x00, byte(direction), 0x00, 0x00, 0x00}
return bytes.NewBuffer(buf)
}
func (b *Driver) generatePcmd() *bytes.Buffer {
b.pcmdMutex.Lock()
defer b.pcmdMutex.Unlock()
b.stepsfa0a++
pcmd := b.Pcmd
cmd := &bytes.Buffer{}
binary.Write(cmd, binary.LittleEndian, int8(2))
binary.Write(cmd, binary.LittleEndian, int8(b.stepsfa0a))
binary.Write(cmd, binary.LittleEndian, int8(2))
binary.Write(cmd, binary.LittleEndian, int8(0))
binary.Write(cmd, binary.LittleEndian, int8(2))
binary.Write(cmd, binary.LittleEndian, int8(0))
binary.Write(cmd, binary.LittleEndian, int8(pcmd.Flag))
binary.Write(cmd, binary.LittleEndian, int8(pcmd.Roll))
binary.Write(cmd, binary.LittleEndian, int8(pcmd.Pitch))
binary.Write(cmd, binary.LittleEndian, int8(pcmd.Yaw))
binary.Write(cmd, binary.LittleEndian, int8(pcmd.Gaz))
binary.Write(cmd, binary.LittleEndian, float32(pcmd.Psi))
binary.Write(cmd, binary.LittleEndian, int16(0))
binary.Write(cmd, binary.LittleEndian, int16(0))
return cmd
}
func (b *Driver) processFlightStatus(data []byte) {
if len(data) < 5 {
// ignore, just a sync
return
}
b.Publish(FlightStatus, data[4])
switch data[4] {
case flatTrimChanged:
b.Publish(FlatTrimChange, true)
case flyingStateChanged:
switch data[6] {
case flyingStateLanded:
if b.flying {
b.flying = false
b.Publish(Landed, true)
}
case flyingStateTakeoff:
b.Publish(Takeoff, true)
case flyingStateHovering:
if !b.flying {
b.flying = true
b.Publish(Hovering, true)
}
case flyingStateFlying:
if !b.flying {
b.flying = true
b.Publish(Flying, true)
}
case flyingStateLanding:
b.Publish(Landing, true)
case flyingStateEmergency:
b.Publish(Emergency, true)
case flyingStateRolling:
b.Publish(Rolling, true)
}
}
}
func validatePitch(val int) int {
if val > 100 {
return 100
} else if val < 0 {
return 0
}
return val
}
You can’t perform that action at this time.