def test_rc_relay(self):
'''test toggling channel 12 toggles relay'''
self.set_parameter("RC12_OPTION", 28) # Relay On/Off
self.set_rc(12, 1000)
self.reboot_sitl() # needed for RC12_OPTION to take effect
off = self.get_parameter("SIM_PIN_MASK")
if off:
raise PreconditionFailedException("SIM_MASK_PIN off")
# allow time for the RC library to register initial value:
self.delay_sim_time(1)
self.set_rc(12, 2000)
self.wait_heartbeat()
self.wait_heartbeat()
on = self.get_parameter("SIM_PIN_MASK")
if not on:
raise NotAchievedException("SIM_PIN_MASK doesn't reflect ON")
self.set_rc(12, 1000)
self.wait_heartbeat()
self.wait_heartbeat()
off = self.get_parameter("SIM_PIN_MASK")
if off:
raise NotAchievedException("SIM_PIN_MASK doesn't reflect OFF")
def test_offboard(self, timeout=90):
self.load_mission("rover-guided-mission.txt")
self.wait_ready_to_arm(require_absolute=True)
self.arm_vehicle()
self.change_mode("AUTO")
offboard_expected_duration = 10 # see mission file
if self.mav.messages.get("SET_POSITION_TARGET_GLOBAL_INT", None):
raise PreconditionFailedException(
"Already have SET_POSITION_TARGET_GLOBAL_INT")
tstart = self.get_sim_time_cached()
last_heartbeat_sent = 0
got_sptgi = False
magic_waypoint_tstart = 0
magic_waypoint_tstop = 0
while True:
if self.mode_is("HOLD", cached=True):
break
now = self.get_sim_time_cached()
if now - last_heartbeat_sent > 1:
last_heartbeat_sent = now
self.mav.mav.heartbeat_send(
mavutil.mavlink.MAV_TYPE_ONBOARD_CONTROLLER,
mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)
if now - tstart > timeout:
raise AutoTestTimeoutException("Didn't complete")
magic_waypoint = 3
# mc = self.mav.messages.get("MISSION_CURRENT", None)
mc = self.mav.recv_match(type="MISSION_CURRENT", blocking=False)
if mc is not None:
print("%s" % str(mc))
if mc.seq == magic_waypoint:
print("At magic waypoint")
if magic_waypoint_tstart == 0:
magic_waypoint_tstart = self.get_sim_time_cached()
sptgi = self.mav.messages.get(
"SET_POSITION_TARGET_GLOBAL_INT", None)
if sptgi is not None:
got_sptgi = True
elif mc.seq > magic_waypoint:
if magic_waypoint_tstop == 0:
magic_waypoint_tstop = self.get_sim_time_cached()
self.disarm_vehicle()
offboard_duration = magic_waypoint_tstop - magic_waypoint_tstart
if abs(offboard_duration - offboard_expected_duration) > 1:
raise NotAchievedException(
"Did not stay in offboard control for correct time (want=%f got=%f)"
% (offboard_expected_duration, offboard_duration))
if not got_sptgi:
raise NotAchievedException("Did not get sptgi message")
print("spgti: %s" % str(sptgi))
def drive_rtl_mission(self):
self.wait_ready_to_arm()
self.arm_vehicle()
mission_filepath = os.path.join("ArduRover-Missions", "rtl.txt")
self.load_mission(mission_filepath)
self.change_mode("AUTO")
self.mavproxy.expect('Executing RTL')
self.drain_mav()
m = self.mav.recv_match(type='NAV_CONTROLLER_OUTPUT',
blocking=True,
timeout=1)
if m is None:
raise MsgRcvTimeoutException(
"Did not receive NAV_CONTROLLER_OUTPUT message")
wp_dist_min = 5
if m.wp_dist < wp_dist_min:
raise PreconditionFailedException(
"Did not start at least %f metres from destination (is=%f)" %
(wp_dist_min, m.wp_dist))
self.progress("NAV_CONTROLLER_OUTPUT.wp_dist looks good (%u >= %u)" % (
m.wp_dist,
wp_dist_min,
))
tstart = self.get_sim_time()
while True:
if self.get_sim_time_cached() - tstart > 600:
raise NotAchievedException("Did not get home")
self.progress("Distance home: %f (mode=%s)" %
(self.distance_to_home(), self.mav.flightmode))
if self.mode_is('HOLD') or self.mode_is(
'LOITER'): # loiter for balancebot
break
# the EKF doesn't pull us down to 0 speed:
self.wait_groundspeed(0, 0.5, timeout=600)
# current Rover blows straight past the home position and ends
# up ~6m past the home point.
home_distance = self.distance_to_home()
home_distance_min = 5.5
home_distance_max = self.drive_rtl_mission_max_distance_from_home()
if home_distance > home_distance_max:
raise NotAchievedException(
"Did not stop near home (%f metres distant (%f > want > %f))" %
(home_distance, home_distance_min, home_distance_max))
self.disarm_vehicle()
self.progress("RTL Mission OK (%fm)" % home_distance)
def test_rc_option_camera_trigger(self):
'''test toggling channel 12 takes picture'''
x = self.mav.messages.get("CAMERA_FEEDBACK", None)
if x is not None:
raise PreconditionFailedException("Receiving CAMERA_FEEDBACK?!")
self.set_rc(12, 2000)
tstart = self.get_sim_time()
while self.get_sim_time() - tstart < 10:
x = self.mav.messages.get("CAMERA_FEEDBACK", None)
if x is not None:
break
self.mav.wait_heartbeat()
self.set_rc(12, 1000)
if x is None:
raise NotAchievedException("No CAMERA_FEEDBACK message received")
def test_rc_relay(self):
'''test toggling channel 12 toggles relay'''
off = self.get_parameter("SIM_PIN_MASK")
if off:
raise PreconditionFailedException("SIM_MASK_PIN off")
self.set_rc(12, 2000)
self.mav.wait_heartbeat()
self.mav.wait_heartbeat()
on = self.get_parameter("SIM_PIN_MASK")
if not on:
raise NotAchievedException("SIM_PIN_MASK doesn't reflect ON")
self.set_rc(12, 1000)
self.mav.wait_heartbeat()
self.mav.wait_heartbeat()
off = self.get_parameter("SIM_PIN_MASK")
if off:
raise NotAchievedException("SIM_PIN_MASK doesn't reflect OFF")
def test_rc_option_camera_trigger(self):
'''test toggling channel 12 takes picture'''
self.set_parameter("RC12_OPTION", 9) # CameraTrigger
self.reboot_sitl() # needed for RC12_OPTION to take effect
x = self.mav.messages.get("CAMERA_FEEDBACK", None)
if x is not None:
raise PreconditionFailedException("Receiving CAMERA_FEEDBACK?!")
self.set_rc(12, 2000)
tstart = self.get_sim_time()
while self.get_sim_time_cached() - tstart < 10:
x = self.mav.messages.get("CAMERA_FEEDBACK", None)
if x is not None:
break
self.wait_heartbeat()
self.set_rc(12, 1000)
if x is None:
raise NotAchievedException("No CAMERA_FEEDBACK message received")
def drive_rtl_mission(self):
self.wait_ready_to_arm()
self.arm_vehicle()
mission_filepath = os.path.join("ArduRover-Missions", "rtl.txt")
self.load_mission(mission_filepath)
self.mavproxy.send('switch 4\n') # auto mode
self.set_rc(3, 1500)
self.wait_mode('AUTO')
self.mavproxy.expect('Executing RTL')
m = self.mav.recv_match(type='NAV_CONTROLLER_OUTPUT',
blocking=True,
timeout=0.1)
if m is None:
raise MsgRcvTimeoutException(
"Did not receive NAV_CONTROLLER_OUTPUT message")
wp_dist_min = 5
if m.wp_dist < wp_dist_min:
raise PreconditionFailedException(
"Did not start at least %u metres from destination" %
(wp_dist_min))
self.progress("NAV_CONTROLLER_OUTPUT.wp_dist looks good (%u >= %u)" % (
m.wp_dist,
wp_dist_min,
))
self.wait_mode('HOLD', timeout=600) # balancebot can take a long time!
pos = self.mav.location()
home_distance = self.get_distance(HOME, pos)
home_distance_max = 5
if home_distance > home_distance_max:
raise NotAchievedException(
"Did not get home (%u metres distant > %u)" %
(home_distance, home_distance_max))
self.mavproxy.send('switch 6\n')
self.wait_mode('MANUAL')
self.disarm_vehicle()
self.progress("RTL Mission OK")
def drive_rtl_mission(self):
mission_filepath = os.path.join(testdir, "ArduRover-Missions",
"rtl.txt")
self.mavproxy.send('wp load %s\n' % mission_filepath)
self.mavproxy.expect('Flight plan received')
self.mavproxy.send('switch 4\n') # auto mode
self.set_rc(3, 1500)
self.wait_mode('AUTO')
self.mavproxy.expect('Executing RTL')
m = self.mav.recv_match(type='NAV_CONTROLLER_OUTPUT',
blocking=True,
timeout=0.1)
if m is None:
self.progress("Did not receive NAV_CONTROLLER_OUTPUT message")
raise MsgRcvTimeoutException()
wp_dist_min = 5
if m.wp_dist < wp_dist_min:
self.progress("Did not start at least 5 metres from destination")
raise PreconditionFailedException()
self.progress("NAV_CONTROLLER_OUTPUT.wp_dist looks good (%u >= %u)" % (
m.wp_dist,
wp_dist_min,
))
self.wait_mode('HOLD')
pos = self.mav.location()
home_distance = self.get_distance(HOME, pos)
home_distance_max = 5
if home_distance > home_distance_max:
self.progress("Did not get home (%u metres distant > %u)" %
(home_distance, home_distance_max))
raise NotAchievedException()
self.mavproxy.send('switch 6\n')
self.wait_mode('MANUAL')
self.progress("RTL Mission OK")
def test_throttle_failsafe(self):
self.change_mode('MANUAL')
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
receiver_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_RC_RECEIVER
self.progress("Testing receiver enabled")
if (not (m.onboard_control_sensors_enabled & receiver_bit)):
raise PreconditionFailedException()
self.progress("Testing receiver present")
if (not (m.onboard_control_sensors_present & receiver_bit)):
raise PreconditionFailedException()
self.progress("Testing receiver health")
if (not (m.onboard_control_sensors_health & receiver_bit)):
raise PreconditionFailedException()
self.progress("Ensure we know original throttle value")
self.wait_rc_channel_value(3, 1000)
self.set_parameter("THR_FS_VALUE", 960)
self.progress("Failing receiver (throttle-to-950)")
self.set_parameter("SIM_RC_FAIL", 2) # throttle-to-950
self.wait_mode('CIRCLE') # short failsafe
self.wait_mode('RTL') # long failsafe
self.progress("Ensure we've had our throttle squashed to 950")
self.wait_rc_channel_value(3, 950)
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
print("%s" % str(m))
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
print("%s" % str(m))
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
print("%s" % str(m))
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
print("%s" % str(m))
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
print("%s" % str(m))
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
print("%s" % str(m))
self.progress("Testing receiver enabled")
if (not (m.onboard_control_sensors_enabled & receiver_bit)):
raise NotAchievedException("Receiver not enabled")
self.progress("Testing receiver present")
if (not (m.onboard_control_sensors_present & receiver_bit)):
raise NotAchievedException("Receiver not present")
# skip this until RC is fixed
# self.progress("Testing receiver health")
# if (m.onboard_control_sensors_health & receiver_bit):
# raise NotAchievedException("Sensor healthy when it shouldn't be")
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)
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
self.progress("Testing receiver enabled")
if (not (m.onboard_control_sensors_enabled & receiver_bit)):
raise NotAchievedException("Receiver not enabled")
self.progress("Testing receiver present")
if (not (m.onboard_control_sensors_present & receiver_bit)):
raise NotAchievedException("Receiver not present")
self.progress("Testing receiver health")
if (not (m.onboard_control_sensors_health & receiver_bit)):
raise NotAchievedException("Receiver not healthy")
self.change_mode('MANUAL')
self.progress("Failing receiver (no-pulses)")
self.set_parameter("SIM_RC_FAIL", 1) # no-pulses
self.wait_mode('CIRCLE') # short failsafe
self.wait_mode('RTL') # long failsafe
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
print("%s" % str(m))
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
print("%s" % str(m))
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
print("%s" % str(m))
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
print("%s" % str(m))
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
print("%s" % str(m))
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
print("%s" % str(m))
self.progress("Testing receiver enabled")
if (not (m.onboard_control_sensors_enabled & receiver_bit)):
raise NotAchievedException("Receiver not enabled")
self.progress("Testing receiver present")
if (not (m.onboard_control_sensors_present & receiver_bit)):
raise NotAchievedException("Receiver not present")
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)
self.progress("Giving receiver time to recover")
for i in range(1, 10):
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
self.progress("Testing receiver enabled")
if (not (m.onboard_control_sensors_enabled & receiver_bit)):
raise NotAchievedException("Receiver not enabled")
self.progress("Testing receiver present")
if (not (m.onboard_control_sensors_present & receiver_bit)):
raise NotAchievedException("Receiver not present")
self.progress("Testing receiver health")
if (not (m.onboard_control_sensors_health & receiver_bit)):
raise NotAchievedException("Receiver not healthy")
self.change_mode('MANUAL')
def test_airmode(self):
"""Check that plane.air_mode turns on and off as required"""
self.progress("########## Testing AirMode operation")
self.set_parameter("AHRS_EKF_TYPE", 10)
self.change_mode('QSTABILIZE')
self.wait_ready_to_arm()
"""
SPIN_ARM and SPIN_MIN default to 0.10 and 0.15
when armed with zero throttle in AirMode, motor PWM should be at SPIN_MIN
If AirMode is off, motor PWM will drop to SPIN_ARM
"""
self.progress("Verify that SERVO5 is Motor1 (default)")
motor1_servo_function_lp = 33
if (self.get_parameter('SERVO5_FUNCTION') != motor1_servo_function_lp):
raise PreconditionFailedException("SERVO5_FUNCTION not %d" %
motor1_servo_function_lp)
self.progress("Verify that flightmode channel is 5 (default)")
default_fltmode_ch = 5
if (self.get_parameter("FLTMODE_CH") != default_fltmode_ch):
raise PreconditionFailedException("FLTMODE_CH not %d" %
default_fltmode_ch)
"""When disarmed, motor PWM will drop to min_pwm"""
min_pwm = self.get_parameter("Q_THR_MIN_PWM")
self.progress("Verify Motor1 is at min_pwm when disarmed")
self.wait_servo_channel_value(5, min_pwm, comparator=operator.eq)
"""set Q_OPTIONS bit AIRMODE"""
airmode_option_bit = (1 << 9)
self.set_parameter("Q_OPTIONS", airmode_option_bit)
armdisarm_option = 41
arm_ch = 8
self.set_parameter("RC%d_OPTION" % arm_ch, armdisarm_option)
self.progress("Configured RC%d as ARMDISARM switch" % arm_ch)
"""arm with GCS, record Motor1 SPIN_ARM PWM output and disarm"""
spool_delay = self.get_parameter("Q_M_SPOOL_TIME") + 0.25
self.zero_throttle()
self.arm_vehicle()
self.progress("Waiting for Motor1 to spool up to SPIN_ARM")
self.delay_sim_time(spool_delay)
spin_arm_pwm = self.wait_servo_channel_value(5,
min_pwm,
comparator=operator.gt)
self.progress("spin_arm_pwm: %d" % spin_arm_pwm)
self.disarm_vehicle()
"""arm with switch, record Motor1 SPIN_MIN PWM output and disarm"""
self.set_rc(8, 2000)
self.delay_sim_time(spool_delay)
self.progress("Waiting for Motor1 to spool up to SPIN_MIN")
spin_min_pwm = self.wait_servo_channel_value(5,
spin_arm_pwm,
comparator=operator.gt)
self.progress("spin_min_pwm: %d" % spin_min_pwm)
self.set_rc(8, 1000)
if (spin_arm_pwm >= spin_min_pwm):
raise PreconditionFailedException(
"SPIN_MIN pwm not greater than SPIN_ARM pwm")
self.start_subtest("Test auxswitch arming with Q_OPTIONS=AirMode")
for mode in ('QSTABILIZE', 'QACRO'):
"""verify that arming with switch results in higher PWM output"""
self.progress("Testing %s mode" % mode)
self.change_mode(mode)
self.zero_throttle()
self.progress("Arming with switch at zero throttle")
self.arm_motors_with_switch(arm_ch)
self.progress("Waiting for Motor1 to speed up")
self.wait_servo_channel_value(5,
spin_min_pwm,
comparator=operator.ge)
self.progress("Verify that rudder disarm is disabled")
if self.disarm_motors_with_rc_input():
raise NotAchievedException("Rudder disarm not disabled")
self.progress("Disarming with switch")
self.disarm_motors_with_switch(arm_ch)
self.progress("Waiting for Motor1 to stop")
self.wait_servo_channel_value(5, min_pwm, comparator=operator.le)
self.wait_ready_to_arm()
self.start_subtest(
"Verify that arming with switch does not spin motors in other modes"
)
# introduce a large attitude error to verify that stabilization is not active
ahrs_trim_x = self.get_parameter("AHRS_TRIM_X")
self.set_parameter("AHRS_TRIM_X", math.radians(-60))
self.wait_roll(60, 1)
# test all modes except QSTABILIZE, QACRO, AUTO and QAUTOTUNE
for mode in (
'ACRO',
'AUTOTUNE',
'AVOID_ADSB',
'CIRCLE',
'CRUISE',
'FBWA',
'FBWB',
'GUIDED',
'LOITER',
'QHOVER',
'QLAND',
'QLOITER',
'QRTL',
'RTL',
'STABILIZE',
'TRAINING',
):
self.progress("Testing %s mode" % mode)
self.change_mode(mode)
self.zero_throttle()
self.progress("Arming with switch at zero throttle")
self.arm_motors_with_switch(arm_ch)
self.progress("Waiting for Motor1 to (not) speed up")
self.delay_sim_time(spool_delay)
self.wait_servo_channel_value(5,
spin_arm_pwm,
comparator=operator.le)
self.wait_servo_channel_value(6,
spin_arm_pwm,
comparator=operator.le)
self.wait_servo_channel_value(7,
spin_arm_pwm,
comparator=operator.le)
self.wait_servo_channel_value(8,
spin_arm_pwm,
comparator=operator.le)
self.progress("Disarming with switch")
self.disarm_motors_with_switch(arm_ch)
self.progress("Waiting for Motor1 to stop")
self.wait_servo_channel_value(5, min_pwm, comparator=operator.le)
self.wait_ready_to_arm()
# remove attitude error
self.set_parameter("AHRS_TRIM_X", ahrs_trim_x)
self.start_subtest(
"verify that AIRMODE auxswitch turns airmode on/off while armed")
"""set RC7_OPTION to AIRMODE"""
option_airmode = 84
self.set_parameter("RC7_OPTION", option_airmode)
for mode in ('QSTABILIZE', 'QACRO'):
self.progress("Testing %s mode" % mode)
self.change_mode(mode)
self.zero_throttle()
self.progress("Arming with GCS at zero throttle")
self.arm_vehicle()
self.progress("Turn airmode on with auxswitch")
self.set_rc(7, 2000)
self.progress("Waiting for Motor1 to speed up")
self.wait_servo_channel_value(5,
spin_min_pwm,
comparator=operator.ge)
self.progress("Turn airmode off with auxswitch")
self.set_rc(7, 1000)
self.progress("Waiting for Motor1 to slow down")
self.wait_servo_channel_value(5,
spin_arm_pwm,
comparator=operator.le)
self.disarm_vehicle()
self.wait_ready_to_arm()
self.start_subtest("Test GCS arming")
for mode in ('QSTABILIZE', 'QACRO'):
self.progress("Testing %s mode" % mode)
self.change_mode(mode)
self.zero_throttle()
self.progress("Arming with GCS at zero throttle")
self.arm_vehicle()
self.progress("Turn airmode on with auxswitch")
self.set_rc(7, 2000)
self.progress("Waiting for Motor1 to speed up")
self.wait_servo_channel_value(5,
spin_min_pwm,
comparator=operator.ge)
self.progress("Disarm/rearm with GCS")
self.disarm_vehicle()
self.arm_vehicle()
self.progress("Verify that airmode is still on")
self.wait_servo_channel_value(5,
spin_min_pwm,
comparator=operator.ge)
self.disarm_vehicle()
self.wait_ready_to_arm()
