diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index e98cabb5b7..fb02727757 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -9,9 +9,10 @@ import time import pexpect from pymavlink import mavutil +from pymavlink import mavextra +from pymavlink import quaternion from pysim import util - from common import AutoTest from common import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException @@ -1702,6 +1703,40 @@ class AutoTestCopter(AutoTest): if ex is not None: raise ex + def fly_guided_move_relative(self, lat, lon, alt): + startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT', + blocking=True) + + tstart = self.get_sim_time() + while True: + if self.get_sim_time() - tstart > 200: + raise NotAchievedException("Did not move far enough") + # send a position-control command + self.mav.mav.set_position_target_global_int_send( + 0, # timestamp + 1, # target system_id + 1, # target component id + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + 0b1111111111111000, # mask specifying use-only-lat-lon-alt + lat, # lat + lon, # lon + alt, # alt + 0, # vx + 0, # vy + 0, # vz + 0, # afx + 0, # afy + 0, # afz + 0, # yaw + 0, # yawrate + ) + pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', + blocking=True) + delta = self.get_distance_int(startpos, pos) + self.progress("delta=%f (want >10)" % delta) + if delta > 10: + break + def fly_guided_change_submode(self): """"Ensure we can move around in guided after a takeoff command.""" @@ -1720,43 +1755,12 @@ class AutoTestCopter(AutoTest): self.user_takeoff(alt_min=10) - startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT', - blocking=True) - """yaw through absolute angles using MAV_CMD_CONDITION_YAW""" self.guided_achieve_heading(45) self.guided_achieve_heading(135) """move the vehicle using set_position_target_global_int""" - tstart = self.get_sim_time() - while True: - if self.get_sim_time() - tstart > 200: - raise NotAchievedException("Did not move far enough") - # send a position-control command - self.mav.mav.set_position_target_global_int_send( - 0, # timestamp - 1, # target system_id - 1, # target component id - mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, - 0b1111111111111000, # mask specifying use-only-lat-lon-alt - 5, # lat - 5, # lon - 10, # alt - 0, # vx - 0, # vy - 0, # vz - 0, # afx - 0, # afy - 0, # afz - 0, # yaw - 0, # yawrate - ) - pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', - blocking=True) - delta = self.get_distance_int(startpos, pos) - self.progress("delta=%f (want >10)" % delta) - if delta > 10: - break + self.fly_guided_move_relative(5, 5, 10) self.progress("Landing") self.mavproxy.send('switch 2\n') # land mode @@ -1796,6 +1800,325 @@ class AutoTestCopter(AutoTest): if ex is not None: raise ex + def test_mount_pitch(self, despitch, despitch_tolerance, timeout=5): + tstart = self.get_sim_time() + while True: + if self.get_sim_time() - tstart > timeout: + raise NotAchievedException() + + m = self.mav.recv_match(type='MOUNT_STATUS', + blocking=True, + timeout=5) +# self.progress("pitch=%f roll=%f yaw=%f" % +# (m.pointing_a, m.pointing_b, m.pointing_c)) + mount_pitch = m.pointing_a/100.0 # centidegrees to degrees + if abs(despitch - mount_pitch) > despitch_tolerance: + self.progress("Mount pitch incorrect: %f != %f" % + (mount_pitch, despitch)) + continue + self.progress("Mount pitch correct: %f degrees == %f" % + (mount_pitch, despitch)) + return + + def do_pitch(self, pitch): + '''pitch aircraft in guided/angle mode''' + self.mav.mav.set_attitude_target_send( + 0, # time_boot_ms + 1, # target sysid + 1, # target compid + 0, # bitmask of things to ignore + mavextra.euler_to_quat([0, math.radians(pitch), 0]), # att + 0, # roll rate (rad/s) + 1, # pitch rate + 0, # yaw rate + 0.5) # thrust, 0 to 1, translated to a climb/descent rate + + def test_mount(self): + ex = None + self.context_push() + try: + '''start by disabling GCS failsafe, otherwise we immediately disarm + due to (apparently) not receiving traffic from the GCS for + too long. This is probably a function of --speedup''' + self.set_parameter("FS_GCS_ENABLE", 0) + + self.progress("Setting up servo mount") + roll_servo = 5 + pitch_servo = 6 + yaw_servo = 7 + self.set_parameter("MNT_TYPE", 1) + self.set_parameter("SERVO%u_FUNCTION" % roll_servo, 8) # roll + self.set_parameter("SERVO%u_FUNCTION" % pitch_servo, 7) # pitch + self.set_parameter("SERVO%u_FUNCTION" % yaw_servo, 6) # yaw + self.reboot_sitl() # to handle MNT_TYPE changing + + # make sure we're getting mount status and gimbal reports + self.mav.recv_match(type='MOUNT_STATUS', + blocking=True, + timeout=5) + self.mav.recv_match(type='GIMBAL_REPORT', + blocking=True, + timeout=5) + + # test pitch isn't stabilising: + m = self.mav.recv_match(type='MOUNT_STATUS', + blocking=True, + timeout=5) + if m.pointing_a != 0 or m.pointing_b != 0 or m.pointing_c != 0: + self.progress("Stabilising when not requested") + raise NotAchievedException() + + self.mavproxy.send('mode guided\n') + self.wait_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + + self.user_takeoff() + + despitch = 10 + despitch_tolerance = 3 + + self.progress("Pitching vehicle") + self.do_pitch(despitch) # will time out! + + self.wait_pitch(despitch, despitch_tolerance) + + # check we haven't modified: + m = self.mav.recv_match(type='MOUNT_STATUS', + blocking=True, + timeout=5) + if m.pointing_a != 0 or m.pointing_b != 0 or m.pointing_c != 0: + self.progress("Stabilising when not requested") + raise NotAchievedException() + + self.progress("Enable pitch stabilization using MOUNT_CONFIGURE") + self.do_pitch(despitch) + self.mav.mav.mount_configure_send( + 1, # target system + 1, # target component + mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, + 0, # stab-roll + 1, # stab-pitch + 0) + + self.test_mount_pitch(-despitch, 1) + + self.progress("Disable pitch using MAV_CMD_DO_MOUNT_CONFIGURE") + self.do_pitch(despitch) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE, + mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, + 0, + 0, + 0, + 0, + 0, + 0, + ) + self.test_mount_pitch(0, 0) + + self.progress("Point somewhere using MOUNT_CONTROL (ANGLE)") + self.do_pitch(despitch) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE, + mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, + 0, + 0, + 0, + 0, + 0, + 0, + ) + self.mav.mav.mount_control_send( + 1, # target system + 1, # target component + 20 *100, # pitch + 20 *100, # roll (centidegrees) + 0, # yaw + 0 # save position + ) + self.test_mount_pitch(20, 1) + + self.progress("Point somewhere using MOUNT_CONTROL (GPS)") + self.do_pitch(despitch) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE, + mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT, + 0, + 0, + 0, + 0, + 0, + 0, + ) + start = self.mav.location() + self.progress("start=%s" % str(start)) + (t_lat, t_lon) = mavextra.gps_offset(start.lat, start.lng, 10, 20) + t_alt = 0 + + self.progress("loc %f %f %f" % (start.lat, start.lng, start.alt)) + self.progress("targetting %f %f %f" % (t_lat, t_lon, t_alt)) + self.do_pitch(despitch) + self.mav.mav.mount_control_send( + 1, # target system + 1, # target component + t_lat * 1e7, # lat + t_lon * 1e7, # lon + t_alt * 100, # alt + 0 # save position + ) + self.test_mount_pitch(-52, 5) + + # now test RC targetting + self.progress("Testing mount RC targetting") + + # this is a one-off; ArduCopter *will* time out this directive! + self.progress("Levelling aircraft") + self.mav.mav.set_attitude_target_send( + 0, # time_boot_ms + 1, # target sysid + 1, # target compid + 0, # bitmask of things to ignore + mavextra.euler_to_quat([0, 0, 0]), # att + 1, # roll rate (rad/s) + 1, # pitch rate + 1, # yaw rate + 0.5) # thrust, 0 to 1, translated to a climb/descent rate + + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE, + mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, + 0, + 0, + 0, + 0, + 0, + 0, + ) + try: + self.context_push() + self.set_parameter('MNT_RC_IN_ROLL', 11) + self.set_parameter('MNT_RC_IN_TILT', 12) + self.set_parameter('MNT_RC_IN_PAN', 13) + self.progress("Testing RC angular control") + self.set_rc(11, 1500) + self.set_rc(12, 1500) + self.set_rc(13, 1500) + self.test_mount_pitch(0, 1) + self.set_rc(12, 1400) + self.test_mount_pitch(-11.25, 0.01) + self.set_rc(12, 1800) + self.test_mount_pitch(33.75, 0.01) + self.set_rc(11, 1500) + self.set_rc(12, 1500) + self.set_rc(13, 1500) + + self.progress("Testing RC rate control") + self.set_parameter('MNT_JSTICK_SPD', 10) + self.test_mount_pitch(0, 1) + self.set_rc(12, 1300) + self.test_mount_pitch(-5, 1) + self.test_mount_pitch(-10, 1) + self.test_mount_pitch(-15, 1) + self.test_mount_pitch(-20, 1) + self.set_rc(12, 1700) + self.test_mount_pitch(-15, 1) + self.test_mount_pitch(-10, 1) + self.test_mount_pitch(-5, 1) + self.test_mount_pitch(0, 1) + self.test_mount_pitch(5, 1) + + self.progress("Reverting to angle mode") + self.set_parameter('MNT_JSTICK_SPD', 0) + self.set_rc(12, 1500) + self.test_mount_pitch(0, 0.1) + + self.context_pop() + + except Exception: + self.context_pop() + raise + + self.progress("Testing mount ROI behaviour") + self.test_mount_pitch(0, 0.1) + start = self.mav.location() + self.progress("start=%s" % str(start)) + (roi_lat, roi_lon) = mavextra.gps_offset(start.lat, + start.lng, + 10, + 20) + roi_alt = 0 + self.progress("Using MAV_CMD_DO_SET_ROI_LOCATION") + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, + 0, + 0, + 0, + 0, + roi_lat, + roi_lon, + roi_alt, + ) + self.test_mount_pitch(-52, 5) + + start = self.mav.location() + (roi_lat, roi_lon) = mavextra.gps_offset(start.lat, + start.lng, + -100, + -200) + roi_alt = 0 + self.progress("Using MAV_CMD_DO_SET_ROI") + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI, + 0, + 0, + 0, + 0, + roi_lat, + roi_lon, + roi_alt, + ) + self.test_mount_pitch(-7.5, 1) + + self.progress("checking ArduCopter yaw-aircraft-for-roi") + try: + self.context_push() + + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + self.progress("current heading %u" % m.heading) + self.set_parameter("SERVO%u_FUNCTION" % yaw_servo, 0) # yaw + self.progress("Waiting for check_servo_map to do its job") + self.wait_seconds(5) + start = self.mav.location() + self.progress("Moving to guided/position controller") + self.fly_guided_move_relative(0, 0, 0) + self.guided_achieve_heading(0) + (roi_lat, roi_lon) = mavextra.gps_offset(start.lat, + start.lng, + -100, + -200) + roi_alt = 0 + self.progress("Using MAV_CMD_DO_SET_ROI") + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI, + 0, + 0, + 0, + 0, + roi_lat, + roi_lon, + roi_alt, + ) + + self.wait_heading(110, timeout=600) + + self.context_pop() + except Exception: + self.context_pop() + raise + + except Exception as e: + ex = e + self.context_pop() + + self.reboot_sitl() # to handle MNT_TYPE changing + + if ex is not None: + raise ex + def autotest(self): """Autotest ArduCopter in SITL.""" self.check_test_syntax(test_file=os.path.realpath(__file__)) @@ -2016,6 +2339,9 @@ class AutoTestCopter(AutoTest): '''vision position''' # expects vehicle to be disarmed self.run_test("Fly Vision Position", self.fly_vision_position) + '''tests for camera/antenna mount''' + self.run_test("Test Mount", self.test_mount) + # Download logs self.run_test("log download", lambda: self.log_download(