mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 09:58:28 -04:00
Tools: autotest: increase test diagnostics
Tools: autotest: improve wait_distance_home_gt output Tools: autotest: emit progress for parameter sets Tools: autotest: add progress for RC health checks Tools: autotest: remove pointless context
This commit is contained in:
parent
c5c67362fa
commit
6418226106
@ -500,8 +500,11 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
||||
tstart = self.get_sim_time()
|
||||
while self.get_sim_time_cached() - tstart < timeout:
|
||||
# m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
if self.distance_to_home() > distance:
|
||||
distance_home = self.distance_to_home(use_cached_home=True)
|
||||
self.progress("distance_home=%f want=%f" % (distance_home, distance))
|
||||
if distance_home > distance:
|
||||
return
|
||||
self.drain_mav()
|
||||
raise NotAchievedException("Failed to get %fm from home (now=%f)" %
|
||||
(distance, home_distance))
|
||||
|
||||
|
@ -771,6 +771,7 @@ class AutoTestPlane(AutoTest):
|
||||
self.progress("Testing receiver health")
|
||||
if (m.onboard_control_sensors_health & receiver_bit):
|
||||
raise NotAchievedException("Sensor healthy when it shouldn't be")
|
||||
self.progress("Making RC work again")
|
||||
self.set_parameter("SIM_RC_FAIL", 0)
|
||||
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
|
||||
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
|
||||
|
@ -944,6 +944,7 @@ class AutoTest(ABC):
|
||||
|
||||
def set_parameter(self, name, value, add_to_context=True, epsilon=0.00001):
|
||||
"""Set parameters from vehicle."""
|
||||
self.progress("Setting %s to %f" % (name, value))
|
||||
old_value = self.get_parameter(name, retry=2)
|
||||
for i in range(1, 10):
|
||||
self.mavproxy.send("param set %s %s\n" % (name, str(value)))
|
||||
@ -1461,19 +1462,24 @@ class AutoTest(ABC):
|
||||
"%.4f,%.4f at altitude %.1f height_accuracy=%.1f" %
|
||||
(loc.lat, loc.lng, target_altitude, height_accuracy))
|
||||
last_distance_message = 0
|
||||
closest = 10000000
|
||||
last = 0
|
||||
while self.get_sim_time() < tstart + timeout:
|
||||
pos = self.mav.location()
|
||||
delta = self.get_distance(loc, pos)
|
||||
if self.get_sim_time_cached() - last_distance_message >= 1:
|
||||
self.progress("Distance %.2f meters alt %.1f" % (delta, pos.alt))
|
||||
last_distance_message = self.get_sim_time_cached()
|
||||
if closest > delta:
|
||||
closest = delta
|
||||
last = delta
|
||||
if delta <= accuracy:
|
||||
height_delta = math.fabs(pos.alt - target_altitude)
|
||||
if height_accuracy != -1 and height_delta > height_accuracy:
|
||||
continue
|
||||
self.progress("Reached location (%.2f meters)" % delta)
|
||||
return True
|
||||
raise WaitLocationTimeout("Failed to attain location")
|
||||
raise WaitLocationTimeout("Failed to attain location (want=<%f) (closest=%f)" % (accuracy, closest, last))
|
||||
|
||||
def wait_current_waypoint(self, wpnum, timeout=60):
|
||||
tstart = self.get_sim_time()
|
||||
@ -1849,7 +1855,6 @@ class AutoTest(ABC):
|
||||
|
||||
def test_arm_feature(self):
|
||||
"""Common feature to test."""
|
||||
self.context_push()
|
||||
# TEST ARMING/DISARM
|
||||
self.set_parameter("ARMING_RUDDER", 2) # allow arm and disarm with rudder on first tests
|
||||
interlock_channel = 8 # Plane got flighmode_ch on channel 8
|
||||
@ -1980,7 +1985,6 @@ class AutoTest(ABC):
|
||||
raise NotAchievedException("Motor interlock was changed while disarmed")
|
||||
self.set_rc(interlock_channel, 1000)
|
||||
self.progress("ALL PASS")
|
||||
self.context_pop()
|
||||
# TODO : add failure test : arming check, wrong mode; Test arming magic; Same for disarm
|
||||
|
||||
def get_message_rate(self, victim_message, timeout):
|
||||
|
Loading…
Reference in New Issue
Block a user