diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp index a688580c30e96..88840520ca6ab 100644 --- a/ArduPlane/mode_autoland.cpp +++ b/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,7 +99,7 @@ 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 */ @@ -107,29 +107,29 @@ 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; - // 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)) {