Sub: Use PosControl fixes

ArduSub/Attitude.cpp

@@ -147,8 +147,9 @@ float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_al
     }
 
     // do not let target altitude get too far from current altitude above ground
-    // Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition
+    target_rangefinder_alt = constrain_float(target_rangefinder_alt,
-    target_rangefinder_alt = constrain_float(target_rangefinder_alt,rangefinder_state.alt_cm-pos_control.get_leash_down_z(),rangefinder_state.alt_cm+pos_control.get_leash_up_z());
+        rangefinder_state.alt_cm - pos_control.get_pos_error_min_z_cm(),
+        rangefinder_state.alt_cm + pos_control.get_pos_error_max_z_cm());
 
     // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
     distance_error = (target_rangefinder_alt - rangefinder_state.alt_cm) - (current_alt_target - current_alt);

ArduSub/GCS_Mavlink.cpp

@@ -92,7 +92,7 @@ void GCS_MAVLINK_Sub::send_nav_controller_output() const
         targets.z * 1.0e-2f,
         sub.wp_nav.get_wp_bearing_to_destination() * 1.0e-2f,
         MIN(sub.wp_nav.get_wp_distance_to_destination() * 1.0e-2f, UINT16_MAX),
-        sub.pos_control.get_alt_error() * 1.0e-2f,
+        sub.pos_control.get_pos_error_z_cm() * 1.0e-2f,
         0,
         0);
 }
@@ -679,7 +679,7 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg)
          */
 
         if (!pos_ignore && sub.control_mode == ALT_HOLD) { // Control only target depth when in ALT_HOLD
-            sub.pos_control.set_alt_target(packet.alt*100);
+            sub.pos_control.set_pos_target_z_cm(packet.alt*100);
             break;
         }
 

ArduSub/Log.cpp

@@ -38,13 +38,13 @@ void Sub::Log_Write_Control_Tuning()
         angle_boost         : attitude_control.angle_boost(),
         throttle_out        : motors.get_throttle(),
         throttle_hover      : motors.get_throttle_hover(),
-        desired_alt         : pos_control.get_alt_target() / 100.0f,
+        desired_alt         : pos_control.get_pos_target_z_cm() / 100.0f,
         inav_alt            : inertial_nav.get_altitude() / 100.0f,
         baro_alt            : barometer.get_altitude(),
         desired_rangefinder_alt   : (int16_t)target_rangefinder_alt,
         rangefinder_alt           : rangefinder_state.alt_cm,
         terr_alt            : terr_alt,
-        target_climb_rate   : (int16_t)pos_control.get_vel_target_z(),
+        target_climb_rate   : (int16_t)pos_control.get_vel_target_z_cms(),
         climb_rate          : climb_rate
     };
     logger.WriteBlock(&pkt, sizeof(pkt));

ArduSub/Sub.cpp

@@ -34,7 +34,7 @@ Sub::Sub()
           inertial_nav(ahrs),
           ahrs_view(ahrs, ROTATION_NONE),
           attitude_control(ahrs_view, aparm, motors, MAIN_LOOP_SECONDS),
-          pos_control(ahrs_view, inertial_nav, motors, attitude_control),
+          pos_control(ahrs_view, inertial_nav, motors, attitude_control, MAIN_LOOP_SECONDS),
           wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control),
           loiter_nav(inertial_nav, ahrs_view, pos_control, attitude_control),
           circle_nav(inertial_nav, ahrs_view, pos_control),

ArduSub/control_acro.cpp

@@ -9,7 +9,7 @@
 bool Sub::acro_init()
 {
     // set target altitude to zero for reporting
-    pos_control.set_alt_target(0);
+    pos_control.set_pos_target_z_cm(0);
 
     // attitude hold inputs become thrust inputs in acro mode
     // set to neutral to prevent chaotic behavior (esp. roll/pitch)

ArduSub/control_althold.cpp

@@ -12,14 +12,15 @@ bool Sub::althold_init()
         return false;
     }
 
-    // initialize vertical speeds and leash lengths
+    // initialize vertical maximum speeds and acceleration
     // sets the maximum speed up and down returned by position controller
-    pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
+    pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
-    pos_control.set_max_accel_z(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);
 
     // initialise position and desired velocity
-    pos_control.set_alt_target(inertial_nav.get_altitude());
+    pos_control.init_z_controller();
-    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
 
     last_pilot_heading = ahrs.yaw_sensor;
 
