Plane: limiting POS1 landing target speed

prevent very high target speeds when the target velocity profile is
above the initial speed in POSITION1. Always allow up to 2*Q_WP_SPEED,
but don't go above the initial speed
This commit is contained in:
Andrew Tridgell 2021-11-23 09:53:50 +11:00 committed by Randy Mackay
parent e95c4658a9
commit 477599b3d7
2 changed files with 5 additions and 0 deletions

View File

@ -2622,6 +2622,7 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s)
if (s == QPOS_POSITION1) { if (s == QPOS_POSITION1) {
reached_wp_speed = false; reached_wp_speed = false;
qp.attitude_control->reset_yaw_target_and_rate(); qp.attitude_control->reset_yaw_target_and_rate();
pos1_start_speed = plane.ahrs.groundspeed_vector().length();
} else if (s == QPOS_POSITION2) { } else if (s == QPOS_POSITION2) {
// POSITION2 changes target speed, so we need to change it // POSITION2 changes target speed, so we need to change it
// back to normal // back to normal
@ -2846,6 +2847,9 @@ void QuadPlane::vtol_position_controller(void)
const float current_speed_sq = plane.ahrs.groundspeed_vector().length_squared(); const float current_speed_sq = plane.ahrs.groundspeed_vector().length_squared();
const float scaled_wp_speed = get_scaled_wp_speed(degrees(diff_wp.angle())); const float scaled_wp_speed = get_scaled_wp_speed(degrees(diff_wp.angle()));
// limit target speed to initial pos1 speed, but at least twice the Q_WP_SPEED
target_speed = MIN(MAX(poscontrol.pos1_start_speed, 2*wp_speed), target_speed);
if (poscontrol.reached_wp_speed || if (poscontrol.reached_wp_speed ||
current_speed_sq < sq(wp_speed) || current_speed_sq < sq(wp_speed) ||
wp_speed > 1.35*scaled_wp_speed) { wp_speed > 1.35*scaled_wp_speed) {

View File

@ -492,6 +492,7 @@ private:
uint32_t last_log_ms; uint32_t last_log_ms;
bool reached_wp_speed; bool reached_wp_speed;
uint32_t last_run_ms; uint32_t last_run_ms;
float pos1_start_speed;
private: private:
uint32_t last_state_change_ms; uint32_t last_state_change_ms;
enum position_control_state state; enum position_control_state state;