WIP2

magicrub · Oct 13, 2023 · e58f23a · e58f23a
1 parent dec0c8d
commit e58f23a
Showing 1 changed file with 24 additions and 15 deletions.
diff --git a/ArduPlane/mode_LoiterAltQLand.cpp b/ArduPlane/mode_LoiterAltQLand.cpp
@@ -19,10 +19,10 @@ bool ModeLoiterAltQLand::_enter()
     // at this point plane.next_WP_loc == current_loc
     const Location loiter_wp = plane.previous_mode->is_guided_mode() ? previous_wp : plane.next_WP_loc;
 
-    AP_Mission::Mission_Command cmd {};
-    cmd.content.location = loiter_wp;
+    // AP_Mission::Mission_Command cmd {};
+    // cmd.content.location = loiter_wp;
 
-    plane.do_landing_vtol_approach(cmd);
+    // plane.do_landing_vtol_approach(cmd);
 
     handle_guided_request(loiter_wp);
 
@@ -35,25 +35,34 @@ bool ModeLoiterAltQLand::_enter()
 
 void ModeLoiterAltQLand::navigate()
 {
-    AP_Mission::Mission_Command cmd {};
-    cmd.content.location = plane.next_WP_loc;
-    plane.verify_landing_vtol_approach(cmd);
+    // AP_Mission::Mission_Command cmd {};
+    // cmd.content.location = plane.next_WP_loc;
+    // plane.verify_landing_vtol_approach(cmd);
 
-    // switch_qland();
+    //switch_qland();
 
     ModeLoiter::navigate();
+
+
+    ftype dist;
+    if ((!plane.current_loc.get_alt_distance(plane.next_WP_loc, dist) || is_negative(dist)) && plane.nav_controller->reached_loiter_target()) {
+        const Location loiter_wp = plane.next_WP_loc;
+        plane.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_REACHED_QLAND);
+        plane.set_next_WP(loiter_wp);
+        poscontrol.set_state(QuadPlane::QPOS_AIRBRAKE);
+    }
 }
 
 void ModeLoiterAltQLand::switch_qland()
 {
-    // if (!has_reached_transition_altitude) {
-    //     ftype dist;
-    //     if ((!plane.current_loc.get_alt_distance(plane.next_WP_loc, dist) || is_negative(dist)) && plane.nav_controller->reached_loiter_target()) {
-    //         // we only need to satisfy the altitude requirement once
-    //         has_reached_transition_altitude = true;
-    //         gcs().send_text(MAV_SEVERITY_INFO, "ALT reached %.2f", plane.current_loc.alt*0.01);
-    //     }
-    // }
+    if (!has_reached_transition_altitude) {
+        ftype dist;
+        if ((!plane.current_loc.get_alt_distance(plane.next_WP_loc, dist) || is_negative(dist)) && plane.nav_controller->reached_loiter_target()) {
+            // we only need to satisfy the altitude requirement once
+            has_reached_transition_altitude = true;
+            gcs().send_text(MAV_SEVERITY_INFO, "ALT reached %.2f", plane.current_loc.alt*0.01);
+        }
+    }
 
     // if (has_reached_transition_altitude)
     {