@@ -33,15 +34,14 @@ void Sub::althold_run()
     uint32_t tnow = AP_HAL::millis();
 
     // initialize vertical speeds and acceleration
-    pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
+    pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
-    pos_control.set_max_accel_z(g.pilot_accel_z);
 
     if (!motors.armed()) {
         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
         // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
         attitude_control.set_throttle_out(0,true,g.throttle_filt);
         attitude_control.relax_attitude_controllers();
-        pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
+        pos_control.relax_z_controller(motors.get_throttle_hover());
         last_pilot_heading = ahrs.yaw_sensor;
         return;
     }
@@ -111,14 +111,14 @@ void Sub::control_depth() {
         // output pilot's throttle
         attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);
         // reset z targets to current values
-        pos_control.relax_alt_hold_controllers();
+        pos_control.relax_z_controller(channel_throttle->norm_input());
         engageStopZ = true;
         lastVelocityZWasNegative = is_negative(inertial_nav.get_velocity_z());
     } else { // hold z
 
         if (ap.at_bottom) {
-            pos_control.relax_alt_hold_controllers(); // clear velocity and position targets
+            pos_control.init_z_controller();
-            pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
+            pos_control.set_pos_target_z_cm(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
         }
 
         // Detects a zero derivative
@@ -127,7 +127,7 @@ void Sub::control_depth() {
         // or smaller input signals
         if(engageStopZ && (lastVelocityZWasNegative ^ is_negative(inertial_nav.get_velocity_z()))) {
             engageStopZ = false;
-            pos_control.relax_alt_hold_controllers();
+            pos_control.init_z_controller();
         }
 
         pos_control.update_z_controller();

ArduSub/control_auto.cpp

@@ -514,9 +514,9 @@ float Sub::get_auto_heading()
         float track_bearing = get_bearing_cd(wp_nav.get_wp_origin(), wp_nav.get_wp_destination());
 
         // Bearing from current position towards intermediate position target (centidegrees)
-        const Vector2f target_vel_xy{pos_control.get_vel_target().x, pos_control.get_vel_target().y};
+        const Vector2f target_vel_xy{pos_control.get_vel_target_cms().x, pos_control.get_vel_target_cms().y};
         float angle_error = 0.0f;
-        if (target_vel_xy.length() >= pos_control.get_max_speed_xy() * 0.1f) {
+        if (target_vel_xy.length() >= pos_control.get_max_speed_xy_cms() * 0.1f) {
             const float desired_angle_cd = degrees(target_vel_xy.angle()) * 100.0f;
             angle_error = wrap_180_cd(desired_angle_cd - track_bearing);
         }
@@ -565,15 +565,10 @@ bool Sub::auto_terrain_recover_start()
     loiter_nav.init_target();
 
     // Reset z axis controller
-    pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
+    pos_control.relax_z_controller(motors.get_throttle_hover());
 
-    // initialize vertical speeds and leash lengths
+    // initialize vertical maximum speeds and acceleration
-    pos_control.set_max_speed_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up());
+    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_max_accel_z(wp_nav.get_accel_z());
-
-    // Reset vertical position and velocity targets
-    pos_control.set_alt_target(inertial_nav.get_altitude());
-    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
 
     gcs().send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery");
     return true;
@@ -616,7 +611,7 @@ void Sub::auto_terrain_recover_run()
             // Start timer as soon as rangefinder is healthy
             if (rangefinder_recovery_ms == 0) {
                 rangefinder_recovery_ms = AP_HAL::millis();
-                pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); // Reset alt hold targets
+                pos_control.relax_z_controller(motors.get_throttle_hover()); // Reset alt hold targets
             }
 
             // 1.5 seconds of healthy rangefinder means we can resume mission with terrain enabled
@@ -663,7 +658,7 @@ void Sub::auto_terrain_recover_run()
 
     /////////////////////
     // update z target //
-    pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, true);
+    pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate, true);
     pos_control.update_z_controller();
 
     ////////////////////////////

ArduSub/control_circle.cpp

@@ -14,10 +14,8 @@ bool Sub::circle_init()
     circle_pilot_yaw_override = false;
 
     // initialize speeds and accelerations
-    pos_control.set_max_speed_xy(wp_nav.get_default_speed_xy());
+    pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration());
-    pos_control.set_max_accel_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_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
-    pos_control.set_max_accel_z(g.pilot_accel_z);
 
     // initialise circle controller including setting the circle center based on vehicle speed
     circle_nav.init();
