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

Plane: fixed slowing descent in final phase of QLAND

Browse Source
This commit is contained in:
Andrew Tridgell 2021-05-15 07:20:39 +10:00
parent 53e5bef0c2
commit 71fa52d5c6
2 changed files with 21 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
ArduPlane/quadplane.cpp
View File

@ -1425,6 +1425,10 @@ bool QuadPlane::is_flying_vtol(void) const
*/ */
float QuadPlane::landing_descent_rate_cms(float height_above_ground) const float QuadPlane::landing_descent_rate_cms(float height_above_ground) const
{ {
if (poscontrol.state == QPOS_LAND_FINAL) {
// when in final use descent rate for final even if alt has climbed again
height_above_ground = MIN(height_above_ground, land_final_alt);
}
const float max_climb_speed = wp_nav->get_default_speed_up(); const float max_climb_speed = wp_nav->get_default_speed_up();
float ret = linear_interpolate(land_speed_cms, wp_nav->get_default_speed_down(), float ret = linear_interpolate(land_speed_cms, wp_nav->get_default_speed_down(),
height_above_ground, height_above_ground,
@ -1492,6 +1496,9 @@ void QuadPlane::control_loiter()
loiter_nav->set_pilot_desired_acceleration(target_roll_cd, target_pitch_cd); loiter_nav->set_pilot_desired_acceleration(target_roll_cd, target_pitch_cd);
// run loiter controller // run loiter controller
if (!pos_control->is_active_xy()) {
pos_control->init_xy_controller();
}
loiter_nav->update(); loiter_nav->update();
// nav roll and pitch are controller by loiter controller // nav roll and pitch are controller by loiter controller
@ -1525,9 +1532,13 @@ void QuadPlane::control_loiter()
} }
} }
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
float descent_rate_cms = (poscontrol.state == QPOS_LAND_FINAL)? land_speed_cms:landing_descent_rate_cms(height_above_ground); float descent_rate_cms = landing_descent_rate_cms(height_above_ground);
set_climb_rate_cms(-descent_rate_cms, true); if (poscontrol.state == QPOS_LAND_FINAL && (options & OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) {
ahrs.setTouchdownExpected(true);
}
set_climb_rate_cms(-descent_rate_cms, descent_rate_cms>0);
check_land_complete(); check_land_complete();
} else if (plane.control_mode == &plane.mode_guided && guided_takeoff) { } else if (plane.control_mode == &plane.mode_guided && guided_takeoff) {
set_climb_rate_cms(0, false); set_climb_rate_cms(0, false);
@ -2492,6 +2503,7 @@ void QuadPlane::update_land_positioning(void)
if ((options & OPTION_THR_LANDING_CONTROL) == 0) { if ((options & OPTION_THR_LANDING_CONTROL) == 0) {
// not enabled // not enabled
poscontrol.pilot_correction_active = false; poscontrol.pilot_correction_active = false;
poscontrol.target_vel_cms.zero();
return; return;
} }
const float scale = 1.0 / 4500; const float scale = 1.0 / 4500;
@ -2507,12 +2519,10 @@ void QuadPlane::update_land_positioning(void)
const float speed_max_cms = wp_nav->get_default_speed_xy() * 0.25; const float speed_max_cms = wp_nav->get_default_speed_xy() * 0.25;
const float dt = plane.scheduler.get_loop_period_s(); const float dt = plane.scheduler.get_loop_period_s();
Vector2f pos_change_cm(-pitch_in, roll_in); poscontrol.target_vel_cms = Vector3f(-pitch_in, roll_in, 0) * speed_max_cms;
pos_change_cm *= dt * speed_max_cms; poscontrol.target_vel_cms.rotate_xy(plane.ahrs.yaw);
pos_change_cm.rotate(plane.ahrs.yaw);
poscontrol.target_cm.x += pos_change_cm.x; poscontrol.target_cm += poscontrol.target_vel_cms * dt;
poscontrol.target_cm.y += pos_change_cm.y;
poscontrol.pilot_correction_active = (fabsf(roll_in) > 0 || fabsf(pitch_in) > 0); poscontrol.pilot_correction_active = (fabsf(roll_in) > 0 || fabsf(pitch_in) > 0);
} }
@ -2673,16 +2683,12 @@ void QuadPlane::vtol_position_controller(void)
update_land_positioning(); update_land_positioning();
// relax when close to the ground // relax when close to the ground
if (poscontrol.pilot_correction_active) { if (should_relax()) {
Vector3f zero;
pos_control->input_pos_vel_accel_xy(poscontrol.target_cm, zero, zero);
} else if (should_relax()) {
pos_control->relax_velocity_controller_xy(); pos_control->relax_velocity_controller_xy();
} else { } else {
// we stop doing position control in LAND_FINAL to allow for GPS glitch handling without // we use velocity control in QPOS_LAND_FINAL to allow for GPS glitch handling
// a sharp reposition
Vector3f zero; Vector3f zero;
pos_control->input_vel_accel_xy(zero, zero); pos_control->input_vel_accel_xy(poscontrol.target_vel_cms, zero);
} }
run_xy_controller(); run_xy_controller();
@ -2755,8 +2761,6 @@ void QuadPlane::vtol_position_controller(void)
case QPOS_LAND_FINAL: { case QPOS_LAND_FINAL: {
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
if (poscontrol.state == QPOS_LAND_FINAL) { if (poscontrol.state == QPOS_LAND_FINAL) {
// when in final use descent rate for final even if alt has climbed again
height_above_ground = MIN(height_above_ground, land_final_alt);
if ((options & OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) { if ((options & OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) {
ahrs.setTouchdownExpected(true); ahrs.setTouchdownExpected(true);
} }

1
ArduPlane/quadplane.h
View File

@ -471,6 +471,7 @@ private:
Vector2f target_velocity; Vector2f target_velocity;
float max_speed; float max_speed;
Vector3f target_cm; Vector3f target_cm;
Vector3f target_vel_cms;
bool slow_descent:1; bool slow_descent:1;
bool pilot_correction_active; bool pilot_correction_active;
} poscontrol; } poscontrol;