AC_PosControl: minor comment fixes

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -820,7 +820,7 @@ void AC_PosControl::relax_z_controller(float throttle_setting)
 void AC_PosControl::init_z_controller()
 {
     _pos_target.z = _inav.get_position_z_up_cm();
-    _pos_desired.z = _pos_target.z - _pos_offset.z;
+    _pos_desired.z = _pos_target.z - _pos_offset.z;     // _pos_terrain is not included because it is initialised to zero below 
 
     _vel_target.z = _inav.get_velocity_z_up_cms();
     _vel_desired.z = _vel_target.z - _vel_offset.z;
@@ -941,7 +941,7 @@ void AC_PosControl::set_alt_target_with_slew(float pos)
     input_pos_vel_accel_z(pos, zero, 0);
 }
 
-/// update_z_offsets - updates the vertical offsets used by terrain following
+/// update_offsets_z - updates the vertical offsets used by terrain following
 void AC_PosControl::update_offsets_z()
 {
     // check for offset target timeout
@@ -957,7 +957,7 @@ void AC_PosControl::update_offsets_z()
     update_pos_vel_accel(p_offset_z, _vel_offset.z, _accel_offset.z, _dt, MIN(_limit_vector.z, 0.0f), _p_pos_z.get_error(), _pid_vel_z.get_error());
     _pos_offset.z = p_offset_z;
 
-    // input shape horizontal position, velocity and acceleration offsets
+    // input shape vertical position, velocity and acceleration offsets
     shape_pos_vel_accel(_pos_offset_target.z, _vel_offset_target.z, _accel_offset_target.z,
         _pos_offset.z, _vel_offset.z, _accel_offset.z,
         get_max_speed_down_cms(), get_max_speed_up_cms(),
@@ -1129,7 +1129,7 @@ void AC_PosControl::init_terrain()
     _accel_terrain = 0.0;
 }
 
-// remove offsets from the position velocity and acceleration targets
+// init_pos_terrain_cm - initialises the current terrain altitude and target altitude to pos_offset_terrain_cm
 void AC_PosControl::init_pos_terrain_cm(float pos_terrain_cm)
 {
     _pos_desired.z -= (pos_terrain_cm - _pos_terrain);
@@ -1496,11 +1496,8 @@ void AC_PosControl::handle_ekf_xy_reset()
     uint32_t reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);
     if (reset_ms != _ekf_xy_reset_ms) {
 
-        // if we want to move EKF steps to the offsets for modes setting absolute position and velocity
-        // we need some sort of switch to select what type of EKF handling we use
-        // To minimise estimated position errors for Auto, RTL and Guided position control.
-        //_pos_offset.xy() += (_inav.get_position_xy_cm() + _p_pos_xy.get_error()).topostype() - _pos_target.xy() ;
-        //_vel_offset.xy() += (_inav.get_velocity_xy_cms() + _pid_vel_xy.get_error()) - _vel_target.xy();
+        // ToDo: move EKF steps into the offsets for modes setting absolute position and velocity
+        // for this we need some sort of switch to select what type of EKF handling we want to use
 
         // To zero real position shift during relative position modes like Loiter, PosHold, Guided velocity and accleration control.
         _pos_target.xy() = (_inav.get_position_xy_cm() + _p_pos_xy.get_error()).topostype();
@@ -1527,11 +1524,8 @@ void AC_PosControl::handle_ekf_z_reset()
     uint32_t reset_ms = _ahrs.getLastPosDownReset(alt_shift);
     if (reset_ms != 0 && reset_ms != _ekf_z_reset_ms) {
 
-        // if we want to move EKF steps to the offsets for modes setting absolute position and velocity
-        // we need some sort of switch to select what type of EKF handling we use
-        // To minimise estimated position errors for Auto, RTL and Guided position control.
-        //_pos_offset.z += (_inav.get_position_z_cm() + _p_pos_z.get_error()) - _pos_target.z ;
-        //_vel_offset.z += (_inav.get_velocity_z_cms() + _pid_vel_z.get_error()) - _vel_target.z;
+        // ToDo: move EKF steps into the offsets for modes setting absolute position and velocity
+        // for this we need some sort of switch to select what type of EKF handling we want to use
 
         // To zero real position shift during relative position modes like Loiter, PosHold, Guided velocity and accleration control.
         _pos_target.z = _inav.get_position_z_up_cm() + _p_pos_z.get_error();

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -255,14 +255,14 @@ class AC_PosControl
     /// set_pos_desired_xy_cm - sets the position target, frame NEU in cm relative to the EKF origin
     void set_pos_desired_xy_cm(const Vector2f& pos) { _pos_desired.xy() = pos.topostype(); }
 
-    /// get_pos_target_cm - returns the position target, frame NEU in cm relative to the EKF origin
+    /// get_pos_desired_cm - returns the position desired, frame NEU in cm relative to the EKF origin
     const Vector3p& get_pos_desired_cm() const { return _pos_desired; }
 
     /// get_pos_target_z_cm - get target altitude (in cm above the EKF origin)
     float get_pos_target_z_cm() const { return _pos_target.z; }
 
     /// set_pos_desired_z_cm - set altitude target in cm above the EKF origin
-    void set_pos_desired_z_cm(float pos) { _pos_desired.z = pos; }
+    void set_pos_desired_z_cm(float pos_z) { _pos_desired.z = pos_z; }
 
     /// get_pos_desired_z_cm - get target altitude (in cm above the EKF origin)
     float get_pos_desired_z_cm() const { return _pos_desired.z; }
@@ -541,8 +541,8 @@ class AC_PosControl
     float       _yaw_rate_target;       // desired yaw rate in centi-degrees per second calculated by position controller
 
     // position controller internal variables
-    Vector3p    _pos_desired;           // target location, frame NEU in cm relative to the EKF origin
-    Vector3p    _pos_target;            // target location, frame NEU in cm relative to the EKF origin
+    Vector3p    _pos_desired;           // desired location, frame NEU in cm relative to the EKF origin.  This is equal to the _pos_target minus offsets
+    Vector3p    _pos_target;            // target location, frame NEU in cm relative to the EKF origin.  This is equal to the _pos_desired plus offsets
     Vector3f    _vel_desired;           // desired velocity in NEU cm/s
     Vector3f    _vel_target;            // velocity target in NEU cm/s calculated by pos_to_rate step
     Vector3f    _accel_desired;         // desired acceleration in NEU cm/s/s (feed forward)