diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 47326d88a3..76f0b8f9aa 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -470,6 +470,7 @@ static const struct AP_Param::defaults_table_struct defaults_table[] = { { "Q_LOIT_SPEED", 500 }, { "Q_WP_SPEED", 500 }, { "Q_WP_ACCEL", 100 }, + { "Q_P_JERK_XY", 2 }, }; /* @@ -2175,7 +2176,8 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s) // never do a rate reset, if attitude control is not active it will be automaticaly reset before running, see: last_att_control_ms // if it is active then the rate control should not be reset at all qp.attitude_control->reset_yaw_target_and_rate(false); - pos1_start_speed = plane.ahrs.groundspeed_vector().length(); + pos1_speed_limit = plane.ahrs.groundspeed_vector().length(); + done_accel_init = false; } else if (s == QPOS_AIRBRAKE) { // start with zero integrator on vertical throttle qp.pos_control->get_accel_z_pid().set_integrator(0); @@ -2422,11 +2424,13 @@ void QuadPlane::vtol_position_controller(void) float target_speed = stopping_speed; // maximum configured VTOL speed - const float wp_speed = wp_nav->get_default_speed_xy() * 0.01; + const float wp_speed = MAX(1.0, wp_nav->get_default_speed_xy() * 0.01); 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); + // limit target speed to a the pos1 speed limit, which starts out at the initial speed + // but is adjusted if we start putting our nose down. We always allow at least twice + // the WP speed + target_speed = MIN(MAX(poscontrol.pos1_speed_limit, 2*wp_speed), target_speed); if (poscontrol.reached_wp_speed || rel_groundspeed_sq < sq(wp_speed) || @@ -2452,17 +2456,39 @@ void QuadPlane::vtol_position_controller(void) target_speed_xy_cms = diff_wp_norm * target_speed * 100; target_accel_cms = diff_wp_norm * (-target_accel*100); } + const float target_speed_ms = target_speed_xy_cms.length() * 0.01; target_speed_xy_cms += landing_velocity * 100; - poscontrol.target_speed = target_speed_xy_cms.length()*0.01; + poscontrol.target_speed = target_speed_ms; poscontrol.target_accel = target_accel; + if (!poscontrol.reached_wp_speed && + rel_groundspeed_sq < sq(target_speed_ms) && + rel_groundspeed_sq > sq(2*wp_speed) && + plane.nav_pitch_cd < 0) { + // we have slowed down more than expected, likely due to + // drag from the props and we're starting to put our nose + // down as a result. We want to accept the slowdown and + // re-calculate the target speed profile + poscontrol.pos1_speed_limit = sqrtf(rel_groundspeed_sq); + } + // use input shaping and abide by accel and jerk limits pos_control->input_vel_accel_xy(target_speed_xy_cms, target_accel_cms); // run horizontal velocity controller run_xy_controller(MAX(target_accel, transition_decel)*1.5); + if (!poscontrol.done_accel_init) { + /* + the pos controller init assumes zero accel, we need to + override that so that we can start decelerating more + quickly at the start of POSITION1 + */ + poscontrol.done_accel_init = true; + pos_control->set_accel_desired_xy_cmss(target_accel_cms); + } + // nav roll and pitch are controller by position controller plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 68b3ad5035..33bbc2296b 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -447,7 +447,8 @@ private: uint32_t last_log_ms; bool reached_wp_speed; uint32_t last_run_ms; - float pos1_start_speed; + float pos1_speed_limit; + bool done_accel_init; Vector2f velocity_match; uint32_t last_velocity_match_ms; float target_speed;