// autopilot_test.go
// Copyright (C) 2018 Steve Merrony and then FlandersNed
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
package tello
import (
"log"
"testing"
"time"
"os/exec"
)
var (
drone Tello
)
func TestLaava(t *testing.T) {
err := drone.ControlConnectDefault()
if err != nil {
log.Fatalf("CCD failed with error %v", err)
}
log.Println("Connected to Tello control channel")
time.Sleep(2 * time.Second)
startVideo()
time.Sleep(250 * time.Millisecond)
startDisplay()
time.Sleep(3 * time.Second)
log.Println("Taking off...")
drone.TakeOff()
time.Sleep(6 * time.Second)
log.Println("Flying to height 1.2 Metres")
done, err := drone.AutoFlyToHeight(12)
if err != nil { // should go up to 1.25m
t.Errorf("Error %v calling AutoFlyToHeight(12)", err)
}
<-done
log.Println("Done to fly height")
time.Sleep(2 * time.Second)
log.Println("Set Home")
err = drone.SetHome()
if err != nil {
t.Errorf("Error %v calling SetOrigin()", err)
}
currentYaw := drone.fd.IMU.Yaw
currentX := drone.fd.MVO.PositionX
currentY := drone.fd.MVO.PositionY
log.Println(currentYaw, currentX, currentY)
time.Sleep(1 * time.Second)
log.Println("Commence coordinate flying.")
time.Sleep(1 * time.Second)
done, err = drone.AutoFlyToXY(0, 0.5)
if err != nil { // should fly forward 50cm
t.Errorf("Error %v calling AutoFlyTo(0.0, 0.50)", err)
}
<-done
time.Sleep(3 * time.Second)
currentYaw1 := drone.fd.IMU.Yaw
currentX1 := drone.fd.MVO.PositionX
currentY1 := drone.fd.MVO.PositionY
log.Println(currentYaw1, currentX1, currentY1)
log.Println("Navigation 1 completion notified")
log.Println("Turning by 90 degrees counterclockwise")
doneYB, err := drone.AutoTurnByDeg(-90)
if err != nil { // should rotate counterclockwise 90 deg
t.Errorf("Error %v calling AutoTurnByDeg(-90)", err)
}
<-doneYB
log.Println("Done to turn counterclockwise")
time.Sleep(1 * time.Second)
done, err = drone.AutoFlyToXY(0.5, 0.5)
if err != nil { // should fly to next location
t.Errorf("Error %v calling AutoFlyTo(0.5, 0.5)", err)
}
<-done
time.Sleep(3 * time.Second)
currentYaw2 := drone.fd.IMU.Yaw
currentX2 := drone.fd.MVO.PositionX
currentY2 := drone.fd.MVO.PositionY
log.Println(currentYaw2, currentX2, currentY2)
log.Println("Navigation 2 completion notified")
done, err = drone.AutoFlyToXY(0, 0)
if err != nil { // should fly to home
t.Errorf("Error %v calling AutoFlyTo(0.0, 0.0)", err)
}
<-done
time.Sleep(3 * time.Second)
log.Println("Navigation completion notified")
currentYaw3 := drone.fd.IMU.Yaw
currentX3 := drone.fd.MVO.PositionX
currentY3 := drone.fd.MVO.PositionY
log.Println(currentYaw3, currentX3, currentY3)
log.Println("Turning by 90 degrees clockwise")
doneBB, err := drone.AutoTurnByDeg(90)
if err != nil { // should rotate counterclockwise 40 deg
t.Errorf("Error %v calling AutoTurnByDeg(90)", err)
}
<-doneBB
log.Println("Done to turn counterclockwise")
time.Sleep(1 * time.Second)
log.Println("Landing...")
drone.Land()
time.Sleep(3 * time.Second)
drone.ControlDisconnect()
log.Println("Disconnected normally from Tello")
}
func startVideo() {
videochan, err := drone.VideoConnectDefault()
if err != nil {
log.Fatalf("Tello VideoConnectDefault() failed with error %v", err)
}
//log.Println(videochan)
// start external mplayer instance...
// the -vo X11 parm allows it to run nicely inside a virtual machine
// setting the FPS to 60 seems to produce smoother video
var record *exec.Cmd
record = exec.Command("mplayer", "-", "-nosound", "-fps", "60", "-dumpstream", "-dumpfile", "test.mp4")
recordIn, err := record.StdinPipe()
if err != nil {
log.Fatalf("Unable to get STDIN for mplayer %v", err)
}
if err := record.Start(); err != nil {
log.Fatalf("Unable to start mplayer - %v", err)
return
}
// start video feed when drone connects
drone.GetVideoSpsPps()
go func() {
for {
drone.GetVideoSpsPps()
time.Sleep(500 * time.Millisecond)
}
}()
go func() {
for {
vbuf := <-videochan
_, err := recordIn.Write(vbuf)
if err != nil {
log.Fatalf("Error writing to mplayer %v\n", err)
}
}
}()
// recording using mplayer. WARNING: Either the bitrate or the RAM is not large enough to record and view the screen at the same time with multiple channels!
/* go func() {
for {
vbufR := <-videochan
_, err := recordIn.Write(vbufR)
if err != nil {
log.Fatalf("Error recording %v\n", err)
}
}
}()*/
}
func startDisplay() {
var player *exec.Cmd
//you may be able to use 'tee' to copy the pipe to another process
player = exec.Command("mplayer", "-fps", "30", "test.mp4")
if err := player.Start(); err != nil {
log.Fatalf("Unable to start player - %v", err)
return
}
}