@@ -33,10 +31,8 @@ void Sub::circle_run()
     float target_climb_rate = 0;
 
     // update parameters, to allow changing at runtime
-    pos_control.set_max_speed_xy(wp_nav.get_default_speed_xy());
+    pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration());
-    pos_control.set_max_accel_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_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
-    pos_control.set_max_accel_z(g.pilot_accel_z);
 
     // if not armed set throttle to zero and exit immediately
     if (!motors.armed()) {
@@ -45,7 +41,6 @@ void Sub::circle_run()
         // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
         attitude_control.set_throttle_out(0,true,g.throttle_filt);
         attitude_control.relax_attitude_controllers();
-        pos_control.set_alt_target_to_current_alt();
         return;
     }
 
@@ -83,6 +78,6 @@ void Sub::circle_run()
     }
 
     // update altitude target and call position controller
-    pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
+    pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
     pos_control.update_z_controller();
 }

ArduSub/control_guided.cpp

@@ -73,12 +73,12 @@ void Sub::guided_vel_control_start()
     // set guided_mode to velocity controller
     guided_mode = Guided_Velocity;
 
-    // initialize vertical speeds and leash lengths
+    // initialize vertical maximum speeds and acceleration
-    pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
+    pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
-    pos_control.set_max_accel_z(g.pilot_accel_z);
 
     // initialise velocity controller
-    pos_control.init_vel_controller_xyz();
+    pos_control.init_z_controller();
+    pos_control.init_xy_controller();
 }
 
 // initialise guided mode's posvel controller
@@ -87,22 +87,12 @@ void Sub::guided_posvel_control_start()
     // set guided_mode to velocity controller
     guided_mode = Guided_PosVel;
 
-    pos_control.init_xy_controller();
-
-    // set speed and acceleration from wpnav's speed and acceleration
-    pos_control.set_max_speed_xy(wp_nav.get_default_speed_xy());
-    pos_control.set_max_accel_xy(wp_nav.get_wp_acceleration());
-
-    const Vector3f& curr_pos = inertial_nav.get_position();
-    const Vector3f& curr_vel = inertial_nav.get_velocity();
-
-    // set target position and velocity to current position and velocity
-    pos_control.set_xy_target(curr_pos.x, curr_pos.y);
-    pos_control.set_desired_velocity_xy(curr_vel.x, curr_vel.y);
-
     // set vertical speed and acceleration
-    pos_control.set_max_speed_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up());
+    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_max_accel_z(wp_nav.get_accel_z());
+
+    // initialise velocity controller
+    pos_control.init_z_controller();
+    pos_control.init_xy_controller();
 
     // pilot always controls yaw
     set_auto_yaw_mode(AUTO_YAW_HOLD);
@@ -115,12 +105,10 @@ void Sub::guided_angle_control_start()
     guided_mode = Guided_Angle;
 
     // set vertical speed and acceleration
-    pos_control.set_max_speed_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up());
+    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_max_accel_z(wp_nav.get_accel_z());
 
-    // initialise position and desired velocity
+    // initialise velocity controller
-    pos_control.set_alt_target(inertial_nav.get_altitude());
+    pos_control.init_z_controller();
-    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
 
     // initialise targets
     guided_angle_state.update_time_ms = AP_HAL::millis();
@@ -204,7 +192,7 @@ void Sub::guided_set_velocity(const Vector3f& velocity)
     vel_update_time_ms = AP_HAL::millis();
 
     // set position controller velocity target
-    pos_control.set_desired_velocity(velocity);
+    pos_control.set_vel_desired_cms(velocity);
 }
 
 // set guided mode posvel target
@@ -229,7 +217,8 @@ bool Sub::guided_set_destination_posvel(const Vector3f& destination, const Vecto
     posvel_pos_target_cm = destination;
     posvel_vel_target_cms = velocity;
 
-    pos_control.set_pos_target(posvel_pos_target_cm);
+    pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm, posvel_vel_target_cms, Vector3f());
+    pos_control.input_pos_vel_accel_z(posvel_pos_target_cm, posvel_vel_target_cms, Vector3f());
 
     // log target
     Log_Write_GuidedTarget(guided_mode, destination, velocity);
@@ -293,6 +282,9 @@ void Sub::guided_pos_control_run()
         // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
         attitude_control.set_throttle_out(0,true,g.throttle_filt);
         attitude_control.relax_attitude_controllers();
+        // initialise velocity controller
+        pos_control.init_z_controller();
+        pos_control.init_xy_controller();
         return;
     }
 
