mirror of https://github.com/ArduPilot/ardupilot
Tools: autotest: add test for follow mode
This commit is contained in:
parent
929426abf8
commit
36ba3a6e4c
|
@ -2,6 +2,7 @@
|
||||||
|
|
||||||
# Fly ArduCopter in SITL
|
# Fly ArduCopter in SITL
|
||||||
from __future__ import print_function
|
from __future__ import print_function
|
||||||
|
import copy
|
||||||
import math
|
import math
|
||||||
import os
|
import os
|
||||||
import shutil
|
import shutil
|
||||||
|
@ -3339,6 +3340,64 @@ class AutoTestCopter(AutoTest):
|
||||||
if ex is not None:
|
if ex is not None:
|
||||||
raise ex
|
raise ex
|
||||||
|
|
||||||
|
def global_position_int_for_location(self, loc, time, heading=0):
|
||||||
|
return self.mav.mav.global_position_int_encode(
|
||||||
|
int(time * 1000), # time_boot_ms
|
||||||
|
int(loc.lat * 1e7),
|
||||||
|
int(loc.lng * 1e7),
|
||||||
|
int(loc.alt * 1000), # alt in mm
|
||||||
|
20, # relative alt - urp.
|
||||||
|
vx = 0,
|
||||||
|
vy = 0,
|
||||||
|
vz = 0,
|
||||||
|
hdg = heading
|
||||||
|
)
|
||||||
|
|
||||||
|
def fly_follow_mode(self):
|
||||||
|
self.set_parameter("FOLL_ENABLE", 1)
|
||||||
|
self.set_parameter("FOLL_SYSID", 255)
|
||||||
|
foll_ofs_x = 30 # metres
|
||||||
|
self.set_parameter("FOLL_OFS_X", -foll_ofs_x)
|
||||||
|
self.set_parameter("FOLL_OFS_TYPE", 1) # relative to other vehicle heading
|
||||||
|
self.takeoff(10, mode="LOITER")
|
||||||
|
self.set_parameter("SIM_SPEEDUP", 1)
|
||||||
|
self.change_mode("FOLLOW")
|
||||||
|
ex = None
|
||||||
|
tstart = self.get_sim_time_cached()
|
||||||
|
new_loc = self.mav.location()
|
||||||
|
new_loc_offset_n = 20
|
||||||
|
new_loc_offset_e = 30
|
||||||
|
self.location_offset_ne(new_loc, new_loc_offset_n, new_loc_offset_e)
|
||||||
|
self.progress("new_loc: %s" % str(new_loc))
|
||||||
|
heading = 0
|
||||||
|
self.mavproxy.send("map icon %f %f greenplane %f\n" %
|
||||||
|
(new_loc.lat, new_loc.lng, heading))
|
||||||
|
|
||||||
|
expected_loc = copy.copy(new_loc)
|
||||||
|
self.location_offset_ne(expected_loc, -foll_ofs_x, 0)
|
||||||
|
self.mavproxy.send("map icon %f %f hoop\n" %
|
||||||
|
(expected_loc.lat, expected_loc.lng))
|
||||||
|
self.progress("expected_loc: %s" % str(expected_loc))
|
||||||
|
|
||||||
|
last_sent = 0
|
||||||
|
while True:
|
||||||
|
now = self.get_sim_time_cached()
|
||||||
|
if now - last_sent > 0.5:
|
||||||
|
last_run = now
|
||||||
|
gpi = self.global_position_int_for_location(new_loc,
|
||||||
|
now,
|
||||||
|
heading=heading)
|
||||||
|
gpi.pack(self.mav.mav)
|
||||||
|
self.mav.mav.send(gpi)
|
||||||
|
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
||||||
|
pos = self.mav.location()
|
||||||
|
delta = self.get_distance(expected_loc, pos)
|
||||||
|
max_delta = 2
|
||||||
|
self.progress("position delta=%f (want <%f)" % (delta, max_delta))
|
||||||
|
if delta < max_delta:
|
||||||
|
break
|
||||||
|
self.do_RTL()
|
||||||
|
|
||||||
def fly_beacon_position(self):
|
def fly_beacon_position(self):
|
||||||
self.reboot_sitl()
|
self.reboot_sitl()
|
||||||
|
|
||||||
|
@ -3670,6 +3729,10 @@ class AutoTestCopter(AutoTest):
|
||||||
"Fly POSHOLD takeoff",
|
"Fly POSHOLD takeoff",
|
||||||
self.fly_poshold_takeoff),
|
self.fly_poshold_takeoff),
|
||||||
|
|
||||||
|
("FOLLOW",
|
||||||
|
"Fly follow mode",
|
||||||
|
self.fly_follow_mode),
|
||||||
|
|
||||||
("OnboardCompassCalibration",
|
("OnboardCompassCalibration",
|
||||||
"Test onboard compass calibration",
|
"Test onboard compass calibration",
|
||||||
self.test_onboard_compass_calibration),
|
self.test_onboard_compass_calibration),
|
||||||
|
|
|
@ -1104,14 +1104,6 @@ class AutoTestPlane(AutoTest):
|
||||||
self.test_fence_breach_circle_at(self.home_position_as_mav_location(),
|
self.test_fence_breach_circle_at(self.home_position_as_mav_location(),
|
||||||
disable_on_breach=True)
|
disable_on_breach=True)
|
||||||
|
|
||||||
def location_offset_ne(self, location, north, east):
|
|
||||||
'''move location in metres'''
|
|
||||||
print("old: %f %f" % (location.lat, location.lng))
|
|
||||||
(lat, lng) = mp_util.gps_offset(location.lat, location.lng, east, north)
|
|
||||||
location.lat = lat
|
|
||||||
location.lng = lng
|
|
||||||
print("new: %f %f" % (location.lat, location.lng))
|
|
||||||
|
|
||||||
def test_fence_rtl_rally(self):
|
def test_fence_rtl_rally(self):
|
||||||
ex = None
|
ex = None
|
||||||
target_system = 1
|
target_system = 1
|
||||||
|
|
|
@ -847,6 +847,14 @@ class AutoTest(ABC):
|
||||||
return True
|
return True
|
||||||
raise SetRCTimeout("Failed to send RC commands to channel %s" % str(chan))
|
raise SetRCTimeout("Failed to send RC commands to channel %s" % str(chan))
|
||||||
|
|
||||||
|
def location_offset_ne(self, location, north, east):
|
||||||
|
'''move location in metres'''
|
||||||
|
print("old: %f %f" % (location.lat, location.lng))
|
||||||
|
(lat, lng) = mp_util.gps_offset(location.lat, location.lng, east, north)
|
||||||
|
location.lat = lat
|
||||||
|
location.lng = lng
|
||||||
|
print("new: %f %f" % (location.lat, location.lng))
|
||||||
|
|
||||||
def zero_throttle(self):
|
def zero_throttle(self):
|
||||||
"""Set throttle to zero."""
|
"""Set throttle to zero."""
|
||||||
if self.is_rover():
|
if self.is_rover():
|
||||||
|
|
Loading…
Reference in New Issue