fixes

ArduCopter/Copter.h

@@ -266,10 +266,6 @@ class Copter : public AP_Vehicle {
         //   measured ground or ceiling level measured using the range finder.
         void update_surface_offset();
 
-        // get/set target altitude (in cm) above ground
-        bool get_target_alt_cm(float &target_alt_cm) const;
-        void set_target_alt_cm(float target_alt_cm);
-
         // get target and actual distances (in m) for logging purposes
         bool get_target_dist_for_logging(float &target_dist) const;
         float get_dist_for_logging() const;

ArduCopter/mode_auto.cpp

@@ -1492,6 +1492,7 @@ bool ModeAuto::shift_alt_to_current_alt(Location& target_loc) const
 // should be used when the location will be used as a target for the position controller
 void ModeAuto::subtract_pos_offsets(Location& target_loc) const
 {
+    // QUESTION: do we need to remove terrain altitude here too?
     // subtract position controller offsets from target location
     target_loc.offset(-pos_control->get_pos_offset_cm() * 0.01);
 }

ArduCopter/mode_poshold.cpp

@@ -573,7 +573,7 @@ void ModePosHold::update_wind_comp_estimate()
     }
 
     // get position controller accel target
-    const Vector3f accel_target = pos_control->get_accel_target_cmss();
+    const Vector3f& accel_target = pos_control->get_accel_target_cmss();
 
     // update wind compensation in earth-frame lean angles
     if (is_zero(wind_comp_ef.x)) {

ArduCopter/mode_zigzag.cpp

@@ -244,11 +244,6 @@ void ModeZigZag::return_to_manual_control(bool maintain_target)
         if (maintain_target) {
             const Vector3f& wp_dest = wp_nav->get_wp_destination();
             loiter_nav->init_target(wp_dest.xy());
-#if AP_RANGEFINDER_ENABLED
-            if (wp_nav->origin_and_destination_are_terrain_alt()) {
-                copter.surface_tracking.set_target_alt_cm(wp_dest.z);
-            }
-#endif
         } else {
             loiter_nav->init_target();
         }
@@ -455,17 +450,8 @@ bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Ve
         terrain_alt = wp_nav->origin_and_destination_are_terrain_alt();
         next_dest.z = wp_nav->get_wp_destination().z;
     } else {
-        // if we have a downward facing range finder then use terrain altitude targets
         terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy();
-        if (terrain_alt) {
-#if AP_RANGEFINDER_ENABLED
-            if (!copter.surface_tracking.get_target_alt_cm(next_dest.z)) {
-                next_dest.z = copter.rangefinder_state.alt_cm_filt.get();
-            }
-#endif
-        } else {
-            next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_desired_z_cm() : inertial_nav.get_position_z_up_cm();
-        }
+        next_dest.z = pos_control->get_pos_desired_z_cm();
     }
 
     return true;
@@ -506,20 +492,11 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con
 
     // get distance from vehicle to start_pos
     const Vector2f curr_pos2d {inertial_nav.get_position_xy_cm()};
-    next_dest.x = curr_pos2d.x + (AB_side.x * scalar);
-    next_dest.y = curr_pos2d.y + (AB_side.y * scalar);
+    next_dest.xy() = curr_pos2d + (AB_side * scalar);
 
     // if we have a downward facing range finder then use terrain altitude targets
     terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy();
-    if (terrain_alt) {
-#if AP_RANGEFINDER_ENABLED
-        if (!copter.surface_tracking.get_target_alt_cm(next_dest.z)) {
-            next_dest.z = copter.rangefinder_state.alt_cm_filt.get();
-        }
-#endif
-    } else {
-        next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_desired_z_cm() : inertial_nav.get_position_z_up_cm();
-    }
+    next_dest.z = pos_control->get_pos_desired_z_cm();
 
     return true;
 }

ArduCopter/surface_tracking.cpp

@@ -43,34 +43,6 @@ void Copter::SurfaceTracking::update_surface_offset()
     }
 }
 
