From 7db568c85a18a49fa3f1eb270614265cdd92d821 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 8 Jul 2021 13:46:41 +0930 Subject: [PATCH] Plane: seperate kinimatic shaping from pid limit setting --- ArduPlane/qautotune.cpp | 3 +++ ArduPlane/quadplane.cpp | 8 ++++++++ 2 files changed, 11 insertions(+) diff --git a/ArduPlane/qautotune.cpp b/ArduPlane/qautotune.cpp index 3f9a385202..141ae621d8 100644 --- a/ArduPlane/qautotune.cpp +++ b/ArduPlane/qautotune.cpp @@ -45,6 +45,9 @@ void QAutoTune::init_z_limits() plane.quadplane.pos_control->set_max_speed_accel_z(-plane.quadplane.get_pilot_velocity_z_max_dn(), plane.quadplane.pilot_velocity_z_max_up, plane.quadplane.pilot_accel_z); + plane.quadplane.pos_control->set_correction_speed_accel_z(-plane.quadplane.get_pilot_velocity_z_max_dn(), + plane.quadplane.pilot_velocity_z_max_up, + plane.quadplane.pilot_accel_z); } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 11a0f41b5b..64800a4fb2 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1134,6 +1134,7 @@ void QuadPlane::init_hover(void) { // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); + pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); set_climb_rate_cms(0, false); init_throttle_wait(); @@ -1320,6 +1321,7 @@ void QuadPlane::init_loiter(void) // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); + pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); init_throttle_wait(); @@ -2562,6 +2564,7 @@ void QuadPlane::run_xy_controller(void) { if (!pos_control->is_active_xy()) { pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); pos_control->init_xy_controller(); } pos_control->update_xy_controller(); @@ -2600,6 +2603,8 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s) // back to normal qp.pos_control->set_max_speed_accel_xy(qp.wp_nav->get_default_speed_xy(), qp.wp_nav->get_wp_acceleration()); + qp.pos_control->set_correction_speed_accel_xy(qp.wp_nav->get_default_speed_xy(), + qp.wp_nav->get_wp_acceleration()); } else if (s == QPOS_AIRBRAKE) { // start with zero integrator on vertical throttle qp.pos_control->get_accel_z_pid().set_integrator(0); @@ -2907,6 +2912,7 @@ void QuadPlane::vtol_position_controller(void) const float scaled_wp_speed = get_scaled_wp_speed(degrees(diff_wp.angle())); pos_control->set_max_speed_accel_xy(scaled_wp_speed*100, wp_nav->get_wp_acceleration()); + pos_control->set_correction_speed_accel_xy(scaled_wp_speed*100, wp_nav->get_wp_acceleration()); run_xy_controller(); @@ -3098,6 +3104,7 @@ void QuadPlane::setup_target_position(void) // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); + pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); } /* @@ -3275,6 +3282,7 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd) // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); + pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); // initialise the vertical position controller pos_control->init_z_controller();