mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
2784f8fa7f
So instead of updating plane.guided_WP_loc and then calling set_guided_WP(void) to copy that state into plane.next_WP_loc we pass the new location in the call to set_guided_WP(const Location &loc). avoidance was the only place which was not entirely over-writing plane.guided_WP_loc. However, plane.next_WP_loc was updated to be the current location when we entered guided mode. If we update the horizontal/vertical avoidance now it is relative to the current location, not the guided wp location, which could be quite important.
64 lines
1.6 KiB
C++
64 lines
1.6 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)
|
|
{
|
|
// setup altitude
|
|
#if AP_TERRAIN_AVAILABLE
|
|
if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) {
|
|
target_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN);
|
|
} else {
|
|
target_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
|
|
}
|
|
#else
|
|
target_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
|
|
#endif
|
|
|
|
plane.set_guided_WP(target_loc);
|
|
|
|
return true;
|
|
}
|
|
|
|
#endif
|