Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 10:38:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Avoidance: add adjust velocity by beacon fence

Browse Source
This commit is contained in:
Shingo Matsuura 2017-05-02 18:10:37 +09:00 committed by Randy Mackay
parent cded78022c
commit 7b0f6edf31
2 changed files with 46 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -5,10 +5,10 @@ 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,2:UseProximitySensor,3:All
// @Bitmask: 0:StopAtFence,1:UseProximitySensor
// @Values: 0:None,1:StopAtFence,2:UseProximitySensor,3:StopAtFence and UseProximitySensor,4:StopAtBeaconFence,7:All
// @Bitmask: 0:StopAtFence,1:UseProximitySensor,2:StopAtBeaconFence
// @User: Standard
AP_GROUPINFO("ENABLE", 1, AC_Avoid, _enabled, AC_AVOID_ALL),
AP_GROUPINFO("ENABLE", 1, AC_Avoid, _enabled, AC_AVOID_DEFAULT),
// @Param: ANGLE_MAX
// @DisplayName: Avoidance max lean angle in non-GPS flight modes
@ -38,11 +38,12 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = {
};
/// Constructor
AC_Avoid::AC_Avoid(const AP_AHRS& ahrs, const AP_InertialNav& inav, const AC_Fence& fence, const AP_Proximity& proximity)
AC_Avoid::AC_Avoid(const AP_AHRS& ahrs, const AP_InertialNav& inav, const AC_Fence& fence, const AP_Proximity& proximity, const AP_Beacon* beacon)
: _ahrs(ahrs),
_inav(inav),
_fence(fence),
_proximity(proximity)
_proximity(proximity),
_beacon(beacon)
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -62,6 +63,10 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel
adjust_velocity_polygon_fence(kP, accel_cmss_limited, desired_vel);
}
if ((_enabled & AC_AVOID_STOP_AT_BEACON_FENCE) > 0) {
adjust_velocity_beacon_fence(kP, accel_cmss_limited, desired_vel);
}
if ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) > 0 && _proximity_enabled) {
adjust_velocity_proximity(kP, accel_cmss_limited, desired_vel);
}
@ -255,6 +260,32 @@ void AC_Avoid::adjust_velocity_polygon_fence(float kP, float accel_cmss, Vector2
adjust_velocity_polygon(kP, accel_cmss, desired_vel, boundary, num_points, true, _fence.get_margin());
}
/*
* Adjusts the desired velocity for the beacon fence.
*/
void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f &desired_vel)
{
// exit if the beacon is not present
if (_beacon == nullptr) {
return;
}
// exit immediately if no desired velocity
if (desired_vel.is_zero()) {
return;
}
// get boundary from beacons
uint16_t num_points;
const Vector2f* boundary = _beacon->get_boundary_points(num_points);
if (boundary == nullptr || num_points == 0) {
return;
}
// adjust velocity using beacon
adjust_velocity_polygon(kP, accel_cmss, desired_vel, boundary, num_points, true, _fence.get_margin());
}
/*
* Adjusts the desired velocity based on output from the proximity sensor
*/

12
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -8,6 +8,7 @@
#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>
#include <AP_Beacon/AP_Beacon.h>
#define AC_AVOID_ACCEL_CMSS_MAX 100.0f // maximum acceleration/deceleration in cm/s/s used to avoid hitting fence
@ -15,7 +16,8 @@
#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
#define AC_AVOID_STOP_AT_BEACON_FENCE 4 // stop based on beacon perimeter
#define AC_AVOID_DEFAULT (AC_AVOID_STOP_AT_FENCE | AC_AVOID_USE_PROXIMITY_SENSOR)
// definitions for non-GPS avoidance
#define AC_AVOID_NONGPS_DIST_MAX_DEFAULT 10.0f // objects over 10m away are ignored (default value for DIST_MAX parameter)
@ -29,7 +31,7 @@ class AC_Avoid {
public:
/// Constructor
AC_Avoid(const AP_AHRS& ahrs, const AP_InertialNav& inav, const AC_Fence& fence, const AP_Proximity& proximity);
AC_Avoid(const AP_AHRS& ahrs, const AP_InertialNav& inav, const AC_Fence& fence, const AP_Proximity& proximity, const AP_Beacon* beacon = nullptr);
/*
* Adjusts the desired velocity so that the vehicle can stop
@ -65,6 +67,11 @@ private:
*/
void adjust_velocity_polygon_fence(float kP, float accel_cmss, Vector2f &desired_vel);
/*
* Adjusts the desired velocity for the beacon fence.
*/
void adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f &desired_vel);
/*
* Adjusts the desired velocity based on output from the proximity sensor
*/
@ -118,6 +125,7 @@ private:
const AP_InertialNav& _inav;
const AC_Fence& _fence;
const AP_Proximity& _proximity;
const AP_Beacon* _beacon;
// parameters
AP_Int8 _enabled;