diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index a4237ddaa9df5..c9c9a8c951af9 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -777,17 +777,28 @@ bool Plane::verify_loiter_to_alt(const AP_Mission::Mission_Command &cmd) update_loiter(cmd.p1); + // type_specific_bit 0 stores the MAVLink param1 "heading required" flag: + // 1 = continue loitering until heading towards next waypoint (original behaviour) + // 0 = complete as soon as target altitude is reached + const bool heading_required = (cmd.type_specific_bits & (1U << 0)) != 0; + // condition_value == 0 means alt has never been reached if (condition_value == 0) { // primary goal, loiter to alt if (labs(loiter.sum_cd) > 1 && (loiter.reached_target_alt || loiter.unable_to_achieve_target_alt)) { - // primary goal completed, initialize secondary heading goal + // primary goal completed if (loiter.unable_to_achieve_target_alt) { gcs().send_text(MAV_SEVERITY_INFO,"Loiter to alt was stuck at %d", int(current_loc.alt/100)); } condition_value = 1; - result = verify_loiter_heading(true); + if (!heading_required) { + // altitude reached and heading alignment not required, we are done + result = true; + } else { + // initialize secondary heading goal + result = verify_loiter_heading(true); + } } } else { // secondary goal, loiter to heading diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 1aaadd885dedf..22180618dd263 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -7159,6 +7159,68 @@ def MAV_CMD_NAV_LOITER_TO_ALT(self): self.wait_current_waypoint(4) self.fly_home_land_and_disarm() + def MAV_CMD_NAV_LOITER_TO_ALT_HeadingRequired(self): + '''test MAV_CMD_NAV_LOITER_TO_ALT param1=1 (heading required)''' + # waypoint after loiter is 1000 m north of home. The loiter is + # directly overhead home so the plane must turn to a northerly heading + # before it is allowed to exit when heading_required=1. + loiter_alt = 200 # metres AGL + wp_north_m = 1000 + self.start_flying_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30), + self.create_MISSION_ITEM_INT( + mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT, + p1=1, # heading required + p2=100, # loiter radius (m) + z=loiter_alt, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + ), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, wp_north_m, 0, loiter_alt), + ]) + # Wait until the loiter altitude band is reached. + self.wait_altitude(loiter_alt - 20, loiter_alt + 20, relative=True, timeout=600) + t_alt_reached = self.get_sim_time() + # plane must complete at least a partial orbit to line up on the next waypoint before advancing. + # verify time passes and plane doesn't advance to next waypoint + self.wait_current_waypoint(3, timeout=600) + elapsed = self.get_sim_time() - t_alt_reached + if elapsed < 10: + raise NotAchievedException( + f"Heading-required loiter exited too quickly ({elapsed:.1f}s); expected heading alignment delay" + ) + self.progress(f"Heading-required loiter held for {elapsed:.1f}s after altitude reached - PASS") + self.fly_home_land_and_disarm(timeout=300) + + def MAV_CMD_NAV_LOITER_TO_ALT_HeadingNotRequired(self): + '''test MAV_CMD_NAV_LOITER_TO_ALT param1=0 (heading not required)''' + loiter_alt = 200 # metres AGL + wp_north_m = 1000 + self.start_flying_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30), + self.create_MISSION_ITEM_INT( + mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT, + p1=0, # heading NOT required + p2=100, # loiter radius (m) + z=loiter_alt, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + ), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, wp_north_m, 0, loiter_alt), + ]) + # Wait until the loiter altitude band is reached. + self.wait_altitude(loiter_alt - 20, loiter_alt + 20, relative=True, timeout=600) + t_alt_reached = self.get_sim_time() + # With heading_required=0 the mission should advance within a few + # seconds of hitting the target altitude band, well before a full + # orbit could be completed (~30 s at 100 m radius / 30 m/s). + self.wait_current_waypoint(3, timeout=60) + elapsed = self.get_sim_time() - t_alt_reached + if elapsed > 30: + raise NotAchievedException( + f"Heading-not-required loiter took too long to exit ({elapsed:.1f}s); expected near-immediate advance" + ) + self.progress("Heading-not-required loiter exited in {elapsed:.1f}s after altitude reached - PASS") + self.fly_home_land_and_disarm(timeout=300) + def RudderArmedTakeoffRequiresNeutralThrottle(self): '''auto-takeoff should not occur while rudder continues to be held over''' self.change_mode('TAKEOFF') @@ -8082,6 +8144,8 @@ def tests1a(self): self.LOITER, self.MAV_CMD_NAV_LOITER_TURNS, self.MAV_CMD_NAV_LOITER_TO_ALT, + self.MAV_CMD_NAV_LOITER_TO_ALT_HeadingRequired, + self.MAV_CMD_NAV_LOITER_TO_ALT_HeadingNotRequired, self.DeepStall, self.WatchdogHome, self.LargeMissions, diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index dcd3463fe17e1..621e4594f1aeb 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -1172,6 +1172,10 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ cmd.p1 = fabsf(packet.param2); // param2 is radius in meters cmd.content.location.loiter_ccw = (packet.param2 < 0); cmd.content.location.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location + // param1: heading required (1=heading required before exiting, 0=exit when alt reached) + if (is_equal(packet.param1, 1.0f)) { + cmd.type_specific_bits |= (1U << 0); + } break; case MAV_CMD_NAV_ARC_WAYPOINT: // MAV ID: 36 @@ -1691,6 +1695,7 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c break; case MAV_CMD_NAV_LOITER_TO_ALT: // MAV ID: 31 + packet.param1 = (cmd.type_specific_bits & (1U << 0)) ? 1 : 0; // heading required packet.param2 = cmd.p1; // loiter radius(m) if (cmd.content.location.loiter_ccw) { packet.param2 = -packet.param2;