Sub: Fix before squash

ArduSub/Attitude.cpp

@@ -148,8 +148,8 @@ 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
     target_rangefinder_alt = constrain_float(target_rangefinder_alt,
-        rangefinder_state.alt_cm - pos_control.get_pos_error_min_z_cm(),
-        rangefinder_state.alt_cm + pos_control.get_pos_error_max_z_cm());
+        rangefinder_state.alt_cm - pos_control.get_pos_error_z_down_cm(),
+        rangefinder_state.alt_cm + pos_control.get_pos_error_z_up_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/control_auto.cpp

@@ -148,7 +148,8 @@ void Sub::auto_wp_run()
     motors.set_lateral(lateral_out);
     motors.set_forward(forward_out);
 
-    // call z-axis position controller (wpnav should have already updated it's alt target)
+    // WP_Nav has set the vertical position control targets
+    // run the vertical position controller and set output throttle
     pos_control.update_z_controller();
 
     ////////////////////////////
@@ -242,7 +243,8 @@ void Sub::auto_circle_run()
     motors.set_lateral(lateral_out);
     motors.set_forward(forward_out);
 
-    // call z-axis position controller
+    // WP_Nav has set the vertical position control targets
+    // run the vertical position controller and set output throttle
     pos_control.update_z_controller();
 
     // roll & pitch from waypoint controller, yaw rate from pilot
@@ -329,7 +331,8 @@ void Sub::auto_loiter_run()
     motors.set_lateral(lateral_out);
     motors.set_forward(forward_out);
 
-    // call z-axis position controller (wpnav should have already updated it's alt target)
+    // WP_Nav has set the vertical position control targets
+    // run the vertical position controller and set output throttle
     pos_control.update_z_controller();
 
     // get pilot desired lean angles

ArduSub/control_guided.cpp

@@ -311,7 +311,8 @@ void Sub::guided_pos_control_run()
     motors.set_lateral(lateral_out);
     motors.set_forward(forward_out);
 
-    // call z-axis position controller (wpnav should have already updated it's alt target)
+    // WP_Nav has set the vertical position control targets
+    // run the vertical position controller and set output throttle
     pos_control.update_z_controller();
 
     // call attitude controller
@@ -416,11 +417,8 @@ void Sub::guided_posvel_control_run()
         posvel_vel_target_cms.zero();
     }
 
-    // calculate dt
-    float dt = pos_control.get_dt();
-
     // advance position target using velocity target
-    posvel_pos_target_cm += posvel_vel_target_cms * dt;
+    posvel_pos_target_cm += posvel_vel_target_cms * pos_control.get_dt();
 
     // send position and velocity targets to position controller
     pos_control.input_pos_vel_accel_xy(posvel_pos_target_cm, posvel_vel_target_cms, Vector3f());

ArduSub/control_surface.cpp

@@ -28,7 +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());
+        pos_control.init_z_controller();
         return;
     }