Sub: seperate kinimatic shaping from pid limit setting

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();

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;

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();

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();

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();

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();