diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 15a28e8876..1d704f35cf 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -37,6 +37,15 @@ void AC_Avoid::adjust_velocity(const float kP, const float accel_cmss, Vector2f } } +// convenience function to accept Vector3f. Only x and y are adjusted +void AC_Avoid::adjust_velocity(const float kP, const float accel_cmss, Vector3f &desired_vel) +{ + Vector2f des_vel_xy(desired_vel.x, desired_vel.y); + adjust_velocity(kP, accel_cmss, des_vel_xy); + desired_vel.x = des_vel_xy.x; + desired_vel.y = des_vel_xy.y; +} + /* * Adjusts the desired velocity for the circular fence. */ diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index 62a52a4c29..bc4157f201 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -27,8 +27,10 @@ public: /* * Adjusts the desired velocity so that the vehicle can stop * before the fence/object. + * Note: Vector3f version is for convenience and only adjusts x and y axis */ void adjust_velocity(const float kP, const float accel_cmss, Vector2f &desired_vel); + void adjust_velocity(const float kP, const float accel_cmss, Vector3f &desired_vel); static const struct AP_Param::GroupInfo var_info[];