Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_Avoid: add adjust_speed 

this method accepts a heading and speed instead of a velocity vector
This commit is contained in:
Randy Mackay 2017-12-13 18:16:07 +09:00
parent 9d74d82ff6
commit dfaabb543c
2 changed files with 29 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -87,6 +87,27 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector3f &desired_vel
desired_vel.y = des_vel_xy.y;
}
// adjust desired horizontal speed so that the vehicle stops before the fence or object
// accel (maximum acceleration/deceleration) is in m/s/s
// heading is in radians
// speed is in m/s
// kP should be zero for linear response, non-zero for non-linear response
void AC_Avoid::adjust_speed(float kP, float accel, float heading, float &speed, float dt)
{
// convert heading and speed into velocity vector
Vector2f vel_xy;
vel_xy.x = cosf(heading) * speed * 100.0f;
vel_xy.y = sinf(heading) * speed * 100.0f;
adjust_velocity(kP, accel * 100.0f, vel_xy, dt);
// adjust speed towards zero
if (is_negative(speed)) {
speed = -vel_xy.length() * 0.01f;
} else {
speed = vel_xy.length() * 0.01f;
}
}
// adjust vertical climb rate so vehicle does not break the vertical fence
void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt)
{

8
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -42,6 +42,14 @@ public:
void adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel, float dt);
void adjust_velocity(float kP, float accel_cmss, Vector3f &desired_vel, float dt);
// adjust desired horizontal speed so that the vehicle stops before the fence or object
// accel (maximum acceleration/deceleration) is in m/s/s
// heading is in radians
// speed is in m/s
// kP should be zero for linear response, non-zero for non-linear response
// dt is the time since the last call in seconds
void adjust_speed(float kP, float accel, float heading, float &speed, float dt);
// adjust vertical climb rate so vehicle does not break the vertical fence
void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt);