diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index b06039e86c..62f2985654 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -874,7 +874,7 @@ void QuadPlane::init_loiter(void) last_loiter_ms = AP_HAL::millis(); } -void QuadPlane::init_land(void) +void QuadPlane::init_qland(void) { init_loiter(); throttle_wait = false; @@ -1653,7 +1653,7 @@ bool QuadPlane::init_mode(void) init_loiter(); break; case QLAND: - init_land(); + init_qland(); break; case QRTL: init_qrtl(); @@ -1777,7 +1777,7 @@ void QuadPlane::vtol_position_controller(void) case QPOS_POSITION1: { Vector2f diff_wp = location_diff(plane.current_loc, loc); - float distance = diff_wp.length(); + const float distance = diff_wp.length(); if (poscontrol.speed_scale <= 0) { // initialise scaling so we start off targeting our @@ -1886,7 +1886,11 @@ void QuadPlane::vtol_position_controller(void) pos_control->set_desired_accel_xy(0.0f,0.0f); // set position control target and update - pos_control->set_xy_target(poscontrol.target.x, poscontrol.target.y); + if (should_relax()) { + loiter_nav->soften_for_landing(); + } else { + pos_control->set_xy_target(poscontrol.target.x, poscontrol.target.y); + } pos_control->update_xy_controller(ekfNavVelGainScaler); // nav roll and pitch are controller by position controller @@ -2291,10 +2295,6 @@ bool QuadPlane::verify_vtol_land(void) plane.set_next_WP(plane.next_WP_loc); } - if (should_relax()) { - loiter_nav->soften_for_landing(); - } - // at land_final_alt begin final landing if (poscontrol.state == QPOS_LAND_DESCEND && check_land_final()) { poscontrol.state = QPOS_LAND_FINAL; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index cafc635d5e..1fce00c06f 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -189,7 +189,7 @@ private: void run_rate_controller(void); void init_loiter(void); - void init_land(void); + void init_qland(void); void control_loiter(void); void check_land_complete(void); bool land_detector(uint32_t timeout_ms);