|
|
|
@ -895,7 +895,7 @@ void QuadPlane::init_loiter(void)
@@ -895,7 +895,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; |
|
|
|
@ -1750,7 +1750,7 @@ bool QuadPlane::init_mode(void)
@@ -1750,7 +1750,7 @@ bool QuadPlane::init_mode(void)
|
|
|
|
|
init_loiter(); |
|
|
|
|
break; |
|
|
|
|
case QLAND: |
|
|
|
|
init_land(); |
|
|
|
|
init_qland(); |
|
|
|
|
break; |
|
|
|
|
case QRTL: |
|
|
|
|
init_qrtl(); |
|
|
|
@ -1877,7 +1877,7 @@ void QuadPlane::vtol_position_controller(void)
@@ -1877,7 +1877,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
|
|
|
|
@ -1986,7 +1986,11 @@ void QuadPlane::vtol_position_controller(void)
@@ -1986,7 +1986,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(); |
|
|
|
|
|
|
|
|
|
// nav roll and pitch are controller by position controller
|
|
|
|
@ -2369,10 +2373,6 @@ bool QuadPlane::verify_vtol_land(void)
@@ -2369,10 +2373,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
|
|
|
|
|
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); |
|
|
|
|
if (poscontrol.state == QPOS_LAND_DESCEND && height_above_ground < land_final_alt) { |
|
|
|
|