Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-03 22:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Avoidance: add adjust_velocity that accepts Vector3f for convenience

Browse Source 
We should actually modify this function to scale back the z axis in order to avoid breaching the vertical fence.  Currently breaching the vertical fence is handled within the position controller which is inconsistent.
This commit is contained in:
Randy Mackay 2016-07-25 12:16:26 +09:00
parent 692ff22453
commit f27cf8d388
2 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -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.
*/

2
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -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[];