add land cmd

Hwurzburg · Nov 24, 2024 · 3a47dc2 · 3a47dc2
1 parent a1defd0
commit 3a47dc2
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 2 deletions.
diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
@@ -681,11 +681,13 @@ void Plane::update_flight_stage(void)
             }
 #endif
             set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
-        } else if (control_mode != &mode_takeoff) {
+        } else if ((control_mode != &mode_takeoff) && (control_mode != &mode_autoland)) {
             // If not in AUTO then assume normal operation for normal TECS operation.
             // This prevents TECS from being stuck in the wrong stage if you switch from
             // AUTO to, say, FBWB during a landing, an aborted landing or takeoff.
             set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
+        } else  if (control_mode == &mode_autoland) {
+            set_flight_stage(AP_FixedWing::FlightStage::LAND);
         }
         return;
     }

diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
@@ -867,6 +867,8 @@ class ModeAutoLand: public Mode
 
 protected:
     bool _enter() override;
+    AP_Mission::Mission_Command cmd {};
+    bool land_started;
 
 
 private:

diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp
@@ -43,6 +43,9 @@ bool ModeAutoLand::_enter()
     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;
+    cmd.id = MAV_CMD_NAV_WAYPOINT;
+    cmd.content.location = plane.next_WP_loc;
+    land_started = false;
     return true;
 }
 
@@ -56,5 +59,16 @@ void ModeAutoLand::update()
 
 void ModeAutoLand::navigate()
 {    
-    plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
+    if (!land_started){
+        if (plane.verify_nav_wp(cmd)){
+            const Location &home_loc = ahrs.get_home();
+            cmd.id = MAV_CMD_NAV_LAND;
+            cmd.content.location = home_loc;
+            land_started = true;
+            plane.start_command(cmd);
+        }
+        return;
+    } else {
+       plane.verify_command(cmd);
+    }
 }