mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 23:18:28 -04:00
46 lines
1.1 KiB
C++
46 lines
1.1 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.set_mode(plane.mode_qland, ModeReason::LOITER_ALT_REACHED_QLAND);
|
|
}
|
|
}
|
|
|
|
#endif
|