added final wp and fly to it

Hwurzburg · Nov 21, 2024 · a1defd0 · a1defd0
1 parent 9e6f6f9
commit a1defd0
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 7 deletions.
diff --git 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
@@ -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);
 }