From dec0c8d0e8029ee729c7bebe8f0fc1e9941adbf0 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Thu, 12 Oct 2023 17:57:24 -0700 Subject: [PATCH] WIP --- ArduPlane/commands_logic.cpp | 2 +- ArduPlane/mode.h | 2 + ArduPlane/mode_LoiterAltQLand.cpp | 92 ++++++++++++++++++++++++++----- 3 files changed, 81 insertions(+), 15 deletions(-) diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 9ad4f9531318b1..9e8f92d7c0cade 100644 --- 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 index a88dbec5b6e3c1..6647e23e2461dc 100644 --- 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 index 0cc8b9457c11e7..da8cf3af9bef74 100644 --- 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; }