mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 22:24:32 +02:00
310 lines
22 KiB
Python
310 lines
22 KiB
Python
# Unit tests for ROI
|
|
import mock
|
|
from mock import Mock
|
|
import os
|
|
from os import sys, path
|
|
from pymavlink import mavutil
|
|
from shotManagerConstants import *
|
|
import unittest
|
|
import random
|
|
import math
|
|
|
|
from dronekit import Vehicle, LocationGlobalRelative
|
|
|
|
sys.path.append(os.path.realpath('..'))
|
|
import pathHandler
|
|
from shotManager import ShotManager
|
|
import shots
|
|
#Random number generator seed
|
|
SEED = 94739473
|
|
|
|
#Number of tests to run
|
|
REPEAT = 10
|
|
|
|
class TestMoveTowardsEndpt(unittest.TestCase):
|
|
def setUp(self):
|
|
mgr = mock.create_autospec(ShotManager)
|
|
mgr.currentShot = shots.APP_SHOT_CABLECAM
|
|
self.loc1 = LocationGlobalRelative(45.68464156, 58.68464, 5.54684)
|
|
self.loc2 = LocationGlobalRelative(45.684641555, 58.684655, 5.54684)
|
|
v = mock.create_autospec(Vehicle)
|
|
v.message_factory = Mock()
|
|
v.commands = Mock()
|
|
self.handler = pathHandler.TwoPointPathHandler( self.loc1, self.loc2, v, mgr )
|
|
random.seed(SEED)
|
|
|
|
def testTogglePauseShouldPause(self):
|
|
'''If cruiseSpeed != 0 then pause() should be executed'''
|
|
self.handler.pause = Mock()
|
|
self.handler.cruiseSpeed = 4.0
|
|
self.handler.togglePause()
|
|
self.handler.pause.assert_called_with()
|
|
|
|
def testTogglePauseShouldResume(self):
|
|
'''If cruiseSpeed == 0 then resume() should be executed'''
|
|
self.handler.resume = Mock()
|
|
self.handler.cruiseSpeed = 0.0
|
|
self.handler.togglePause()
|
|
self.handler.resume.assert_called_with()
|
|
|
|
def testNoCruiseForwardsFullStick(self):
|
|
""" Command should reach (and not exceed) MAX_SPEED [Flying in forwards (+) direction, full stick in positive direction] """
|
|
for test in range(REPEAT): #test REPEAT times with random values based on SEED
|
|
self.handler.cruiseSpeed = 0.0 #No cruise
|
|
initialSpeed = random.uniform(0.0,MAX_SPEED) #Start at a random positive speed within acceptable limits
|
|
self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
|
|
channels = [1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0] #Full positive stick
|
|
#calculate number of ticks required to reach MAX_SPEED
|
|
deltaSpeed = MAX_SPEED-self.handler.currentSpeed
|
|
ticksRequired = int(math.ceil(deltaSpeed/pathHandler.ACCEL_PER_TICK))
|
|
for ticks in range(1,ticksRequired+1):
|
|
#run controller tick
|
|
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
|
|
#assert but handle floating point errors (limit speed to MAX_SPEED)
|
|
self.assertAlmostEqual(self.handler.currentSpeed, min(initialSpeed + ticks*pathHandler.ACCEL_PER_TICK,MAX_SPEED), places=7, msg=None, delta=None)
|
|
#make sure the command is called
|
|
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
|
|
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
|
|
0, # confirmation
|
|
1, abs(self.handler.currentSpeed), -1, # params 1-3
|
|
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
|
|
#make sure we are targeted to the right loc
|
|
self.handler.vehicle.simple_goto.assert_called_with( self.loc2 )
|
|
self.assertGreaterEqual(returnSpeed, 0.0)
|
|
|
|
def testNoCruiseBackwardsFullNegStick(self):
|
|
""" Command should reach (and not exceed) -MAX_SPEED [Flying in backwards (-) direction, full stick in negative direction] """
|
|
for test in range(REPEAT):
|
|
self.handler.cruiseSpeed = 0.0 #No cruise
|
|
initialSpeed = random.uniform(-MAX_SPEED,0.0) #Start at a random speed within acceptable limits
|
|
self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
|
|
channels = [1.0, -1.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0] #Full negative stick
|
|
#calculate number of ticks required to reach -MAX_SPEED
|
|
deltaSpeed = -MAX_SPEED-self.handler.currentSpeed
|
|
ticksRequired = int(math.ceil(abs(deltaSpeed/pathHandler.ACCEL_PER_TICK)))
|
|
for ticks in range(1,ticksRequired+1):
|
|
#run controller tick
|
|
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
|
|
#assert but handle floating point errors (limit speed to -MAX_SPEED)
|
|
self.assertAlmostEqual(self.handler.currentSpeed, max(initialSpeed - ticks*pathHandler.ACCEL_PER_TICK,-MAX_SPEED), places=7, msg=None, delta=None)
|
|
#make sure the command is called
|
|
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
|
|
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
|
|
0, # confirmation
|
|
1, abs(self.handler.currentSpeed), -1, # params 1-3
|
|
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
|
|
#make sure we are targeted to the right loc
|
|
self.handler.vehicle.simple_goto.assert_called_with( self.loc1 )
|
|
self.assertGreaterEqual(returnSpeed, 0.0)
|
|
|
|
def testNoCruiseSwitchDirections(self):
|
|
''' Command should decelerate to zero and accelerate in new direction [Flying in forwards (+) direction, full stick in negative direction] '''
|
|
for test in range(REPEAT):
|
|
self.handler.cruiseSpeed = 0.0 #No cruise
|
|
initialSpeed = random.uniform(0.0, MAX_SPEED) #Start at a random speed within acceptable limits
|
|
self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
|
|
channels = [1.0, -1.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0] #Full negative stick
|
|
#calculate number of decel ticks required to reach 0.0
|
|
deltaSpeed = 0.0-self.handler.currentSpeed
|
|
ticksRequired = int(math.ceil(abs(deltaSpeed/pathHandler.ACCEL_PER_TICK)))
|
|
for ticks in range(1,ticksRequired+1):
|
|
#run controller tick
|
|
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
|
|
#assert but handle floating point errors (limit speed to -MAX_SPEED)
|
|
self.assertAlmostEqual(self.handler.currentSpeed, initialSpeed - ticks*pathHandler.ACCEL_PER_TICK, places=7, msg=None, delta=None)
|
|
#make sure the command is called
|
|
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
|
|
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
|
|
0, # confirmation
|
|
1, abs(self.handler.currentSpeed), -1, # params 1-3
|
|
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
|
|
#should be heading in reverse now
|
|
self.handler.vehicle.simple_goto.assert_called_with( self.loc1 )
|
|
|
|
#resync initial speed to current
|
|
initialSpeed = self.handler.currentSpeed
|
|
|
|
#redo math to reach full neg speed
|
|
deltaSpeed = -MAX_SPEED-self.handler.currentSpeed
|
|
ticksRequired = int(math.ceil(abs(deltaSpeed/pathHandler.ACCEL_PER_TICK)))
|
|
for ticks in range(1,ticksRequired+1):
|
|
#run controller tick
|
|
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
|
|
#assert but handle floating point errors (limit speed to -MAX_SPEED)
|
|
self.assertAlmostEqual(self.handler.currentSpeed, max(initialSpeed - ticks*pathHandler.ACCEL_PER_TICK,-MAX_SPEED), places=7, msg=None, delta=None)
|
|
#make sure the command is called
|
|
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
|
|
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
|
|
0, # confirmation
|
|
1, abs(self.handler.currentSpeed), -1, # params 1-3
|
|
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
|
|
self.assertEqual(-MAX_SPEED,self.handler.currentSpeed)
|
|
self.assertGreaterEqual(returnSpeed, 0.0)
|
|
|
|
def testNoCruisePitchBigger(self):
|
|
""" Pitch beats roll """
|
|
channels = [1.0, 0.4, -0.8, 1.0, 1.0, 1.0, 1.0, 1.0]
|
|
self.handler.cruiseSpeed = 0.0 #No cruise
|
|
self.handler.currentSpeed = 0.0 #Start at a standstill
|
|
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
|
|
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
|
|
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
|
|
0, # confirmation
|
|
1, pathHandler.ACCEL_PER_TICK, -1, # params 1-3
|
|
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
|
|
self.handler.vehicle.simple_goto.assert_called_with( self.loc2 )
|
|
self.assertGreaterEqual(returnSpeed, 0.0)
|
|
|
|
|
|
def testNoCruiseRollBigger(self):
|
|
""" Roll beats pitch """
|
|
channels = [1.0, -0.72, 0.33, 1.0, 1.0, 1.0, 1.0, 1.0]
|
|
self.handler.cruiseSpeed = 0.0 #No cruise
|
|
self.handler.currentSpeed = 0.0 #Start at a standstill
|
|
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
|
|
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
|
|
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
|
|
0, # confirmation
|
|
1, pathHandler.ACCEL_PER_TICK, -1, # params 1-3
|
|
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
|
|
self.handler.vehicle.simple_goto.assert_called_with( self.loc1 )
|
|
self.assertGreaterEqual(returnSpeed, 0.0)
|
|
|
|
def testNoCruisePositiveAccelerate(self):
|
|
''' Vehicle flying in positive direction, increase stick in positive direction - should accelerate to steady state speed'''
|
|
for test in range(REPEAT):
|
|
initialSpeed = random.uniform(0.0,MAX_SPEED) #Start at a random speed within acceptable limits
|
|
self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
|
|
stickSpeed = random.uniform(initialSpeed,MAX_SPEED) #set a random stick speed above the randomly chosen initial speed
|
|
stickVal = stickSpeed/MAX_SPEED #calculate corresponding stick value
|
|
channels = [1.0, stickVal, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
|
#calculate number of accel ticks required to reach stickSpeed
|
|
deltaSpeed = stickSpeed-self.handler.currentSpeed
|
|
ticksRequired = int(math.ceil(abs(deltaSpeed/pathHandler.ACCEL_PER_TICK)))
|
|
for ticks in range(1,ticksRequired+1):
|
|
#run controller tick
|
|
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
|
|
#assert but handle floating point errors (limit to stickSpeed)
|
|
self.assertAlmostEqual(self.handler.currentSpeed, min(initialSpeed + ticks*pathHandler.ACCEL_PER_TICK,stickSpeed), places=7, msg=None, delta=None)
|
|
#make sure the command is called
|
|
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
|
|
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
|
|
0, # confirmation
|
|
1, abs(self.handler.currentSpeed), -1, # params 1-3
|
|
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
|
|
self.assertEqual(stickSpeed,self.handler.currentSpeed)
|
|
self.handler.vehicle.simple_goto.assert_called_with( self.loc2 )
|
|
self.assertGreaterEqual(returnSpeed, 0.0)
|
|
|
|
def testNoCruisePositiveDecelerate(self):
|
|
''' Vehicle flying in positive direction, decrease stick in positive direction - should decelerate '''
|
|
for test in range(REPEAT):
|
|
initialSpeed = random.uniform(0.0,MAX_SPEED) #Start at a random speed within acceptable limits
|
|
self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
|
|
stickSpeed = random.uniform(0.0,initialSpeed) #set a random stick speed above the randomly chosen initial speed
|
|
stickVal = stickSpeed/MAX_SPEED #calculate corresponding stick value
|
|
channels = [1.0, stickVal, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
|
#calculate number of accel ticks required to reach stickSpeed
|
|
deltaSpeed = stickSpeed-self.handler.currentSpeed
|
|
ticksRequired = int(math.ceil(abs(deltaSpeed/pathHandler.ACCEL_PER_TICK)))
|
|
for ticks in range(1,ticksRequired+1):
|
|
#run controller tick
|
|
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
|
|
#assert but handle floating point errors (limit to stickSpeed)
|
|
self.assertAlmostEqual(self.handler.currentSpeed, max(initialSpeed - ticks*pathHandler.ACCEL_PER_TICK,stickSpeed), places=7, msg=None, delta=None)
|
|
#make sure the command is called
|
|
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
|
|
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
|
|
0, # confirmation
|
|
1, abs(self.handler.currentSpeed), -1, # params 1-3
|
|
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
|
|
self.assertEqual(stickSpeed,self.handler.currentSpeed)
|
|
self.handler.vehicle.simple_goto.assert_called_with( self.loc2 )
|
|
self.assertGreaterEqual(returnSpeed, 0.0)
|
|
|
|
def testNoCruiseNegativeAccelerate(self):
|
|
''' Vehicle flying in negative direction, increase stick in negative direction - should accelerate '''
|
|
for test in range(REPEAT):
|
|
initialSpeed = random.uniform(-MAX_SPEED,0.0) #Start at a random speed within acceptable limits
|
|
self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
|
|
stickSpeed = random.uniform(-MAX_SPEED,initialSpeed) #set a random stick speed above the randomly chosen initial speed
|
|
stickVal = stickSpeed/MAX_SPEED #calculate corresponding stick value
|
|
channels = [1.0, stickVal, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
|
#calculate number of accel ticks required to reach stickSpeed
|
|
deltaSpeed = stickSpeed-self.handler.currentSpeed
|
|
ticksRequired = int(math.ceil(abs(deltaSpeed/pathHandler.ACCEL_PER_TICK)))
|
|
for ticks in range(1,ticksRequired+1):
|
|
#run controller tick
|
|
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
|
|
#assert but handle floating point errors (limit to stickSpeed)
|
|
self.assertAlmostEqual(self.handler.currentSpeed, max(initialSpeed - ticks*pathHandler.ACCEL_PER_TICK,stickSpeed), places=7, msg=None, delta=None)
|
|
#make sure the command is called
|
|
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
|
|
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
|
|
0, # confirmation
|
|
1, abs(self.handler.currentSpeed), -1, # params 1-3
|
|
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
|
|
self.assertEqual(stickSpeed,self.handler.currentSpeed)
|
|
self.handler.vehicle.simple_goto.assert_called_with( self.loc1 )
|
|
self.assertGreaterEqual(returnSpeed, 0.0)
|
|
|
|
def testNoCruiseNegativeDecelerate(self):
|
|
''' Vehicle flying in negative direction, decrease stick in negative direction - should decelerate '''
|
|
for test in range(REPEAT):
|
|
initialSpeed = random.uniform(-MAX_SPEED,0.0) #Start at a random speed within acceptable limits
|
|
self.handler.currentSpeed = initialSpeed #set currentSpeed to initial
|
|
stickSpeed = random.uniform(initialSpeed,0.0) #set a random stick speed above the randomly chosen initial speed
|
|
stickVal = stickSpeed/MAX_SPEED #calculate corresponding stick value
|
|
channels = [1.0, stickVal, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0]
|
|
#calculate number of accel ticks required to reach stickSpeed
|
|
deltaSpeed = stickSpeed-self.handler.currentSpeed
|
|
ticksRequired = int(math.ceil(abs(deltaSpeed/pathHandler.ACCEL_PER_TICK)))
|
|
for ticks in range(1,ticksRequired+1):
|
|
#run controller tick
|
|
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
|
|
#assert but handle floating point errors (limit to stickSpeed)
|
|
self.assertAlmostEqual(self.handler.currentSpeed, min(initialSpeed + ticks*pathHandler.ACCEL_PER_TICK,stickSpeed), places=7, msg=None, delta=None)
|
|
#make sure the command is called
|
|
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
|
|
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
|
|
0, # confirmation
|
|
1, abs(self.handler.currentSpeed), -1, # params 1-3
|
|
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
|
|
self.assertEqual(stickSpeed,self.handler.currentSpeed)
|
|
self.handler.vehicle.simple_goto.assert_called_with( self.loc1 )
|
|
self.assertGreaterEqual(returnSpeed, 0.0)
|
|
|
|
###CRUISE TESTS###
|
|
|
|
def testCruiseSameDir(self):
|
|
""" Cruising with full same stick towards loc 2- should accelerate """
|
|
self.handler.cruiseSpeed = 4.0 #random positive cruise speed
|
|
self.handler.currentSpeed = self.handler.cruiseSpeed #start out at cruise
|
|
channels = [1.0, 0.5, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0] #Stick forward
|
|
|
|
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
|
|
#make sure the command is called
|
|
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
|
|
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
|
|
0, # confirmation
|
|
1, self.handler.cruiseSpeed + pathHandler.ACCEL_PER_TICK, -1, # params 1-3
|
|
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
|
|
self.handler.vehicle.simple_goto.assert_called_with( self.loc2 )
|
|
self.assertGreaterEqual(returnSpeed, 0.0)
|
|
|
|
def testCruiseOppositeDir(self):
|
|
""" Cruising towards loc 2 with opposite stick - should decelerate to a steady state speed"""
|
|
self.handler.cruiseSpeed = 4.0 #random positive cruise speed
|
|
self.handler.currentSpeed = self.handler.cruiseSpeed #start out at cruise
|
|
channels = [1.0, -0.5, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0] #Stick backward
|
|
|
|
[returnSpeed, returnDirection] = self.handler.MoveTowardsEndpt( channels )
|
|
#make sure the command is called
|
|
self.handler.vehicle.message_factory.command_long_encode.assert_called_with(0, 1, # target system, target component
|
|
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, # frame
|
|
0, # confirmation
|
|
1, self.handler.cruiseSpeed - pathHandler.ACCEL_PER_TICK, -1, # params 1-3
|
|
0.0, 0.0, 0.0, 0.0 ) # params 4-7 (not used)
|
|
self.handler.vehicle.simple_goto.assert_called_with( self.loc2 )
|
|
self.assertGreaterEqual(returnSpeed, 0.0)
|