Plane:add loiter down to alt to AUTOLAND

Hwurzburg · Jan 7, 2025 · d4373c3 · d4373c3
1 parent f1a4cb6
commit d4373c3
Show file tree

Hide file tree

Showing 3 changed files with 59 additions and 29 deletions.
diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
@@ -516,9 +516,9 @@ void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
     loc.sanitize(current_loc);
     set_next_WP(loc);
     loiter_set_direction_wp(cmd);
-
     // init to 0, set to 1 when altitude is reached
     condition_value = 0;
+
 }
 
 // do_nav_delay - Delay the next navigation command
@@ -757,7 +757,6 @@ bool Plane::verify_loiter_turns(const AP_Mission::Mission_Command &cmd)
 bool Plane::verify_loiter_to_alt(const AP_Mission::Mission_Command &cmd)
 {
     bool result = false;
-
     update_loiter(cmd.p1);
 
     // condition_value == 0 means alt has never been reached

diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
@@ -933,9 +933,10 @@ class ModeAutoLand: public Mode
 
 protected:
     bool _enter() override;
-    AP_Mission::Mission_Command cmd[3];
+    AP_Mission::Mission_Command cmd[4];
     uint8_t stage;
     void set_autoland_direction(void);
+    uint16_t max_nonloiter_alt;
 };
 #endif
 #if HAL_SOARING_ENABLED

diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp
@@ -68,20 +68,24 @@ bool ModeAutoLand::_enter()
     plane.set_target_altitude_current();
 
     /*
-      landing is in 3 steps:
-        1) a base leg waypoint
-        2) a land start WP, with crosstrack
-        3) the landing WP, with crosstrack
+      landing is in 4 steps:
+        1) if necesary, loiter down to an altitude which can use TECS sink to get to the alt of:
+        2) a base leg waypoint
+        3) a land start WP, with crosstrack
+        4) the landing WP, with crosstrack
 
       the base leg point is 90 degrees off from the landing leg
      */
-    const Location &home = ahrs.get_home();
-
+     
+    
     /*
-      first calculate the starting waypoint we will use when doing the
+      calculate the starting waypoint we will use when doing the
       NAV_LAND. This is offset by final_wp_dist from home, in a
       direction 180 degrees from takeoff direction
      */
+
+    const Location &home = ahrs.get_home();
+
     Location land_start_WP = home;
     land_start_WP.offset_bearing(wrap_360(plane.takeoff_state.initial_direction.heading + 180), final_wp_dist);
     land_start_WP.set_alt_m(final_wp_alt, Location::AltFrame::ABOVE_HOME);
@@ -96,29 +100,39 @@ bool ModeAutoLand::_enter()
     const float bearing_err_deg = wrap_180(wrap_180(plane.takeoff_state.initial_direction.heading) - bearing_to_current_deg);
     const float bearing_offset_deg = bearing_err_deg > 0? -90 : 90;
     const float base_leg_length = final_wp_dist * 0.333;
-    cmd[0].id = MAV_CMD_NAV_WAYPOINT;
-    cmd[0].content.location = land_start_WP;
-    cmd[0].content.location.offset_bearing(plane.takeoff_state.initial_direction.heading + bearing_offset_deg, base_leg_length);
+    cmd[1].id = MAV_CMD_NAV_WAYPOINT;
+    cmd[1].content.location = land_start_WP;
+    cmd[1].content.location.offset_bearing(plane.takeoff_state.initial_direction.heading + bearing_offset_deg, base_leg_length);
     // set a 1m acceptance radius, we want to fly all the way to this waypoint
-    cmd[0].p1 = 1;
-
+    cmd[1].p1 = 1;
+    
     /*
       create the WP for the start of the landing
      */
-    cmd[1].content.location = land_start_WP;
-    cmd[1].id = MAV_CMD_NAV_WAYPOINT;
+    cmd[2].content.location = land_start_WP;
+    cmd[2].id = MAV_CMD_NAV_WAYPOINT;
 
     // and the land WP
-    cmd[2].id = MAV_CMD_NAV_LAND;
-    cmd[2].content.location = home;
-
-    // start first leg
-    stage = 0;
-    plane.start_command(cmd[0]);
+    cmd[3].id = MAV_CMD_NAV_LAND;
+    cmd[3].content.location = home;
+
+    //determine if a loiter down in altitude is required
+    const uint16_t altitude = plane.relative_ground_altitude(false,true);
+    max_nonloiter_alt = 0.177 * plane.current_loc.get_distance(cmd[1].content.location);
+    max_nonloiter_alt += final_wp_alt;
+    if (altitude > max_nonloiter_alt) {
+        plane.do_loiter_at_location();
+        gcs().send_text(MAV_SEVERITY_WARNING,"Loitering down to alt for approach");
+        stage = 0;
+    } else {
+        stage =1;
+        plane.prev_WP_loc = plane.current_loc;
+        plane.set_target_altitude_current();
+        plane.start_command(cmd[1]);
+        plane.auto_state.crosstrack = false;
+        plane.auto_state.next_wp_crosstrack = true;
 
-    // don't crosstrack initially
-    plane.auto_state.crosstrack = false;
-    plane.auto_state.next_wp_crosstrack = true;
+    }
 
     return true;
 }
@@ -138,13 +152,29 @@ void ModeAutoLand::update()
 
 void ModeAutoLand::navigate()
 {
-    if (stage == 2) {
+    if (stage == 3) {
         // we are on final landing leg
         plane.set_flight_stage(AP_FixedWing::FlightStage::LAND);
-        plane.verify_command(cmd[stage]);
+        plane.verify_command(cmd[3]);
+        return;
+    }
+    if (stage == 0 ) {
+        //we are loitering down to max alt for a 10deg slope to base waypoint
+        plane.target_altitude.amsl_cm = max_nonloiter_alt * 100;
+        const uint16_t altitude = plane.relative_ground_altitude(false,true);
+        if (altitude > max_nonloiter_alt){
+        plane.update_loiter(0);
+        } else {
+        //move to next stage (1) once enough altitude is bled off
+        stage++;
+        plane.prev_WP_loc = plane.current_loc;
+        plane.set_target_altitude_current();
+        plane.start_command(cmd[1]);
+        plane.auto_state.crosstrack = false;
+        plane.auto_state.next_wp_crosstrack = true;
+        }
         return;
     }
-
     // see if we have completed the leg
     if (plane.verify_nav_wp(cmd[stage])) {
         stage++;