diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index fc0659c8dfc1dc..b3b71c23d5a7e1 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -4887,6 +4887,62 @@ def ManualThrottleModeChange(self): self.run_cmd_do_set_mode("ACRO") self.wait_disarmed() + def MountWPNextOffsetMission(self, target_system=1, target_component=1): + '''check the MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET mission item''' + # setup mount parameters + self.setup_servo_mount() + self.reboot_sitl() # to handle MNT_TYPE changing + + alt = 50 + + loc = self.mav.location() + + items = [ + self.mission_item_home(), + self.mission_item_copter_takeoff(alt=50), + ] + + # third waypoint + loc = self.offset_location_ne(loc, 100, 0) + items.append(self.mission_item_do_cmd_roi_set_wpnext_offset(p=0)) + items.append(self.mission_item_waypoint(loc.lat, loc.lng, alt)) + + # third waypoint + loc = self.offset_location_ne(loc, 100, 0) + items.append(self.mission_item_do_cmd_roi_set_wpnext_offset(p=-45)) + items.append(self.mission_item_waypoint(loc.lat, loc.lng, alt)) + + # fourth waypoint (offset from second) + loc = self.offset_location_ne(loc, 100, 0) + items.append(self.mission_item_do_cmd_roi_set_wpnext_offset(y=45)) + items.append(self.mission_item_waypoint(loc.lat, loc.lng, alt)) + + # now RTL + items.append(self.mission_item_rtl()) + + self.renumber_mission_items(items) + self.check_mission_upload_download(items) + + self.set_parameter('AUTO_OPTIONS', 3) + self.wait_ready_to_arm() + self.arm_vehicle() + self.change_mode('AUTO') + + self.wait_current_waypoint(3) + # the 30 here is because the vehicle is pitching down and the + # gimbal is compensating + self.wait_mount_roll_pitch_yaw_deg(r=0, p=30, y=0) + + self.wait_current_waypoint(5) + # the -15 here is because the vehicle is pitching down and the + # gimbal is compensating, and is being asked to pitch down 45 degrees + self.wait_mount_roll_pitch_yaw_deg(r=0, p=-15, y=0) + + self.wait_current_waypoint(7) + self.wait_mount_roll_pitch_yaw_deg(y=45) + + self.wait_disarmed() + def test_mount_pitch(self, despitch, despitch_tolerance, mount_mode, timeout=10, hold=0): tstart = self.get_sim_time() success_start = 0 @@ -4949,11 +5005,33 @@ def setup_servo_mount(self, roll_servo=5, pitch_servo=6, yaw_servo=7): "SERVO%u_FUNCTION" % yaw_servo: 6, # yaw }) + # FIXME: change this to use wait_and_maintain + def wait_mount_roll_pitch_yaw_deg(self, r=None, p=None, y=None, timeout=20): + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > timeout: + raise NotAchievedException("Did not get rpy") + got_r, got_p, got_y = self.get_mount_roll_pitch_yaw_deg() + self.progress("want=(%s, %s, %s)" % (r, p , y)) + self.progress("got=(%s, %s, %s)" % (got_r, got_p , got_y)) + passed = True + for (want, got) in [(r, got_r), (p, got_p), (y, got_y)]: + if want is None: + continue + if abs(want - got) > 5: + passed = False + break + if passed: + self.progress("achieved mount rpy") + break + def get_mount_roll_pitch_yaw_deg(self): - '''return mount (aka gimbal) roll, pitch and yaw angles in degrees''' + '''return mount (aka gimbal) roll, pitch and yaw angles in degrees - in body frame''' # wait for gimbal attitude message m = self.assert_receive_message('GIMBAL_DEVICE_ATTITUDE_STATUS', timeout=5) + return self.eulers_in_degrees_from_GIMBAL_DEVICE_ATTITUDE_STATUS(m) + def eulers_in_degrees_from_GIMBAL_DEVICE_ATTITUDE_STATUS(self, m): # convert quaternion to euler angles and return q = quaternion.Quaternion(m.q) euler = q.euler @@ -5065,6 +5143,7 @@ def test_mount_rc_targetting(self): def Mount(self): '''Test Camera/Antenna Mount''' + ex = None self.context_push() old_srcSystem = self.mav.mav.srcSystem @@ -5328,6 +5407,9 @@ def Mount(self): self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL) self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL) + self.start_subtest("Testing MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET") + self.test_mount_mav_cmd_do_set_roi_wpnext_offset_mission() + except Exception as e: self.print_exception_caught(e) ex = e @@ -9975,6 +10057,7 @@ def tests1e(self): self.MountYawVehicleForMountROI, self.MAV_CMD_DO_MOUNT_CONTROL, self.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, + self.MountWPNextOffsetMission, self.Button, self.ShipTakeoff, self.RangeFinder, diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 90519e6d627f3f..d9331f42dfd8ec 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -3764,6 +3764,107 @@ def create_simple_relhome_mission(self, items_in, target_system=1, target_compon return ret + def renumber_mission_items(self, items): + '''make item's seq sequential starting from zero''' + count = 0 + for item in items: + item.seq = count + count += 1 + + def mission_item_copter_takeoff(self, alt=30, target_system=1, target_component=1): + '''returns a mission_item_int which can be used as takeoff in a mission''' + return self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 1, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + 0, # current + 0, # autocontinue + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0000 * 1e7), # latitude + int(1.0000 * 1e7), # longitude + alt, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_MISSION) + + def mission_item_rtl(self, target_system=1, target_component=1): + '''returns a mission_item_int which can be used as RTL in a mission''' + return self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 1, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL, + mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, + 0, # current + 0, # autocontinue + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + 0, # latitude + 0, # longitude + 0.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_MISSION) + + def mission_item_do_cmd_roi_set_wpnext_offset(self, r=0, p=0, y=0, target_system=1, target_component=1): + return self.mav.mav.mission_item_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL, + mavutil.mavlink.MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET, + 0, # current + 0, # autocontinue + 0, # param1 + 0, # param2 + 0, # param3 + 0, # param4 + r, # param5 + p, # param6 + y # param7 + ) + + def mission_item_waypoint(self, lat, lng, alt, target_system=1, target_component=1): + '''returns a mission_item_int which can be used as waypoint in a mission''' + return self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, + 0, # current + 0, # autocontinue + 0, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(lat*1e7), # latitude + int(lng*1e7), # longitude + alt, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_MISSION) + + def mission_item_home(self, target_system=1, target_component=1): + '''returns a mission_item_int which can be used as home in a mission''' + return self.mav.mav.mission_item_int_encode( + target_system, + target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_INT, + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, + 0, # current + 0, # autocontinue + 3, # p1 + 0, # p2 + 0, # p3 + 0, # p4 + int(1.0000 * 1e7), # latitude + int(2.0000 * 1e7), # longitude + 31.0000, # altitude + mavutil.mavlink.MAV_MISSION_TYPE_MISSION) + def upload_simple_relhome_mission(self, items, target_system=1, target_component=1): mission = self.create_simple_relhome_mission( items, diff --git a/Tools/autotest/helicopter.py b/Tools/autotest/helicopter.py index 0dddcea057b831..8c2fe19dc9ae78 100644 --- a/Tools/autotest/helicopter.py +++ b/Tools/autotest/helicopter.py @@ -436,44 +436,6 @@ def ManAutoRotation(self, timeout=600): self.wait_servo_channel_value(8, 1000, timeout=3) self.wait_disarmed() - def mission_item_home(self, target_system, target_component): - '''returns a mission_item_int which can be used as home in a mission''' - return self.mav.mav.mission_item_int_encode( - target_system, - target_component, - 0, # seq - mavutil.mavlink.MAV_FRAME_GLOBAL_INT, - mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, - 0, # current - 0, # autocontinue - 3, # p1 - 0, # p2 - 0, # p3 - 0, # p4 - int(1.0000 * 1e7), # latitude - int(2.0000 * 1e7), # longitude - 31.0000, # altitude - mavutil.mavlink.MAV_MISSION_TYPE_MISSION) - - def mission_item_takeoff(self, target_system, target_component): - '''returns a mission_item_int which can be used as takeoff in a mission''' - return self.mav.mav.mission_item_int_encode( - target_system, - target_component, - 1, # seq - mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, - mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, - 0, # current - 0, # autocontinue - 0, # p1 - 0, # p2 - 0, # p3 - 0, # p4 - int(1.0000 * 1e7), # latitude - int(1.0000 * 1e7), # longitude - 31.0000, # altitude - mavutil.mavlink.MAV_MISSION_TYPE_MISSION) - def mission_item_rtl(self, target_system, target_component): '''returns a mission_item_int which can be used as takeoff in a mission''' return self.mav.mav.mission_item_int_encode( @@ -545,7 +507,7 @@ def scurve_nasty_mission(self, target_system=1, target_component=1): # slot 0 is home self.mission_item_home(target_system=target_system, target_component=target_component), # slot 1 is takeoff - self.mission_item_takeoff(target_system=target_system, target_component=target_component), + self.mission_item_copter_takeoff(target_system=target_system, target_component=target_component), # now three spline waypoints right on top of one another: copy.copy(wp2_by_three), copy.copy(wp2_by_three), @@ -619,7 +581,7 @@ def scurve_nasty_up_mission(self, target_system=1, target_component=1): # slot 0 is home self.mission_item_home(target_system=target_system, target_component=target_component), # slot 1 is takeoff - self.mission_item_takeoff(target_system=target_system, target_component=target_component), + self.mission_item_copter_takeoff(target_system=target_system, target_component=target_component), wp2, wp3, wp4, diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 61766d9025aaea..72f0bc01d720b8 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -537,6 +537,8 @@ MAV_RESULT AP_Mount::handle_command(const mavlink_command_int_t &packet, const m return handle_command_do_set_roi_sysid(packet); case MAV_CMD_DO_SET_ROI_NONE: return handle_command_do_set_roi_none(); + case MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET: + return handle_command_do_set_roi_wpnext_offset(packet); default: return MAV_RESULT_UNSUPPORTED; }