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()