mirror of https://github.com/ArduPilot/ardupilot
66 lines
1.7 KiB
C++
66 lines
1.7 KiB
C++
#include "mode.h"
|
|
#include "Plane.h"
|
|
|
|
#if HAL_QUADPLANE_ENABLED
|
|
|
|
bool ModeLoiterAltQLand::_enter()
|
|
{
|
|
if (plane.previous_mode->is_vtol_mode() || plane.quadplane.in_vtol_mode()) {
|
|
plane.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_IN_VTOL);
|
|
return true;
|
|
}
|
|
|
|
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
|
|
|
|
switch_qland();
|
|
|
|
return true;
|
|
}
|
|
|
|
void ModeLoiterAltQLand::navigate()
|
|
{
|
|
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);
|
|
}
|
|
}
|
|
|
|
bool ModeLoiterAltQLand::handle_guided_request(Location target_loc)
|
|
{
|
|
plane.guided_WP_loc = target_loc;
|
|
|
|
// setup altitude
|
|
#if AP_TERRAIN_AVAILABLE
|
|
if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) {
|
|
plane.guided_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN);
|
|
} else {
|
|
plane.guided_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
|
|
}
|
|
#else
|
|
plane.guided_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
|
|
#endif
|
|
|
|
plane.set_guided_WP();
|
|
|
|
return true;
|
|
}
|
|
|
|
#endif
|