fix loiter radius and neaten comments

ArduPlane/mode_autoland.cpp

@@ -69,20 +69,20 @@ bool ModeAutoLand::_enter()
 #endif
 
     if (!plane.takeoff_state.initial_direction.initialized) {
-        gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff initial direction not set,must use autotakeoff");
+        gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff initial direction not set");
         return false;
     }
 
     plane.set_target_altitude_current();
-    plane.auto_state.next_wp_crosstrack = true; // not really needed since previous/next_WP is not used by loiter cmd navigation
+    plane.auto_state.next_wp_crosstrack = true;
 
     plane.prev_WP_loc = plane.current_loc;
 
     /*
       landing is in 3 steps:
-        1) a base leg loitering to alt waypoint
-        2) a land start WP, with crosstrack
-        3) the landing WP, with crosstrack
+        1) a loitering to alt waypoint centered on base leg
+        2) exiting and proceeeing to a final approach land start WP, with crosstrack
+        3) the landing WP at home, with crosstrack
 
       the base leg point is 90 degrees off from the landing leg
      */
@@ -99,37 +99,37 @@ bool ModeAutoLand::_enter()
     land_start_WP.change_alt_frame(Location::AltFrame::ABSOLUTE);
 
     /*
-      now create the initial target waypoint for the loitering to alt base leg. We
+      now create the initial target waypoint for the loitering to alt centered on base leg waypoint. We
       choose if we will do a right or left turn onto the landing based
       on where we are when we enter the landing mode
      */
     const float bearing_to_current_deg = wrap_180(degrees(land_start_WP.get_bearing(plane.current_loc)));
     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;
 
-    // make the "base leg" the smaller of the loiter radius and 1/3 of the final WP distance
+    // make the "base leg" the smaller of the waypoint loiter radius or 1/3 of the final WP distance
     // this results in a neat landing no matter the loiter radius
     const float base_leg_length = MIN(final_wp_dist * 0.333, fabsf(plane.aparm.loiter_radius));
 
     cmd[0].id = MAV_CMD_NAV_LOITER_TO_ALT;
+    cmd[0].p1 = base_leg_length;
     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[0].content.location.loiter_ccw = bearing_err_deg>0? 1 :0;
 
     /*
-      create the WP for the start of the landing
+      create the WP for the start of the landing final approach
      */
     cmd[1].content.location = land_start_WP;
     cmd[1].id = MAV_CMD_NAV_WAYPOINT;
 
-    // and the land WP
+    // and the land WP at home
     cmd[2].id = MAV_CMD_NAV_LAND;
     cmd[2].content.location = home;
 
-    // start first leg
+    // start first leg toward the base leg loiter to alt point
     stage = AutoLandStage::LOITER;
     plane.start_command(cmd[0]);
-
     return true;
 }
 
@@ -150,6 +150,7 @@ void ModeAutoLand::navigate()
 {
     switch (stage) {
     case AutoLandStage::LOITER:
+        // check if we have arrived and completed loiter at base leg waypoint
         if (plane.verify_loiter_to_alt(cmd[0])) {
             stage = AutoLandStage::BASE_LEG;
             plane.start_command(cmd[1]);
@@ -160,6 +161,7 @@ void ModeAutoLand::navigate()
         break;
 
     case AutoLandStage::BASE_LEG:
+        // check if we have reached the final approach waypoint
         if (plane.verify_nav_wp(cmd[1])) {
             stage = AutoLandStage::LANDING;
             plane.start_command(cmd[2]);
@@ -210,16 +212,16 @@ void ModeAutoLand::set_autoland_direction()
 }
 
 /*
-  return true when the LOITER_TO_ALT is lined up ready for the landing
+  return true when the LOITER_TO_ALT is lined up ready for the landing, used in commands_logic verify loiter to alt
  */
 bool ModeAutoLand::landing_lined_up(void)
 {
-    // use the line between the center of the LOITER_TO_ALT and the
-    // start of the landing leg
+    // use the line between the center of the LOITER_TO_ALT on the base leg and the
+    // start of the landing leg (land_start_WP)
     return plane.mode_loiter.isHeadingLinedUp_cd(cmd[0].content.location.get_bearing(cmd[1].content.location)*100);
 }
 
-// possibly capture heading for autoland mode if option is set and using a compass
+// possibly capture heading on arming for autoland mode if option is set and using a compass
 void ModeAutoLand::arm_check(void)
 {
     if (plane.ahrs.use_compass() && autoland_option_is_set(ModeAutoLand::AutoLandOption::AUTOLAND_DIR_ON_ARM)) {