-
-// get target altitude (in cm) above ground
-// returns true if there is a valid target
-bool Copter::SurfaceTracking::get_target_alt_cm(float &target_alt_cm) const
-{
-    // fail if we are not tracking downwards
-    if (surface != Surface::GROUND) {
-        return false;
-    }
-    // check target has been updated recently
-    if (AP_HAL::millis() - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
-        return false;
-    }
-    target_alt_cm = (copter.pos_control->get_pos_target_z_cm() - copter.pos_control->get_pos_offset_z_cm());
-    return true;
-}
-
-// set target altitude (in cm) above ground
-void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm)
-{
-    // fail if we are not tracking downwards
-    if (surface != Surface::GROUND) {
-        return;
-    }
-    copter.pos_control->set_pos_terrain_target_cm(copter.inertial_nav.get_position_z_up_cm() - _target_alt_cm);
-    last_update_ms = AP_HAL::millis();
-}
-
 bool Copter::SurfaceTracking::get_target_dist_for_logging(float &target_dist) const
 {
     if (!valid_for_logging || (surface == Surface::NONE)) {
@@ -79,7 +51,7 @@ bool Copter::SurfaceTracking::get_target_dist_for_logging(float &target_dist) co
 
     const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
     // fix me
-    target_dist = dir * (copter.pos_control->get_pos_target_z_cm() - copter.pos_control->get_pos_offset_z_cm()) * 0.01f;
+    target_dist = dir * copter.pos_control->get_pos_desired_z_cm() * 0.01f;
     return true;
 }
 

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -565,6 +565,7 @@ void AC_PosControl::input_accel_xy(const Vector3f& accel)
 void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel, bool limit_output)
 {
     update_pos_vel_accel_xy(_pos_desired.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error());
+
     shape_vel_accel_xy(vel, accel, _vel_desired.xy(), _accel_desired.xy(),
         _accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, limit_output);
 
@@ -579,6 +580,7 @@ void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel, boo
 void AC_PosControl::input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, bool limit_output)
 {
     update_pos_vel_accel_xy(_pos_desired.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error());
+
     shape_pos_vel_accel_xy(pos, vel, accel, _pos_desired.xy(), _vel_desired.xy(), _accel_desired.xy(),
                            _vel_max_xy_cms, _accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, limit_output);
 
@@ -663,9 +665,9 @@ void AC_PosControl::update_xy_controller()
     // Position Controller
 
     _pos_target.xy() = _pos_desired.xy() + _pos_offset.xy();
-    const Vector3f &curr_pos = _inav.get_position_neu_cm();
 
     // determine the combined position of the actual position and the disturbance from system ID mode
+    const Vector3f &curr_pos = _inav.get_position_neu_cm();
     Vector3f comb_pos = curr_pos;
     comb_pos.xy() += _disturb_pos;
 
@@ -679,9 +681,9 @@ void AC_PosControl::update_xy_controller()
 
     _vel_target.xy() = vel_target;
     _vel_target.xy() += _vel_desired.xy() + _vel_offset.xy();
-    const Vector2f &curr_vel = _inav.get_velocity_xy_cms();
 
     // determine the combined velocity of the actual velocity and the disturbance from system ID mode
+    const Vector2f &curr_vel = _inav.get_velocity_xy_cms();
     Vector2f comb_vel = curr_vel;
     comb_vel += _disturb_vel;
 
@@ -888,8 +890,7 @@ void AC_PosControl::input_vel_accel_z(float &vel, float accel, bool limit_output
 ///     The zero target altitude is varied to follow pos_offset_z
 void AC_PosControl::set_pos_target_z_from_climb_rate_cm(float vel)
 {
-    float vel_temp = vel;
-    input_vel_accel_z(vel_temp, 0.0);
+    input_vel_accel_z(vel, 0.0);
 }
 
 /// land_at_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s
@@ -1235,7 +1236,7 @@ void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point) const
 {
     // todo: we should use the current target position and velocity if we are currently running the position controller
     stopping_point = _inav.get_position_xy_cm().topostype();
-    float kP = _p_pos_xy.kP();
+    stopping_point -= _pos_offset.xy();
 
     Vector2f curr_vel = _inav.get_velocity_xy_cms();
     curr_vel -= _vel_offset.xy();
@@ -1246,7 +1247,8 @@ void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point) const
     if (!is_positive(vel_total)) {
         return;
     }
-
+
+    float kP = _p_pos_xy.kP();
     const float stopping_dist = stopping_distance(constrain_float(vel_total, 0.0, _vel_max_xy_cms), kP, _accel_max_xy_cmss);
     if (!is_positive(stopping_dist)) {
         return;
@@ -1255,7 +1257,6 @@ void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point) const
     // convert the stopping distance into a stopping point using velocity vector
     const float t = stopping_dist / vel_total;
     stopping_point += (curr_vel * t).topostype();
-    stopping_point -= _pos_offset.xy();
 }
 
 /// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
@@ -1328,27 +1329,30 @@ void AC_PosControl::write_log()
                    _vel_desired.y, _vel_target.y, _inav.get_velocity_neu_cms().y,
                    _accel_desired.y, _accel_target.y, accel_y);
 
