AC_PosControl: combine z position control into single method

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -482,12 +482,12 @@ void AC_PosControl::update_z_controller()
     // check if leash lengths need to be recalculated
     calc_leash_length_z();
 
-    // call position controller
-    pos_to_rate_z();
+    // call z-axis position controller
+    run_z_controller();
 }
 
 /// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
-///     called by pos_to_rate_z if z-axis speed or accelerations are changed
+///     called by update_z_controller if z-axis speed or accelerations are changed
 void AC_PosControl::calc_leash_length_z()
 {
     if (_flags.recalc_leash_z) {
@@ -497,10 +497,11 @@ void AC_PosControl::calc_leash_length_z()
     }
 }
 
-// pos_to_rate_z - position to rate controller for Z axis
+// run position control for Z axis
+// target altitude should be set with one of these functions: set_alt_target, set_target_to_stopping_point_z, init_takeoff
 // calculates desired rate in earth-frame z axis and passes to rate controller
 // vel_up_max, vel_down_max should have already been set before calling this method
-void AC_PosControl::pos_to_rate_z()
+void AC_PosControl::run_z_controller()
 {
     float curr_alt = _inav.get_altitude();
 
@@ -544,17 +545,11 @@ void AC_PosControl::pos_to_rate_z()
         _vel_target.z += _vel_desired.z;
     }
 
-    // call rate based throttle controller which will update accel based throttle controller targets
-    rate_to_accel_z();
-}
+    // the following section calculates acceleration required to achieve the velocity target
 
-// rate_to_accel_z - calculates desired accel required to achieve the velocity target
-// calculates desired acceleration and calls accel throttle controller
-void AC_PosControl::rate_to_accel_z()
-{
     const Vector3f& curr_vel = _inav.get_velocity();
-    float p;                                // used to capture pid values for logging
 
+    // TODO: remove velocity derivative calculation
     // reset last velocity target to current target
     if (_flags.reset_rate_to_accel_z) {
         _vel_last.z = _vel_target.z;
@@ -586,20 +581,12 @@ void AC_PosControl::rate_to_accel_z()
         _vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z, _dt);
     }
 
-    // calculate p
-    p = _p_vel_z.kP() * _vel_error.z;
+    _accel_target.z = _p_vel_z.get_p(_vel_error.z);
 
-    // consolidate and constrain target acceleration
-    _accel_target.z = _accel_desired.z + p;
+    _accel_target.z += _accel_desired.z;
 
-    // set target for accel based throttle controller
-    accel_to_throttle(_accel_target.z);
-}
 
-// accel_to_throttle - alt hold's acceleration controller
-// calculates a desired throttle which is sent directly to the motors
-void AC_PosControl::accel_to_throttle(float accel_target_z)
-{
+    // the following section calculates a desired throttle needed to achieve the acceleration target
     float z_accel_meas;         // actual acceleration
     float p,i,d;              // used to capture pid values for logging
 
@@ -613,12 +600,12 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
         _flags.reset_accel_to_throttle = false;
     } else {
         // calculate accel error
-        _accel_error.z = accel_target_z - z_accel_meas;
+        _accel_error.z = _accel_target.z - z_accel_meas;
     }
 
     // set input to PID
     _pid_accel_z.set_input_filter_all(_accel_error.z);
-    _pid_accel_z.set_desired_rate(accel_target_z);
+    _pid_accel_z.set_desired_rate(_accel_target.z);
 
     // separately calculate p, i, d values for logging
     p = _pid_accel_z.get_p();
@@ -647,7 +634,7 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
 }
 
 ///
-/// position controller
+/// lateral position controller
 ///
 
 /// set_accel_xy - set horizontal acceleration in cm/s/s
@@ -1000,7 +987,7 @@ void AC_PosControl::run_xy_controller(float dt, float ekfNavVelGainScaler)
     _vel_target.x += _vel_desired.x;
     _vel_target.y += _vel_desired.y;
 
-    // this section converts desired velocities in lat/lon directions to accelerations in lat/lon frame
+    // the following section converts desired velocities in lat/lon directions to accelerations in lat/lon frame
 
     Vector2f accel_target, vel_xy_p, vel_xy_i, vel_xy_d;
 
@@ -1057,7 +1044,7 @@ void AC_PosControl::run_xy_controller(float dt, float ekfNavVelGainScaler)
     _accel_target.x += _accel_desired.x;
     _accel_target.y += _accel_desired.y;
 
-    // This section converts desired accelerations provided in lat/lon frame to roll/pitch angles
+    // the following section converts desired accelerations provided in lat/lon frame to roll/pitch angles
 
     float accel_right, accel_forward;
 

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -87,7 +87,7 @@ public:
     float get_accel_z() const { return _accel_z_cms; }
 
     /// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
-    ///     called by pos_to_rate_z if z-axis speed or accelerations are changed
+    ///     called by update_z_controller if z-axis speed or accelerations are changed
     void calc_leash_length_z();
 
     /// set_alt_target - set altitude target in cm above home
@@ -317,18 +317,12 @@ protected:
     /// z controller private methods
     ///
 
-    // pos_to_rate_z - position to rate controller for Z axis
-    // target altitude should be placed into _pos_target.z using or set with one of these functions
+    // run position control for Z axis
+    // target altitude should be set with one of these functions
     //          set_alt_target
     //          set_target_to_stopping_point_z
     //          init_takeoff
-    void pos_to_rate_z();
-
-    // rate_to_accel_z - calculates desired accel required to achieve the velocity target
-    void rate_to_accel_z();
-
-    // accel_to_throttle - alt hold's acceleration controller
-    void accel_to_throttle(float accel_target_z);
+    void run_z_controller();
 
     ///
     /// xy controller private methods