diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index f98197033e..0bce362152 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2194,12 +2194,13 @@ void QuadPlane::poscontrol_init_approach(void) */ void QuadPlane::log_QPOS(void) { - AP::logger().WriteStreaming("QPOS", "TimeUS,State,Dist,TSpd,TAcc", "QBfff", + AP::logger().WriteStreaming("QPOS", "TimeUS,State,Dist,TSpd,TAcc,OShoot", "QBfffB", AP_HAL::micros64(), poscontrol.get_state(), plane.auto_state.wp_distance, poscontrol.target_speed, - poscontrol.target_accel); + poscontrol.target_accel, + poscontrol.overshoot); } /* @@ -2237,6 +2238,7 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s) state = s; qp.log_QPOS(); last_log_ms = now; + overshoot = false; } last_state_change_ms = now; @@ -2458,6 +2460,7 @@ void QuadPlane::vtol_position_controller(void) const float distance = diff_wp.length(); const Vector2f rel_groundspeed_vector = landing_closing_velocity(); const float rel_groundspeed_sq = rel_groundspeed_vector.length_squared(); + const float closing_groundspeed = rel_groundspeed_vector * diff_wp.normalized(); // calculate speed we should be at to reach the position2 // target speed at the position2 distance threshold, assuming @@ -2493,11 +2496,31 @@ void QuadPlane::vtol_position_controller(void) Vector2f target_speed_xy_cms; Vector2f target_accel_cms; - const float target_accel = accel_needed(distance, rel_groundspeed_sq); + bool have_target_yaw = false; + float target_yaw_deg; + const float target_accel = accel_needed(distance, sq(closing_groundspeed)); if (distance > 0.1) { Vector2f diff_wp_norm = diff_wp.normalized(); target_speed_xy_cms = diff_wp_norm * target_speed * 100; target_accel_cms = diff_wp_norm * (-target_accel*100); + target_yaw_deg = degrees(diff_wp_norm.angle()); + const float yaw_err_deg = wrap_180(target_yaw_deg - degrees(plane.ahrs.yaw)); + bool overshoot = (closing_groundspeed < 0 || fabsf(yaw_err_deg) > 60); + if (overshoot && !poscontrol.overshoot) { + gcs().send_text(MAV_SEVERITY_INFO,"VTOL Overshoot d=%.1f cs=%.1f yerr=%.1f", + distance, closing_groundspeed, yaw_err_deg); + poscontrol.overshoot = true; + } + if (poscontrol.overshoot) { + /* we have overshot the landing point or our nose is + off by more than 60 degrees. Zero target accel and + point nose at the landing point. Set target speed + to our position2 threshold speed + */ + target_accel_cms.zero(); + target_speed_xy_cms = diff_wp_norm * position2_target_speed * 100; + have_target_yaw = true; + } } const float target_speed_ms = target_speed_xy_cms.length() * 0.01; @@ -2541,10 +2564,17 @@ void QuadPlane::vtol_position_controller(void) } // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, - plane.nav_pitch_cd, - desired_auto_yaw_rate_cds() + get_weathervane_yaw_rate_cds()); - if ((plane.auto_state.wp_distance < position2_dist_threshold) && tiltrotor.tilt_angle_achieved()) { + if (have_target_yaw) { + attitude_control->input_euler_angle_roll_pitch_yaw(plane.nav_roll_cd, + plane.nav_pitch_cd, + target_yaw_deg*100, true); + } else { + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, + plane.nav_pitch_cd, + desired_auto_yaw_rate_cds() + get_weathervane_yaw_rate_cds()); + } + if ((plane.auto_state.wp_distance < position2_dist_threshold) && tiltrotor.tilt_angle_achieved() && + fabsf(rel_groundspeed_sq) < sq(3*position2_target_speed)) { // if continuous tiltrotor only advance to position 2 once tilts have finished moving poscontrol.set_state(QPOS_POSITION2); poscontrol.pilot_correction_done = false; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 2ee4d863f2..7dd4e53126 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -478,6 +478,7 @@ private: float target_speed; float target_accel; uint32_t last_pos_reset_ms; + bool overshoot; private: uint32_t last_state_change_ms; enum position_control_state state;