@@ -338,12 +330,13 @@ void Sub::guided_vel_control_run()
 {
     // ifmotors not enabled set throttle to zero and exit immediately
     if (!motors.armed()) {
-        // initialise velocity controller
-        pos_control.init_vel_controller_xyz();
         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
         // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
         attitude_control.set_throttle_out(0,true,g.throttle_filt);
         attitude_control.relax_attitude_controllers();
+        // initialise velocity controller
+        pos_control.init_z_controller();
+        pos_control.init_xy_controller();
         return;
     }
 
@@ -362,12 +355,13 @@ void Sub::guided_vel_control_run()
 
     // set velocity to zero if no updates received for 3 seconds
     uint32_t tnow = AP_HAL::millis();
-    if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_desired_velocity().is_zero()) {
+    if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_vel_desired_cms().is_zero()) {
-        pos_control.set_desired_velocity(Vector3f(0,0,0));
+        pos_control.set_vel_desired_cms(Vector3f(0,0,0));
     }
 
     // call velocity controller which includes z axis controller
-    pos_control.update_vel_controller_xyz();
+    pos_control.update_xy_controller();
+    pos_control.update_z_controller();
 
     float lateral_out, forward_out;
     translate_pos_control_rp(lateral_out, forward_out);
@@ -392,13 +386,13 @@ void Sub::guided_posvel_control_run()
 {
     // if motors not enabled set throttle to zero and exit immediately
     if (!motors.armed()) {
-        // set target position and velocity to current position and velocity
-        pos_control.set_pos_target(inertial_nav.get_position());
-        pos_control.set_desired_velocity(Vector3f(0,0,0));
         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
         // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
         attitude_control.set_throttle_out(0,true,g.throttle_filt);
         attitude_control.relax_attitude_controllers();
+        // initialise velocity controller
+        pos_control.init_z_controller();
+        pos_control.init_xy_controller();
         return;
     }
 
@@ -423,22 +417,18 @@ void Sub::guided_posvel_control_run()
     }
 
     // calculate dt
-    float dt = pos_control.time_since_last_xy_update();
+    float dt = pos_control.get_dt();
-
-    // sanity check dt
-    if (dt >= 0.2f) {
-        dt = 0.0f;
-    }
 
     // advance position target using velocity target
     posvel_pos_target_cm += posvel_vel_target_cms * dt;
 
     // send position and velocity targets to position controller
-    pos_control.set_pos_target(posvel_pos_target_cm);
+    pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm, posvel_vel_target_cms, Vector3f());
-    pos_control.set_desired_velocity_xy(posvel_vel_target_cms.x, posvel_vel_target_cms.y);
+    pos_control.input_pos_vel_accel_z(posvel_pos_target_cm, posvel_vel_target_cms, Vector3f());
 
     // run position controller
     pos_control.update_xy_controller();
+    pos_control.update_z_controller();
 
     float lateral_out, forward_out;
     translate_pos_control_rp(lateral_out, forward_out);
@@ -447,8 +437,6 @@ void Sub::guided_posvel_control_run()
     motors.set_lateral(lateral_out);
     motors.set_forward(forward_out);
 
-    pos_control.update_z_controller();
-
     // call attitude controller
     if (auto_yaw_mode == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
@@ -469,7 +457,8 @@ void Sub::guided_angle_control_run()
         // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
         attitude_control.set_throttle_out(0.0f,true,g.throttle_filt);
         attitude_control.relax_attitude_controllers();
-        pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
+        // initialise velocity controller
+        pos_control.init_z_controller();
         return;
     }
 
@@ -505,7 +494,7 @@ void Sub::guided_angle_control_run()
     attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);
 
     // call position controller
-    pos_control.set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);
+    pos_control.set_pos_target_z_from_climb_rate_cm(climb_rate_cms, false);
     pos_control.update_z_controller();
 }
 

ArduSub/control_manual.cpp

@@ -4,7 +4,7 @@
 bool Sub::manual_init()
 {
     // set target altitude to zero for reporting
-    pos_control.set_alt_target(0);
+    pos_control.set_pos_target_z_cm(0);
 
     // attitude hold inputs become thrust inputs in manual mode
     // set to neutral to prevent chaotic behavior (esp. roll/pitch)

ArduSub/control_poshold.cpp

@@ -13,22 +13,19 @@ bool Sub::poshold_init()
     if (!position_ok()) {
         return false;
     }
-    pos_control.init_vel_controller_xyz();
-    pos_control.set_desired_velocity_xy(0, 0);
-    pos_control.set_target_to_stopping_point_xy();
 
     // initialize vertical speeds and acceleration
-    pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
+    pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration());
-    pos_control.set_max_accel_z(g.pilot_accel_z);
+    pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
 
     // initialise position and desired velocity
