diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 95844e89e2..2b2654c96c 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -741,7 +741,7 @@ void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const /// get_distance_to_target - get horizontal distance to target position in cm float AC_PosControl::get_distance_to_target() const { - return _distance_to_target; + return norm(_pos_error.x, _pos_error.y); } /// get_bearing_to_target - get bearing to target position in centi-degrees @@ -1016,7 +1016,6 @@ void AC_PosControl::run_xy_controller(float dt) _pos_target.x = curr_pos.x + _pos_error.x; _pos_target.y = curr_pos.y + _pos_error.y; } - _distance_to_target = norm(_pos_error.x, _pos_error.y); _vel_target = sqrt_controller(_pos_error, kP, _accel_cms); } diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 7ff1a4c4fe..0d3f0b4336 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -407,7 +407,6 @@ protected: Vector3f _accel_target; // acceleration target in cm/s/s Vector3f _accel_error; // acceleration error in cm/s/s Vector2f _vehicle_horiz_vel; // velocity to use if _flags.vehicle_horiz_vel_override is set - float _distance_to_target; // distance to position target - for reporting only LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error LowPassFilterVector2f _accel_target_filter; // acceleration target filter