From a1defd0a9b0bcdf65280e0f159c62b36b4e1d426 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Thu, 21 Nov 2024 14:11:08 -0600 Subject: [PATCH] added final wp and fly to it --- ArduPlane/Plane.h | 1 + ArduPlane/mode_autoland.cpp | 18 +++++++++++------- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index f56ad3ec8ffc0a..2d309746e04b8d 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -172,6 +172,7 @@ class Plane : public AP_Vehicle { friend class ModeTakeoff; friend class ModeThermal; friend class ModeLoiterAltQLand; + friend class ModeAutoLand; #if AP_EXTERNAL_CONTROL_ENABLED friend class AP_ExternalControl_Plane; diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp index b18622e59d5cbf..228bbea3071fcb 100644 --- a/ArduPlane/mode_autoland.cpp +++ b/ArduPlane/mode_autoland.cpp @@ -35,22 +35,26 @@ ModeAutoLand::ModeAutoLand() : bool ModeAutoLand::_enter() { -// upon enter we setup a final wp and land wp based on home and takeoff_dir + //is flying check here and DO_LAND_START check here + plane.prev_WP_loc = plane.current_loc; const Location &home = ahrs.get_home(); plane.set_target_altitude_current(); - plane.next_WP_loc.offset_bearing(bearing_cd, home.alt*0.01f + final_wp_alt); - plane.prev_WP_loc = plane.current_loc; - plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc); + plane.next_WP_loc = home; + uint16_t bearing_cd = wrap_360((plane.takeoff_state.takeoff_initial_direction + 180)); + plane.next_WP_loc.offset_bearing(bearing_cd, final_wp_dist); + plane.next_WP_loc.alt = home.alt + final_wp_alt*100; return true; } void ModeAutoLand::update() { + plane.set_target_altitude_location(plane.next_WP_loc); plane.calc_nav_roll(); - plane.update_fbwb_speed_height(); + plane.calc_nav_pitch(); + plane.calc_throttle(); } void ModeAutoLand::navigate() -{ - +{ + plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc); }