-    pos_control.set_alt_target(inertial_nav.get_altitude());
+    pos_control.init_xy_controller_stopping_point();
-    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
+    pos_control.init_z_controller();
 
     // Stop all thrusters
     attitude_control.set_throttle_out(0.5f ,true, g.throttle_filt);
     attitude_control.relax_attitude_controllers();
-    pos_control.relax_alt_hold_controllers();
+    pos_control.relax_z_controller(0.5f);
 
     last_pilot_heading = ahrs.yaw_sensor;
 
@@ -46,8 +43,8 @@ void Sub::poshold_run()
         // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
         attitude_control.set_throttle_out(0.5f ,true, g.throttle_filt);
         attitude_control.relax_attitude_controllers();
-        pos_control.set_target_to_stopping_point_xy();
+        pos_control.init_xy_controller_stopping_point();
-        pos_control.relax_alt_hold_controllers();
+        pos_control.relax_z_controller(0.5f);
         last_pilot_heading = ahrs.yaw_sensor;
         return;
     }
@@ -63,19 +60,15 @@ void Sub::poshold_run()
     float lateral_out = 0;
     float forward_out = 0;
 
-    pos_control.set_desired_velocity_xy(0,0);
-
     if (position_ok()) {
         // Allow pilot to reposition the sub
         if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) {
-            pos_control.set_target_to_stopping_point_xy();
+            pos_control.init_xy_controller_stopping_point();
         }
         translate_pos_control_rp(lateral_out, forward_out);
         pos_control.update_xy_controller();
     } else {
-        pos_control.init_vel_controller_xyz();
+        pos_control.init_xy_controller_stopping_point();
-        pos_control.set_desired_velocity_xy(0, 0);
-        pos_control.set_target_to_stopping_point_xy();
     }
     motors.set_forward(forward_out + pilot_forward);
     motors.set_lateral(lateral_out + pilot_lateral);

ArduSub/control_stabilize.cpp

@@ -4,7 +4,7 @@
 bool Sub::stabilize_init()
 {
     // set target altitude to zero for reporting
-    pos_control.set_alt_target(0);
+    pos_control.set_pos_target_z_cm(0);
     last_pilot_heading = ahrs.yaw_sensor;
 
     return true;

ArduSub/control_surface.cpp

@@ -7,13 +7,11 @@ bool Sub::surface_init()
         return false;
     }
 
-    // initialize vertical speeds and leash lengths
+    // initialize vertical speeds and acceleration
-    pos_control.set_max_speed_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up());
+    pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
-    pos_control.set_max_accel_z(wp_nav.get_accel_z());
 
     // initialise position and desired velocity
-    pos_control.set_alt_target(inertial_nav.get_altitude());
+    pos_control.init_z_controller();
-    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
 
     return true;
 
@@ -30,6 +28,7 @@ void Sub::surface_run()
         motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
         attitude_control.set_throttle_out(0,true,g.throttle_filt);
         attitude_control.relax_attitude_controllers();
+        pos_control.relax_z_controller(motors.get_throttle_hover());
         return;
     }
 
@@ -49,13 +48,13 @@ void Sub::surface_run()
     attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
 
     // set target climb rate
-    float cmb_rate = constrain_float(fabsf(wp_nav.get_default_speed_up()), 1, pos_control.get_max_speed_up());
+    float cmb_rate = constrain_float(fabsf(wp_nav.get_default_speed_up()), 1, pos_control.get_max_speed_up_cms());
 
     // record desired climb rate for logging
     desired_climb_rate = cmb_rate;
 
     // update altitude target and call position controller
-    pos_control.set_alt_target_from_climb_rate_ff(cmb_rate, G_Dt, true);
+    pos_control.set_pos_target_z_from_climb_rate_cm(cmb_rate, true);
     pos_control.update_z_controller();
 
     // pilot has control for repositioning

ArduSub/system.cpp

@@ -94,8 +94,6 @@ void Sub::init_ardupilot()
     wp_nav.set_terrain(&terrain);
 #endif
 
-    pos_control.set_dt(MAIN_LOOP_SECONDS);
-
     // init the optical flow sensor
 #if OPTFLOW == ENABLED
     init_optflow();