From e1000d25ed1a34e4ccece5d1efd29acac0bbd4f7 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 8 Jul 2021 13:46:08 +0930 Subject: [PATCH] Sub: seperate kinimatic shaping from pid limit setting --- ArduSub/control_althold.cpp | 4 +--- ArduSub/control_auto.cpp | 1 + ArduSub/control_circle.cpp | 2 ++ ArduSub/control_guided.cpp | 3 +++ ArduSub/control_poshold.cpp | 2 ++ ArduSub/control_surface.cpp | 1 + 6 files changed, 10 insertions(+), 3 deletions(-) diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index 268c06a170..c72a2401e8 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -15,9 +15,7 @@ bool Sub::althold_init() // initialize vertical maximum speeds and acceleration // sets the maximum speed up and down returned by position controller pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); - - // initialize vertical speeds and acceleration - pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity pos_control.init_z_controller(); diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index a381cb4e0d..8f100a5432 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -572,6 +572,7 @@ bool Sub::auto_terrain_recover_start() // initialize vertical maximum speeds and acceleration pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); + pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); gcs().send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery"); return true; diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp index 0dec0cd2b8..9f057d9111 100644 --- a/ArduSub/control_circle.cpp +++ b/ArduSub/control_circle.cpp @@ -15,7 +15,9 @@ bool Sub::circle_init() // initialize speeds and accelerations 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.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise circle controller including setting the circle center based on vehicle speed circle_nav.init(); diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index 13337b61b4..987c9b2383 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -74,6 +74,7 @@ void Sub::guided_vel_control_start() // initialize vertical maximum speeds and acceleration pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise velocity controller pos_control.init_z_controller(); @@ -88,6 +89,7 @@ void Sub::guided_posvel_control_start() // set vertical speed and acceleration pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); + pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); // initialise velocity controller pos_control.init_z_controller(); @@ -105,6 +107,7 @@ void Sub::guided_angle_control_start() // set vertical speed and acceleration pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); + pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); // initialise velocity controller pos_control.init_z_controller(); diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp index e70d3589a3..46121952ae 100644 --- a/ArduSub/control_poshold.cpp +++ b/ArduSub/control_poshold.cpp @@ -16,7 +16,9 @@ bool Sub::poshold_init() // initialize vertical speeds and acceleration 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.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity pos_control.init_xy_controller_stopping_point(); diff --git a/ArduSub/control_surface.cpp b/ArduSub/control_surface.cpp index d3becc5366..6c1cca1109 100644 --- a/ArduSub/control_surface.cpp +++ b/ArduSub/control_surface.cpp @@ -9,6 +9,7 @@ bool Sub::surface_init() // initialize vertical speeds and acceleration pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); + pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity pos_control.init_z_controller();