Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: LoiterAltQLand: support handle_guided_request

Browse Source
This commit is contained in:
Iampete1 2021-09-27 20:29:03 +01:00 committed by Andrew Tridgell
parent a0260b05b3
commit 0a1cad772c
2 changed files with 24 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/mode.h
View File

@ -287,6 +287,9 @@ public:
const char *name() const override { return "Loiter to QLAND"; }
const char *name4() const override { return "L2QL"; }
// handle a guided target request from GCS
bool handle_guided_request(Location target_loc) override;
protected:
bool _enter() override;

22
ArduPlane/mode_LoiterAltQLand.cpp
View File

@ -37,9 +37,29 @@ void ModeLoiterAltQLand::navigate()
void ModeLoiterAltQLand::switch_qland()
{
ftype dist;
if (!plane.current_loc.get_alt_distance(plane.next_WP_loc, dist) || is_negative(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