-        // log offsets if they have ever been used
-//        if (!_pos_offset_target.xy().is_zero()) {
+        // log offsets if they are being used
+        if (!_pos_offset.xy().is_zero()) {
             Write_PSCO(_pos_offset_target.xy(), _pos_offset.xy(), _vel_offset_target.xy(), _vel_offset.xy(), _accel_offset_target.xy(), _accel_offset.xy());
-//        }
+        }
     }
 
     if (is_active_z()) {
         Write_PSCD(-_pos_target.z, -_inav.get_position_z_up_cm(),
                    -_vel_desired.z, -_vel_target.z, -_inav.get_velocity_z_up_cms(),
                    -_accel_desired.z, -_accel_target.z, -get_z_accel_cmss());
-        AP::logger().Write("PSCV",
-                        "TimeUS,PTT,PT,VT,AT",
-                        "smmno",
-                        "F0000",
-                        "Qffff",
-                        AP_HAL::micros64(),
-                        double(_pos_terrain_target * 0.01),
-                        double(_pos_terrain * 0.01),
-                        double(_vel_terrain * 0.01),
-                        double(_accel_terrain * 0.01));
 
+        // log offsets if they are being used
+        if (!is_zero(_pos_terrain)) {
+            AP::logger().Write("PSCV",
+                            "TimeUS,PTT,PT,VT,AT",
+                            "smmno",
+                            "F0000",
+                            "Qffff",
+                            AP_HAL::micros64(),
+                            double(_pos_terrain_target * 0.01),
+                            double(_pos_terrain * 0.01),
+                            double(_vel_terrain * 0.01),
+                            double(_accel_terrain * 0.01));
+        }
     }
 }
 #endif  // HAL_LOGGING_ENABLED
@@ -1476,9 +1480,13 @@ void AC_PosControl::handle_ekf_xy_reset()
     uint32_t reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);
     if (reset_ms != _ekf_xy_reset_ms) {
 
-        //_pos_offset.xy() += _pos_target.xy() - (_inav.get_position_xy_cm() + _p_pos_xy.get_error()).topostype();
-        //_vel_offset.xy() += _vel_target.xy() - (_inav.get_velocity_xy_cms() + _pid_vel_xy.get_error());
+        // 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();
 
+        // 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();
         _pos_desired.xy() = _pos_target.xy() - _pos_offset.xy();
         _vel_target.xy() = _inav.get_velocity_xy_cms() + _pid_vel_xy.get_error();
@@ -1503,8 +1511,13 @@ 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) {
 
-        //_pos_offset.z += _pos_target.z - (_inav.get_position_z_up_cm() + _p_pos_z.get_error());
-        //_vel_offset.z += _vel_target.z - (_inav.get_velocity_z_up_cms() + _pid_vel_z.get_error());
+        // 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;
+
+        // 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();
         _pos_desired.z = _pos_target.z - (_pos_offset.z - _pos_terrain);
         _vel_target.z = _inav.get_velocity_z_up_cms() + _pid_vel_z.get_error();

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -297,10 +297,10 @@ class AC_PosControl
     /// set_vel_desired_xy_cms - sets horizontal desired velocity in NEU cm/s
     void set_vel_desired_xy_cms(const Vector2f &vel) {_vel_desired.xy() = vel; }
 
-    /// returns desired velocity (i.e. feed forward) in cm/s in NEU
+    /// get_vel_desired_cms - returns desired velocity in cm/s in NEU
     const Vector3f& get_vel_desired_cms() { return _vel_desired; }
 
-    // returns the target velocity (not including offsets) in NEU cm/s
+    // get_vel_target_cms - returns the target velocity in NEU cm/s
     const Vector3f& get_vel_target_cms() const { return _vel_target; }
 
     /// set_vel_desired_z_cms - sets desired velocity in cm/s in z axis
@@ -312,17 +312,22 @@ class AC_PosControl
 
     /// Acceleration
 
-    // set_accel_desired_xy_cmss set desired acceleration in cm/s in xy axis
+    // set_accel_desired_xy_cmss - set desired acceleration in cm/s in xy axis
     void set_accel_desired_xy_cmss(const Vector2f &accel_cms) { _accel_desired.xy() = accel_cms; }
 
-    // returns the target acceleration (not including offsets) in NEU cm/s/s
+    // get_accel_target_cmss - returns the target acceleration in NEU cm/s/s
     const Vector3f& get_accel_target_cmss() const { return _accel_target; }
 
 
     /// Terrain
 
+    // set_pos_terrain_target_cm - set target terrain altitude in cm
     void set_pos_terrain_target_cm(float pos_terrain_target) {_pos_terrain_target = pos_terrain_target;}
+
+    // init_pos_terrain_cm - initialises the current terrain altitude and target altitude to pos_offset_terrain_cm
     void init_pos_terrain_cm(float pos_offset_terrain_cm);
+
+    // get_pos_terrain_cm - returns the current terrain altitude in cm
     float get_pos_terrain_cm() { return _pos_terrain; }
 
 
@@ -342,7 +347,6 @@ class AC_PosControl
     void set_posvelaccel_offset_target_z_cm(float pos_offset_target_z_cm, float vel_offset_target_z_cms, float accel_offset_target_z_cmss);
 
     /// get the position, velocity or acceleration offets in cm from EKF origin in NEU frame
-    const Vector3p& get_pos_offset_target_cm() const { return _pos_offset_target; }
     const Vector3p& get_pos_offset_cm() const { return _pos_offset; }
     const Vector3f& get_vel_offset_cms() const { return _vel_offset; }
     const Vector3f& get_accel_offset_cmss() const { return _accel_offset; }

libraries/AC_WPNav/AC_Circle.cpp

@@ -314,12 +314,13 @@ void AC_Circle::init_start_angle(bool use_heading)
         _angle = wrap_PI(_ahrs.yaw-M_PI);
     } else {
         // if we are exactly at the center of the circle, init angle to directly behind vehicle (so vehicle will backup but not change heading)
-        const Vector3f &curr_pos_minus_offset = _inav.get_position_neu_cm() - _pos_control.get_pos_offset_cm().tofloat();
-        if (is_equal(curr_pos_minus_offset.x,float(_center.x)) && is_equal(curr_pos_minus_offset.y,float(_center.y))) {
+        // curr_pos_desired is the position before we add offsets and terrain
+        const Vector3f &curr_pos_desired= _pos_control.get_pos_desired_cm().tofloat();
+        if (is_equal(curr_pos_desired.x,float(_center.x)) && is_equal(curr_pos_desired.y,float(_center.y))) {
             _angle = wrap_PI(_ahrs.yaw-M_PI);
         } else {
             // get bearing from circle center to vehicle in radians
-            float bearing_rad = atan2f(curr_pos_minus_offset.y-_center.y, curr_pos_minus_offset.x-_center.x);
+            float bearing_rad = atan2f(curr_pos_desired.y-_center.y, curr_pos_desired.x-_center.x);
             _angle = wrap_PI(bearing_rad);
         }
     }

libraries/AC_WPNav/AC_Loiter.cpp

@@ -221,12 +221,10 @@ void AC_Loiter::calc_desired_velocity(bool avoidance_on)
     }
 
     // get loiters desired velocity from the position controller where it is being stored.
