aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_Avoid: limit velocity and get-max-speed become public

Browse Source 
this allows AP_Follow to use these function
This commit is contained in:
Randy Mackay 2018-01-29 21:36:37 +09:00
parent f7751ec44a
commit be804aa74e
2 changed files with 44 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -229,6 +229,37 @@ void AC_Avoid::adjust_roll_pitch(float &roll, float &pitch, float veh_angle_max)
pitch = rp_out.y;
}
/*
* Limits the component of desired_vel in the direction of the unit vector
* limit_direction to be at most the maximum speed permitted by the limit_distance.
*
* Uses velocity adjustment idea from Randy's second email on this thread:
* https://groups.google.com/forum/#!searchin/drones-discuss/obstacle/drones-discuss/QwUXz__WuqY/qo3G8iTLSJAJ
*/
void AC_Avoid::limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f& limit_direction, float limit_distance, float dt) const
{
const float max_speed = get_max_speed(kP, accel_cmss, limit_distance, dt);
// project onto limit direction
const float speed = desired_vel * limit_direction;
if (speed > max_speed) {
// subtract difference between desired speed and maximum acceptable speed
desired_vel += limit_direction*(max_speed - speed);
}
}
/*
* Computes the speed such that the stopping distance
* of the vehicle will be exactly the input distance.
*/
float AC_Avoid::get_max_speed(float kP, float accel_cmss, float distance_cm, float dt) const
{
if (is_zero(kP)) {
return safe_sqrt(2.0f * distance_cm * accel_cmss);
} else {
return AC_AttitudeControl::sqrt_controller(distance_cm, kP, accel_cmss, dt);
}
}
/*
* Adjusts the desired velocity for the circular fence.
*/
@ -461,37 +492,6 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des
}
}
/*
* Limits the component of desired_vel in the direction of the unit vector
* limit_direction to be at most the maximum speed permitted by the limit_distance.
*
* Uses velocity adjustment idea from Randy's second email on this thread:
* https://groups.google.com/forum/#!searchin/drones-discuss/obstacle/drones-discuss/QwUXz__WuqY/qo3G8iTLSJAJ
*/
void AC_Avoid::limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f& limit_direction, float limit_distance, float dt) const
{
const float max_speed = get_max_speed(kP, accel_cmss, limit_distance, dt);
// project onto limit direction
const float speed = desired_vel * limit_direction;
if (speed > max_speed) {
// subtract difference between desired speed and maximum acceptable speed
desired_vel += limit_direction*(max_speed - speed);
}
}
/*
* Computes the speed such that the stopping distance
* of the vehicle will be exactly the input distance.
*/
float AC_Avoid::get_max_speed(float kP, float accel_cmss, float distance_cm, float dt) const
{
if (is_zero(kP)) {
return safe_sqrt(2.0f * distance_cm * accel_cmss);
} else {
return AC_AttitudeControl::sqrt_controller(distance_cm, kP, accel_cmss, dt);
}
}
/*
* Computes distance required to stop, given current speed.
*

29
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -62,6 +62,19 @@ public:
void proximity_avoidance_enable(bool on_off) { _proximity_enabled = on_off; }
bool proximity_avoidance_enabled() { return _proximity_enabled; }
// helper functions
// Limits the component of desired_vel in the direction of the unit vector
// limit_direction to be at most the maximum speed permitted by the limit_distance.
// uses velocity adjustment idea from Randy's second email on this thread:
// https://groups.google.com/forum/#!searchin/drones-discuss/obstacle/drones-discuss/QwUXz__WuqY/qo3G8iTLSJAJ
void limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f& limit_direction, float limit_distance, float dt) const;
// compute the speed such that the stopping distance of the vehicle will
// be exactly the input distance.
// kP should be non-zero for Copter which has a non-linear response
float get_max_speed(float kP, float accel_cmss, float distance_cm, float dt) const;
static const struct AP_Param::GroupInfo var_info[];
private:
@ -98,22 +111,6 @@ private:
*/
void adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f* boundary, uint16_t num_points, bool earth_frame, float margin, float dt);
/*
* Limits the component of desired_vel in the direction of the unit vector
* limit_direction to be at most the maximum speed permitted by the limit_distance.
*
* Uses velocity adjustment idea from Randy's second email on this thread:
* https://groups.google.com/forum/#!searchin/drones-discuss/obstacle/drones-discuss/QwUXz__WuqY/qo3G8iTLSJAJ
*/
void limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f& limit_direction, float limit_distance, float dt) const;
/*
* Computes the speed such that the stopping distance
* of the vehicle will be exactly the input distance.
* kP should be non-zero for Copter which has a non-linear response
*/
float get_max_speed(float kP, float accel_cmss, float distance_cm, float dt) const;
/*
* Computes distance required to stop, given current speed.
*/