AP_Mission: Add heading required param to LOITER_TO_ALT

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

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,

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;