Tools: autotest: add a test for Copter surface tracking
This commit is contained in:
Peter Barker
Peter Barker
parent
0ca71ba725
commit
98cb68157f
@ -1500,7 +1500,8 @@ class AutoTestCopter(AutoTest):
|
||||
break
|
||||
|
||||
except Exception as e:
|
||||
self.progress("Exception caught")
|
||||
self.progress("Exception caught: %s" % (
|
||||
self.get_exception_stacktrace(e)))
|
||||
ex = e
|
||||
|
||||
self.context_pop()
|
||||
@ -1667,7 +1668,69 @@ class AutoTestCopter(AutoTest):
|
||||
raise NotAchievedException("Did not see expected RFND message")
|
||||
|
||||
except Exception as e:
|
||||
self.progress("Exception caught")
|
||||
self.progress("Exception caught: %s" % (
|
||||
self.get_exception_stacktrace(e)))
|
||||
ex = e
|
||||
self.context_pop()
|
||||
self.reboot_sitl()
|
||||
if ex is not None:
|
||||
raise ex
|
||||
|
||||
def test_surface_tracking(self):
|
||||
ex = None
|
||||
self.context_push()
|
||||
|
||||
try:
|
||||
self.set_parameter("RNGFND1_TYPE", 1)
|
||||
self.set_parameter("RNGFND1_MIN_CM", 0)
|
||||
self.set_parameter("RNGFND1_MAX_CM", 4000)
|
||||
self.set_parameter("RNGFND1_PIN", 0)
|
||||
self.set_parameter("RNGFND1_SCALING", 12.12)
|
||||
self.set_parameter("RC9_OPTION", 10) # rangefinder
|
||||
self.set_rc(9, 2000)
|
||||
|
||||
self.reboot_sitl() # needed for both rangefinder and initial position
|
||||
self.assert_vehicle_location_is_at_startup_location()
|
||||
|
||||
self.takeoff(10, mode="LOITER")
|
||||
lower_surface_pos = mavutil.location(-35.362421, 149.164534, 584, 270)
|
||||
here = self.mav.location()
|
||||
bearing = self.get_bearing(here, lower_surface_pos)
|
||||
|
||||
self.change_mode("GUIDED")
|
||||
self.guided_achieve_heading(bearing)
|
||||
self.change_mode("LOITER")
|
||||
self.delay_sim_time(2)
|
||||
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
||||
orig_absolute_alt_mm = m.alt
|
||||
|
||||
self.progress("Original alt: absolute=%f" % orig_absolute_alt_mm)
|
||||
|
||||
self.progress("Flying somewhere which surface is known lower compared to takeoff point")
|
||||
self.set_rc(2, 1450)
|
||||
tstart = self.get_sim_time()
|
||||
while True:
|
||||
if self.get_sim_time() - tstart > 200:
|
||||
raise NotAchievedException("Did not reach lower point")
|
||||
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
||||
x = mavutil.location(m.lat/1e7, m.lon/1e7, m.alt/1e3, 0)
|
||||
dist = self.get_distance(x, lower_surface_pos)
|
||||
delta = (orig_absolute_alt_mm - m.alt)/1000.0
|
||||
|
||||
self.progress("Distance: %fm abs-alt-delta: %fm" %
|
||||
(dist, delta))
|
||||
if dist < 10:
|
||||
if delta < 0.8:
|
||||
raise NotAchievedException("Did not dip in altitude as expected")
|
||||
break
|
||||
|
||||
self.set_rc(2, 1500)
|
||||
self.do_RTL()
|
||||
|
||||
except Exception as e:
|
||||
self.progress("Exception caught: %s" % (
|
||||
self.get_exception_stacktrace(e)))
|
||||
self.disarm_vehicle(force=True)
|
||||
ex = e
|
||||
self.context_pop()
|
||||
self.reboot_sitl()
|
||||
@ -2139,7 +2202,8 @@ class AutoTestCopter(AutoTest):
|
||||
self.wait_mode("ACRO")
|
||||
|
||||
except Exception as e:
|
||||
self.progress("Exception caught")
|
||||
self.progress("Exception caught: %s" % (
|
||||
self.get_exception_stacktrace(e)))
|
||||
ex = e
|
||||
|
||||
self.context_pop()
|
||||
@ -2165,7 +2229,8 @@ class AutoTestCopter(AutoTest):
|
||||
self.set_rc(10, 1000) # this re-polls the mode switch
|
||||
self.wait_mode("CIRCLE")
|
||||
except Exception as e:
|
||||
self.progress("Exception caught")
|
||||
self.progress("Exception caught: %s" % (
|
||||
self.get_exception_stacktrace(e)))
|
||||
ex = e
|
||||
|
||||
self.context_pop()
|
||||
@ -2489,7 +2554,8 @@ class AutoTestCopter(AutoTest):
|
||||
self.mav.motors_disarmed_wait()
|
||||
|
||||
except Exception as e:
|
||||
self.progress("Exception caught")
|
||||
self.progress("Exception caught: %s" % (
|
||||
self.get_exception_stacktrace(e)))
|
||||
ex = e
|
||||
|
||||
self.context_pop()
|
||||
@ -2538,6 +2604,7 @@ class AutoTestCopter(AutoTest):
|
||||
self.load_mission("copter-gripper-mission.txt")
|
||||
self.mavproxy.send('mode loiter\n')
|
||||
self.wait_ready_to_arm()
|
||||
self.assert_vehicle_location_is_at_startup_location()
|
||||
self.arm_vehicle()
|
||||
self.mavproxy.send('mode auto\n')
|
||||
self.wait_mode('AUTO')
|
||||
@ -2545,7 +2612,8 @@ class AutoTestCopter(AutoTest):
|
||||
self.mavproxy.expect("Gripper Grabbed")
|
||||
self.mavproxy.expect("Gripper Released")
|
||||
except Exception as e:
|
||||
self.progress("Exception caught: %s" % str(e))
|
||||
self.progress("Exception caught: %s" % (
|
||||
self.get_exception_stacktrace(e)))
|
||||
self.mavproxy.send('mode land\n')
|
||||
ex = e
|
||||
self.context_pop()
|
||||
@ -2983,7 +3051,8 @@ class AutoTestCopter(AutoTest):
|
||||
self.progress("Attitude: %f %f %f" %
|
||||
(math.degrees(att.roll), math.degrees(att.pitch), math.degrees(att.yaw)))
|
||||
except Exception as e:
|
||||
print("Exception caught: %s" % str(e))
|
||||
self.progress("Exception caught: %s" % (
|
||||
self.get_exception_stacktrace(e)))
|
||||
ex = e
|
||||
|
||||
self.context_pop()
|
||||
@ -3067,7 +3136,8 @@ class AutoTestCopter(AutoTest):
|
||||
self.loiter_to_ne(start.x + 5, start.y - 10, start.z + 10)
|
||||
|
||||
except Exception as e:
|
||||
self.progress("Exception caught: %s" % self.get_exception_stacktrace(e))
|
||||
self.progress("Exception caught: %s" % (
|
||||
self.get_exception_stacktrace(e)))
|
||||
ex = e
|
||||
|
||||
self.context_pop()
|
||||
@ -3521,6 +3591,10 @@ class AutoTestCopter(AutoTest):
|
||||
"Test RangeFinder Basic Functionality",
|
||||
self.test_rangefinder),
|
||||
|
||||
("SurfaceTracking",
|
||||
"Test Surface Tracking",
|
||||
self.test_surface_tracking),
|
||||
|
||||
("Parachute",
|
||||
"Test Parachute Functionality",
|
||||
self.test_parachute),
|
||||
|
||||
@ -314,6 +314,7 @@ class AutoTest(ABC):
|
||||
def reboot_sitl(self):
|
||||
"""Reboot SITL instance and wait for it to reconnect."""
|
||||
self.reboot_sitl_mav()
|
||||
self.assert_simstate_location_is_at_startup_location()
|
||||
|
||||
def reboot_sitl_mavproxy(self):
|
||||
"""Reboot SITL instance using MAVProxy and wait for it to reconnect."""
|
||||
@ -1379,6 +1380,26 @@ class AutoTest(ABC):
|
||||
self.set_rc(3, 1500)
|
||||
self.set_rc(1, 1500)
|
||||
|
||||
def assert_vehicle_location_is_at_startup_location(self, dist_max=1):
|
||||
here = self.mav.location()
|
||||
start_loc = self.sitl_start_location()
|
||||
dist = self.get_distance(here, start_loc)
|
||||
data = "dist=%f max=%f (here: %s start-loc: %s)" % (dist, dist_max, here, start_loc)
|
||||
|
||||
if dist > dist_max:
|
||||
raise NotAchievedException("Far from startup location: %s" % data)
|
||||
self.progress("Close to startup location: %s" % data)
|
||||
|
||||
def assert_simstate_location_is_at_startup_location(self, dist_max=1):
|
||||
simstate_loc = self.sim_location()
|
||||
start_loc = self.sitl_start_location()
|
||||
dist = self.get_distance(simstate_loc, start_loc)
|
||||
data = "dist=%f max=%f (simstate: %s start-loc: %s)" % (dist, dist_max, simstate_loc, start_loc)
|
||||
|
||||
if dist > dist_max:
|
||||
raise NotAchievedException("simstate from startup location: %s" % data)
|
||||
self.progress("Simstate Close to startup location: %s" % data)
|
||||
|
||||
def reach_distance_manual(self, distance):
|
||||
"""Manually direct the vehicle to the target distance from home."""
|
||||
if self.is_copter():
|
||||
@ -1394,21 +1415,21 @@ class AutoTest(ABC):
|
||||
|
||||
def guided_achieve_heading(self, heading):
|
||||
tstart = self.get_sim_time()
|
||||
self.run_cmd(mavutil.mavlink.MAV_CMD_CONDITION_YAW,
|
||||
heading, # target angle
|
||||
10, # degrees/second
|
||||
1, # -1 is counter-clockwise, 1 clockwise
|
||||
0, # 1 for relative, 0 for absolute
|
||||
0, # p5
|
||||
0, # p6
|
||||
0, # p7
|
||||
)
|
||||
while True:
|
||||
if self.get_sim_time_cached() - tstart > 200:
|
||||
raise NotAchievedException("Did not achieve heading")
|
||||
self.run_cmd(mavutil.mavlink.MAV_CMD_CONDITION_YAW,
|
||||
heading, # target angle
|
||||
10, # degrees/second
|
||||
1, # -1 is counter-clockwise, 1 clockwise
|
||||
0, # 1 for relative, 0 for absolute
|
||||
0, # p5
|
||||
0, # p6
|
||||
0, # p7
|
||||
)
|
||||
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
self.progress("heading=%d want=%f" % (m.heading, heading))
|
||||
if m.heading == heading:
|
||||
self.progress("heading=%d want=%d" % (m.heading, int(heading)))
|
||||
if m.heading == int(heading):
|
||||
return
|
||||
|
||||
#################################################
|
||||
|
||||
Loading…
Reference in New Issue
Block a user