WIP

magicrub · Oct 13, 2023 · dec0c8d · dec0c8d
1 parent 659db36
commit dec0c8d
Show file tree

Hide file tree

Showing 3 changed files with 81 additions and 15 deletions.
diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
@@ -1092,7 +1092,7 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
                 nav_controller->update_loiter(cmd.content.location, radius, direction);
 
                 const float breakout_direction_rad = radians(vtol_approach_s.approach_direction_deg + (direction > 0 ? 270 : 90));
-
+ 
                 // breakout when within 5 degrees of the opposite direction
                 if (fabsF(wrap_PI(ahrs.yaw - breakout_direction_rad)) < radians(5.0f)) {
                     gcs().send_text(MAV_SEVERITY_INFO, "Starting VTOL land approach path");

diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
@@ -370,6 +370,8 @@ class ModeLoiterAltQLand : public ModeLoiter
 private:
     void switch_qland();
 
+    bool has_reached_transition_altitude;
+
 };
 #endif // HAL_QUADPLANE_ENABLED
 

diff --git a/ArduPlane/mode_LoiterAltQLand.cpp b/ArduPlane/mode_LoiterAltQLand.cpp
@@ -10,35 +10,98 @@ bool ModeLoiterAltQLand::_enter()
         return true;
     }
 
+    // cache next_WP_loc in case we're already heading somewhere in Guided mode
+    // because ModeLoiter::_enter() will set plane.next_WP_loc = current_loc
+    const Location previous_wp = plane.next_WP_loc;
+
     ModeLoiter::_enter();
 
-#if AP_TERRAIN_AVAILABLE
-    if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) {
-        plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN);
-    } else {
-        plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
-    }
-#else
-    plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
-#endif
+    // 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;
+
+    plane.do_landing_vtol_approach(cmd);
+
+    handle_guided_request(loiter_wp);
+
+
+    // switch_qland();
 
-    switch_qland();
 
     return true;
 }
 
 void ModeLoiterAltQLand::navigate()
 {
-    switch_qland();
+    AP_Mission::Mission_Command cmd {};
+    cmd.content.location = plane.next_WP_loc;
+    plane.verify_landing_vtol_approach(cmd);
+
+    // switch_qland();
 
     ModeLoiter::navigate();
 }
 
 void ModeLoiterAltQLand::switch_qland()
 {
-    ftype dist;
-    if ((!plane.current_loc.get_alt_distance(plane.next_WP_loc, dist) || is_negative(dist)) && plane.nav_controller->reached_loiter_target()) {
-        plane.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_REACHED_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)
+    {
+        // if (plane.any_failsafe_triggered()) {
+        //     // land at current_loc which will be on the loiter radius. Useful for emergencies to get down faster
+        //     // but it lands in a somewhat random location on the radius which is not ideal
+        //     plane.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_REACHED_QLAND);
+        //     return;
+        // }
+
+        const Location loiter_wp = plane.next_WP_loc;
+
+
+        // // wait until we are down-wind so our approch is into the wind.
+        // // we do this by creating a waypoint to cpmare against that is 10km downwind from the loiter center
+        // const Vector2f wind = AP::ahrs().wind_estimate().xy();
+        // const float wind_speed = wind.length();
+        bool is_lined_up = false;
+
+        // const float wind_direction_cd = degrees(wind.angle()) * 100;
+
+
+        // if (wind_speed < 0.5) {
+        //     // not enough wind to matter
+        //     gcs().send_text(MAV_SEVERITY_INFO, "Wind: %.1fm/s %.1fdeg", wind_speed, wind_direction_cd*0.01);
+        //     gcs().send_text(MAV_SEVERITY_INFO, "Wind is weak, landing now");
+        //     is_lined_up = true;
+
+        // } else {
+        //     const float heading_to_exit_loiter_cd = (plane.loiter.direction < 0) ? wind_direction_cd + 27000 : wind_direction_cd + 9000;
+        //     is_lined_up = plane.mode_loiter.isHeadingLinedUp_cd(heading_to_exit_loiter_cd);
+
+        //     const int32_t heading_cd = (wrap_360(degrees(AP::ahrs().groundspeed_vector().angle())))*100;
+        //     const int32_t heading_err_cd = wrap_180_cd(heading_to_exit_loiter_cd - heading_cd);
+
+        //     gcs().send_text(MAV_SEVERITY_INFO, "Wind: %.1fm/s %.1fdeg, hdg: %.1f %.1f", wind_speed, wind_direction_cd*0.01, heading_cd*0.01, heading_err_cd*0.01);
+        // }
+
+        AP_Mission::Mission_Command cmd {};
+        cmd.content.location = loiter_wp;
+        is_lined_up = plane.verify_landing_vtol_approach(cmd);
+
+        if (is_lined_up) {
+            gcs().send_text(MAV_SEVERITY_INFO, "SWITCHING TO QRTL");
+            // NOTE: setting the mode will change next_WP_loc so we must change it back to the cached loiter_wp
+            plane.set_mode(plane.mode_qrtl, ModeReason::LOITER_ALT_REACHED_QLAND);
+            plane.set_next_WP(loiter_wp);
+        }
     }
 }
 
@@ -57,6 +120,7 @@ bool ModeLoiterAltQLand::handle_guided_request(Location target_loc)
 
     plane.set_guided_WP(target_loc);
 
+    has_reached_transition_altitude = false;
     return true;
 }