From 921116e7a3be3a45d02b3c834bb20aa221e9948f Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 14 Aug 2021 19:55:03 +0100 Subject: [PATCH] Plane: QLand: move functions to ModeQLand --- ArduPlane/mode_qland.cpp | 22 ++++++++++++++++++++++ ArduPlane/quadplane.cpp | 24 ------------------------ 2 files changed, 22 insertions(+), 24 deletions(-) diff --git a/ArduPlane/mode_qland.cpp b/ArduPlane/mode_qland.cpp index 0d2a272d7a..3a2d2421d0 100644 --- a/ArduPlane/mode_qland.cpp +++ b/ArduPlane/mode_qland.cpp @@ -6,8 +6,30 @@ bool ModeQLand::_enter() return plane.mode_qstabilize._enter(); } +void ModeQLand::init() +{ + plane.mode_qloiter.init(); + quadplane.throttle_wait = false; + quadplane.setup_target_position(); + poscontrol.set_state(QuadPlane::QPOS_LAND_DESCEND); + poscontrol.pilot_correction_done = false; + quadplane.last_land_final_agl = plane.relative_ground_altitude(plane.g.rangefinder_landing); + quadplane.landing_detect.lower_limit_start_ms = 0; + quadplane.landing_detect.land_start_ms = 0; +#if LANDING_GEAR_ENABLED == ENABLED + plane.g2.landing_gear.deploy_for_landing(); +#endif +#if AC_FENCE == ENABLED + plane.fence.auto_disable_fence_for_landing(); +#endif +} + void ModeQLand::update() { plane.mode_qstabilize.update(); } +void ModeQLand::run() +{ + plane.mode_qloiter.run(); +} diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index eb9a8f7e62..263fc90937 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1189,25 +1189,6 @@ void ModeQLoiter::init() quadplane.last_loiter_ms = AP_HAL::millis(); } -void ModeQLand::init() -{ - plane.mode_qloiter.init(); - quadplane.throttle_wait = false; - quadplane.setup_target_position(); - poscontrol.set_state(QuadPlane::QPOS_LAND_DESCEND); - poscontrol.pilot_correction_done = false; - quadplane.last_land_final_agl = plane.relative_ground_altitude(plane.g.rangefinder_landing); - quadplane.landing_detect.lower_limit_start_ms = 0; - quadplane.landing_detect.land_start_ms = 0; -#if LANDING_GEAR_ENABLED == ENABLED - plane.g2.landing_gear.deploy_for_landing(); -#endif -#if AC_FENCE == ENABLED - plane.fence.auto_disable_fence_for_landing(); -#endif -} - - // helper for is_flying() bool QuadPlane::is_flying(void) { @@ -2219,11 +2200,6 @@ void QuadPlane::control_run(void) } } -void ModeQLand::run() -{ - plane.mode_qloiter.run(); -} - /* enter a quadplane mode */