diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index b0b153423c..c31ff9dc24 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -532,6 +532,19 @@ void AC_PosControl::set_xy_target(float x, float y) _pos_target.y = y; } +/// shift position target target in x, y axis +void AC_PosControl::shift_pos_xy_target(float x_cm, float y_cm) +{ + // move pos controller target + _pos_target.x += x_cm; + _pos_target.y += y_cm; + + // disable feed forward + if (!is_zero(x_cm) || !is_zero(y_cm)) { + freeze_ff_xy(); + } +} + /// set_target_to_stopping_point_xy - sets horizontal target to reasonable stopping position in cm from home void AC_PosControl::set_target_to_stopping_point_xy() { diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 9a74f033df..202f512ec8 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -209,6 +209,9 @@ public: /// set_xy_target in cm from home void set_xy_target(float x, float y); + /// shift position target target in x, y axis + void shift_pos_xy_target(float x_cm, float y_cm); + /// get_desired_velocity - returns xy desired velocity (i.e. feed forward) in cm/s in lat and lon direction const Vector3f& get_desired_velocity() { return _vel_desired; }