minidrone_driver.go

package minidrone

import (
	"bytes"
	"encoding/binary"
	"fmt"
	"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
	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"
)

// 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.ClientAdaptor) *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.ClientAdaptor {
	return b.Connection().(*ble.ClientAdaptor)
}

// Start tells driver to get ready to do work
func (b *Driver) Start() (err error) {
	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) {
		if len(data) < 5 {
			// ignore, just a sync
			return
		}

		b.Publish(FlightStatus, data[4])

		if data[4] == flatTrimChanged {
			b.Publish(FlatTrimChange, true)
		}
		if data[4] == 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)
			}
		}
	})

	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)
	if err != nil {
		fmt.Println("GenerateAllStates error:", err)
		return err
	}

	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)
	if err != nil {
		fmt.Println("takeoff error:", err)
		return err
	}

	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
func (b *Driver) Up(val int) error {
	b.Pcmd.Flag = 1
	b.Pcmd.Gaz = validatePitch(val)
	return nil
}

// Down tells the drone to descend
func (b *Driver) Down(val int) error {
	b.Pcmd.Flag = 1
	b.Pcmd.Gaz = validatePitch(val) * -1
	return nil
}

// Forward tells the drone to go forward
func (b *Driver) Forward(val int) error {
	b.Pcmd.Flag = 1
	b.Pcmd.Pitch = validatePitch(val)
	return nil
}

// Backward tells drone to go in reverse
func (b *Driver) Backward(val int) error {
	b.Pcmd.Flag = 1
	b.Pcmd.Pitch = validatePitch(val) * -1
	return nil
}

// Right tells drone to go right
func (b *Driver) Right(val int) error {
	b.Pcmd.Flag = 1
	b.Pcmd.Roll = validatePitch(val)
	return nil
}

// Left tells drone to go left
func (b *Driver) Left(val int) error {
	b.Pcmd.Flag = 1
	b.Pcmd.Roll = validatePitch(val) * -1
	return nil
}

// Clockwise tells drone to rotate in a clockwise direction
func (b *Driver) Clockwise(val int) error {
	b.Pcmd.Flag = 1
	b.Pcmd.Yaw = validatePitch(val)
	return nil
}

// CounterClockwise tells drone to rotate in a counter-clockwise direction
func (b *Driver) CounterClockwise(val int) error {
	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.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 mdoe 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())
}

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.stepsfa0a++

	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(b.Pcmd.Flag))
	binary.Write(cmd, binary.LittleEndian, int8(b.Pcmd.Roll))
	binary.Write(cmd, binary.LittleEndian, int8(b.Pcmd.Pitch))
	binary.Write(cmd, binary.LittleEndian, int8(b.Pcmd.Yaw))
	binary.Write(cmd, binary.LittleEndian, int8(b.Pcmd.Gaz))
	binary.Write(cmd, binary.LittleEndian, float32(b.Pcmd.Psi))
	binary.Write(cmd, binary.LittleEndian, int16(0))
	binary.Write(cmd, binary.LittleEndian, int16(0))

	return cmd
}

func validatePitch(val int) int {
	if val > 100 {
		return 100
	} else if val < 0 {
		return 0
	}

	return val
}