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

Plane: use pos control for most of LAND_FINAL

Browse Source 
this gives more accurate landing with some velocity drift
This commit is contained in:
Andrew Tridgell 2022-03-09 16:56:47 +11:00
parent ce4580efd2
commit 1d2a64951c
2 changed files with 20 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduPlane/quadplane.cpp
View File

@ -2178,6 +2178,10 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s)
} else if (s == QPOS_LAND_DESCEND) {
// reset throttle descent control
qp.thr_ctrl_land = false;
} else if (s == QPOS_LAND_FINAL) {
// remember last pos reset to handle GPS glitch in LAND_FINAL
Vector2f rpos;
last_pos_reset_ms = plane.ahrs.getLastPosNorthEastReset(rpos);
}
qp.log_QPOS();
}
@ -2510,10 +2514,23 @@ void QuadPlane::vtol_position_controller(void)
if (should_relax()) {
pos_control->relax_velocity_controller_xy();
} else {
// we use velocity control in QPOS_LAND_FINAL to allow for GPS glitch handling
Vector2f zero;
Vector2f vel_cms = poscontrol.target_vel_cms.xy() + landing_velocity*100;
Vector2f rpos;
const uint32_t last_reset_ms = plane.ahrs.getLastPosNorthEastReset(rpos);
/* we use velocity control when we may be touching the
ground or if we've had a position reset from AHRS. This
helps us handle a GPS glitch in the final land phase,
and also prevents trying to reposition after touchdown
*/
if (motors->limit.throttle_lower ||
motors->get_throttle() < 0.5*motors->get_throttle_hover() ||
last_reset_ms != poscontrol.last_pos_reset_ms) {
pos_control->input_vel_accel_xy(vel_cms, zero);
} else {
// otherwise use full pos control
pos_control->input_pos_vel_accel_xy(poscontrol.target_cm.xy(), vel_cms, zero);
}
}
run_xy_controller();

1
ArduPlane/quadplane.h
View File

@ -452,6 +452,7 @@ private:
uint32_t last_velocity_match_ms;
float target_speed;
float target_accel;
uint32_t last_pos_reset_ms;
private:
uint32_t last_state_change_ms;
enum position_control_state state;