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

AC_Avoidance: allow use of AP_Proximity

This commit is contained in:
Randy Mackay 2016-08-15 20:59:35 +09:00
parent c3087edbe8
commit 4808664fb6
2 changed files with 53 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -5,18 +5,20 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = {
// @Param: ENABLE
// @DisplayName: Avoidance control enable/disable
// @Description: Enabled/disable stopping at fence
// @Values: 0:None,1:StopAtFence
// @Values: 0:None,1:StopAtFence,2:UseProximitySensor,3:All
// @Bitmask: 0:StopAtFence,1:UseProximitySensor
// @User: Standard
AP_GROUPINFO("ENABLE", 1, AC_Avoid, _enabled, AC_AVOID_STOP_AT_FENCE),
AP_GROUPINFO("ENABLE", 1, AC_Avoid, _enabled, AC_AVOID_ALL),
AP_GROUPEND
};
/// Constructor
AC_Avoid::AC_Avoid(const AP_AHRS& ahrs, const AP_InertialNav& inav, const AC_Fence& fence)
AC_Avoid::AC_Avoid(const AP_AHRS& ahrs, const AP_InertialNav& inav, const AC_Fence& fence, const AP_Proximity& proximity)
: _ahrs(ahrs),
_inav(inav),
_fence(fence)
_fence(fence),
_proximity(proximity)
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -31,10 +33,14 @@ void AC_Avoid::adjust_velocity(const float kP, const float accel_cmss, Vector2f
// limit acceleration
float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX);
if (_enabled == AC_AVOID_STOP_AT_FENCE) {
if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0) {
adjust_velocity_circle(kP, accel_cmss_limited, desired_vel);
adjust_velocity_poly(kP, accel_cmss_limited, desired_vel);
}
if ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) > 0) {
adjust_velocity_proximity(kP, accel_cmss_limited, desired_vel);
}
}
// convenience function to accept Vector3f. Only x and y are adjusted
@ -138,7 +144,7 @@ void AC_Avoid::adjust_velocity_poly(const float kP, const float accel_cmss, Vect
// We are strictly inside the given edge.
// Adjust velocity to not violate this edge.
limit_direction /= limit_distance;
limit_velocity(kP, accel_cmss, safe_vel, limit_direction, limit_distance);
limit_velocity(kP, accel_cmss, safe_vel, limit_direction, MAX(limit_distance - get_margin(),0.0f));
} else {
// We are exactly on the edge - treat this as a fence breach.
// i.e. do not adjust velocity.
@ -149,6 +155,37 @@ void AC_Avoid::adjust_velocity_poly(const float kP, const float accel_cmss, Vect
desired_vel = safe_vel;
}
/*
* Adjusts the desired velocity based on output from the proximity sensor
*/
void AC_Avoid::adjust_velocity_proximity(const float kP, const float accel_cmss, Vector2f &desired_vel)
{
// exit immediately if proximity sensor is not present
if (_proximity.get_status() != AP_Proximity::Proximity_Good) {
return;
}
// exit immediately if no desired velocity
if (desired_vel.is_zero()) {
return;
}
// normalise desired velocity vector
Vector2f vel_dir = desired_vel.normalized();
// get angle of desired velocity
float heading_rad = atan2f(vel_dir.y, vel_dir.x);
// rotate desired velocity angle into body-frame angle
float heading_bf_rad = wrap_PI(heading_rad - _ahrs.yaw);
// get nearest object using body-frame angle and shorten desired velocity (which must remain in earth-frame)
float distance_m;
if (_proximity.get_horizontal_distance(degrees(heading_bf_rad), distance_m)) {
limit_velocity(kP, accel_cmss, desired_vel, vel_dir, MAX(distance_m*100.0f - 200.0f, 0.0f));
}
}
/*
* 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.
@ -158,7 +195,7 @@ void AC_Avoid::adjust_velocity_poly(const float kP, const float accel_cmss, Vect
*/
void AC_Avoid::limit_velocity(const float kP, const float accel_cmss, Vector2f &desired_vel, const Vector2f limit_direction, const float limit_distance) const
{
const float max_speed = get_max_speed(kP, accel_cmss, limit_distance - get_margin());
const float max_speed = get_max_speed(kP, accel_cmss, limit_distance);
// project onto limit direction
const float speed = desired_vel * limit_direction;
if (speed > max_speed) {

10
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -7,12 +7,15 @@
#include <AP_InertialNav/AP_InertialNav.h> // Inertial Navigation library
#include <AC_AttitudeControl/AC_AttitudeControl.h> // Attitude controller library for sqrt controller
#include <AC_Fence/AC_Fence.h> // Failsafe fence library
#include <AP_Proximity/AP_Proximity.h>
#define AC_AVOID_ACCEL_CMSS_MAX 100.0f // maximum acceleration/deceleration in cm/s/s used to avoid hitting fence
// bit masks for enabled fence types.
#define AC_AVOID_DISABLED 0 // avoidance disabled
#define AC_AVOID_STOP_AT_FENCE 1 // stop at fence
#define AC_AVOID_USE_PROXIMITY_SENSOR 2 // stop based on proximity sensor output
#define AC_AVOID_ALL 3 // use fence and promiximity sensor
/*
* This class prevents the vehicle from leaving a polygon fence in
@ -22,7 +25,7 @@ class AC_Avoid {
public:
/// Constructor
AC_Avoid(const AP_AHRS& ahrs, const AP_InertialNav& inav, const AC_Fence& fence);
AC_Avoid(const AP_AHRS& ahrs, const AP_InertialNav& inav, const AC_Fence& fence, const AP_Proximity& proximity);
/*
* Adjusts the desired velocity so that the vehicle can stop
@ -46,6 +49,10 @@ private:
*/
void adjust_velocity_poly(const float kP, const float accel_cmss, Vector2f &desired_vel);
/*
* Adjusts the desired velocity based on output from the proximity sensor
*/
void adjust_velocity_proximity(const float kP, const float accel_cmss, Vector2f &desired_vel);
/*
* Limits the component of desired_vel in the direction of the unit vector
@ -81,6 +88,7 @@ private:
const AP_AHRS& _ahrs;
const AP_InertialNav& _inav;
const AC_Fence& _fence;
const AP_Proximity& _proximity;
// parameters
AP_Int8 _enabled;