mirror of https://github.com/ArduPilot/ardupilot
Tools: autotest: move code up
This commit is contained in:
Peter Barker
Randy Mackay
parent
f2dc8f9282
commit
0d34052cef
|
|
@ -775,101 +775,6 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
|||
if ex is not None:
|
||||
raise ex
|
||||
|
||||
def get_message_rate(self, victim_message, timeout):
|
||||
tstart = self.get_sim_time()
|
||||
count = 0
|
||||
while self.get_sim_time_cached() < tstart + timeout:
|
||||
m = self.mav.recv_match(type=victim_message,
|
||||
blocking=True,
|
||||
timeout=0.1
|
||||
)
|
||||
if m is not None:
|
||||
count += 1
|
||||
time_delta = self.get_sim_time_cached() - tstart
|
||||
self.progress("%s count after %f seconds: %u" %
|
||||
(victim_message, time_delta, count))
|
||||
return count/time_delta
|
||||
|
||||
def rate_to_interval_us(self, rate):
|
||||
return 1/float(rate)*1000000.0
|
||||
|
||||
def test_rate(self, desc, in_rate, expected_rate):
|
||||
self.progress("###### %s" % desc)
|
||||
self.progress("Setting rate to %u" % in_rate)
|
||||
# SET_MESSAGE_INTERVAL rates are given in microseconds
|
||||
if in_rate == 0 or in_rate == -1:
|
||||
set_interval = in_rate
|
||||
else:
|
||||
set_interval = self.rate_to_interval_us(in_rate)
|
||||
|
||||
self.mavproxy.send("long SET_MESSAGE_INTERVAL %u %d\n" %
|
||||
(self.victim_message_id, set_interval))
|
||||
self.mav.recv_match(type='COMMAND_ACK', blocking=True)
|
||||
new_measured_rate = self.get_message_rate(self.victim_message, 10)
|
||||
self.progress("Measured rate: %f (want %u)" %
|
||||
(new_measured_rate, expected_rate))
|
||||
if round(new_measured_rate) != expected_rate:
|
||||
raise NotAchievedException("Rate not achieved (got %f want %u)" %
|
||||
(new_measured_rate, expected_rate))
|
||||
|
||||
def test_set_message_interval(self):
|
||||
self.victim_message = 'VFR_HUD'
|
||||
self.victim_message_id = mavutil.mavlink.MAVLINK_MSG_ID_VFR_HUD
|
||||
try:
|
||||
# tell MAVProxy to stop stuffing around with the rates:
|
||||
self.mavproxy.send("set streamrate -1\n")
|
||||
|
||||
rate = round(self.get_message_rate(self.victim_message, 20))
|
||||
self.progress("Initial rate: %u" % rate)
|
||||
|
||||
self.test_rate("Test set to %u" % (rate/2,), rate/2, rate/2)
|
||||
# this assumes the streamrates have not been played with:
|
||||
self.test_rate("Resetting original rate using 0-value", 0, rate)
|
||||
self.test_rate("Disabling using -1-value", -1, 0)
|
||||
self.test_rate("Resetting original rate", rate, rate)
|
||||
|
||||
self.progress("try getting a message which is not ordinarily streamed out")
|
||||
rate = round(self.get_message_rate("CAMERA_FEEDBACK", 20))
|
||||
if rate != 0:
|
||||
raise PreconditionFailedException("Already getting CAMERA_FEEDBACK")
|
||||
self.progress("try various message rates")
|
||||
camera_feedback_id = 180
|
||||
for want_rate in 5, 6, 7, 8, 9, 10, 11, 12, 13:
|
||||
self.mavproxy.send("long SET_MESSAGE_INTERVAL %u %d\n" %
|
||||
(camera_feedback_id,
|
||||
self.rate_to_interval_us(want_rate)))
|
||||
rate = round(self.get_message_rate("CAMERA_FEEDBACK", 20))
|
||||
self.progress("Want=%f got=%f" % (want_rate, rate))
|
||||
if rate != want_rate:
|
||||
raise NotAchievedException("Did not get expected rate")
|
||||
|
||||
|
||||
self.progress("try at the main loop rate")
|
||||
# have to reset the speedup as MAVProxy can't keep up otherwise
|
||||
old_speedup = self.get_parameter("SIM_SPEEDUP")
|
||||
self.set_parameter("SIM_SPEEDUP", 1.0)
|
||||
camera_feedback_id = 180
|
||||
# ArduPilot currently limits message rate to 80% of main loop rate:
|
||||
want_rate = self.get_parameter("SCHED_LOOP_RATE") * 0.8
|
||||
self.mavproxy.send("long SET_MESSAGE_INTERVAL %u %d\n" %
|
||||
(camera_feedback_id,
|
||||
self.rate_to_interval_us(want_rate)))
|
||||
rate = round(self.get_message_rate("CAMERA_FEEDBACK", 20))
|
||||
self.set_parameter("SIM_SPEEDUP", old_speedup)
|
||||
self.progress("Want=%f got=%f" % (want_rate, rate))
|
||||
if abs(rate - want_rate) > 2:
|
||||
raise NotAchievedException("Did not get expected rate")
|
||||
|
||||
|
||||
sr = self.sitl_streamrate()
|
||||
self.mavproxy.send("set streamrate %u\n" % sr)
|
||||
|
||||
except Exception as e:
|
||||
# tell MAVProxy to start stuffing around with the rates:
|
||||
sr = self.sitl_streamrate()
|
||||
self.mavproxy.send("set streamrate %u\n" % sr)
|
||||
raise e
|
||||
|
||||
def autotest(self):
|
||||
"""Autotest APMrover2 in SITL."""
|
||||
self.check_test_syntax(test_file=os.path.realpath(__file__))
|
||||
|
|
|
|||
|
|
@ -1545,6 +1545,100 @@ class AutoTest(ABC):
|
|||
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):
|
||||
tstart = self.get_sim_time()
|
||||
count = 0
|
||||
while self.get_sim_time_cached() < tstart + timeout:
|
||||
m = self.mav.recv_match(type=victim_message,
|
||||
blocking=True,
|
||||
timeout=0.1
|
||||
)
|
||||
if m is not None:
|
||||
count += 1
|
||||
time_delta = self.get_sim_time_cached() - tstart
|
||||
self.progress("%s count after %f seconds: %u" %
|
||||
(victim_message, time_delta, count))
|
||||
return count/time_delta
|
||||
|
||||
def rate_to_interval_us(self, rate):
|
||||
return 1/float(rate)*1000000.0
|
||||
|
||||
def test_rate(self, desc, in_rate, expected_rate):
|
||||
self.progress("###### %s" % desc)
|
||||
self.progress("Setting rate to %u" % in_rate)
|
||||
# SET_MESSAGE_INTERVAL rates are given in microseconds
|
||||
if in_rate == 0 or in_rate == -1:
|
||||
set_interval = in_rate
|
||||
else:
|
||||
set_interval = self.rate_to_interval_us(in_rate)
|
||||
|
||||
self.mavproxy.send("long SET_MESSAGE_INTERVAL %u %d\n" %
|
||||
(self.victim_message_id, set_interval))
|
||||
self.mav.recv_match(type='COMMAND_ACK', blocking=True)
|
||||
new_measured_rate = self.get_message_rate(self.victim_message, 10)
|
||||
self.progress("Measured rate: %f (want %u)" %
|
||||
(new_measured_rate, expected_rate))
|
||||
if round(new_measured_rate) != expected_rate:
|
||||
raise NotAchievedException("Rate not achieved (got %f want %u)" %
|
||||
(new_measured_rate, expected_rate))
|
||||
|
||||
def test_set_message_interval(self):
|
||||
self.victim_message = 'VFR_HUD'
|
||||
self.victim_message_id = mavutil.mavlink.MAVLINK_MSG_ID_VFR_HUD
|
||||
try:
|
||||
# tell MAVProxy to stop stuffing around with the rates:
|
||||
self.mavproxy.send("set streamrate -1\n")
|
||||
|
||||
rate = round(self.get_message_rate(self.victim_message, 20))
|
||||
self.progress("Initial rate: %u" % rate)
|
||||
|
||||
self.test_rate("Test set to %u" % (rate/2,), rate/2, rate/2)
|
||||
# this assumes the streamrates have not been played with:
|
||||
self.test_rate("Resetting original rate using 0-value", 0, rate)
|
||||
self.test_rate("Disabling using -1-value", -1, 0)
|
||||
self.test_rate("Resetting original rate", rate, rate)
|
||||
|
||||
self.progress("try getting a message which is not ordinarily streamed out")
|
||||
rate = round(self.get_message_rate("CAMERA_FEEDBACK", 20))
|
||||
if rate != 0:
|
||||
raise PreconditionFailedException("Already getting CAMERA_FEEDBACK")
|
||||
self.progress("try various message rates")
|
||||
for want_rate in range(5, 14):
|
||||
self.mavproxy.send(
|
||||
"long SET_MESSAGE_INTERVAL %u %d\n" %
|
||||
(mavutil.mavlink.MAVLINK_MSG_ID_CAMERA_FEEDBACK,
|
||||
self.rate_to_interval_us(want_rate)))
|
||||
rate = round(self.get_message_rate("CAMERA_FEEDBACK", 20))
|
||||
self.progress("Want=%f got=%f" % (want_rate, rate))
|
||||
if rate != want_rate:
|
||||
raise NotAchievedException("Did not get expected rate")
|
||||
|
||||
|
||||
self.progress("try at the main loop rate")
|
||||
# have to reset the speedup as MAVProxy can't keep up otherwise
|
||||
old_speedup = self.get_parameter("SIM_SPEEDUP")
|
||||
self.set_parameter("SIM_SPEEDUP", 1.0)
|
||||
# ArduPilot currently limits message rate to 80% of main loop rate:
|
||||
want_rate = self.get_parameter("SCHED_LOOP_RATE") * 0.8
|
||||
self.mavproxy.send("long SET_MESSAGE_INTERVAL %u %d\n" %
|
||||
(mavutil.mavlink.MAVLINK_MSG_ID_CAMERA_FEEDBACK,
|
||||
self.rate_to_interval_us(want_rate)))
|
||||
rate = round(self.get_message_rate("CAMERA_FEEDBACK", 20))
|
||||
self.set_parameter("SIM_SPEEDUP", old_speedup)
|
||||
self.progress("Want=%f got=%f" % (want_rate, rate))
|
||||
if abs(rate - want_rate) > 2:
|
||||
raise NotAchievedException("Did not get expected rate")
|
||||
|
||||
|
||||
sr = self.sitl_streamrate()
|
||||
self.mavproxy.send("set streamrate %u\n" % sr)
|
||||
|
||||
except Exception as e:
|
||||
# tell MAVProxy to start stuffing around with the rates:
|
||||
sr = self.sitl_streamrate()
|
||||
self.mavproxy.send("set streamrate %u\n" % sr)
|
||||
raise e
|
||||
|
||||
def test_gripper(self):
|
||||
self.context_push()
|
||||
self.set_parameter("GRIP_ENABLE", 1)
|
||||
|
|
|
|||
Loading…
Reference in New Issue