-    const Vector3f desired_vel_3d = _pos_control.get_vel_desired_cms();
-    Vector2f desired_vel{desired_vel_3d.x,desired_vel_3d.y};
+    Vector2f desired_vel = _pos_control.get_vel_desired_cms().xy();
 
     // update the desired velocity using our predicted acceleration
-    desired_vel.x += _predicted_accel.x * dt;
-    desired_vel.y += _predicted_accel.y * dt;
+    desired_vel += _predicted_accel * dt;
 
     Vector2f loiter_accel_brake;
     float desired_speed = desired_vel.length();
@@ -261,8 +259,7 @@ void AC_Loiter::calc_desired_velocity(bool avoidance_on)
     // Apply EKF limit to desired velocity -  this limit is calculated by the EKF and adjusted as required to ensure certain sensor limits are respected (eg optical flow sensing)
     float horizSpdDem = desired_vel.length();
     if (horizSpdDem > gnd_speed_limit_cms) {
-        desired_vel.x = desired_vel.x * gnd_speed_limit_cms / horizSpdDem;
-        desired_vel.y = desired_vel.y * gnd_speed_limit_cms / horizSpdDem;
+        desired_vel = desired_vel * gnd_speed_limit_cms / horizSpdDem;
     }
 
 #if AP_AVOIDANCE_ENABLED && !APM_BUILD_TYPE(APM_BUILD_ArduPlane)

libraries/AC_WPNav/AC_WPNav.cpp

@@ -465,7 +465,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
     }
     const float offset_z_scaler = _pos_control.pos_offset_z_scaler(terr_offset, get_terrain_margin() * 100.0);
 
-    // input shape the horizontal and terrain offsets
+    // input shape the terrain offset
     _pos_control.set_pos_terrain_target_cm(terr_offset);
 
     // get position controller's position offset (post input shaping) so it can be used in position error calculation